Support for Agent discovery in RMW (#79)

* Initial support for Agent discovery in RMW

* Add locator check session init

Author: pablogs9

.github/workflows/ci.yml

@@ -28,7 +28,7 @@ jobs:
 
         - name: Test
           run: | 
-            . /opt/ros/foxy/setup.sh && . /uros_ws/install/local_setup.sh && ros2 run micro_ros_agent micro_ros_agent udp4 --port 8888 -v6 &
+            . /opt/ros/foxy/setup.sh && . /uros_ws/install/local_setup.sh && ros2 run micro_ros_agent micro_ros_agent udp4 --port 8888 -d -v6 &
             sleep 1 
             . /opt/ros/foxy/setup.sh && . install/local_setup.sh 
             cd build/rmw_microxrcedds/test

rmw_microxrcedds_c/include/rmw_uros/options.h

@@ -18,6 +18,11 @@
 #include <rmw/ret_types.h>
 #include <rmw/init_options.h>
 
+#if defined(__cplusplus)
+extern "C"
+{
+#endif
+
 /**
  * \brief Parses command line args and fills rmw implementation-specific options.
  * `rmw_init_options allocator` is used to allocate the specific rmw options.
@@ -55,6 +60,16 @@ rmw_ret_t rmw_uros_options_set_udp_address(
   const char * ip, const char * port,
   rmw_init_options_t * rmw_options);
 
+/**
+ * \brief Fills rmw implementation-specific options with the autodicovered address of an micro-ROS Agent.
+ *
+ * \param[in,out] rmw_options Updated options with rmw specifics.
+ * \return RMW_RET_OK If arguments were valid and set in rmw_init_options.
+ * \return RMW_RET_TIMEOUT If micro-ROS agent autodiscovery is timeout.
+ * \return RMW_RET_INVALID_ARGUMENT If rmw_init_options is not valid or unexpected arguments.
+ */
+rmw_ret_t rmw_uros_discover_agent(rmw_init_options_t * rmw_options);
+
 /**
  * \brief Fills rmw implementation-specific options with the given parameters.
  *
@@ -65,5 +80,8 @@ rmw_ret_t rmw_uros_options_set_udp_address(
  */
 rmw_ret_t rmw_uros_options_set_client_key(uint32_t client_key, rmw_init_options_t * rmw_options);
 
+#if defined(__cplusplus)
+}
+#endif
 
 #endif  // RMW_UROS__OPTIONS_H_

rmw_microxrcedds_c/src/rmw_uros_options.c

@@ -21,6 +21,8 @@
 #include <rmw/ret_types.h>
 #include <rmw/error_handling.h>
 
+// #include <uxr/client/client.h>
+
 rmw_ret_t rmw_uros_init_options(
   int argc, const char * const argv[],
   rmw_init_options_t * rmw_options)
@@ -117,6 +119,58 @@ rmw_ret_t rmw_uros_options_set_udp_address(
 #endif
 }
 
+bool on_agent_found(const TransportLocator* locator, void* args)
+{
+  rmw_init_options_t * rmw_options = (rmw_init_options_t *) args;
+  uxrIpProtocol ip_protocol;
+  char ip[MAX_IP_LEN];
+  char port_str[MAX_PORT_LEN];
+  uint16_t port;
+
+  uxr_locator_to_ip(locator, ip, sizeof(ip), &port, &ip_protocol);
+  sprintf(port_str, "%d", port);
+
+  uxrUDPTransport transport;
+  uxrUDPPlatform udp_platform;
+  if(uxr_init_udp_transport(&transport, &udp_platform, ip_protocol, ip, port_str))
+  {
+    uxrSession session;
+    uxr_init_session(&session, &transport.comm, rmw_options->impl->connection_params.client_key);
+    if(uxr_create_session_retries(&session, 1))
+    {
+      sprintf(rmw_options->impl->connection_params.agent_port, "%d", port);
+      sprintf(rmw_options->impl->connection_params.agent_address, "%s", ip);
+      uxr_delete_session(&session);
+      return true;
+    }
+  }
+  return false;
+}
+
+rmw_ret_t rmw_uros_discover_agent(rmw_init_options_t * rmw_options)
+{
+#if defined(MICRO_XRCEDDS_UDP) && defined(UCLIENT_PROFILE_DISCOVERY)
+  if (NULL == rmw_options) {
+    RMW_SET_ERROR_MSG("Uninitialised rmw_init_options.");
+    return RMW_RET_INVALID_ARGUMENT;
+  }
+
+  memset(rmw_options->impl->connection_params.agent_address, 0, MAX_IP_LEN);
+  memset(rmw_options->impl->connection_params.agent_port, 0, MAX_PORT_LEN);
+
+  uxr_discovery_agents_default(1, 1000, on_agent_found, (void*) rmw_options);
+
+  return (strlen(rmw_options->impl->connection_params.agent_address) > 0 )? RMW_RET_OK : RMW_RET_TIMEOUT;
+#else
+  (void) ip;
+  (void) port;
+  (void) rmw_options;
+
+  RMW_SET_ERROR_MSG("MICRO_XRCEDDS_UDP or UCLIENT_PROFILE_DISCOVERY not set.");
+  return RMW_RET_INVALID_ARGUMENT;
+#endif
+}
+
 rmw_ret_t rmw_uros_options_set_client_key(uint32_t client_key, rmw_init_options_t * rmw_options)
 {
   if (NULL == rmw_options) {

rmw_microxrcedds_c/test/test_rmw.cpp

@@ -18,6 +18,8 @@
 #include <rmw/init_options.h>
 #include <rcutils/allocator.h>
 
+#include <rmw_uros/options.h>
+
 /*
    Testing rmw init and shutdown. htps://github.com/microROS/rmw-microxrcedds/issues/14
 */
@@ -28,3 +30,15 @@ TEST(rmw_microxrcedds, init_shutdown) {
   ASSERT_EQ(rmw_init(&test_options, &test_context), RMW_RET_OK);
   ASSERT_EQ(rmw_shutdown(&test_context), RMW_RET_OK);
 }
+
+/*
+   Testing rmw agent autodiscovery.
+*/
+TEST(rmw_microxrcedds, autodiscover) {
+  rmw_context_t test_context = rmw_get_zero_initialized_context();
+  rmw_init_options_t test_options = rmw_get_zero_initialized_init_options();
+  ASSERT_EQ(rmw_init_options_init(&test_options, rcutils_get_default_allocator()), RMW_RET_OK);
+  ASSERT_EQ(rmw_uros_discover_agent(&test_options), RMW_RET_OK);
+  ASSERT_EQ(rmw_init(&test_options, &test_context), RMW_RET_OK);
+  ASSERT_EQ(rmw_shutdown(&test_context), RMW_RET_OK);
+}