Add ping with defined transport (#159) (#162)

* Add ping with custom transport

* Add mising ,

* Fix ident

* Uncrustify

(cherry picked from commit 02373a7)

Co-authored-by: Antonio Cuadros <49162117+Acuadros95@users.noreply.github.com>

Author: mergify[bot]

rmw_microxrcedds_c/include/rmw_microros/custom_transport.h

@@ -37,7 +37,7 @@ extern "C"
  */
 
 /**
- * \brief Check if micro-ROS Agent answers to micro-ROS client
+ * \brief Sets micro-ROS default custom transport.
  *
  * \param[in] framing Enable XRCE framing.
  * \param[in] args Arguments for open function.
@@ -56,6 +56,28 @@ rmw_ret_t rmw_uros_set_custom_transport(
   write_custom_func write_cb,
   read_custom_func read_cb);
 
+/**
+ * \brief  Fills rmw implementation-specific options with the given custom transport.
+ *
+ * \param[in] framing Enable XRCE framing.
+ * \param[in] args Arguments for open function.
+ * \param[in] open_cb Open transport callback.
+ * \param[in] close_cb Close transport callback.
+ * \param[in] write_cb Write transport callback.
+ * \param[in] read_cb Read transport callback.
+ * \param[in,out] rmw_options Updated options with updated transport.
+ * \return RMW_RET_OK If arguments were valid and set in rmw_init_options.
+ * \return RMW_RET_INVALID_ARGUMENT If rmw_init_options is not valid or unexpected arguments.
+ */
+rmw_ret_t rmw_uros_options_set_custom_transport(
+  bool framing,
+  void * args,
+  open_custom_func open_cb,
+  close_custom_func close_cb,
+  write_custom_func write_cb,
+  read_custom_func read_cb,
+  rmw_init_options_t * rmw_options);
+
 /** @}*/
 
 #if defined(__cplusplus)

rmw_microxrcedds_c/include/rmw_microros/ping.h

@@ -52,6 +52,21 @@ rmw_ret_t rmw_uros_ping_agent(
   const int timeout_ms,
   const uint8_t attempts);
 
+/**
+ * \brief Check if micro-ROS Agent is up and running using the transport set on the given rmw options.
+ *        This function can be called even when the micro-ROS context has not yet been initialized.
+ *        The transport will be initialized and closed once during the ping process.
+ * \param[in] timeout_ms Timeout in ms (per attempt).
+ * \param[in] attempts Number of tries before considering the ping as failed.
+ * \param[in] rmw_options rmw options with populated transport parameters.
+ * \return RMW_RET_OK If micro-ROS Agent is available.
+ * \return RMW_RET_ERROR If micro-ROS Agent is not available.
+ */
+rmw_ret_t rmw_uros_ping_agent_options(
+  const int timeout_ms,
+  const uint8_t attempts,
+  rmw_init_options_t * rmw_options);
+
 /** @}*/
 
 #if defined(__cplusplus)

rmw_microxrcedds_c/src/rmw_microros/custom_transport.c

@@ -47,3 +47,32 @@ rmw_ret_t rmw_uros_set_custom_transport(
   }
   return RMW_RET_OK;
 }
+
+rmw_ret_t rmw_uros_options_set_custom_transport(
+  bool framing,
+  void * args,
+  open_custom_func open_cb,
+  close_custom_func close_cb,
+  write_custom_func write_cb,
+  read_custom_func read_cb,
+  rmw_init_options_t * rmw_options)
+{
+  if (NULL != open_cb &&
+    NULL != close_cb &&
+    NULL != write_cb &&
+    NULL != read_cb &&
+    NULL != rmw_options)
+  {
+    rmw_options->impl->transport_params.framing = framing;
+    rmw_options->impl->transport_params.args = args;
+    rmw_options->impl->transport_params.open_cb = open_cb;
+    rmw_options->impl->transport_params.close_cb = close_cb;
+    rmw_options->impl->transport_params.write_cb = write_cb;
+    rmw_options->impl->transport_params.read_cb = read_cb;
+  } else {
+    RMW_SET_ERROR_MSG("Uninitialised arguments.");
+    return RMW_RET_INVALID_ARGUMENT;
+  }
+
+  return RMW_RET_OK;
+}

rmw_microxrcedds_c/src/rmw_microros/ping.c

@@ -67,3 +67,35 @@ rmw_ret_t rmw_uros_ping_agent(
 
   return success ? RMW_RET_OK : RMW_RET_ERROR;
 }
