From b62b33ce005e591ccd8e712ab6ef5ae69422589b Mon Sep 17 00:00:00 2001 From: Borja Outerelo Date: Thu, 26 Dec 2019 10:07:08 +0100 Subject: [PATCH 1/9] Add configuration examples. --- rcl/CMakeLists.txt | 38 +++++--- .../configured_publisher/CMakeLists.txt | 20 ++++ .../configured_publisher/main.c | 92 +++++++++++++++++++ .../configured_subscriber/CMakeLists.txt | 20 ++++ .../configured_subscriber/main.c | 91 ++++++++++++++++++ rcl/int32_publisher/CMakeLists.txt | 2 + rcl/int32_publisher/main.c | 3 + rcl/int32_subscriber/CMakeLists.txt | 2 + rcl/int32_subscriber/main.c | 2 + rcl/package.xml | 15 +-- 10 files changed, 264 insertions(+), 21 deletions(-) create mode 100644 rcl/configuration_example/configured_publisher/CMakeLists.txt create mode 100644 rcl/configuration_example/configured_publisher/main.c create mode 100644 rcl/configuration_example/configured_subscriber/CMakeLists.txt create mode 100644 rcl/configuration_example/configured_subscriber/main.c diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index 734bb99..3fe0ccc 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -5,26 +5,34 @@ project(micro_ros_demos_rcl) find_package(ament_cmake REQUIRED) include(ExternalProject) +include(CMakeParseArguments) -function(export_executable subdirectory) -ExternalProject_Add(${subdirectory} - PREFIX - ${PROJECT_BINARY_DIR}/${subdirectory} - SOURCE_DIR - ${PROJECT_SOURCE_DIR}/${subdirectory} - INSTALL_DIR - ${PROJECT_BINARY_DIR}/temp_install - BUILD_ALWAYS 1 - CMAKE_CACHE_ARGS - -DCMAKE_INSTALL_PREFIX:PATH= - -DBUILD_SHARED_LIBS:BOOL=${BUILD_SHARED_LIBS} - -DCMAKE_PREFIX_PATH:PATH=${CMAKE_PREFIX_PATH} - -DCMAKE_BUILD_TYPE:STRING=${CMAKE_BUILD_TYPE} - ) +function(export_executable) + set(subtree "") + foreach(arg IN LISTS ARGN) + set(subtree ${subtree}/${arg}) + endforeach() + get_filename_component(leaf ${subtree} NAME) + ExternalProject_Add(${leaf} + PREFIX + ${PROJECT_BINARY_DIR}${subtree} + SOURCE_DIR + ${PROJECT_SOURCE_DIR}${subtree} + INSTALL_DIR + ${PROJECT_BINARY_DIR}/temp_install + BUILD_ALWAYS 1 + CMAKE_CACHE_ARGS + -DCMAKE_INSTALL_PREFIX:PATH= + -DBUILD_SHARED_LIBS:BOOL=${BUILD_SHARED_LIBS} + -DCMAKE_PREFIX_PATH:PATH=${CMAKE_PREFIX_PATH} + -DCMAKE_BUILD_TYPE:STRING=${CMAKE_BUILD_TYPE} + ) endfunction() export_executable(int32_publisher) export_executable(int32_subscriber) +export_executable(configuration_example configured_publisher) +export_executable(configuration_example configured_subscriber) export_executable(addtwoints_server) export_executable(addtwoints_client) export_executable(fibonacci_action_server) diff --git a/rcl/configuration_example/configured_publisher/CMakeLists.txt b/rcl/configuration_example/configured_publisher/CMakeLists.txt new file mode 100644 index 0000000..6042207 --- /dev/null +++ b/rcl/configuration_example/configured_publisher/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 3.5) +set(CMAKE_C_CLANG_TIDY clang-tidy -checks=*) + +project(configured_publisher) + +find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(rmw_microxrcedds REQUIRED) +find_package(rcl REQUIRED) + +add_executable(${PROJECT_NAME} main.c) +ament_target_dependencies(${PROJECT_NAME} + geometry_msgs + rmw_microxrcedds + rcl + ) + +install(TARGETS ${PROJECT_NAME} + DESTINATION ${PROJECT_NAME} + ) \ No newline at end of file diff --git a/rcl/configuration_example/configured_publisher/main.c b/rcl/configuration_example/configured_publisher/main.c new file mode 100644 index 0000000..342ab6a --- /dev/null +++ b/rcl/configuration_example/configured_publisher/main.c @@ -0,0 +1,92 @@ +// Copyright 2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#include +#include + +#include +#include +#include + +int main(int argc, char * const argv[]) +{ + rcl_ret_t ret; + + //Init RCL context + rcl_context_t context; + rcl_init_options_t init_options; + + init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + + printf("Connecting to agent %s:%d\n",argv[1],atoi(argv[2])); + rmw_init_options_t* rmw_options = rcl_init_options_get_rmw_init_options(&init_options); + ret = rmw_uros_init_options(argc, argv, rmw_options); + + context = rcl_get_zero_initialized_context(); + + ret = rcl_init(0, NULL, &init_options, &context); + + //Init Node + rcl_node_t node = rcl_get_zero_initialized_node(); + rcl_node_options_t node_ops = rcl_node_get_default_options(); + + ret = rcl_node_init(&node, "vector3_publisher", "", &context, &node_ops); + + //Init Publisher + const char* topic_name = "sample_vector3"; + + rcl_publisher_t publisher_vector3 = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * pub_type_support = ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Vector3); + rcl_publisher_options_t pub_opt = rcl_publisher_get_default_options(); + + ret = rcl_publisher_init(&publisher_vector3, &node, pub_type_support, topic_name, &pub_opt); + + geometry_msgs__msg__Vector3 topic_data; + geometry_msgs__msg__Vector3__init(&topic_data); + + // Spin + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + ret = rcl_wait_set_init(&wait_set, 1, 0, 0, 0, 0, 0, &context, rcl_get_default_allocator()); + + float values[3]; + for(uint8_t i = 0; i < 3; i++){ + values[i] = i; + } + + topic_data.x = values[0]; + topic_data.y = values[1]; + topic_data.z = values[2]; + + while (rcl_context_is_valid(&context)) + { + ret = rcl_publish( &publisher_vector3, (const void *) &topic_data, NULL); + + for(uint8_t i = 0; i < 3; i++){ + values[i]++; + } + + topic_data.x = values[0]; + topic_data.y = values[1]; + topic_data.z = values[2]; + + printf("Publish: %f, %f, %f\n",topic_data.x,topic_data.y,topic_data.z); + + sleep(1); + } + + ret = rcl_node_fini(&node); + +} diff --git a/rcl/configuration_example/configured_subscriber/CMakeLists.txt b/rcl/configuration_example/configured_subscriber/CMakeLists.txt new file mode 100644 index 0000000..734f3bc --- /dev/null +++ b/rcl/configuration_example/configured_subscriber/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 3.5) +set(CMAKE_C_CLANG_TIDY clang-tidy -checks=*) + +project(configured_subscriber) + +find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(rmw_microxrcedds REQUIRED) +find_package(rcl REQUIRED) + +add_executable(${PROJECT_NAME} main.c) +ament_target_dependencies(${PROJECT_NAME} + geometry_msgs + rmw_microxrcedds + rcl + ) + +install(TARGETS ${PROJECT_NAME} + DESTINATION ${PROJECT_NAME} + ) diff --git a/rcl/configuration_example/configured_subscriber/main.c b/rcl/configuration_example/configured_subscriber/main.c new file mode 100644 index 0000000..12ca178 --- /dev/null +++ b/rcl/configuration_example/configured_subscriber/main.c @@ -0,0 +1,91 @@ +// Copyright 2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#include +#include + +#include +#include +#include + +int main(int argc, char * argv[]) +{ + rcl_ret_t ret; + + //Init RCL context + rcl_context_t context; + rcl_init_options_t init_options; + + init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + + printf("Connecting to agent %s:%d\n",argv[1],atoi(argv[2])); + rmw_init_options_t* rmw_options = rcl_init_options_get_rmw_init_options(&init_options); + ret = rmw_uros_init_options(argc, argv, rmw_options); + + context = rcl_get_zero_initialized_context(); + + ret = rcl_init(0, NULL, &init_options, &context); + + //Init Node + rcl_node_t node = rcl_get_zero_initialized_node(); + rcl_node_options_t node_ops = rcl_node_get_default_options(); + + ret = rcl_node_init(&node, "vector3_subscriber", "", &context, &node_ops); + + //Init Subscriber + const char* topic_name = "sample_vector3"; + rcl_subscription_t subscriber_vector3 = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t subs_ops = rcl_subscription_get_default_options(); + + const rosidl_message_type_support_t * sub_type_support = ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Vector3); + + ret = rcl_subscription_init(&subscriber_vector3, &node, sub_type_support, topic_name, &subs_ops); + + geometry_msgs__msg__Vector3 topic_data; + geometry_msgs__msg__Vector3__init(&topic_data); + + // Spin + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + ret = rcl_wait_set_init(&wait_set, 1, 0, 0, 0, 0, 0, &context, rcl_get_default_allocator()); + + uint32_t timeout_ms = 500; + + while (rcl_context_is_valid(&context)) + { + // get empty wait set + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + ret = rcl_wait_set_init(&wait_set, 1, 0, 0, 0, 0, 0, &context, rcl_get_default_allocator()); + + // set rmw fields to NULL + ret = rcl_wait_set_clear(&wait_set); + + size_t index = 0; + ret = rcl_wait_set_add_subscription(&wait_set, &subscriber_vector3, &index); + + ret = rcl_wait(&wait_set, RCL_MS_TO_NS(timeout_ms)); + + if (ret == RCL_RET_OK && wait_set.subscriptions[0]) { + rmw_message_info_t messageInfo; + ret = rcl_take(wait_set.subscriptions[0], &topic_data, &messageInfo, NULL); + printf("Publish: %f, %f, %f\n",topic_data.x,topic_data.y,topic_data.z); + } + + rcl_wait_set_fini(&wait_set); + } + + ret = rcl_node_fini(&node); + +} diff --git a/rcl/int32_publisher/CMakeLists.txt b/rcl/int32_publisher/CMakeLists.txt index 3f3d88d..455dac9 100644 --- a/rcl/int32_publisher/CMakeLists.txt +++ b/rcl/int32_publisher/CMakeLists.txt @@ -5,12 +5,14 @@ project(int32_publisher LANGUAGES C) find_package(ament_cmake REQUIRED) find_package(rcl REQUIRED) find_package(std_msgs REQUIRED) +find_package(rmw_microxrcedds REQUIRED) add_executable(${PROJECT_NAME} main.c) ament_target_dependencies(${PROJECT_NAME} rcl std_msgs + rmw_microxrcedds ) install(TARGETS ${PROJECT_NAME} diff --git a/rcl/int32_publisher/main.c b/rcl/int32_publisher/main.c index 5fb1d07..38fc935 100644 --- a/rcl/int32_publisher/main.c +++ b/rcl/int32_publisher/main.c @@ -2,6 +2,8 @@ #include #include +#include + #include #include @@ -17,6 +19,7 @@ int main(int argc, const char * const * argv) RCCHECK(rcl_init(argc, argv, &options, &context)) rcl_node_options_t node_ops = rcl_node_get_default_options(); + rcl_node_t node = rcl_get_zero_initialized_node(); RCCHECK(rcl_node_init(&node, "int32_publisher_rcl", "", &context, &node_ops)) diff --git a/rcl/int32_subscriber/CMakeLists.txt b/rcl/int32_subscriber/CMakeLists.txt index 1bab916..4addd6b 100644 --- a/rcl/int32_subscriber/CMakeLists.txt +++ b/rcl/int32_subscriber/CMakeLists.txt @@ -5,12 +5,14 @@ project(int32_subscriber LANGUAGES C) find_package(ament_cmake REQUIRED) find_package(rcl REQUIRED) find_package(std_msgs REQUIRED) +find_package(rmw_microxrcedds REQUIRED) add_executable(${PROJECT_NAME} main.c) ament_target_dependencies(${PROJECT_NAME} rcl std_msgs + rmw_microxrcedds ) install(TARGETS ${PROJECT_NAME} diff --git a/rcl/int32_subscriber/main.c b/rcl/int32_subscriber/main.c index e367eda..6d4a5c7 100644 --- a/rcl/int32_subscriber/main.c +++ b/rcl/int32_subscriber/main.c @@ -2,6 +2,8 @@ #include #include +#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;}} diff --git a/rcl/package.xml b/rcl/package.xml index 894cc7c..1126c50 100644 --- a/rcl/package.xml +++ b/rcl/package.xml @@ -2,11 +2,11 @@ micro_ros_demos_rcl - 0.0.1 - Example of a publisher using int32 - Julian Bermudez + 0.0.2 + Examples using RCL API + Borja Outerelo Apache License 2.0 - Julian Bermudez + Borja Outerelo ament_cmake @@ -15,9 +15,12 @@ std_msgs example_interfaces + + geometry_msgs + rmw_microxrcedds + ament_cmake - - + \ No newline at end of file From 5f6571229cbec69767d00e203c5b0e6004a2c419 Mon Sep 17 00:00:00 2001 From: Pablo Garrido Date: Tue, 7 Jan 2020 12:09:24 +0100 Subject: [PATCH 2/9] Added configuration examples --- rcl/configuration_example/configured_publisher/main.c | 2 +- rcl/configuration_example/configured_subscriber/main.c | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rcl/configuration_example/configured_publisher/main.c b/rcl/configuration_example/configured_publisher/main.c index 342ab6a..1432cfb 100644 --- a/rcl/configuration_example/configured_publisher/main.c +++ b/rcl/configuration_example/configured_publisher/main.c @@ -33,7 +33,7 @@ int main(int argc, char * const argv[]) printf("Connecting to agent %s:%d\n",argv[1],atoi(argv[2])); rmw_init_options_t* rmw_options = rcl_init_options_get_rmw_init_options(&init_options); - ret = rmw_uros_init_options(argc, argv, rmw_options); + ret = rmw_uros_options_set_udp_address(argv[1], argv[2], rmw_options); context = rcl_get_zero_initialized_context(); diff --git a/rcl/configuration_example/configured_subscriber/main.c b/rcl/configuration_example/configured_subscriber/main.c index 12ca178..2e5c393 100644 --- a/rcl/configuration_example/configured_subscriber/main.c +++ b/rcl/configuration_example/configured_subscriber/main.c @@ -33,7 +33,7 @@ int main(int argc, char * argv[]) printf("Connecting to agent %s:%d\n",argv[1],atoi(argv[2])); rmw_init_options_t* rmw_options = rcl_init_options_get_rmw_init_options(&init_options); - ret = rmw_uros_init_options(argc, argv, rmw_options); + ret = rmw_uros_options_set_udp_address(argv[1], argv[2], rmw_options); context = rcl_get_zero_initialized_context(); @@ -77,10 +77,10 @@ int main(int argc, char * argv[]) ret = rcl_wait(&wait_set, RCL_MS_TO_NS(timeout_ms)); - if (ret == RCL_RET_OK && wait_set.subscriptions[0]) { + if (ret == RCL_RET_OK && wait_set.subscriptions[0]) { rmw_message_info_t messageInfo; ret = rcl_take(wait_set.subscriptions[0], &topic_data, &messageInfo, NULL); - printf("Publish: %f, %f, %f\n",topic_data.x,topic_data.y,topic_data.z); + printf("Received: %f, %f, %f\n",topic_data.x,topic_data.y,topic_data.z); } rcl_wait_set_fini(&wait_set); From b7f764b049e59e729b6185a87ff822d93119023d Mon Sep 17 00:00:00 2001 From: Pablo Garrido Date: Wed, 15 Jan 2020 08:45:59 +0100 Subject: [PATCH 3/9] Client key API --- rcl/configuration_example/configured_publisher/main.c | 3 ++- rcl/configuration_example/configured_subscriber/main.c | 1 + 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/rcl/configuration_example/configured_publisher/main.c b/rcl/configuration_example/configured_publisher/main.c index 1432cfb..58cb191 100644 --- a/rcl/configuration_example/configured_publisher/main.c +++ b/rcl/configuration_example/configured_publisher/main.c @@ -33,7 +33,8 @@ int main(int argc, char * const argv[]) printf("Connecting to agent %s:%d\n",argv[1],atoi(argv[2])); rmw_init_options_t* rmw_options = rcl_init_options_get_rmw_init_options(&init_options); - ret = rmw_uros_options_set_udp_address(argv[1], argv[2], rmw_options); + rmw_uros_options_set_udp_address(argv[1], argv[2], rmw_options); + rmw_uros_options_set_client_key(0xBA5EBA11, rmw_options); context = rcl_get_zero_initialized_context(); diff --git a/rcl/configuration_example/configured_subscriber/main.c b/rcl/configuration_example/configured_subscriber/main.c index 2e5c393..81f19aa 100644 --- a/rcl/configuration_example/configured_subscriber/main.c +++ b/rcl/configuration_example/configured_subscriber/main.c @@ -34,6 +34,7 @@ int main(int argc, char * argv[]) printf("Connecting to agent %s:%d\n",argv[1],atoi(argv[2])); rmw_init_options_t* rmw_options = rcl_init_options_get_rmw_init_options(&init_options); ret = rmw_uros_options_set_udp_address(argv[1], argv[2], rmw_options); + rmw_uros_options_set_client_key(0xCAFEBABE, rmw_options); context = rcl_get_zero_initialized_context(); From 811b154505d6a2426094205ba880420d0bec46a9 Mon Sep 17 00:00:00 2001 From: Pablo Garrido Date: Wed, 15 Jan 2020 09:45:17 +0100 Subject: [PATCH 4/9] Fix --- rcl/configuration_example/configured_subscriber/main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rcl/configuration_example/configured_subscriber/main.c b/rcl/configuration_example/configured_subscriber/main.c index 81f19aa..44b3bde 100644 --- a/rcl/configuration_example/configured_subscriber/main.c +++ b/rcl/configuration_example/configured_subscriber/main.c @@ -33,7 +33,7 @@ int main(int argc, char * argv[]) printf("Connecting to agent %s:%d\n",argv[1],atoi(argv[2])); rmw_init_options_t* rmw_options = rcl_init_options_get_rmw_init_options(&init_options); - ret = rmw_uros_options_set_udp_address(argv[1], argv[2], rmw_options); + rmw_uros_options_set_udp_address(argv[1], argv[2], rmw_options); rmw_uros_options_set_client_key(0xCAFEBABE, rmw_options); context = rcl_get_zero_initialized_context(); From 086be097e4a09a0953c00689de7b458eb3dc073d Mon Sep 17 00:00:00 2001 From: Pablo Garrido Date: Wed, 15 Jan 2020 16:28:28 +0100 Subject: [PATCH 5/9] Examples cleanup ad error code check --- .../configured_publisher/main.c | 33 ++++++++----------- .../configured_subscriber/main.c | 28 ++++++++-------- 2 files changed, 27 insertions(+), 34 deletions(-) diff --git a/rcl/configuration_example/configured_publisher/main.c b/rcl/configuration_example/configured_publisher/main.c index 58cb191..fbd0d16 100644 --- a/rcl/configuration_example/configured_publisher/main.c +++ b/rcl/configuration_example/configured_publisher/main.c @@ -22,29 +22,27 @@ int main(int argc, char * const argv[]) { - rcl_ret_t ret; - //Init RCL context rcl_context_t context; rcl_init_options_t init_options; init_options = rcl_get_zero_initialized_init_options(); - ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + RCCHECK(rcl_init_options_init(&init_options, rcl_get_default_allocator())) printf("Connecting to agent %s:%d\n",argv[1],atoi(argv[2])); rmw_init_options_t* rmw_options = rcl_init_options_get_rmw_init_options(&init_options); - rmw_uros_options_set_udp_address(argv[1], argv[2], rmw_options); - rmw_uros_options_set_client_key(0xBA5EBA11, rmw_options); + RCCHECK(rmw_uros_options_set_udp_address(argv[1], argv[2], rmw_options)) + RCCHECK(rmw_uros_options_set_client_key(0xBA5EBA11, rmw_options)) context = rcl_get_zero_initialized_context(); - ret = rcl_init(0, NULL, &init_options, &context); + RCCHECK(rcl_init(0, NULL, &init_options, &context)) //Init Node rcl_node_t node = rcl_get_zero_initialized_node(); rcl_node_options_t node_ops = rcl_node_get_default_options(); - ret = rcl_node_init(&node, "vector3_publisher", "", &context, &node_ops); + RCCHECK(rcl_node_init(&node, "vector3_publisher", "", &context, &node_ops)) //Init Publisher const char* topic_name = "sample_vector3"; @@ -53,29 +51,28 @@ int main(int argc, char * const argv[]) const rosidl_message_type_support_t * pub_type_support = ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Vector3); rcl_publisher_options_t pub_opt = rcl_publisher_get_default_options(); - ret = rcl_publisher_init(&publisher_vector3, &node, pub_type_support, topic_name, &pub_opt); + RCCHECK(rcl_publisher_init(&publisher_vector3, &node, pub_type_support, topic_name, &pub_opt)) geometry_msgs__msg__Vector3 topic_data; geometry_msgs__msg__Vector3__init(&topic_data); // Spin rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); - ret = rcl_wait_set_init(&wait_set, 1, 0, 0, 0, 0, 0, &context, rcl_get_default_allocator()); + RCCHECK(rcl_wait_set_init(&wait_set, 1, 0, 0, 0, 0, 0, &context, rcl_get_default_allocator())) float values[3]; - for(uint8_t i = 0; i < 3; i++){ - values[i] = i; - } + values[0] = 0.0; + values[1] = 1.0; + values[2] = 2.0; topic_data.x = values[0]; topic_data.y = values[1]; topic_data.z = values[2]; - while (rcl_context_is_valid(&context)) - { - ret = rcl_publish( &publisher_vector3, (const void *) &topic_data, NULL); + while (rcl_context_is_valid(&context)) { + RCSOFTCHECK(rcl_publish( &publisher_vector3, (const void *) &topic_data, NULL)) - for(uint8_t i = 0; i < 3; i++){ + for(uint8_t i = 0; i < 3; i++) { values[i]++; } @@ -84,10 +81,8 @@ int main(int argc, char * const argv[]) topic_data.z = values[2]; printf("Publish: %f, %f, %f\n",topic_data.x,topic_data.y,topic_data.z); - sleep(1); } - ret = rcl_node_fini(&node); - + RCCHECK(rcl_node_fini(&node)) } diff --git a/rcl/configuration_example/configured_subscriber/main.c b/rcl/configuration_example/configured_subscriber/main.c index 44b3bde..ba19b7a 100644 --- a/rcl/configuration_example/configured_subscriber/main.c +++ b/rcl/configuration_example/configured_subscriber/main.c @@ -22,29 +22,27 @@ int main(int argc, char * argv[]) { - rcl_ret_t ret; - //Init RCL context rcl_context_t context; rcl_init_options_t init_options; init_options = rcl_get_zero_initialized_init_options(); - ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + RCCHECK(rcl_init_options_init(&init_options, rcl_get_default_allocator())) printf("Connecting to agent %s:%d\n",argv[1],atoi(argv[2])); rmw_init_options_t* rmw_options = rcl_init_options_get_rmw_init_options(&init_options); - rmw_uros_options_set_udp_address(argv[1], argv[2], rmw_options); - rmw_uros_options_set_client_key(0xCAFEBABE, rmw_options); + RCCHECK(rmw_uros_options_set_udp_address(argv[1], argv[2], rmw_options)) + RCCHECK(rmw_uros_options_set_client_key(0xCAFEBABE, rmw_options)) context = rcl_get_zero_initialized_context(); - ret = rcl_init(0, NULL, &init_options, &context); + RCCHECK(rcl_init(0, NULL, &init_options, &context)) //Init Node rcl_node_t node = rcl_get_zero_initialized_node(); rcl_node_options_t node_ops = rcl_node_get_default_options(); - ret = rcl_node_init(&node, "vector3_subscriber", "", &context, &node_ops); + RCCHECK(rcl_node_init(&node, "vector3_subscriber", "", &context, &node_ops)) //Init Subscriber const char* topic_name = "sample_vector3"; @@ -53,14 +51,14 @@ int main(int argc, char * argv[]) const rosidl_message_type_support_t * sub_type_support = ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Vector3); - ret = rcl_subscription_init(&subscriber_vector3, &node, sub_type_support, topic_name, &subs_ops); + RCCHECK(rcl_subscription_init(&subscriber_vector3, &node, sub_type_support, topic_name, &subs_ops)) geometry_msgs__msg__Vector3 topic_data; geometry_msgs__msg__Vector3__init(&topic_data); // Spin rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); - ret = rcl_wait_set_init(&wait_set, 1, 0, 0, 0, 0, 0, &context, rcl_get_default_allocator()); + RCCHECK(rcl_wait_set_init(&wait_set, 1, 0, 0, 0, 0, 0, &context, rcl_get_default_allocator())) uint32_t timeout_ms = 500; @@ -68,25 +66,25 @@ int main(int argc, char * argv[]) { // get empty wait set rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); - ret = rcl_wait_set_init(&wait_set, 1, 0, 0, 0, 0, 0, &context, rcl_get_default_allocator()); + RCSOFTCHECK(rcl_wait_set_init(&wait_set, 1, 0, 0, 0, 0, 0, &context, rcl_get_default_allocator())) // set rmw fields to NULL - ret = rcl_wait_set_clear(&wait_set); + RCSOFTCHECK(rcl_wait_set_clear(&wait_set)) size_t index = 0; - ret = rcl_wait_set_add_subscription(&wait_set, &subscriber_vector3, &index); + RCSOFTCHECK(rcl_wait_set_add_subscription(&wait_set, &subscriber_vector3, &index)) - ret = rcl_wait(&wait_set, RCL_MS_TO_NS(timeout_ms)); + RCSOFTCHECK(rcl_wait(&wait_set, RCL_MS_TO_NS(timeout_ms))) if (ret == RCL_RET_OK && wait_set.subscriptions[0]) { rmw_message_info_t messageInfo; - ret = rcl_take(wait_set.subscriptions[0], &topic_data, &messageInfo, NULL); + RCSOFTCHECK(rcl_take(wait_set.subscriptions[0], &topic_data, &messageInfo, NULL)) printf("Received: %f, %f, %f\n",topic_data.x,topic_data.y,topic_data.z); } rcl_wait_set_fini(&wait_set); } - ret = rcl_node_fini(&node); + RCCHECK(rcl_node_fini(&node)) } From c5af16d69354ff04a6ccf7fe35d306b5f6b49dcb Mon Sep 17 00:00:00 2001 From: Pablo Garrido Date: Thu, 16 Jan 2020 07:31:28 +0100 Subject: [PATCH 6/9] Added error check macros --- rcl/configuration_example/configured_publisher/main.c | 3 +++ rcl/configuration_example/configured_subscriber/main.c | 9 ++++++--- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/rcl/configuration_example/configured_publisher/main.c b/rcl/configuration_example/configured_publisher/main.c index fbd0d16..51ab0a6 100644 --- a/rcl/configuration_example/configured_publisher/main.c +++ b/rcl/configuration_example/configured_publisher/main.c @@ -20,6 +20,9 @@ #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);}} + int main(int argc, char * const argv[]) { //Init RCL context diff --git a/rcl/configuration_example/configured_subscriber/main.c b/rcl/configuration_example/configured_subscriber/main.c index ba19b7a..2abb54c 100644 --- a/rcl/configuration_example/configured_subscriber/main.c +++ b/rcl/configuration_example/configured_subscriber/main.c @@ -20,6 +20,9 @@ #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);}} + int main(int argc, char * argv[]) { //Init RCL context @@ -74,15 +77,15 @@ int main(int argc, char * argv[]) size_t index = 0; RCSOFTCHECK(rcl_wait_set_add_subscription(&wait_set, &subscriber_vector3, &index)) - RCSOFTCHECK(rcl_wait(&wait_set, RCL_MS_TO_NS(timeout_ms))) + rcl_ret_t rc = rcl_wait(&wait_set, RCL_MS_TO_NS(timeout_ms)); - if (ret == RCL_RET_OK && wait_set.subscriptions[0]) { + if (rc == RCL_RET_OK && wait_set.subscriptions[0]) { rmw_message_info_t messageInfo; RCSOFTCHECK(rcl_take(wait_set.subscriptions[0], &topic_data, &messageInfo, NULL)) printf("Received: %f, %f, %f\n",topic_data.x,topic_data.y,topic_data.z); } - rcl_wait_set_fini(&wait_set); + RCSOFTCHECK(rcl_wait_set_fini(&wait_set)) } RCCHECK(rcl_node_fini(&node)) From a10cbbf90843c72f2968b573cb304d8bd07688de Mon Sep 17 00:00:00 2001 From: Pablo Garrido Date: Thu, 16 Jan 2020 08:01:07 +0100 Subject: [PATCH 7/9] Indentation fix --- rcl/configuration_example/configured_publisher/CMakeLists.txt | 2 +- .../configured_subscriber/CMakeLists.txt | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/rcl/configuration_example/configured_publisher/CMakeLists.txt b/rcl/configuration_example/configured_publisher/CMakeLists.txt index 6042207..8494923 100644 --- a/rcl/configuration_example/configured_publisher/CMakeLists.txt +++ b/rcl/configuration_example/configured_publisher/CMakeLists.txt @@ -16,5 +16,5 @@ ament_target_dependencies(${PROJECT_NAME} ) install(TARGETS ${PROJECT_NAME} - DESTINATION ${PROJECT_NAME} + DESTINATION ${PROJECT_NAME} ) \ No newline at end of file diff --git a/rcl/configuration_example/configured_subscriber/CMakeLists.txt b/rcl/configuration_example/configured_subscriber/CMakeLists.txt index 734f3bc..b623707 100644 --- a/rcl/configuration_example/configured_subscriber/CMakeLists.txt +++ b/rcl/configuration_example/configured_subscriber/CMakeLists.txt @@ -16,5 +16,5 @@ ament_target_dependencies(${PROJECT_NAME} ) install(TARGETS ${PROJECT_NAME} - DESTINATION ${PROJECT_NAME} - ) + DESTINATION ${PROJECT_NAME} +) From 44f38cd2f6453b0f0b196ca8917a1dab21889b14 Mon Sep 17 00:00:00 2001 From: Pablo Garrido Date: Thu, 16 Jan 2020 08:01:31 +0100 Subject: [PATCH 8/9] Indentation fix --- rcl/configuration_example/configured_publisher/CMakeLists.txt | 4 ++-- .../configured_subscriber/CMakeLists.txt | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/rcl/configuration_example/configured_publisher/CMakeLists.txt b/rcl/configuration_example/configured_publisher/CMakeLists.txt index 8494923..7acf849 100644 --- a/rcl/configuration_example/configured_publisher/CMakeLists.txt +++ b/rcl/configuration_example/configured_publisher/CMakeLists.txt @@ -13,8 +13,8 @@ ament_target_dependencies(${PROJECT_NAME} geometry_msgs rmw_microxrcedds rcl - ) +) install(TARGETS ${PROJECT_NAME} DESTINATION ${PROJECT_NAME} - ) \ No newline at end of file +) \ No newline at end of file diff --git a/rcl/configuration_example/configured_subscriber/CMakeLists.txt b/rcl/configuration_example/configured_subscriber/CMakeLists.txt index b623707..97ee760 100644 --- a/rcl/configuration_example/configured_subscriber/CMakeLists.txt +++ b/rcl/configuration_example/configured_subscriber/CMakeLists.txt @@ -13,7 +13,7 @@ ament_target_dependencies(${PROJECT_NAME} geometry_msgs rmw_microxrcedds rcl - ) +) install(TARGETS ${PROJECT_NAME} DESTINATION ${PROJECT_NAME} From 8cb6f85ce98d5651ba2a38238f474d067ffe4b4c Mon Sep 17 00:00:00 2001 From: Pablo Garrido Date: Thu, 16 Jan 2020 08:03:16 +0100 Subject: [PATCH 9/9] Indentation fix --- rcl/configuration_example/configured_publisher/CMakeLists.txt | 4 ++-- .../configured_subscriber/CMakeLists.txt | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rcl/configuration_example/configured_publisher/CMakeLists.txt b/rcl/configuration_example/configured_publisher/CMakeLists.txt index 7acf849..73b58d6 100644 --- a/rcl/configuration_example/configured_publisher/CMakeLists.txt +++ b/rcl/configuration_example/configured_publisher/CMakeLists.txt @@ -13,8 +13,8 @@ ament_target_dependencies(${PROJECT_NAME} geometry_msgs rmw_microxrcedds rcl -) + ) install(TARGETS ${PROJECT_NAME} DESTINATION ${PROJECT_NAME} -) \ No newline at end of file + ) \ No newline at end of file diff --git a/rcl/configuration_example/configured_subscriber/CMakeLists.txt b/rcl/configuration_example/configured_subscriber/CMakeLists.txt index 97ee760..52d764d 100644 --- a/rcl/configuration_example/configured_subscriber/CMakeLists.txt +++ b/rcl/configuration_example/configured_subscriber/CMakeLists.txt @@ -13,8 +13,8 @@ ament_target_dependencies(${PROJECT_NAME} geometry_msgs rmw_microxrcedds rcl -) + ) install(TARGETS ${PROJECT_NAME} DESTINATION ${PROJECT_NAME} -) + )