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..73b58d6 --- /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..51ab0a6 --- /dev/null +++ b/rcl/configuration_example/configured_publisher/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 + +#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 + rcl_context_t context; + rcl_init_options_t init_options; + + init_options = rcl_get_zero_initialized_init_options(); + 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); + 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(); + + 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(); + + RCCHECK(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(); + + 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(); + RCCHECK(rcl_wait_set_init(&wait_set, 1, 0, 0, 0, 0, 0, &context, rcl_get_default_allocator())) + + float values[3]; + 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)) { + RCSOFTCHECK(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); + } + + RCCHECK(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..52d764d --- /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..2abb54c --- /dev/null +++ b/rcl/configuration_example/configured_subscriber/main.c @@ -0,0 +1,93 @@ +// 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 + +#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 + rcl_context_t context; + rcl_init_options_t init_options; + + init_options = rcl_get_zero_initialized_init_options(); + 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); + 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(); + + 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(); + + RCCHECK(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); + + 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(); + RCCHECK(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(); + RCSOFTCHECK(rcl_wait_set_init(&wait_set, 1, 0, 0, 0, 0, 0, &context, rcl_get_default_allocator())) + + // set rmw fields to NULL + RCSOFTCHECK(rcl_wait_set_clear(&wait_set)) + + size_t index = 0; + RCSOFTCHECK(rcl_wait_set_add_subscription(&wait_set, &subscriber_vector3, &index)) + + rcl_ret_t rc = rcl_wait(&wait_set, RCL_MS_TO_NS(timeout_ms)); + + 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); + } + + RCSOFTCHECK(rcl_wait_set_fini(&wait_set)) + } + + RCCHECK(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