Skip to content

Commit 800cd68

Browse files
pablogs9pablogs9
authored
Add multithread example (#82)
* Multithread example * Update * Update sleep
1 parent 15c9a3f commit 800cd68

9 files changed

Lines changed: 170 additions & 2 deletions

File tree

‎esp32_toolchain.cmake.in‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -27,7 +27,7 @@ set(CMAKE_CXX_FLAGS_INIT "@CXXFLAGS@" CACHE STRING "" FORCE)
2727
set(idf_path "@IDF_PATH@")
2828
set(idf_target "@IDF_TARGET@")
2929

30-
add_definitions(-DLWIP_IPV4 -DLWIP_IPV6)
30+
add_definitions(-DLWIP_IPV4 -DLWIP_IPV6 -DPLATFORM_NAME_FREERTOS)
3131

3232
include_directories(
3333
"@BUILD_CONFIG_DIR@"
@@ -48,6 +48,7 @@ include_directories(
4848
${idf_path}/components/vfs/include
4949
${idf_path}/components/log/include
5050
${idf_path}/components/freertos/include
51+
${idf_path}/components/freertos/include/freertos
5152
${idf_path}/components/freertos/${CMAKE_SYSTEM_PROCESSOR}/include
5253
${idf_path}/components/soc/soc/${idf_target}/include
5354
${idf_path}/components/wifi_provisioning/include
@@ -90,7 +91,7 @@ include_directories(
9091
${idf_path}/components/sdmmc/include
9192
${idf_path}/components/esp_local_ctrl/include
9293
${idf_path}/components/esp_common/include
93-
94+
9495
${idf_path}/components/lwip/lwip/src/include/lwip/apps
9596
${idf_path}/components/lwip/lwip/src/include/compat/posix
9697
${idf_path}/components/lwip/lwip/src/include

‎examples/multithread_publisher/.gitignore‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
build
2+
sdkconfig
3+
sdkconfig.old

‎examples/multithread_publisher/CMakeLists.txt‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
cmake_minimum_required(VERSION 3.5)
2+
3+
set (EXTRA_COMPONENT_DIRS "./../../.")
4+
5+
include($ENV{IDF_PATH}/tools/cmake/project.cmake)
6+
project(multithread_publisher)
7+

‎examples/multithread_publisher/app-colcon.meta‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
{
2+
"names": {
3+
"microxrcedds_client": {
4+
"cmake-args": [
5+
"-DUCLIENT_PROFILE_MULTITHREAD=ON"
6+
]
7+
},
8+
}
9+
}

‎examples/multithread_publisher/main/CMakeLists.txt‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
idf_component_register(SRCS main.c
2+
INCLUDE_DIRS ""
3+
)

‎examples/multithread_publisher/main/Kconfig.projbuild‎

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,15 @@
1+
menu "micro-ROS example-app settings"
2+
3+
config MICRO_ROS_APP_STACK
4+
int "Stack the micro-ROS app (Bytes)"
5+
default 16000
6+
help
7+
Stack size in Bytes of the micro-ROS app
8+
9+
config MICRO_ROS_APP_TASK_PRIO
10+
int "Priority of the micro-ROS app"
11+
default 5
12+
help
13+
Priority of micro-ros task higher value means higher priority
14+
15+
endmenu

‎examples/multithread_publisher/main/component.mk‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
#
2+
# "main" pseudo-component makefile.
3+
#
4+
# (Uses default behaviour of compiling all source files in directory, adding 'include' to include path.)
5+

‎examples/multithread_publisher/main/main.c‎

Lines changed: 120 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,120 @@
1+
#include <string.h>
2+
#include <stdio.h>
3+
#include <unistd.h>
4+
5+
#include "freertos/FreeRTOS.h"
6+
#include "freertos/task.h"
7+
#include "esp_log.h"
8+
#include "esp_system.h"
9+
10+
#include <uros_network_interfaces.h>
11+
#include <rcl/rcl.h>
12+
#include <rcl/error_handling.h>
13+
#include <std_msgs/msg/int32.h>
14+
#include <rclc/rclc.h>
15+
#include <rclc/executor.h>
16+
#include <rmw_microros/rmw_microros.h>
17+
#include "uxr/client/config.h"
18+
19+
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Aborting.\n",__LINE__,(int)temp_rc);vTaskDelete(NULL);}}
20+
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Continuing.\n",__LINE__,(int)temp_rc);}}
21+
22+
rcl_publisher_t publisher_1;
23+
rcl_publisher_t publisher_2;
24+
25+
void thread_1(void * arg)
26+
{
27+
std_msgs__msg__Int32 msg;
28+
msg.data = 0;
29+
while(1){
30+
RCSOFTCHECK(rcl_publish(&publisher_1, &msg, NULL));
31+
msg.data++;
32+
usleep(1000000);
33+
}
34+
}
35+
36+
void thread_2(void * arg)
37+
{
38+
std_msgs__msg__Int32 msg;
39+
msg.data = 0;
40+
while(1){
41+
RCSOFTCHECK(rcl_publish(&publisher_2, &msg, NULL));
42+
msg.data--;
43+
usleep(500000);
44+
}
45+
}
46+
47+
void micro_ros_task(void * arg)
48+
{
49+
rcl_allocator_t allocator = rcl_get_default_allocator();
50+
rclc_support_t support;
51+
52+
rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
53+
RCCHECK(rcl_init_options_init(&init_options, allocator));
54+
rmw_init_options_t* rmw_options = rcl_init_options_get_rmw_init_options(&init_options);
55+
56+
// Static Agent IP and port can be used instead of autodisvery.
57+
RCCHECK(rmw_uros_options_set_udp_address(CONFIG_MICRO_ROS_AGENT_IP, CONFIG_MICRO_ROS_AGENT_PORT, rmw_options));
58+
59+
// create init_options
60+
RCCHECK(rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator));
61+
62+
// create node
63+
rcl_node_t node;
64+
RCCHECK(rclc_node_init_default(&node, "multithread_node", "", &support));
65+
66+
// create two publishers
67+
RCCHECK(rclc_publisher_init_default(
68+
&publisher_1,
69+
&node,
70+
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
71+
"multithread_publisher_1"));
72+
73+
RCCHECK(rclc_publisher_init_default(
74+
&publisher_2,
75+
&node,
76+
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
77+
"multithread_publisher_2"));
78+
79+
80+
xTaskCreate(thread_1,
81+
"thread_1",
82+
CONFIG_MICRO_ROS_APP_STACK,
83+
NULL,
84+
CONFIG_MICRO_ROS_APP_TASK_PRIO,
85+
NULL);
86+
87+
xTaskCreate(thread_2,
88+
"thread_2",
89+
CONFIG_MICRO_ROS_APP_STACK,
90+
NULL,
91+
CONFIG_MICRO_ROS_APP_TASK_PRIO + 1,
92+
NULL);
93+
94+
while(1){
95+
sleep(100);
96+
}
97+
98+
// free resources
99+
RCCHECK(rcl_publisher_fini(&publisher_1, &node));
100+
RCCHECK(rcl_publisher_fini(&publisher_2, &node));
101+
RCCHECK(rcl_node_fini(&node));
102+
103+
vTaskDelete(NULL);
104+
}
105+
106+
void app_main(void)
107+
{
108+
#ifdef UCLIENT_PROFILE_UDP
109+
// Start the networking if required
110+
ESP_ERROR_CHECK(uros_network_interface_initialize());
111+
#endif // UCLIENT_PROFILE_UDP
112+
113+
//pin micro-ros task in APP_CPU to make PRO_CPU to deal with wifi:
114+
xTaskCreate(micro_ros_task,
115+
"uros_task",
116+
CONFIG_MICRO_ROS_APP_STACK,
117+
NULL,
118+
CONFIG_MICRO_ROS_APP_TASK_PRIO,
119+
NULL);
120+
}

‎examples/multithread_publisher/sdkconfig.defaults‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
CONFIG_ESP_MAIN_TASK_STACK_SIZE=3000
2+
CONFIG_ESP_TASK_WDT=n
3+
CONFIG_FREERTOS_SUPPORT_STATIC_ALLOCATION=y
4+
5+

0 commit comments

Comments
 (0)