Skip to content

Commit 18d8831

Browse files
pablogs9pablogs9
authored
Initial param example (#72)
1 parent 9f69a26 commit 18d8831

8 files changed

Lines changed: 192 additions & 0 deletions

File tree

‎examples/parameters/.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/parameters/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(int32_pub_sub)
7+

‎examples/parameters/app-colcon.meta‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,14 @@
1+
{
2+
"names": {
3+
"rmw_microxrcedds": {
4+
"cmake-args": [
5+
"-DRMW_UXRCE_MAX_NODES=1",
6+
"-DRMW_UXRCE_MAX_PUBLISHERS=10",
7+
"-DRMW_UXRCE_MAX_SUBSCRIPTIONS=1",
8+
"-DRMW_UXRCE_MAX_SERVICES=10",
9+
"-DRMW_UXRCE_MAX_CLIENTS=1",
10+
"-DRMW_UXRCE_MAX_HISTORY=8"
11+
]
12+
}
13+
}
14+
}

‎examples/parameters/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/parameters/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/parameters/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/parameters/main/main.c‎

Lines changed: 142 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,142 @@
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 <rclc/rclc.h>
14+
#include <rclc/executor.h>
15+
#include <rclc_parameter/rclc_parameter.h>
16+
17+
#include <rmw_microros/rmw_microros.h>
18+
#include <uxr/client/config.h>
19+
20+
#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);}}
21+
#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);}}
22+
23+
rclc_parameter_server_t param_server;
24+
25+
void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
26+
{
27+
(void) timer;
28+
(void) last_call_time;
29+
30+
int value;
31+
rclc_parameter_get_int(&param_server, "param2", &value);
32+
value++;
33+
rclc_parameter_set_int(&param_server, "param2", (int64_t) value);
34+
}
35+
36+
void on_parameter_changed(Parameter * param)
37+
{
38+
printf("Parameter %s modified.", param->name.data);
39+
switch (param->value.type)
40+
{
41+
case RCLC_PARAMETER_BOOL:
42+
printf(" New value: %d (bool)", param->value.bool_value);
43+
break;
44+
case RCLC_PARAMETER_INT:
45+
printf(" New value: %lld (int)", param->value.integer_value);
46+
break;
47+
case RCLC_PARAMETER_DOUBLE:
48+
printf(" New value: %f (double)", param->value.double_value);
49+
break;
50+
default:
51+
break;
52+
}
53+
printf("\n");
54+
}
55+
56+
void micro_ros_task(void * arg)
57+
{
58+
59+
rcl_ret_t rc;
60+
rcl_allocator_t allocator = rcl_get_default_allocator();
61+
rclc_support_t support;
62+
63+
rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
64+
RCCHECK(rcl_init_options_init(&init_options, allocator));
65+
rmw_init_options_t* rmw_options = rcl_init_options_get_rmw_init_options(&init_options);
66+
67+
// Static Agent IP and port can be used instead of autodisvery.
68+
RCCHECK(rmw_uros_options_set_udp_address(CONFIG_MICRO_ROS_AGENT_IP, CONFIG_MICRO_ROS_AGENT_PORT, rmw_options));
69+
70+
// create init_options
71+
RCCHECK(rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator));
72+
73+
// Create node
74+
rcl_node_t node;
75+
rclc_node_init_default(&node, "esp32_param_node", "", &support);
76+
77+
// Create parameter service
78+
rclc_parameter_server_init_default(&param_server, &node);
79+
80+
// create timer,
81+
rcl_timer_t timer;
82+
rclc_timer_init_default(
83+
&timer,
84+
&support,
85+
RCL_MS_TO_NS(1000),
86+
timer_callback);
87+
88+
// Create executor
89+
rclc_executor_t executor;
90+
rclc_executor_init(&executor, &support.context, RCLC_PARAMETER_EXECUTOR_HANDLES_NUMBER + 1, &allocator);
91+
rclc_executor_add_parameter_server(&executor, &param_server, on_parameter_changed);
92+
rclc_executor_add_timer(&executor, &timer);
93+
94+
// Add parameters
95+
rclc_add_parameter(&param_server, "param1", RCLC_PARAMETER_BOOL);
96+
rclc_add_parameter(&param_server, "param2", RCLC_PARAMETER_INT);
97+
rclc_add_parameter(&param_server, "param3", RCLC_PARAMETER_DOUBLE);
98+
99+
rclc_parameter_set_bool(&param_server, "param1", false);
100+
rclc_parameter_set_int(&param_server, "param2", 10);
101+
rclc_parameter_set_double(&param_server, "param3", 0.01);
102+
103+
bool param1;
104+
int param2;
105+
double param3;
106+
107+
rclc_parameter_get_bool(&param_server, "param1", &param1);
108+
rclc_parameter_get_int(&param_server, "param2", &param2);
109+
rclc_parameter_get_double(&param_server, "param3", &param3);
110+
111+
while(1){
112+
rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
113+
usleep(100000);
114+
}
115+
116+
// clean up
117+
rc = rclc_executor_fini(&executor);
118+
rc += rclc_parameter_server_fini(&param_server, &node);
119+
rc += rcl_node_fini(&node);
120+
121+
if (rc != RCL_RET_OK) {
122+
printf("Error while cleaning up!\n");
123+
}
124+
125+
vTaskDelete(NULL);
126+
}
127+
128+
void app_main(void)
129+
{
130+
#ifdef UCLIENT_PROFILE_UDP
131+
// Start the networking if required
132+
ESP_ERROR_CHECK(uros_network_interface_initialize());
133+
#endif // UCLIENT_PROFILE_UDP
134+
135+
//pin micro-ros task in APP_CPU to make PRO_CPU to deal with wifi:
136+
xTaskCreate(micro_ros_task,
137+
"uros_task",
138+
CONFIG_MICRO_ROS_APP_STACK,
139+
NULL,
140+
CONFIG_MICRO_ROS_APP_TASK_PRIO,
141+
NULL);
142+
}

‎examples/parameters/sdkconfig.defaults‎

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

0 commit comments

Comments
 (0)