+
+rmw_ret_t rmw_uros_ping_agent_options(
+  const int timeout_ms,
+  const uint8_t attempts,
+  rmw_init_options_t * rmw_options)
+{
+  bool success = false;
+
+#ifdef RMW_UXRCE_TRANSPORT_SERIAL
+  uxrSerialTransport transport;
+#elif defined(RMW_UXRCE_TRANSPORT_UDP)
+  uxrUDPTransport transport;
+#elif defined(RMW_UXRCE_TRANSPORT_CUSTOM)
+  uxrCustomTransport transport;
+  transport.framing = rmw_options->impl->transport_params.framing;
+  transport.args = rmw_options->impl->transport_params.args;
+  transport.open = rmw_options->impl->transport_params.open_cb;
+  transport.close = rmw_options->impl->transport_params.close_cb;
+  transport.write = rmw_options->impl->transport_params.write_cb;
+  transport.read = rmw_options->impl->transport_params.read_cb;
+#endif /* ifdef RMW_UXRCE_TRANSPORT_SERIAL */
+  rmw_ret_t ret = rmw_uxrce_transport_init(NULL, rmw_options->impl, (void *)&transport);
+
+  if (RMW_RET_OK != ret) {
+    return ret;
+  }
+
+  success = uxr_ping_agent_attempts(&transport.comm, timeout_ms, attempts);
+  CLOSE_TRANSPORT(&transport);
+
+  return success ? RMW_RET_OK : RMW_RET_ERROR;
+}

rmw_microxrcedds_c/src/rmw_uxrce_transports.c

@@ -24,7 +24,7 @@ rmw_ret_t rmw_uxrce_transport_init(
   void * transport)
 {
 #ifdef RMW_UXRCE_TRANSPORT_SERIAL
-  const char * serial_device = (NULL == context_impl) ?
+  const char * serial_device = (NULL == init_options_impl) ?
     RMW_UXRCE_DEFAULT_SERIAL_DEVICE :
     init_options_impl->transport_params.serial_device;
 
@@ -92,9 +92,6 @@ rmw_ret_t rmw_uxrce_transport_init(
     RMW_SET_ERROR_MSG("rmw_transport_init SERIAL: invalid serial device file descriptor");
     return RMW_RET_ERROR;
   }
-  printf(
-    "micro-ROS transport: connected using serial mode, dev: '%s'\n",
-    serial_device);
 #elif defined(RMW_UXRCE_TRANSPORT_UDP)
 #ifdef RMW_UXRCE_TRANSPORT_IPV4
   uxrIpProtocol ip_protocol = UXR_IPv4;
@@ -105,25 +102,22 @@ rmw_ret_t rmw_uxrce_transport_init(
   uxrUDPTransport * udp_transport = (NULL == context_impl) ?
     (uxrUDPTransport *)transport :
     &context_impl->transport;
-  const char * agent_ip = (NULL == context_impl) ?
+  const char * agent_ip = (NULL == init_options_impl) ?
     RMW_UXRCE_DEFAULT_UDP_IP :
     init_options_impl->transport_params.agent_address;
-  const char * agent_port = (NULL == context_impl) ?
+  const char * agent_port = (NULL == init_options_impl) ?
     RMW_UXRCE_DEFAULT_UDP_PORT :
     init_options_impl->transport_params.agent_port;
 
   if (!uxr_init_udp_transport(udp_transport, ip_protocol, agent_ip, agent_port)) {
     RMW_SET_ERROR_MSG("rmw_transport_init UDP: cannot init XRCE transport");
     return RMW_RET_ERROR;
   }
-  printf(
-    "micro-ROS transport: connected using UDP mode, ip: '%s', port: '%s'\n",
-    agent_ip, agent_port);
 #elif defined(RMW_UXRCE_TRANSPORT_CUSTOM)
   uxrCustomTransport * custom_transport = (NULL == context_impl) ?
     (uxrCustomTransport *)transport :
     &context_impl->transport;
-  void * args = (NULL == context_impl) ?
+  void * args = (NULL == init_options_impl) ?
     rmw_uxrce_transport_default_params.args :
     init_options_impl->transport_params.args;
 

rmw_microxrcedds_c/src/rmw_uxrce_transports.h

@@ -35,7 +35,7 @@
  * @param   context The RMW context, holding in its implementation the
  *          XRCE transport entity, if it has been initialized.
  * @param   init_options Pointer to struct holding the provided init options for
- *          micro-ROS, if context has been initialized.
+ *          micro-ROS.
  * @param   transport Älternative opaque transport structure, which will
  *          must come as NULL and will be given as a handle to the user.
  *          This structure is filled when the context_impl pointer is NULL.