Add autodiscover micro-ROS agent example (#29)

Author: pablogs9

rclc/CMakeLists.txt

@@ -47,6 +47,7 @@ export_executable(complex_msg_subscriber)
 export_executable(timer)
 export_executable(string_publisher)
 export_executable(string_subscriber)
+export_executable(autodiscover_agent)
 
 if(EXISTS ${CMAKE_BINARY_DIR}/temp_install/)
     install(

rclc/autodiscover_agent/CMakeLists.txt

@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 3.5)
+
+project(autodiscover_agent 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)
+
+add_executable(${PROJECT_NAME} main.c)
+
+ament_target_dependencies(${PROJECT_NAME}
+  rcl
+  rclc
+  std_msgs
+  rmw_microxrcedds
+  )
+
+install(TARGETS ${PROJECT_NAME}
+  DESTINATION ${PROJECT_NAME}
+  )

rclc/autodiscover_agent/main.c

@@ -0,0 +1,81 @@
+#include <rcl/rcl.h>
+#include <rcl/error_handling.h>
+#include <rclc/rclc.h>
+#include <rclc/executor.h>
+
+#include <std_msgs/msg/int32.h>
+
+#include <stdio.h>
+#include <unistd.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);}}
+
+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()
+{	
+	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);
+
+	// Auto discover micro-ROS agent
+	if(rmw_uros_discover_agent(rmw_options) != RCL_RET_OK){
+		printf("micro-ROS agent not found\n");
+		return 1;
+	}
+
+	// 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, "int32_publisher_rclc", "", &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));
+
+	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_timer(&executor, &timer));
+
+	msg.data = 0;
+	
+  	rclc_executor_spin(&executor);
+
+	RCCHECK(rcl_publisher_fini(&publisher, &node));
+	RCCHECK(rcl_node_fini(&node));
+}