Feature entity recreation

rmw_microxrcedds_c/include/rmw_microros/ping.h

@@ -69,6 +69,13 @@ rmw_ret_t rmw_uros_ping_agent_options(
   const uint8_t attempts,
   rmw_init_options_t * rmw_options);
 
+/**
+ * \brief TODO
+ */
+rmw_ret_t rmw_uros_regenerate_entities(
+  const int timeout_ms,
+  const uint8_t attempts)
+
 /** @}*/
 
 #if defined(__cplusplus)

rmw_microxrcedds_c/src/rmw_client.c

@@ -105,16 +105,11 @@ rmw_create_client(
       custom_client->client_id,
       custom_node->participant_id, rmw_uxrce_entity_naming_buffer, UXR_REPLACE | UXR_REUSE);
 #else
-    static char req_type_name[RMW_UXRCE_TYPE_NAME_MAX_LENGTH];
-    static char res_type_name[RMW_UXRCE_TYPE_NAME_MAX_LENGTH];
     generate_service_types(
-      custom_client->topic.type_support_callbacks.srv, req_type_name, res_type_name,
+      custom_client->topic.type_support_callbacks.srv, type_buffer_1, type_buffer_2,
       RMW_UXRCE_TYPE_NAME_MAX_LENGTH);
-
-    static char req_topic_name[RMW_UXRCE_TOPIC_NAME_MAX_LENGTH];
-    static char res_topic_name[RMW_UXRCE_TOPIC_NAME_MAX_LENGTH];
     generate_service_topics(
-      service_name, req_topic_name, res_topic_name,
+      service_name, topic_buffer_1, topic_buffer_2,
       RMW_UXRCE_TOPIC_NAME_MAX_LENGTH);
 
     client_req = uxr_buffer_create_requester_bin(
@@ -123,10 +118,10 @@ rmw_create_client(
       custom_client->client_id,
       custom_node->participant_id,
       (char *) service_name,
-      req_type_name,
-      res_type_name,
-      req_topic_name,
-      res_topic_name,
+      type_buffer_1,
+      type_buffer_2,
+      topic_buffer_1,
+      topic_buffer_2,
       convert_qos_profile(qos_policies),
       UXR_REPLACE | UXR_REUSE);
 #endif /* ifdef RMW_UXRCE_USE_XML */

rmw_microxrcedds_c/src/rmw_microros/ping.c

@@ -23,6 +23,7 @@
 
 #include "../rmw_microros_internal/rmw_uxrce_transports.h"
 #include "../rmw_microros_internal/types.h"
+#include "./rmw_microros_internal/utils.h"
 
 extern rmw_uxrce_transport_params_t rmw_uxrce_transport_default_params;
 
@@ -105,3 +106,305 @@ rmw_ret_t rmw_uros_ping_agent_options(
 
   return success ? RMW_RET_OK : RMW_RET_ERROR;
 }
+
+rmw_ret_t rmw_uros_regenerate_entities(
+  const int timeout_ms,
+  const uint8_t attempts)
+{
+  bool success = true;
+
+  if (!session_memory.is_initialized || NULL == session_memory.allocateditems) {
+    return RMW_RET_ERROR;
+  }
+
+  rmw_uxrce_mempool_item_t * item = session_memory.allocateditems;
+  rmw_context_impl_t * context = (rmw_context_impl_t *)item->data;
+
+  bool ping_success = uxr_ping_agent_attempts(context->session.comm, timeout_ms, attempts);
+
+  if (!ping_success) {
+    return RMW_RET_ERROR;
+  }
+
+  // Regenerate sessions
+  {
+    rmw_uxrce_mempool_item_t * item = session_memory.allocateditems;
+    while (NULL != item) {
+      rmw_context_impl_t * context = (rmw_context_impl_t *)item->data;
+
+      uxr_create_session(&context->session);
+
+      item = item->next;
+    }
+  }
+
+  // Regenerate nodes
+  {
+    rmw_uxrce_mempool_item_t * item = node_memory.allocateditems;
+    while (NULL != item) {
+      rmw_uxrce_node_t * custom_node = (rmw_uxrce_node_t *)item->data;
+      uint16_t req = UXR_INVALID_REQUEST_ID;
+
+      if (strcmp(custom_node->node_namespace, "/") == 0) {
+        snprintf(node_name_buffer, sizeof(node_name_buffer), "%s", custom_node->node_name);
+      } else {
+        snprintf(
+          node_name_buffer, sizeof(node_name_buffer), "%s/%s", custom_node->node_namespace,
+          custom_node->node_name);
+      }
+
+      req = uxr_buffer_create_participant_bin(
+        &custom_node->context->session,
+        *custom_node->context->creation_stream,
+        custom_node->participant_id,
+        custom_node->domain_id,
+        node_name_buffer,
+        UXR_REPLACE | UXR_REUSE);
+
+      run_xrce_session(
+        custom_node->context, custom_node->context->creation_stream, req,
+        custom_node->context->creation_timeout);
+
+      item = item->next;
+    }
+  }
+
+  // Regenerate publishers
+  {
+    rmw_uxrce_mempool_item_t * item = publisher_memory.allocateditems;
+    while (NULL != item) {
+      rmw_uxrce_publisher_t * custom_publisher = (rmw_uxrce_publisher_t *)item->data;
+      uint16_t req = UXR_INVALID_REQUEST_ID;
+
+      generate_topic_name(
+        custom_publishers->topic.topic_name, topic_buffer_1,
+        sizeof(topic_buffer_1));
+      generate_type_name(
+        custom_publisher->topic.type_support_callbacks.msg, type_buffer_1,
+        sizeof(type_buffer_1));
+
+      req = uxr_buffer_create_topic_bin(
+        &custom_publisher->owner_node->context->session,
+        *custom_publisher->owner_node->context->creation_stream,
+        custom_publisher->topic.topic_id,
+        custom_publisher->owner_node->participant_id,
+        topic_buffer_1,
+        type_buffer_1,
+        UXR_REPLACE | UXR_REUSE);
+
+      run_xrce_session(
+        custom_publisher->owner_node->context,
+        custom_publisher->owner_node->context->creation_stream, req,
+        custom_publisher->owner_node->context->creation_timeout);
+
+      req = uxr_buffer_create_publisher_bin(
+        &custom_publisher->owner_node->context->session,
+        *custom_publisher->owner_node->context->creation_stream,
+        custom_publisher->publisher_id,
+        custom_publisher->owner_node->participant_id,
+        UXR_REPLACE | UXR_REUSE);
+
+      run_xrce_session(
+        custom_publisher->owner_node->context,
+        custom_publisher->owner_node->context->creation_stream, req,
+        custom_publisher->owner_node->context->creation_timeout);
+
+      req = uxr_buffer_create_datawriter_bin(
+        &custom_publisher->owner_node->context->session,
+        *custom_publisher->owner_node->context->creation_stream,
+        custom_publisher->datawriter_id,
+        custom_publisher->publisher_id,
+        custom_publisher->topic.topic_id,
+        convert_qos_profile(&custom_publisher->qos),
+        UXR_REPLACE | UXR_REUSE);
+
+      run_xrce_session(
+        custom_publisher->owner_node->context,
+        custom_publisher->owner_node->context->creation_stream, req,
+        custom_publisher->owner_node->context->creation_timeout);
+
+      item = item->next;
+    }
+  }
+
+  // Regenerate subscribers
+  {
+    rmw_uxrce_mempool_item_t * item = subscription_memory.allocateditems;
+    while (NULL != item) {
+      rmw_uxrce_subscription_t * custom_subscription = (rmw_uxrce_subscription_t *)item->data;
+      uint16_t req = UXR_INVALID_REQUEST_ID;
+
+      generate_topic_name(
+        custom_subscription->topic.topic_name, topic_buffer_1,
+        sizeof(topic_buffer_1));
+      generate_type_name(
+        custom_subscription->topic.type_support_callbacks.msg, type_buffer_1,
+        sizeof(type_buffer_1));
+
+      req = uxr_buffer_create_topic_bin(
+        &custom_subscription->owner_node->context->session,
+        *custom_subscription->owner_node->context->creation_stream,
+        custom_subscription->topic.topic_id,
+        custom_subscription->owner_node->participant_id,
+        topic_buffer_1,
+        type_buffer_1,
+        UXR_REPLACE | UXR_REUSE);
+
+      run_xrce_session(
+        custom_subscription->owner_node->context,
+        custom_subscription->owner_node->context->creation_stream, req,
+        custom_subscription->owner_node->context->creation_timeout);
+
+      req = uxr_buffer_create_subscriber_bin(
+        &custom_subscription->owner_node->context->session,
+        *custom_subscription->owner_node->context->creation_stream,
+        custom_subscription->subscriber_id,
+        custom_subscription->owner_node->participant_id,
+        UXR_REPLACE | UXR_REUSE);
+
+      run_xrce_session(
+        custom_subscription->owner_node->context,
+        custom_subscription->owner_node->context->creation_stream, req,
+        custom_subscription->owner_node->context->creation_timeout);
+
+      req = uxr_buffer_create_datareader_bin(
+        &custom_subscription->owner_node->context->session,
+        *custom_subscription->owner_node->context->creation_stream,
+        custom_subscription->datareader_id,
+        custom_subscription->subscriber_id,
+        custom_subscription->topic.topic_id,
+        convert_qos_profile(&custom_subscription->qos),
+        UXR_REPLACE | UXR_REUSE);
+
+      run_xrce_session(
+        custom_subscription->owner_node->context,
+        custom_subscription->owner_node->context->creation_stream, req,
+        custom_subscription->owner_node->context->creation_timeout);
+
+      uxrDeliveryControl delivery_control;
+      delivery_control.max_samples = UXR_MAX_SAMPLES_UNLIMITED;
+      delivery_control.min_pace_period = 0;
+      delivery_control.max_elapsed_time = UXR_MAX_ELAPSED_TIME_UNLIMITED;
+      delivery_control.max_bytes_per_second = UXR_MAX_BYTES_PER_SECOND_UNLIMITED;
+
+      uxrStreamId data_request_stream_id =
+        (custom_subscription->qos.reliability == RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT) ?
+        custom_subscription->owner_node->context->best_effort_input :
+        custom_subscription->owner_node->context->reliable_input;
+
+      uxr_buffer_request_data(
+        &custom_subscription->owner_node->context->session,
+        *custom_subscription->owner_node->context->creation_stream,
+        custom_subscription->datareader_id,
+        data_request_stream_id, &delivery_control);
+
+      item = item->next;
+    }
+  }
+
+  // Regenerate requesters
+  {
+    rmw_uxrce_mempool_item_t * item = service_memory.allocateditems;
+    while (NULL != item) {
+      rmw_uxrce_service_t * custom_service = (rmw_uxrce_service_t *)item->data;
+      uint16_t req = UXR_INVALID_REQUEST_ID;
+
+      generate_service_types(
+        custom_service->topic.type_support_callbacks.srv, type_buffer_1, type_buffer_2,
+        RMW_UXRCE_TYPE_NAME_MAX_LENGTH);
+
+      generate_service_topics(
+        custom_service->topic.topic_name, topic_buffer_1, topic_buffer_2,
+        RMW_UXRCE_TOPIC_NAME_MAX_LENGTH);
+
+      req = uxr_buffer_create_replier_bin(
+        &custom_service->owner_node->context->session,
+        *custom_service->owner_node->context->creation_stream,
+        custom_service->service_id,
+        custom_service->owner_node->participant_id,
+        (char *) custom_service->topic.topic_name,
+        type_buffer_1,
+        type_buffer_2,
+        topic_buffer_1,
+        topic_buffer_2,
+        convert_qos_profile(&custom_service->qos),
+        UXR_REPLACE | UXR_REUSE);
+
+      run_xrce_session(
+        custom_service->owner_node->context,
+        custom_service->owner_node->context->creation_stream, req,
+        custom_service->owner_node->context->creation_timeout);
+
+      uxrStreamId data_request_stream_id =
+        (custom_service->qos.reliability == RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT) ?
+        custom_service->owner_node->context->best_effort_input :
+        custom_service->owner_node->context->reliable_input;
+
+      uxrDeliveryControl delivery_control;
+      delivery_control.max_samples = UXR_MAX_SAMPLES_UNLIMITED;
+      delivery_control.min_pace_period = 0;
+      delivery_control.max_elapsed_time = UXR_MAX_ELAPSED_TIME_UNLIMITED;
+      delivery_control.max_bytes_per_second = UXR_MAX_BYTES_PER_SECOND_UNLIMITED;
+
+      custom_service->service_data_resquest = uxr_buffer_request_data(
+        &custom_service->owner_node->context->session,
+        *custom_service->owner_node->context->creation_stream, custom_service->service_id,
+        data_request_stream_id, &delivery_control);
+      item = item->next;
+    }
+  }
+
+  // Regenerate repliers
+  {
+    rmw_uxrce_mempool_item_t * item = client_memory.allocateditems;
+    while (NULL != item) {
+      rmw_uxrce_client_t * custom_client = (rmw_uxrce_client_t *)item->data;
+      uint16_t req = UXR_INVALID_REQUEST_ID;
+
+      generate_service_types(
+        custom_client->topic.type_support_callbacks.srv, type_buffer_1, type_buffer_2,
+        RMW_UXRCE_TYPE_NAME_MAX_LENGTH);
+      generate_service_topics(
+        custom_client->topic.topic_name, topic_buffer_1, topic_buffer_2,
+        RMW_UXRCE_TOPIC_NAME_MAX_LENGTH);
+
+      req = uxr_buffer_create_requester_bin(
+        &custom_client->owner_node->context->session,
+        *custom_client->owner_node->context->creation_stream,
+        custom_client->client_id,
+        custom_client->owner_node->participant_id,
+        (char *) custom_client->topic.topic_name,
+        type_buffer_1,
+        type_buffer_2,
+        topic_buffer_1,
+        topic_buffer_2,
+        convert_qos_profile(&custom_client->qos),
+        UXR_REPLACE | UXR_REUSE);
+
+      run_xrce_session(
+        custom_client->owner_node->context,
+        custom_client->owner_node->context->creation_stream, req,
+        custom_client->owner_node->context->creation_timeout);
+
+      uxrStreamId data_request_stream_id =
+        (custom_client->qos.reliability == RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT) ?
+        custom_client->owner_node->context->best_effort_input :
+        custom_client->owner_node->context->reliable_input;
+
+      uxrDeliveryControl delivery_control;
+      delivery_control.max_samples = UXR_MAX_SAMPLES_UNLIMITED;
+      delivery_control.min_pace_period = 0;
+      delivery_control.max_elapsed_time = UXR_MAX_ELAPSED_TIME_UNLIMITED;
+      delivery_control.max_bytes_per_second = UXR_MAX_BYTES_PER_SECOND_UNLIMITED;
+
+      custom_client->client_data_request = uxr_buffer_request_data(
+        &custom_client->owner_node->context->session,
+        *custom_client->owner_node->context->creation_stream, custom_client->client_id,
+        data_request_stream_id, &delivery_control);
+
+      item = item->next;
+    }
+  }
+
+  return success ? RMW_RET_OK : RMW_RET_ERROR;
+}

rmw_microxrcedds_c/src/rmw_microros_internal/types.h

@@ -199,6 +199,7 @@ typedef struct rmw_uxrce_node_t
   rmw_uxrce_session_t * context;
 
   uxrObjectId participant_id;
+  size_t domain_id;
 
   rmw_node_t rmw_node;
   char node_name[RMW_UXRCE_NODE_NAME_MAX_LENGTH];