Feature/configuration

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=<INSTALL_DIR>
-                -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=<INSTALL_DIR>
+            -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)

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}
+  )

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 <stdio.h>
+#include <unistd.h>
+
+#include <rcl/rcl.h>
+#include <geometry_msgs/msg/vector3.h>
+#include <rmw_uros/options.h>
+
+#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))
+}

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}
+  )

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 <stdio.h>
+#include <unistd.h>
+
+#include <rcl/rcl.h>
+#include <geometry_msgs/msg/vector3.h>
+#include <rmw_uros/options.h>
+
+#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))
+
+}

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}

rcl/int32_publisher/main.c

@@ -2,6 +2,8 @@
 #include <rcl/error_handling.h>
 #include <std_msgs/msg/int32.h>
 
+#include <rmw_uros/options.h>
+
 #include <stdio.h>
 #include <unistd.h>
 
@@ -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))
 

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}

rcl/int32_subscriber/main.c

@@ -2,6 +2,8 @@
 #include <rcl/error_handling.h>
 #include <std_msgs/msg/int32.h>
 
+#include <rmw_uros/options.h>
+
 #include <stdio.h>
 
 #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;}}

rcl/package.xml

@@ -2,11 +2,11 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>micro_ros_demos_rcl</name>
-  <version>0.0.1</version>
-  <description>Example of a publisher using int32</description>
-  <maintainer email="julianbermudez@eprosima.com">Julian Bermudez</maintainer>
+  <version>0.0.2</version>
+  <description>Examples using RCL API</description>
+  <maintainer email="borjaouterelo@eprosima.com">Borja Outerelo</maintainer>
   <license>Apache License 2.0</license>
-  <author>Julian Bermudez</author>
+  <author>Borja Outerelo</author>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
@@ -15,9 +15,12 @@
   <depend>std_msgs</depend>
   <depend>example_interfaces</depend>
 
+  <!-- configured_publisher and configured_subscriber-->
+  <depend>geometry_msgs</depend>
+  <depend>rmw_microxrcedds</depend>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>
 
-</package>
-
+</package>