diff --git a/rmw_microxrcedds_c/include/rmw_microros/ping.h b/rmw_microxrcedds_c/include/rmw_microros/ping.h index 1375475a..05e4ff4a 100644 --- a/rmw_microxrcedds_c/include/rmw_microros/ping.h +++ b/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) diff --git a/rmw_microxrcedds_c/src/rmw_client.c b/rmw_microxrcedds_c/src/rmw_client.c index 9807ab84..81de02de 100644 --- a/rmw_microxrcedds_c/src/rmw_client.c +++ b/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 */ diff --git a/rmw_microxrcedds_c/src/rmw_microros/ping.c b/rmw_microxrcedds_c/src/rmw_microros/ping.c index 140eaaa7..f328ba6d 100644 --- a/rmw_microxrcedds_c/src/rmw_microros/ping.c +++ b/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; +} diff --git a/rmw_microxrcedds_c/src/rmw_microros_internal/types.h b/rmw_microxrcedds_c/src/rmw_microros_internal/types.h index e559d7e5..f0583a6c 100644 --- a/rmw_microxrcedds_c/src/rmw_microros_internal/types.h +++ b/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]; diff --git a/rmw_microxrcedds_c/src/rmw_microros_internal/utils.h b/rmw_microxrcedds_c/src/rmw_microros_internal/utils.h index eac326c3..09e237af 100644 --- a/rmw_microxrcedds_c/src/rmw_microros_internal/utils.h +++ b/rmw_microxrcedds_c/src/rmw_microros_internal/utils.h @@ -19,6 +19,13 @@ #include "./rmw_microros_internal/types.h" +// Static buffers for name generation +extern char type_buffer_1[RMW_UXRCE_TYPE_NAME_MAX_LENGTH]; +extern char type_buffer_2[RMW_UXRCE_TYPE_NAME_MAX_LENGTH]; +extern char topic_buffer_1[RMW_UXRCE_TYPE_NAME_MAX_LENGTH]; +extern char topic_buffer_2[RMW_UXRCE_TYPE_NAME_MAX_LENGTH]; +extern char node_name_buffer[2 * RMW_UXRCE_NODE_NAME_MAX_LENGTH]; + bool run_xrce_session( rmw_context_impl_t * context, uxrStreamId * target_stream, @@ -63,49 +70,20 @@ int build_service_xml( char xml[], size_t buffer_size); -int build_participant_xml( - size_t domain_id, - const char * participant_name, - char xml[], - size_t buffer_size); -int build_publisher_xml( - const char * publisher_name, - char xml[], - size_t buffer_size); -int build_subscriber_xml( - const char * subscriber_name, - char xml[], - size_t buffer_size); -int build_topic_xml( - const char * topic_name, - const message_type_support_callbacks_t * members, - const rmw_qos_profile_t * qos_policies, - char xml[], - size_t buffer_size); -int build_datawriter_xml( - const char * topic_name, - const message_type_support_callbacks_t * members, - const rmw_qos_profile_t * qos_policies, - char xml[], - size_t buffer_size); -int build_datareader_xml( - const char * topic_name, - const message_type_support_callbacks_t * members, - const rmw_qos_profile_t * qos_policies, - char xml[], - size_t buffer_size); - bool build_participant_profile( char profile_name[], size_t buffer_size); + bool build_topic_profile( const char * topic_name, char profile_name[], size_t buffer_size); + bool build_datawriter_profile( const char * topic_name, char profile_name[], size_t buffer_size); + bool build_datareader_profile( const char * topic_name, char profile_name[], diff --git a/rmw_microxrcedds_c/src/rmw_node.c b/rmw_microxrcedds_c/src/rmw_node.c index e6503628..2b372db0 100644 --- a/rmw_microxrcedds_c/src/rmw_node.c +++ b/rmw_microxrcedds_c/src/rmw_node.c @@ -46,6 +46,7 @@ rmw_node_t * create_node( rmw_uxrce_node_t * custom_node = (rmw_uxrce_node_t *)memory_node->data; custom_node->context = context->impl; + custom_node->domain_id = domain_id; node_handle = &custom_node->rmw_node; @@ -88,12 +89,10 @@ rmw_node_t * create_node( (uint16_t)domain_id, rmw_uxrce_entity_naming_buffer, UXR_REPLACE | UXR_REUSE); #else - static char xrce_node_name[RMW_UXRCE_NODE_NAME_MAX_LENGTH]; - if (strcmp(namespace_, "/") == 0) { - snprintf(xrce_node_name, RMW_UXRCE_NODE_NAME_MAX_LENGTH, "%s", name); + snprintf(node_name_buffer, sizeof(node_name_buffer), "%s", name); } else { - snprintf(xrce_node_name, RMW_UXRCE_NODE_NAME_MAX_LENGTH, "%s/%s", namespace_, name); + snprintf(node_name_buffer, sizeof(node_name_buffer), "%s/%s", namespace_, name); } participant_req = uxr_buffer_create_participant_bin( @@ -101,7 +100,7 @@ rmw_node_t * create_node( *custom_node->context->creation_stream, custom_node->participant_id, domain_id, - xrce_node_name, + node_name_buffer, UXR_REPLACE | UXR_REUSE); #endif /* ifdef RMW_UXRCE_USE_REFS */ diff --git a/rmw_microxrcedds_c/src/rmw_publisher.c b/rmw_microxrcedds_c/src/rmw_publisher.c index 2275c745..4a396f02 100644 --- a/rmw_microxrcedds_c/src/rmw_publisher.c +++ b/rmw_microxrcedds_c/src/rmw_publisher.c @@ -125,13 +125,10 @@ rmw_create_publisher( "%s", topic_name); rmw_publisher->topic_name = custom_publisher->topic.topic_name; - static char full_topic_name[RMW_UXRCE_TOPIC_NAME_MAX_LENGTH]; - static char type_name[RMW_UXRCE_TYPE_NAME_MAX_LENGTH]; - - generate_topic_name(topic_name, full_topic_name, sizeof(full_topic_name)); + generate_topic_name(topic_name, topic_buffer_1, sizeof(topic_buffer_1)); generate_type_name( - custom_publisher->topic.type_support_callbacks.msg, type_name, - sizeof(type_name)); + custom_publisher->topic.type_support_callbacks.msg, type_buffer_1, + sizeof(type_buffer_1)); uint16_t topic_req = UXR_INVALID_REQUEST_ID; @@ -140,8 +137,8 @@ rmw_create_publisher( *custom_node->context->creation_stream, custom_publisher->topic.topic_id, custom_node->participant_id, - full_topic_name, - type_name, + topic_buffer_1, + type_buffer_1, UXR_REPLACE | UXR_REUSE); if (!run_xrce_session( diff --git a/rmw_microxrcedds_c/src/rmw_service.c b/rmw_microxrcedds_c/src/rmw_service.c index fa3cb6a0..48ef971c 100644 --- a/rmw_microxrcedds_c/src/rmw_service.c +++ b/rmw_microxrcedds_c/src/rmw_service.c @@ -103,16 +103,12 @@ rmw_create_service( *custom_node->context->creation_stream, custom_service->service_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_service->topic.type_support_callbacks.srv, req_type_name, res_type_name, + custom_service->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); service_req = uxr_buffer_create_replier_bin( @@ -121,10 +117,10 @@ rmw_create_service( custom_service->service_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 */ diff --git a/rmw_microxrcedds_c/src/rmw_subscription.c b/rmw_microxrcedds_c/src/rmw_subscription.c index 526374c5..14dfccf6 100644 --- a/rmw_microxrcedds_c/src/rmw_subscription.c +++ b/rmw_microxrcedds_c/src/rmw_subscription.c @@ -117,13 +117,10 @@ rmw_create_subscription( sizeof(custom_subscription->topic.topic_name), "%s", topic_name); rmw_subscription->topic_name = custom_subscription->topic.topic_name; - static char full_topic_name[RMW_UXRCE_TOPIC_NAME_MAX_LENGTH]; - static char type_name[RMW_UXRCE_TYPE_NAME_MAX_LENGTH]; - - generate_topic_name(topic_name, full_topic_name, sizeof(full_topic_name)); + generate_topic_name(topic_name, topic_buffer_1, sizeof(topic_buffer_1)); generate_type_name( - custom_subscription->topic.type_support_callbacks.msg, type_name, - sizeof(type_name)); + custom_subscription->topic.type_support_callbacks.msg, type_buffer_1, + sizeof(type_buffer_1)); uint16_t topic_req = UXR_INVALID_REQUEST_ID; @@ -132,8 +129,8 @@ rmw_create_subscription( *custom_node->context->creation_stream, custom_subscription->topic.topic_id, custom_node->participant_id, - full_topic_name, - type_name, + topic_buffer_1, + type_buffer_1, UXR_REPLACE | UXR_REUSE); if (!run_xrce_session( diff --git a/rmw_microxrcedds_c/src/utils.c b/rmw_microxrcedds_c/src/utils.c index 653d32a6..4aa0f655 100644 --- a/rmw_microxrcedds_c/src/utils.c +++ b/rmw_microxrcedds_c/src/utils.c @@ -19,6 +19,14 @@ // TODO(pablogs9) Refactor all this file. +// Static buffers for name generation +char type_buffer_1[RMW_UXRCE_TYPE_NAME_MAX_LENGTH]; +char type_buffer_2[RMW_UXRCE_TYPE_NAME_MAX_LENGTH]; +char topic_buffer_1[RMW_UXRCE_TYPE_NAME_MAX_LENGTH]; +char topic_buffer_2[RMW_UXRCE_TYPE_NAME_MAX_LENGTH]; +char node_name_buffer[2 * RMW_UXRCE_NODE_NAME_MAX_LENGTH]; + +// Prefix strings static const char ros_topic_prefix[] = "rt"; static const char ros_request_prefix[] = "rq"; static const char ros_reply_prefix[] = "rr"; @@ -95,31 +103,6 @@ uxrQoS_t convert_qos_profile(const rmw_qos_profile_t * rmw_qos) return qos; } - -int build_participant_xml( - size_t domain_id, - const char * participant_name, - char xml[], - size_t buffer_size) -{ - (void)domain_id; - static const char format[] = - "" - "" - "" - "%s" - "" - "" - ""; - - int ret = snprintf(xml, buffer_size, format, participant_name); - if ((ret < 0) && (ret >= (int)buffer_size)) { - ret = 0; - } - - return ret; -} - int generate_service_topics( const char * service_name, char * request_topic, @@ -157,120 +140,6 @@ int generate_service_types( return 0; } -int build_service_xml( - const char * service_name_id, - const char * service_name, - bool requester, - const service_type_support_callbacks_t * members, - const rmw_qos_profile_t * qos_policies, - char xml[], - size_t buffer_size) -{ - int ret; - - static const char format[] = "" - "<%s profile_name=\"%s\" " - "service_name=\"%s\" " - "request_type=\"%s\" " - "reply_type=\"%s\">" - "%s" - "%s" - "" - ""; - - // Retrive request and response types - const rosidl_message_type_support_t * req_members = members->request_members_(); - const rosidl_message_type_support_t * res_members = members->response_members_(); - - const message_type_support_callbacks_t * req_callbacks = - (const message_type_support_callbacks_t *)req_members->data; - const message_type_support_callbacks_t * res_callbacks = - (const message_type_support_callbacks_t *)res_members->data; - - - static char req_type_name_buffer[RMW_UXRCE_TYPE_NAME_MAX_LENGTH]; - static char res_type_name_buffer[RMW_UXRCE_TYPE_NAME_MAX_LENGTH]; - - generate_type_name(req_callbacks, req_type_name_buffer, RMW_UXRCE_TYPE_NAME_MAX_LENGTH); - generate_type_name(res_callbacks, res_type_name_buffer, RMW_UXRCE_TYPE_NAME_MAX_LENGTH); - - // Generate request and reply topic names - char req_full_topic_name[RMW_UXRCE_TOPIC_NAME_MAX_LENGTH + 1 + sizeof(ros_request_prefix) + 1 + - sizeof(ros_request_subfix)]; - req_full_topic_name[0] = '\0'; - - char res_full_topic_name[RMW_UXRCE_TOPIC_NAME_MAX_LENGTH + 1 + sizeof(ros_reply_prefix) + 1 + - sizeof(ros_reply_subfix)]; - res_full_topic_name[0] = '\0'; - - if (!qos_policies->avoid_ros_namespace_conventions) { - ret = snprintf( - req_full_topic_name, sizeof(req_full_topic_name), "%s%s%s", ros_request_prefix, - service_name, ros_request_subfix); - if ((ret < 0) || (ret >= (int)sizeof(req_full_topic_name))) { - return 0; - } - - ret = snprintf( - res_full_topic_name, sizeof(res_full_topic_name), "%s%s%s", ros_reply_prefix, - service_name, ros_reply_subfix); - if ((ret < 0) || (ret >= (int)sizeof(res_full_topic_name))) { - return 0; - } - } else { - ret = snprintf(req_full_topic_name, sizeof(req_full_topic_name), "%s", service_name); - if ((ret < 0) || (ret >= (int)sizeof(req_full_topic_name))) { - return 0; - } - ret = snprintf(res_full_topic_name, sizeof(res_full_topic_name), "%s", service_name); - if ((ret < 0) || (ret >= (int)sizeof(req_full_topic_name))) { - return 0; - } - } - - - ret = snprintf( - xml, buffer_size, format, - requester ? "requester" : "replier", - service_name_id, - service_name, - req_type_name_buffer, - res_type_name_buffer, - req_full_topic_name, - res_full_topic_name, - requester ? "requester" : "replier" - ); - if ((ret < 0) && (ret >= (int)buffer_size)) { - ret = 0; - } - - return ret; -} - -int build_publisher_xml( - const char * publisher_name, - char xml[], - size_t buffer_size) -{ - (void)publisher_name; - (void)buffer_size; - - xml[0] = '\0'; - return 1; -} - -int build_subscriber_xml( - const char * subscriber_name, - char xml[], - size_t buffer_size) -{ - (void)subscriber_name; - (void)buffer_size; - - xml[0] = '\0'; - return 1; -} - int generate_name( const uxrObjectId * id, char name[], @@ -333,158 +202,6 @@ int generate_topic_name( return ret; } -int build_topic_xml( - const char * topic_name, - const message_type_support_callbacks_t * members, - const rmw_qos_profile_t * qos_policies, - char xml[], - size_t buffer_size) -{ - static const char format[] = - "" - "" - "%s" - "%s" - "" - ""; - - int ret = 0; - static char type_name_buffer[RMW_UXRCE_TYPE_NAME_MAX_LENGTH]; - - if (RMW_UXRCE_TOPIC_NAME_MAX_LENGTH >= strlen(topic_name) && - 0 != generate_type_name(members, type_name_buffer, sizeof(type_name_buffer))) - { - char full_topic_name[RMW_UXRCE_TOPIC_NAME_MAX_LENGTH + 1 + sizeof(ros_topic_prefix)]; - - if (!qos_policies->avoid_ros_namespace_conventions) { - ret = snprintf( - full_topic_name, sizeof(full_topic_name), "%s%s", ros_topic_prefix, - topic_name); - if ((ret < 0) && (ret >= (int)buffer_size)) { - return 0; - } - } else { - ret = snprintf(full_topic_name, sizeof(full_topic_name), "%s", topic_name); - if ((ret < 0) && (ret >= (int)buffer_size)) { - return 0; - } - } - - ret = snprintf(xml, buffer_size, format, full_topic_name, type_name_buffer); - if ((ret < 0) && (ret >= (int)buffer_size)) { - ret = 0; - } - } - - return ret; -} - -int build_xml( - const char * format, - const char * topic_name, - const message_type_support_callbacks_t * members, - const rmw_qos_profile_t * qos_policies, - char xml[], - size_t buffer_size) -{ - int ret = 0; - static char type_name_buffer[RMW_UXRCE_TYPE_NAME_MAX_LENGTH]; - - if (0 != generate_type_name(members, type_name_buffer, sizeof(type_name_buffer))) { - char full_topic_name[RMW_UXRCE_TOPIC_NAME_MAX_LENGTH + 1 + sizeof(ros_topic_prefix)]; - full_topic_name[0] = '\0'; - - if (!qos_policies->avoid_ros_namespace_conventions) { - ret = snprintf( - full_topic_name, sizeof(full_topic_name), "%s%s", ros_topic_prefix, - topic_name); - if ((ret < 0) && (ret >= (int)buffer_size)) { - return 0; - } - } else { - ret = snprintf(full_topic_name, sizeof(full_topic_name), "%s", topic_name); - if ((ret < 0) && (ret >= (int)buffer_size)) { - return 0; - } - } - - ret = snprintf( - xml, - buffer_size, - format, - (qos_policies->reliability == RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT) ? - "BEST_EFFORT" : "RELIABLE", - full_topic_name, - type_name_buffer); - - if ((ret < 0) && (ret >= (int)buffer_size)) { - ret = 0; - } - } - - return ret; -} - -int build_datawriter_xml( - const char * topic_name, - const message_type_support_callbacks_t * members, - const rmw_qos_profile_t * qos_policies, - char xml[], - size_t buffer_size) -{ - static const char format[] = - "" - "" - "PREALLOCATED_WITH_REALLOC" - "" - "" - "%s" - "" - "" - "" - "NO_KEY" - "%s" - "%s" - "" - "KEEP_ALL" - "" - "" - "" - ""; - - return build_xml(format, topic_name, members, qos_policies, xml, buffer_size); -} - -int build_datareader_xml( - const char * topic_name, - const message_type_support_callbacks_t * members, - const rmw_qos_profile_t * qos_policies, - char xml[], - size_t buffer_size) -{ - static const char format[] = - "" - "" - "PREALLOCATED_WITH_REALLOC" - "" - "" - "%s" - "" - "" - "" - "NO_KEY" - "%s" - "%s" - "" - "KEEP_ALL" - "" - "" - "" - ""; - - return build_xml(format, topic_name, members, qos_policies, xml, buffer_size); -} - bool build_participant_profile( char profile_name[], size_t buffer_size)