Skip to content

Commit 3d73cf3

Browse files
Acuadros95Acuadros95
authored
Update rclc parameter demo (#62)
* Update rclc parameter demo * Update demo
1 parent a46f295 commit 3d73cf3

File tree

1 file changed

+95
-66
lines changed

1 file changed

+95
-66
lines changed

‎rclc/parameter_server/main.c‎

Lines changed: 95 additions & 66 deletions
Original file line numberDiff line numberDiff line change
@@ -9,8 +9,10 @@
99
#include <stdio.h>
1010
#include <unistd.h>
1111

12-
#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); return 1;}}
13-
#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);}}
12+
#define RCCHECK(fn) {rcl_ret_t temp_rc = fn; if ((temp_rc != RCL_RET_OK)) {printf( \
13+
"Failed status on line %d: %d. Aborting.\n", __LINE__, (int)temp_rc); return 1;}}
14+
#define RCSOFTCHECK(fn) {rcl_ret_t temp_rc = fn; if ((temp_rc != RCL_RET_OK)) {printf( \
15+
"Failed status on line %d: %d. Continuing.\n", __LINE__, (int)temp_rc);}}
1416

1517
rcl_publisher_t publisher;
1618
rclc_parameter_server_t param_server;
@@ -20,77 +22,104 @@ std_msgs__msg__Int32 msg;
2022

2123
void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
2224
{
23-
(void) last_call_time;
24-
(void) timer;
25+
(void) last_call_time;
26+
(void) timer;
2527

26-
if (publish)
27-
{
28-
RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
29-
printf("Sent: %d\n", msg.data);
28+
if (publish) {
29+
RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
30+
printf("Sent: %d\n", msg.data);
3031

31-
msg.data++;
32-
}
32+
msg.data++;
33+
}
3334
}
3435

35-
void on_parameter_changed(Parameter * param)
36+
bool on_parameter_changed(const Parameter * old_param, const Parameter * new_param, void * context)
3637
{
37-
if (strcmp(param->name.data, "publish_toogle") == 0 && param->value.type == RCLC_PARAMETER_BOOL)
38-
{
39-
publish = param->value.bool_value;
40-
printf("Publish %s\n", (publish) ? "ON" : "OFF");
41-
}
42-
else if (strcmp(param->name.data, "publish_rate_ms") == 0 && param->value.type == RCLC_PARAMETER_INT)
43-
{
44-
int64_t old;
45-
RCSOFTCHECK(rcl_timer_exchange_period(&timer, RCL_MS_TO_NS(param->value.integer_value), &old));
46-
printf("Publish rate %ld ms\n", param->value.integer_value);
47-
}
38+
(void) context;
39+
40+
if (old_param == NULL && new_param == NULL) {
41+
printf("Callback error, both parameters are NULL\n");
42+
return false;
43+
}
44+
45+
bool ret = true;
46+
if (new_param == NULL) {
47+
printf("Delete parameter %s rejected\n", old_param->name.data);
48+
ret = false;
49+
} else if (strcmp(
50+
new_param->name.data,
51+
"publish_toogle") == 0 && new_param->value.type == RCLC_PARAMETER_BOOL)
52+
{
53+
publish = new_param->value.bool_value;
54+
printf("Publish %s\n", (publish) ? "ON" : "OFF");
55+
} else if (strcmp(
56+
new_param->name.data,
57+
"publish_rate_ms") == 0 && new_param->value.type == RCLC_PARAMETER_INT)
58+
{
59+
int64_t old;
60+
RCSOFTCHECK(rcl_timer_exchange_period(&timer, RCL_MS_TO_NS(new_param->value.integer_value), &old));
61+
printf("Publish rate %ld ms\n", new_param->value.integer_value);
62+
}
63+
64+
return ret;
4865
}
4966

