Add external transport example (#37)

pablogs9 · jamoralp · web-flow · commit ca787147e399 · 2021-02-09T12:24:06.000+01:00
* Initial Rolling support


Update client


Update


Update

* Initial

* Add custom transport example

* Update

* Fix CI

* Fix CI: fix checkout path

* Update .github/workflows/ci.yml

Co-authored-by: Jose Antonio Moral &lt;joseantoniomoralparras@gmail.com&gt;
diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
@@ -24,10 +24,10 @@ jobs:
             rm -rf src/uros/micro-ROS-demos
         - uses: actions/checkout@v2
           with:
-            path: src/uros/micro-ROS-demos
+            path: uros_ws/src/uros/micro-ROS-demos
         - name: Build
           run: |
             cd /uros_ws
             source /opt/ros/foxy/setup.bash
             source install/local_setup.bash
-            ros2 run micro_ros_setup build_firmware.sh
+            ros2 run micro_ros_setup build_firmware.sh
diff --git a/rclc/CMakeLists.txt b/rclc/CMakeLists.txt
@@ -51,6 +51,7 @@ export_executable(timer)
 export_executable(string_publisher)
 export_executable(string_subscriber)
 export_executable(autodiscover_agent)
+export_executable(configuration_example custom_transports)
 
 if(EXISTS ${CMAKE_BINARY_DIR}/temp_install/)
     install(
diff --git a/rclc/addtwoints_client/main.c b/rclc/addtwoints_client/main.c
@@ -14,9 +14,9 @@
 example_interfaces__srv__AddTwoInts_Request req;
 example_interfaces__srv__AddTwoInts_Response res;
 
-void client_callback(const void * msg, rmw_request_id_t * req_id){
+void client_callback(const void * msg){
   example_interfaces__srv__AddTwoInts_Response * msgin = (example_interfaces__srv__AddTwoInts_Response * ) msg;
-  printf("Received service response %ld + %ld = %ld. Seq %ld\n", req.a, req.b, msgin->sum, req_id->sequence_number);
+  printf("Received service response %ld + %ld = %ld\n", req.a, req.b, msgin->sum);
 }
 
 int main(int argc, const char * const * argv)
diff --git a/rclc/autodiscover_agent/CMakeLists.txt b/rclc/autodiscover_agent/CMakeLists.txt
@@ -7,14 +7,20 @@ find_package(rcl REQUIRED)
 find_package(rclc REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(rmw_microxrcedds REQUIRED)
+find_package(microxrcedds_client REQUIRED)
 
 add_executable(${PROJECT_NAME} main.c)
 
+target_link_libraries(${PROJECT_NAME}
+  microxrcedds_client
+)
+
 ament_target_dependencies(${PROJECT_NAME}
   rcl
   rclc
   std_msgs
   rmw_microxrcedds
+  microxrcedds_client
   )
 
 install(TARGETS ${PROJECT_NAME}
diff --git a/rclc/configuration_example/configured_publisher/CMakeLists.txt b/rclc/configuration_example/configured_publisher/CMakeLists.txt
@@ -7,14 +7,20 @@ find_package(rcl REQUIRED)
 find_package(rclc REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(rmw_microxrcedds REQUIRED)
+find_package(microxrcedds_client REQUIRED)
 
 add_executable(${PROJECT_NAME} main.c)
 
+target_link_libraries(${PROJECT_NAME}
+  microxrcedds_client
+)
+
 ament_target_dependencies(${PROJECT_NAME}
   rcl
   rclc
   std_msgs
   rmw_microxrcedds
+  microxrcedds_client
   )
 
 install(TARGETS ${PROJECT_NAME}
diff --git a/rclc/configuration_example/configured_publisher/main.c b/rclc/configuration_example/configured_publisher/main.c
@@ -43,16 +43,18 @@ int main(int argc, char * const argv[])
 	RCCHECK(rmw_uros_options_set_udp_address(argv[1], argv[2], rmw_options))
 	RCCHECK(rmw_uros_options_set_client_key(0xCAFEBABA, rmw_options))
 
+	size_t domain_id = (size_t)(argc == 4 ? atoi(argv[3]) : 0);
+	const char * node_name = "int32_configured_publisher_rclc";
+
+	rcl_init_options_set_domain_id(&init_options, domain_id);
+	printf("Initializing RCL '%s' with ROS Domain ID %ld...\n", node_name, domain_id);
+
 	// create init_options
 	RCCHECK(rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator));
 
 	// 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 = (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));
+	RCCHECK(rclc_node_init_default(&node, node_name, "", &support));
 
 	// create publisher
 	RCCHECK(rclc_publisher_init_default(
diff --git a/rclc/configuration_example/configured_subscriber/CMakeLists.txt b/rclc/configuration_example/configured_subscriber/CMakeLists.txt
@@ -7,14 +7,20 @@ find_package(rcl REQUIRED)
 find_package(rclc REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(rmw_microxrcedds REQUIRED)
+find_package(microxrcedds_client REQUIRED)
 
 add_executable(${PROJECT_NAME} main.c)
 
+target_link_libraries(${PROJECT_NAME}
+  microxrcedds_client
+)
+
 ament_target_dependencies(${PROJECT_NAME}
   rcl
   rclc
   std_msgs
   rmw_microxrcedds
+  microxrcedds_client
   )
 
 install(TARGETS ${PROJECT_NAME}
diff --git a/rclc/configuration_example/configured_subscriber/main.c b/rclc/configuration_example/configured_subscriber/main.c
@@ -39,16 +39,18 @@ int main(int argc, const char * const * argv)
 	RCCHECK(rmw_uros_options_set_udp_address(argv[1], argv[2], rmw_options))
 	RCCHECK(rmw_uros_options_set_client_key(0xCAFEBABE, rmw_options))
 
+	size_t domain_id = (size_t)(argc == 4 ? atoi(argv[3]) : 0);
+	const char * node_name = "int32_configured_subscriber_rclc";
+
+	rcl_init_options_set_domain_id(&init_options, domain_id);
+	printf("Initializing RCL '%s' with ROS Domain ID %ld...\n", node_name, domain_id);
+
 	// create init_options
 	RCCHECK(rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator));
 
 	// 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 = (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));
+	RCCHECK(rclc_node_init_default(&node, node_name, "", &support));
 
 	// create subscriber
 	RCCHECK(rclc_subscription_init_default(
diff --git a/rclc/configuration_example/custom_transports/CMakeLists.txt b/rclc/configuration_example/custom_transports/CMakeLists.txt
@@ -0,0 +1,36 @@
+cmake_minimum_required(VERSION 3.5)
+
+project(custom_transports LANGUAGES C)
+
+find_package(ament_cmake REQUIRED)
+find_package(rcl REQUIRED)
+find_package(rclc REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(rmw_microxrcedds REQUIRED)
+find_package(microxrcedds_client REQUIRED)
+
+add_executable(${PROJECT_NAME} main.c)
+
+target_link_libraries(${PROJECT_NAME}
+  microxrcedds_client
+)
+
+ament_target_dependencies(${PROJECT_NAME}
+  rcl
+  rclc
+  std_msgs
+  rmw_microxrcedds
+  microxrcedds_client
+  )
+
+set_target_properties(${PROJECT_NAME} PROPERTIES
+  CXX_STANDARD
+      11
+  CXX_STANDARD_REQUIRED
+      YES
+  )
+
+install(TARGETS ${PROJECT_NAME}
+  DESTINATION ${PROJECT_NAME}
+  )
+  
diff --git a/rclc/configuration_example/custom_transports/main.c b/rclc/configuration_example/custom_transports/main.c
@@ -0,0 +1,184 @@
+#include <rcl/rcl.h>
+#include <rcl/error_handling.h>
+#include <rclc/rclc.h>
+#include <rclc/executor.h>
+
+#include <std_msgs/msg/int32.h>
+
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <poll.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <sys/socket.h>
+#include <arpa/inet.h>
+#include <string.h>
+#include <errno.h>
+#include <netdb.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);}}
+
+// --- micro-ROS Transports ---
+
+typedef struct {
+	struct pollfd poll_fd;
+} custom_transport_data_t;
+
+bool custom_transport_open(struct uxrCustomTransport * transport){
+    custom_transport_data_t * transport_data = (custom_transport_data_t*) transport->args;
+	
+	bool rv = false;
+    transport_data->poll_fd.fd = socket(AF_INET, SOCK_DGRAM, 0);
+
+    if (-1 != transport_data->poll_fd.fd)
+    {
+        struct addrinfo hints;
+        struct addrinfo* result;
+        struct addrinfo* ptr;
+
+        memset(&hints, 0, sizeof(hints));
+        hints.ai_family = AF_INET;
+        hints.ai_socktype = SOCK_DGRAM;
+
+        if (0 == getaddrinfo("localhost", "8888", &hints, &result))
+        {
+            for (ptr = result; ptr != NULL; ptr = ptr->ai_next)
+            {
+                if (0 == connect(transport_data->poll_fd.fd, ptr->ai_addr, ptr->ai_addrlen))
+                {
+                    transport_data->poll_fd.events = POLLIN;
+                    rv = true;
+                    break;
+                }
+            }
+        }
+        freeaddrinfo(result);
+    }
+    return rcutils_vsnprintf;
+}
+
+bool custom_transport_close(struct uxrCustomTransport * transport){
+    custom_transport_data_t * transport_data = (custom_transport_data_t*) transport->args;
+    return (-1 == transport_data->poll_fd.fd) ? true : (0 == close(transport_data->poll_fd.fd));
+}
+
+size_t custom_transport_write(struct uxrCustomTransport* transport, const uint8_t * buf, size_t len, uint8_t * errcode){
+    custom_transport_data_t * transport_data = (custom_transport_data_t*) transport->args;
+    size_t rv = 0;
+    ssize_t bytes_sent = send(transport_data->poll_fd.fd, (void*)buf, len, 0);
+    if (-1 != bytes_sent)
+    {
+        rv = (size_t)bytes_sent;
+        *errcode = 0;
+    }
+    else
+    {
+        *errcode = 1;
+    }
+    return rv;
+}
+
+size_t custom_transport_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* errcode){
+    custom_transport_data_t * transport_data = (custom_transport_data_t*) transport->args;
+    size_t rv = 0;
+    int poll_rv = poll(&transport_data->poll_fd, 1, timeout);
+    if (0 < poll_rv)
+    {
+        ssize_t bytes_received = recv(transport_data->poll_fd.fd, (void*)buf, len, 0);
+        if (-1 != bytes_received)
+        {
+            rv = (size_t)bytes_received;
+            *errcode = 0;
+        }
+        else
+        {
+            *errcode = 1;
+        }
+    }
+    else
+    {
+        *errcode = (0 == poll_rv) ? 0 : 1;
+    }
+    return rv;
+}
+
+
+// --- micro-ROS App ---
+
+rcl_publisher_t publisher;
+std_msgs__msg__Int32 msg;
+
+void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
+{
+	(void) last_call_time;
+	if (timer != NULL) {
+		RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
+		printf("Sent: %d\n", msg.data);
+		msg.data++;
+	}
+}
+
+int main(int argc, char * const argv[])
+{
+  	rcl_allocator_t allocator = rcl_get_default_allocator();
+	rclc_support_t support;
+	rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
+
+	RCCHECK(rcl_init_options_init(&init_options, allocator));
+	rmw_init_options_t* rmw_options = rcl_init_options_get_rmw_init_options(&init_options);
+	RCCHECK(rmw_uros_options_set_client_key(0xCAFEBABA, rmw_options))
+
+	custom_transport_data_t custom_transport_data;
+
+	RCCHECK(rmw_uros_options_set_custom_transport(
+        false,
+        (void*) &custom_transport_data,
+        custom_transport_open,
+        custom_transport_close,
+        custom_transport_write,
+        custom_transport_read,
+        rmw_options
+    ))
+
+	// create init_options
+	RCCHECK(rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator));
+
+	// create node
+	rcl_node_t node = rcl_get_zero_initialized_node();
+	RCCHECK(rclc_node_init_default(&node, "custom_transport_node", "", &support));
+
+	// create publisher
+	RCCHECK(rclc_publisher_init_default(
+		&publisher,
+		&node,
+		ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
+		"std_msgs_msg_Int32"));
+
+	// create timer,
+	rcl_timer_t timer = rcl_get_zero_initialized_timer();
+	const unsigned int timer_timeout = 1000;
+	RCCHECK(rclc_timer_init_default(
+		&timer,
+		&support,
+		RCL_MS_TO_NS(timer_timeout),
+		timer_callback));
+
+	// create executor
+	rclc_executor_t executor = rclc_executor_get_zero_initialized_executor();
+	RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
+	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/fibonacci_action_server/CMakeLists.txt b/rclc/fibonacci_action_server/CMakeLists.txt
@@ -9,6 +9,9 @@ find_package(example_interfaces REQUIRED)
 
 add_executable(${PROJECT_NAME} main.c)
 
+SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread")
+SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -pthread")
+
 ament_target_dependencies(${PROJECT_NAME}
   rcl
   rcl_action
diff --git a/rclc/package.xml b/rclc/package.xml
@@ -18,6 +18,7 @@
   <depend>example_interfaces</depend>
   <depend>complex_msgs</depend>
   <depend>builtin_interfaces</depend>
+  <depend>microxrcedds_client</depend>
 
   <!-- configured_publisher and configured_subscriber-->
   <depend>geometry_msgs</depend>
