From 130472bf716a737e657ecb65b49bbb425f3f9bf6 Mon Sep 17 00:00:00 2001 From: Pablo Garrido Date: Mon, 9 Nov 2020 13:39:35 +0100 Subject: [PATCH 1/3] Add configured examples with domain ID --- rclc/configuration_example/configured_publisher/main.c | 4 +++- rclc/configuration_example/configured_subscriber/main.c | 4 +++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/rclc/configuration_example/configured_publisher/main.c b/rclc/configuration_example/configured_publisher/main.c index 68e8387..1fe040e 100644 --- a/rclc/configuration_example/configured_publisher/main.c +++ b/rclc/configuration_example/configured_publisher/main.c @@ -42,7 +42,9 @@ int main(int argc, char * const argv[]) // create node rcl_node_t node = rcl_get_zero_initialized_node(); - RCCHECK(rclc_node_init_default(&node, "int32_configured_publisher_rclc", "", &support)); + rcl_node_options_t node_ops = rcl_node_get_default_options(); + node_ops.domain_id = 10; + RCCHECK(rclc_node_init_with_options(&node, "int32_configured_publisher_rclc", "", &support, &node_ops)); // create publisher RCCHECK(rclc_publisher_init_default( diff --git a/rclc/configuration_example/configured_subscriber/main.c b/rclc/configuration_example/configured_subscriber/main.c index 0205f98..249b49f 100644 --- a/rclc/configuration_example/configured_subscriber/main.c +++ b/rclc/configuration_example/configured_subscriber/main.c @@ -38,7 +38,9 @@ int main(int argc, const char * const * argv) // create node rcl_node_t node = rcl_get_zero_initialized_node(); - RCCHECK(rclc_node_init_default(&node, "int32_configured_subscriber_rclc", "", &support)); + rcl_node_options_t node_ops = rcl_node_get_default_options(); + node_ops.domain_id = 10; + RCCHECK(rclc_node_init_with_options(&node, "int32_configured_subscriber_rclc", "", &support, &node_ops)); // create subscriber RCCHECK(rclc_subscription_init_default( From 93b5e908d9341e69c6a854b8a90a5359bf4c820e Mon Sep 17 00:00:00 2001 From: Jose Antonio Moral Date: Tue, 10 Nov 2020 10:02:21 +0100 Subject: [PATCH 2/3] Allow ROS Domain ID to be configurable from CLI --- .../configured_publisher/main.c | 18 ++++++++++++++---- .../configured_subscriber/main.c | 16 +++++++++++++--- 2 files changed, 27 insertions(+), 7 deletions(-) diff --git a/rclc/configuration_example/configured_publisher/main.c b/rclc/configuration_example/configured_publisher/main.c index 1fe040e..cdc03f2 100644 --- a/rclc/configuration_example/configured_publisher/main.c +++ b/rclc/configuration_example/configured_publisher/main.c @@ -26,7 +26,13 @@ void timer_callback(rcl_timer_t * timer, int64_t last_call_time) } int main(int argc, char * const argv[]) -{ +{ + if (argc < 3 || argc > 4) + { + printf("Usage: configured_publisher \n"); + return 1; + } + rcl_allocator_t allocator = rcl_get_default_allocator(); rclc_support_t support; rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); @@ -43,8 +49,10 @@ int main(int argc, char * const argv[]) // create node rcl_node_t node = rcl_get_zero_initialized_node(); rcl_node_options_t node_ops = rcl_node_get_default_options(); - node_ops.domain_id = 10; - RCCHECK(rclc_node_init_with_options(&node, "int32_configured_publisher_rclc", "", &support, &node_ops)); + node_ops.domain_id = (size_t)(argc == 4 ? atoi(argv[3]) : 0); + const char * node_name = "int32_configured_publisher_rclc"; + printf("Initializing node '%s' with ROS Domain ID %ld...\n",node_name, node_ops.domain_id); + RCCHECK(rclc_node_init_with_options(&node, node_name, "", &support, &node_ops)); // create publisher RCCHECK(rclc_publisher_init_default( @@ -71,9 +79,11 @@ int main(int argc, char * const argv[]) RCCHECK(rclc_executor_add_timer(&executor, &timer)); msg.data = 0; - + rclc_executor_spin(&executor); RCCHECK(rcl_publisher_fini(&publisher, &node)); RCCHECK(rcl_node_fini(&node)); + + return 0; } diff --git a/rclc/configuration_example/configured_subscriber/main.c b/rclc/configuration_example/configured_subscriber/main.c index 249b49f..4a1290f 100644 --- a/rclc/configuration_example/configured_subscriber/main.c +++ b/rclc/configuration_example/configured_subscriber/main.c @@ -23,6 +23,12 @@ void subscription_callback(const void * msgin) int main(int argc, const char * const * argv) { + if (argc < 3 || argc > 4) + { + printf("Usage: configured_subscriber \n"); + return 1; + } + rcl_allocator_t allocator = rcl_get_default_allocator(); rclc_support_t support; rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); @@ -39,8 +45,10 @@ int main(int argc, const char * const * argv) // create node rcl_node_t node = rcl_get_zero_initialized_node(); rcl_node_options_t node_ops = rcl_node_get_default_options(); - node_ops.domain_id = 10; - RCCHECK(rclc_node_init_with_options(&node, "int32_configured_subscriber_rclc", "", &support, &node_ops)); + node_ops.domain_id = (size_t)(argc == 4 ? atoi(argv[3]) : 0); + const char * node_name = "int32_configured_subscriber_rclc"; + printf("Initializing node '%s' with ROS Domain ID %ld...\n", node_name, node_ops.domain_id); + RCCHECK(rclc_node_init_with_options(&node, node_name, "", &support, &node_ops)); // create subscriber RCCHECK(rclc_subscription_init_default( @@ -56,9 +64,11 @@ int main(int argc, const char * const * argv) unsigned int rcl_wait_timeout = 1000; // in ms RCCHECK(rclc_executor_set_timeout(&executor, RCL_MS_TO_NS(rcl_wait_timeout))); RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &msg, &subscription_callback, ON_NEW_DATA)); - + rclc_executor_spin(&executor); RCCHECK(rcl_subscription_fini(&subscriber, &node)); RCCHECK(rcl_node_fini(&node)); + + return 0; } From 3081a917f77e1d30d34084d7b537d524ab265ef2 Mon Sep 17 00:00:00 2001 From: Jose Antonio Moral Date: Tue, 10 Nov 2020 10:06:13 +0100 Subject: [PATCH 3/3] Fix indentation --- .../configured_publisher/main.c | 16 ++++++++-------- .../configured_subscriber/main.c | 16 ++++++++-------- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/rclc/configuration_example/configured_publisher/main.c b/rclc/configuration_example/configured_publisher/main.c index cdc03f2..943b1a4 100644 --- a/rclc/configuration_example/configured_publisher/main.c +++ b/rclc/configuration_example/configured_publisher/main.c @@ -27,11 +27,11 @@ void timer_callback(rcl_timer_t * timer, int64_t last_call_time) int main(int argc, char * const argv[]) { - if (argc < 3 || argc > 4) - { - printf("Usage: configured_publisher \n"); - return 1; - } + if (argc < 3 || argc > 4) + { + printf("Usage: configured_publisher \n"); + return 1; + } rcl_allocator_t allocator = rcl_get_default_allocator(); rclc_support_t support; @@ -50,8 +50,8 @@ int main(int argc, char * const argv[]) rcl_node_t node = rcl_get_zero_initialized_node(); rcl_node_options_t node_ops = rcl_node_get_default_options(); node_ops.domain_id = (size_t)(argc == 4 ? atoi(argv[3]) : 0); - const char * node_name = "int32_configured_publisher_rclc"; - printf("Initializing node '%s' with ROS Domain ID %ld...\n",node_name, node_ops.domain_id); + const char * node_name = "int32_configured_publisher_rclc"; + printf("Initializing node '%s' with ROS Domain ID %ld...\n",node_name, node_ops.domain_id); RCCHECK(rclc_node_init_with_options(&node, node_name, "", &support, &node_ops)); // create publisher @@ -85,5 +85,5 @@ int main(int argc, char * const argv[]) RCCHECK(rcl_publisher_fini(&publisher, &node)); RCCHECK(rcl_node_fini(&node)); - return 0; + return 0; } diff --git a/rclc/configuration_example/configured_subscriber/main.c b/rclc/configuration_example/configured_subscriber/main.c index 4a1290f..7925ce9 100644 --- a/rclc/configuration_example/configured_subscriber/main.c +++ b/rclc/configuration_example/configured_subscriber/main.c @@ -23,11 +23,11 @@ void subscription_callback(const void * msgin) int main(int argc, const char * const * argv) { - if (argc < 3 || argc > 4) - { - printf("Usage: configured_subscriber \n"); - return 1; - } + if (argc < 3 || argc > 4) + { + printf("Usage: configured_subscriber \n"); + return 1; + } rcl_allocator_t allocator = rcl_get_default_allocator(); rclc_support_t support; @@ -46,8 +46,8 @@ int main(int argc, const char * const * argv) rcl_node_t node = rcl_get_zero_initialized_node(); rcl_node_options_t node_ops = rcl_node_get_default_options(); node_ops.domain_id = (size_t)(argc == 4 ? atoi(argv[3]) : 0); - const char * node_name = "int32_configured_subscriber_rclc"; - printf("Initializing node '%s' with ROS Domain ID %ld...\n", node_name, node_ops.domain_id); + const char * node_name = "int32_configured_subscriber_rclc"; + printf("Initializing node '%s' with ROS Domain ID %ld...\n", node_name, node_ops.domain_id); RCCHECK(rclc_node_init_with_options(&node, node_name, "", &support, &node_ops)); // create subscriber @@ -70,5 +70,5 @@ int main(int argc, const char * const * argv) RCCHECK(rcl_subscription_fini(&subscriber, &node)); RCCHECK(rcl_node_fini(&node)); - return 0; + return 0; }