5067
int main()
5168
{
52-
rcl_allocator_t allocator = rcl_get_default_allocator();
53-
rclc_support_t support;
54-
55-
// create init_options
56-
RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
57-
58-
// create node
59-
rcl_node_t node;
60-
RCCHECK(rclc_node_init_default(&node, "micro_ros_node", "", &support));
61-
62-
// create publisher
63-
RCCHECK(rclc_publisher_init_default(
64-
&publisher,
65-
&node,
66-
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
67-
"micro_ros_pub"));
68-
69-
// Create parameter service
70-
rclc_parameter_server_init_default(&param_server, &node);
71-
72-
// create timer,
73-
RCCHECK(rclc_timer_init_default(
74-
&timer,
75-
&support,
76-
RCL_MS_TO_NS(1000),
77-
timer_callback));
78-
79-
// create executor
80-
rclc_executor_t executor = rclc_executor_get_zero_initialized_executor();
81-
RCCHECK(rclc_executor_init(&executor, &support.context, RCLC_PARAMETER_EXECUTOR_HANDLES_NUMBER + 1, &allocator));
82-
RCCHECK(rclc_executor_add_parameter_server(&executor, &param_server, on_parameter_changed));
83-
RCCHECK(rclc_executor_add_timer(&executor, &timer));
84-
85-
// Add parameters
86-
rclc_add_parameter(&param_server, "publish_toogle", RCLC_PARAMETER_BOOL);
87-
rclc_add_parameter(&param_server, "publish_rate_ms", RCLC_PARAMETER_INT);
88-
89-
rclc_parameter_set_bool(&param_server, "publish_toogle", true);
90-
rclc_parameter_set_int(&param_server, "publish_rate_ms", 1000);
91-
92-
rclc_executor_spin(&executor);
93-
94-
RCCHECK(rcl_publisher_fini(&publisher, &node));
95-
RCCHECK(rcl_node_fini(&node));
69+
rcl_allocator_t allocator = rcl_get_default_allocator();
70+
rclc_support_t support;
71+
72+
// create init_options
73+
RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
74+
75+
// create node
76+
rcl_node_t node;
77+
RCCHECK(rclc_node_init_default(&node, "micro_ros_node", "", &support));
78+
79+
// create publisher
80+
RCCHECK(
81+
rclc_publisher_init_default(
82+
&publisher,
83+
&node,
84+
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
85+
"micro_ros_pub"));
86+
87+
// Create parameter service
88+
rclc_parameter_server_init_default(&param_server, &node);
89+
90+
// create timer,
91+
RCCHECK(
92+
rclc_timer_init_default(
93+
&timer,
94+
&support,
95+
RCL_MS_TO_NS(1000),
96+
timer_callback));
97+
98+
// create executor
99+
rclc_executor_t executor = rclc_executor_get_zero_initialized_executor();
100+
RCCHECK(
101+
rclc_executor_init(
102+
&executor, &support.context,
103+
RCLC_EXECUTOR_PARAMETER_SERVER_HANDLES + 1, &allocator));
104+
RCCHECK(rclc_executor_add_parameter_server(&executor, &param_server, on_parameter_changed));
105+
RCCHECK(rclc_executor_add_timer(&executor, &timer));
106+
107+
// Add parameters
108+
rclc_add_parameter(&param_server, "publish_toogle", RCLC_PARAMETER_BOOL);
109+
rclc_add_parameter(&param_server, "publish_rate_ms", RCLC_PARAMETER_INT);
110+
111+
// Add parameters constraints
112+
rclc_add_parameter_description(&param_server, "publish_toogle", "Publish ON/OFF parameter", "");
113+
rclc_add_parameter_description(
114+
&param_server, "publish_rate_ms", "Publish rate parameter",
115+
"Unit: milliseconds");
116+
117+
// Set parameter initial values
118+
rclc_parameter_set_bool(&param_server, "publish_toogle", true);
119+
rclc_parameter_set_int(&param_server, "publish_rate_ms", 1000);
120+
121+
rclc_executor_spin(&executor);
122+
123+
RCCHECK(rcl_publisher_fini(&publisher, &node));
124+
RCCHECK(rcl_node_fini(&node));
96125
}

0 commit comments

Comments
 (0)