From 29cf4a1c810414a234fac30c2e1a7c6938a89ebf Mon Sep 17 00:00:00 2001 From: acuadros95 Date: Thu, 7 Apr 2022 12:42:03 +0200 Subject: [PATCH 1/2] Update rclc parameter demo --- rclc/parameter_server/main.c | 156 ++++++++++++++++++++--------------- 1 file changed, 90 insertions(+), 66 deletions(-) diff --git a/rclc/parameter_server/main.c b/rclc/parameter_server/main.c index 5028672..75b3523 100644 --- a/rclc/parameter_server/main.c +++ b/rclc/parameter_server/main.c @@ -9,8 +9,10 @@ #include #include -#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;}} -#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);}} +#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;}} +#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);}} rcl_publisher_t publisher; rclc_parameter_server_t param_server; @@ -20,77 +22,99 @@ std_msgs__msg__Int32 msg; void timer_callback(rcl_timer_t * timer, int64_t last_call_time) { - (void) last_call_time; - (void) timer; + (void) last_call_time; + (void) timer; - if (publish) - { - RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL)); - printf("Sent: %d\n", msg.data); + if (publish) { + RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL)); + printf("Sent: %d\n", msg.data); - msg.data++; - } + msg.data++; + } } -void on_parameter_changed(Parameter * param) +bool on_parameter_changed(const Parameter * old_param, const Parameter * new_param, void * context) { - if (strcmp(param->name.data, "publish_toogle") == 0 && param->value.type == RCLC_PARAMETER_BOOL) - { - publish = param->value.bool_value; - printf("Publish %s\n", (publish) ? "ON" : "OFF"); - } - else if (strcmp(param->name.data, "publish_rate_ms") == 0 && param->value.type == RCLC_PARAMETER_INT) - { - int64_t old; - RCSOFTCHECK(rcl_timer_exchange_period(&timer, RCL_MS_TO_NS(param->value.integer_value), &old)); - printf("Publish rate %ld ms\n", param->value.integer_value); - } + (void) context; + + bool ret = true; + if (new_param == NULL) { + printf("Delete parameter %s rejected\n", old_param->name.data); + ret = false; + } else if (strcmp( + param->name.data, + "publish_toogle") == 0 && param->value.type == RCLC_PARAMETER_BOOL) + { + publish = param->value.bool_value; + printf("Publish %s\n", (publish) ? "ON" : "OFF"); + } else if (strcmp( + param->name.data, + "publish_rate_ms") == 0 && param->value.type == RCLC_PARAMETER_INT) + { + int64_t old; + RCSOFTCHECK(rcl_timer_exchange_period(&timer, RCL_MS_TO_NS(param->value.integer_value), &old)); + printf("Publish rate %ld ms\n", param->value.integer_value); + } + + return ret; } int main() { - rcl_allocator_t allocator = rcl_get_default_allocator(); - rclc_support_t support; - - // create init_options - RCCHECK(rclc_support_init(&support, 0, NULL, &allocator)); - - // create node - rcl_node_t node; - RCCHECK(rclc_node_init_default(&node, "micro_ros_node", "", &support)); - - // create publisher - RCCHECK(rclc_publisher_init_default( - &publisher, - &node, - ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), - "micro_ros_pub")); - - // Create parameter service - rclc_parameter_server_init_default(¶m_server, &node); - - // create timer, - RCCHECK(rclc_timer_init_default( - &timer, - &support, - RCL_MS_TO_NS(1000), - timer_callback)); - - // create executor - rclc_executor_t executor = rclc_executor_get_zero_initialized_executor(); - RCCHECK(rclc_executor_init(&executor, &support.context, RCLC_PARAMETER_EXECUTOR_HANDLES_NUMBER + 1, &allocator)); - RCCHECK(rclc_executor_add_parameter_server(&executor, ¶m_server, on_parameter_changed)); - RCCHECK(rclc_executor_add_timer(&executor, &timer)); - - // Add parameters - rclc_add_parameter(¶m_server, "publish_toogle", RCLC_PARAMETER_BOOL); - rclc_add_parameter(¶m_server, "publish_rate_ms", RCLC_PARAMETER_INT); - - rclc_parameter_set_bool(¶m_server, "publish_toogle", true); - rclc_parameter_set_int(¶m_server, "publish_rate_ms", 1000); - - rclc_executor_spin(&executor); - - RCCHECK(rcl_publisher_fini(&publisher, &node)); - RCCHECK(rcl_node_fini(&node)); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rclc_support_t support; + + // create init_options + RCCHECK(rclc_support_init(&support, 0, NULL, &allocator)); + + // create node + rcl_node_t node; + RCCHECK(rclc_node_init_default(&node, "micro_ros_node", "", &support)); + + // create publisher + RCCHECK( + rclc_publisher_init_default( + &publisher, + &node, + ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), + "micro_ros_pub")); + + // Create parameter service + rclc_parameter_server_init_default(¶m_server, &node); + + // create timer, + RCCHECK( + rclc_timer_init_default( + &timer, + &support, + RCL_MS_TO_NS(1000), + timer_callback)); + + // create executor + rclc_executor_t executor = rclc_executor_get_zero_initialized_executor(); + RCCHECK( + rclc_executor_init( + &executor, &support.context, + RCLC_PARAMETER_EXECUTOR_HANDLES_NUMBER + 1, &allocator)); + RCCHECK(rclc_executor_add_parameter_server(&executor, ¶m_server, on_parameter_changed)); + RCCHECK(rclc_executor_add_timer(&executor, &timer)); + + // Add parameters + rclc_add_parameter(¶m_server, "publish_toogle", RCLC_PARAMETER_BOOL); + rclc_add_parameter(¶m_server, "publish_rate_ms", RCLC_PARAMETER_INT); + + // Add parameters constraints + rclc_add_parameter_description(¶m_server, "publish_toogle", "Publish ON/OFF parameter", ""); + rclc_add_parameter_description( + ¶m_server, "publish_rate_ms", "Publish rate parameter", + "Unit: milliseconds"); + + // Set parameter initial values + rclc_parameter_set_bool(¶m_server, "publish_toogle", true); + rclc_parameter_set_int(¶m_server, "publish_rate_ms", 1000); + + rclc_executor_spin(&executor); + + RCCHECK(rcl_publisher_fini(&publisher, &node)); + RCCHECK(rcl_node_fini(&node)); } From 7539cb41185f070407d5ffa39f2de128761a72ee Mon Sep 17 00:00:00 2001 From: acuadros95 Date: Tue, 19 Apr 2022 12:21:21 +0000 Subject: [PATCH 2/2] Update demo --- rclc/parameter_server/main.c | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/rclc/parameter_server/main.c b/rclc/parameter_server/main.c index 75b3523..47a25bf 100644 --- a/rclc/parameter_server/main.c +++ b/rclc/parameter_server/main.c @@ -37,23 +37,28 @@ bool on_parameter_changed(const Parameter * old_param, const Parameter * new_par { (void) context; + if (old_param == NULL && new_param == NULL) { + printf("Callback error, both parameters are NULL\n"); + return false; + } + bool ret = true; if (new_param == NULL) { printf("Delete parameter %s rejected\n", old_param->name.data); ret = false; } else if (strcmp( - param->name.data, - "publish_toogle") == 0 && param->value.type == RCLC_PARAMETER_BOOL) + new_param->name.data, + "publish_toogle") == 0 && new_param->value.type == RCLC_PARAMETER_BOOL) { - publish = param->value.bool_value; + publish = new_param->value.bool_value; printf("Publish %s\n", (publish) ? "ON" : "OFF"); } else if (strcmp( - param->name.data, - "publish_rate_ms") == 0 && param->value.type == RCLC_PARAMETER_INT) + new_param->name.data, + "publish_rate_ms") == 0 && new_param->value.type == RCLC_PARAMETER_INT) { int64_t old; - RCSOFTCHECK(rcl_timer_exchange_period(&timer, RCL_MS_TO_NS(param->value.integer_value), &old)); - printf("Publish rate %ld ms\n", param->value.integer_value); + RCSOFTCHECK(rcl_timer_exchange_period(&timer, RCL_MS_TO_NS(new_param->value.integer_value), &old)); + printf("Publish rate %ld ms\n", new_param->value.integer_value); } return ret; @@ -95,7 +100,7 @@ int main() RCCHECK( rclc_executor_init( &executor, &support.context, - RCLC_PARAMETER_EXECUTOR_HANDLES_NUMBER + 1, &allocator)); + RCLC_EXECUTOR_PARAMETER_SERVER_HANDLES + 1, &allocator)); RCCHECK(rclc_executor_add_parameter_server(&executor, ¶m_server, on_parameter_changed)); RCCHECK(rclc_executor_add_timer(&executor, &timer));