From 27460e881e42265a2c0f0127869c862cbe7a5083 Mon Sep 17 00:00:00 2001 From: Pablo Garrido Date: Mon, 28 Mar 2022 12:34:51 +0200 Subject: [PATCH 1/6] Initial Signed-off-by: Pablo Garrido --- rmw_microxrcedds_c/src/rmw_microros/ping.c | 277 ++++++++++++++++++ .../src/rmw_microros_internal/types.h | 1 + rmw_microxrcedds_c/src/rmw_node.c | 1 + 3 files changed, 279 insertions(+) diff --git a/rmw_microxrcedds_c/src/rmw_microros/ping.c b/rmw_microxrcedds_c/src/rmw_microros/ping.c index 140eaaa7..d57a549a 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,279 @@ rmw_ret_t rmw_uros_ping_agent_options( return success ? RMW_RET_OK : RMW_RET_ERROR; } + +rmw_ret_t rmw_uros_regenerate_entities() +{ + bool success = true; + + rmw_ret_t ping_ret = rmw_uros_ping_agent(1000, 1); + + if (RMW_RET_OK == ping_ret) + { + return RMW_RET_OK; + } + + // Regenerate sessions + { + rmw_uxrce_mempool_item_t * item = session_memory.allocateditems; + do { + rmw_context_impl_t * context = (rmw_context_impl_t *)item->data; + + uxr_create_session(&context->session); + + item = item->next; + } while (NULL != item); + } + + // Regenerate nodes + { + rmw_uxrce_mempool_item_t * item = node_memory.allocateditems; + do { + rmw_uxrce_node_t * custom_node = (rmw_uxrce_node_t *)item->data; + uint16_t req = UXR_INVALID_REQUEST_ID; + + // TODO(pablogs): Reuse one static buffer. + static char xrce_node_name[RMW_UXRCE_NODE_NAME_MAX_LENGTH + RMW_UXRCE_NODE_NAME_MAX_LENGTH + 1]; + + if (strcmp(custom_node->node_namespace, "/") == 0) { + snprintf(xrce_node_name, RMW_UXRCE_NODE_NAME_MAX_LENGTH, "%s", custom_node->node_name); + } else { + snprintf(xrce_node_name, sizeof(xrce_node_name), "%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, + xrce_node_name, + UXR_REPLACE | UXR_REUSE); + + run_xrce_session(custom_node->context, custom_node->context->creation_stream, req,custom_node->context->creation_timeout); + + item = item->next; + } while (NULL != item); + } + + // Regenerate publishers + { + rmw_uxrce_mempool_item_t * item = publisher_memory.allocateditems; + do { + rmw_uxrce_publisher_t * custom_publisher = (rmw_uxrce_publisher_t *)item->data; + uint16_t req = UXR_INVALID_REQUEST_ID; + + static char full_topic_name[RMW_UXRCE_TOPIC_NAME_MAX_LENGTH]; + static char type_name[RMW_UXRCE_TYPE_NAME_MAX_LENGTH]; + + generate_topic_name(custom_publishers->topic.topic_name, full_topic_name, sizeof(full_topic_name)); + generate_type_name(custom_publisher->topic.type_support_callbacks.msg, type_name, sizeof(type_name)); + + 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, + full_topic_name, + type_name, + 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; + } while (NULL != item); + } + + // Regenerate subscribers + { + rmw_uxrce_mempool_item_t * item = subscription_memory.allocateditems; + do { + rmw_uxrce_subscription_t * custom_subscription = (rmw_uxrce_subscription_t *)item->data; + uint16_t req = UXR_INVALID_REQUEST_ID; + + static char full_topic_name[RMW_UXRCE_TOPIC_NAME_MAX_LENGTH]; + static char type_name[RMW_UXRCE_TYPE_NAME_MAX_LENGTH]; + + generate_topic_name(custom_subscription->topic.topic_name, full_topic_name, sizeof(full_topic_name)); + generate_type_name(custom_subscription->topic.type_support_callbacks.msg, type_name, sizeof(type_name)); + + 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, + full_topic_name, + type_name, + 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; + } while (NULL != item); + } + + // Regenerate requesters + { + rmw_uxrce_mempool_item_t * item = service_memory.allocateditems; + do { + rmw_uxrce_service_t * custom_service = (rmw_uxrce_service_t *)item->data; + uint16_t req = UXR_INVALID_REQUEST_ID; + + 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, + 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( + custom_service->topic.topic_name, req_topic_name, res_topic_name, + 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, + req_type_name, + res_type_name, + req_topic_name, + res_topic_name, + 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; + } while (NULL != item); + } + + // Regenerate repliers + { + rmw_uxrce_mempool_item_t * item = client_memory.allocateditems; + do { + rmw_uxrce_client_t * custom_client = (rmw_uxrce_client_t *)item->data; + uint16_t req = UXR_INVALID_REQUEST_ID; + + 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, + 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( + custom_client->topic.topic_name, req_topic_name, res_topic_name, + 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, + req_type_name, + res_type_name, + req_topic_name, + res_topic_name, + 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; + } while (NULL != item); + } + + 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_node.c b/rmw_microxrcedds_c/src/rmw_node.c index e6503628..219fb146 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; From 0ff8dbaebbf29b232f0aee9112ae4f8a26551678 Mon Sep 17 00:00:00 2001 From: Pablo Garrido Date: Mon, 28 Mar 2022 15:04:15 +0200 Subject: [PATCH 2/6] Update Signed-off-by: Pablo Garrido --- rmw_microxrcedds_c/src/rmw_microros/ping.c | 26 +++++++++++----------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/rmw_microxrcedds_c/src/rmw_microros/ping.c b/rmw_microxrcedds_c/src/rmw_microros/ping.c index d57a549a..5ccd883e 100644 --- a/rmw_microxrcedds_c/src/rmw_microros/ping.c +++ b/rmw_microxrcedds_c/src/rmw_microros/ping.c @@ -113,7 +113,7 @@ rmw_ret_t rmw_uros_regenerate_entities() rmw_ret_t ping_ret = rmw_uros_ping_agent(1000, 1); - if (RMW_RET_OK == ping_ret) + if (RMW_RET_OK != ping_ret) { return RMW_RET_OK; } @@ -121,19 +121,19 @@ rmw_ret_t rmw_uros_regenerate_entities() // Regenerate sessions { rmw_uxrce_mempool_item_t * item = session_memory.allocateditems; - do { + while( NULL != item) { rmw_context_impl_t * context = (rmw_context_impl_t *)item->data; uxr_create_session(&context->session); item = item->next; - } while (NULL != item); + } } // Regenerate nodes { rmw_uxrce_mempool_item_t * item = node_memory.allocateditems; - do { + while( NULL != item) { rmw_uxrce_node_t * custom_node = (rmw_uxrce_node_t *)item->data; uint16_t req = UXR_INVALID_REQUEST_ID; @@ -157,13 +157,13 @@ rmw_ret_t rmw_uros_regenerate_entities() run_xrce_session(custom_node->context, custom_node->context->creation_stream, req,custom_node->context->creation_timeout); item = item->next; - } while (NULL != item); + } } // Regenerate publishers { rmw_uxrce_mempool_item_t * item = publisher_memory.allocateditems; - do { + while( NULL != item) { rmw_uxrce_publisher_t * custom_publisher = (rmw_uxrce_publisher_t *)item->data; uint16_t req = UXR_INVALID_REQUEST_ID; @@ -205,13 +205,13 @@ rmw_ret_t rmw_uros_regenerate_entities() 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; - } while (NULL != item); + }; } // Regenerate subscribers { rmw_uxrce_mempool_item_t * item = subscription_memory.allocateditems; - do { + while( NULL != item) { rmw_uxrce_subscription_t * custom_subscription = (rmw_uxrce_subscription_t *)item->data; uint16_t req = UXR_INVALID_REQUEST_ID; @@ -269,13 +269,13 @@ rmw_ret_t rmw_uros_regenerate_entities() data_request_stream_id, &delivery_control); item = item->next; - } while (NULL != item); + } } // Regenerate requesters { rmw_uxrce_mempool_item_t * item = service_memory.allocateditems; - do { + while( NULL != item) { rmw_uxrce_service_t * custom_service = (rmw_uxrce_service_t *)item->data; uint16_t req = UXR_INVALID_REQUEST_ID; @@ -322,13 +322,13 @@ rmw_ret_t rmw_uros_regenerate_entities() *custom_service->owner_node->context->creation_stream, custom_service->service_id, data_request_stream_id, &delivery_control); item = item->next; - } while (NULL != item); + } } // Regenerate repliers { rmw_uxrce_mempool_item_t * item = client_memory.allocateditems; - do { + while( NULL != item) { rmw_uxrce_client_t * custom_client = (rmw_uxrce_client_t *)item->data; uint16_t req = UXR_INVALID_REQUEST_ID; @@ -376,7 +376,7 @@ rmw_ret_t rmw_uros_regenerate_entities() data_request_stream_id, &delivery_control); item = item->next; - } while (NULL != item); + } } return success ? RMW_RET_OK : RMW_RET_ERROR; From 9982746cb7b5c6dd456ad3697bbb024a1e7bf852 Mon Sep 17 00:00:00 2001 From: Pablo Garrido Date: Mon, 28 Mar 2022 15:26:37 +0200 Subject: [PATCH 3/6] Update Signed-off-by: Pablo Garrido --- rmw_microxrcedds_c/src/rmw_microros/ping.c | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/rmw_microxrcedds_c/src/rmw_microros/ping.c b/rmw_microxrcedds_c/src/rmw_microros/ping.c index 5ccd883e..dc938280 100644 --- a/rmw_microxrcedds_c/src/rmw_microros/ping.c +++ b/rmw_microxrcedds_c/src/rmw_microros/ping.c @@ -111,11 +111,18 @@ rmw_ret_t rmw_uros_regenerate_entities() { bool success = true; - rmw_ret_t ping_ret = rmw_uros_ping_agent(1000, 1); + 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; - if (RMW_RET_OK != ping_ret) + bool ping_success = uxr_ping_agent_attempts(context->session.comm, 1000, 1); + + if (!ping_success) { - return RMW_RET_OK; + return RMW_RET_ERROR; } // Regenerate sessions @@ -380,5 +387,4 @@ rmw_ret_t rmw_uros_regenerate_entities() } return success ? RMW_RET_OK : RMW_RET_ERROR; - } From bb9c113fe7b9acdf6d0a6ceb51a8fe3400075cb4 Mon Sep 17 00:00:00 2001 From: Pablo Garrido Date: Mon, 28 Mar 2022 16:01:19 +0200 Subject: [PATCH 4/6] Update Signed-off-by: Pablo Garrido --- rmw_microxrcedds_c/src/rmw_client.c | 17 +- rmw_microxrcedds_c/src/rmw_microros/ping.c | 64 ++-- .../src/rmw_microros_internal/utils.h | 42 +-- rmw_microxrcedds_c/src/rmw_node.c | 8 +- rmw_microxrcedds_c/src/rmw_publisher.c | 13 +- rmw_microxrcedds_c/src/rmw_service.c | 18 +- rmw_microxrcedds_c/src/rmw_subscription.c | 13 +- rmw_microxrcedds_c/src/utils.c | 299 +----------------- 8 files changed, 67 insertions(+), 407 deletions(-) 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 dc938280..1138af01 100644 --- a/rmw_microxrcedds_c/src/rmw_microros/ping.c +++ b/rmw_microxrcedds_c/src/rmw_microros/ping.c @@ -144,13 +144,10 @@ rmw_ret_t rmw_uros_regenerate_entities() rmw_uxrce_node_t * custom_node = (rmw_uxrce_node_t *)item->data; uint16_t req = UXR_INVALID_REQUEST_ID; - // TODO(pablogs): Reuse one static buffer. - static char xrce_node_name[RMW_UXRCE_NODE_NAME_MAX_LENGTH + RMW_UXRCE_NODE_NAME_MAX_LENGTH + 1]; - if (strcmp(custom_node->node_namespace, "/") == 0) { - snprintf(xrce_node_name, RMW_UXRCE_NODE_NAME_MAX_LENGTH, "%s", custom_node->node_name); + snprintf(node_name_buffer, sizeof(node_name_buffer), "%s", custom_node->node_name); } else { - snprintf(xrce_node_name, sizeof(xrce_node_name), "%s/%s", custom_node->node_namespace, custom_node->node_name); + snprintf(node_name_buffer, sizeof(node_name_buffer), "%s/%s", custom_node->node_namespace, custom_node->node_name); } req = uxr_buffer_create_participant_bin( @@ -158,7 +155,7 @@ rmw_ret_t rmw_uros_regenerate_entities() *custom_node->context->creation_stream, custom_node->participant_id, custom_node->domain_id, - xrce_node_name, + node_name_buffer, UXR_REPLACE | UXR_REUSE); run_xrce_session(custom_node->context, custom_node->context->creation_stream, req,custom_node->context->creation_timeout); @@ -174,19 +171,16 @@ rmw_ret_t rmw_uros_regenerate_entities() rmw_uxrce_publisher_t * custom_publisher = (rmw_uxrce_publisher_t *)item->data; uint16_t req = UXR_INVALID_REQUEST_ID; - static char full_topic_name[RMW_UXRCE_TOPIC_NAME_MAX_LENGTH]; - static char type_name[RMW_UXRCE_TYPE_NAME_MAX_LENGTH]; - - generate_topic_name(custom_publishers->topic.topic_name, full_topic_name, sizeof(full_topic_name)); - generate_type_name(custom_publisher->topic.type_support_callbacks.msg, type_name, sizeof(type_name)); + 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, - full_topic_name, - type_name, + 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); @@ -222,19 +216,16 @@ rmw_ret_t rmw_uros_regenerate_entities() rmw_uxrce_subscription_t * custom_subscription = (rmw_uxrce_subscription_t *)item->data; uint16_t req = UXR_INVALID_REQUEST_ID; - static char full_topic_name[RMW_UXRCE_TOPIC_NAME_MAX_LENGTH]; - static char type_name[RMW_UXRCE_TYPE_NAME_MAX_LENGTH]; - - generate_topic_name(custom_subscription->topic.topic_name, full_topic_name, sizeof(full_topic_name)); - generate_type_name(custom_subscription->topic.type_support_callbacks.msg, type_name, sizeof(type_name)); + 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, - full_topic_name, - type_name, + 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); @@ -286,16 +277,12 @@ rmw_ret_t rmw_uros_regenerate_entities() rmw_uxrce_service_t * custom_service = (rmw_uxrce_service_t *)item->data; uint16_t req = UXR_INVALID_REQUEST_ID; - 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( - custom_service->topic.topic_name, req_topic_name, res_topic_name, + custom_service->topic.topic_name, topic_buffer_1, topic_buffer_2, RMW_UXRCE_TOPIC_NAME_MAX_LENGTH); req = uxr_buffer_create_replier_bin( @@ -304,10 +291,10 @@ rmw_ret_t rmw_uros_regenerate_entities() custom_service->service_id, custom_service->owner_node->participant_id, (char *) custom_service->topic.topic_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(&custom_service->qos), UXR_REPLACE | UXR_REUSE); @@ -339,16 +326,11 @@ rmw_ret_t rmw_uros_regenerate_entities() rmw_uxrce_client_t * custom_client = (rmw_uxrce_client_t *)item->data; uint16_t req = UXR_INVALID_REQUEST_ID; - 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( - custom_client->topic.topic_name, req_topic_name, res_topic_name, + custom_client->topic.topic_name, topic_buffer_1, topic_buffer_2, RMW_UXRCE_TOPIC_NAME_MAX_LENGTH); req = uxr_buffer_create_requester_bin( @@ -357,10 +339,10 @@ rmw_ret_t rmw_uros_regenerate_entities() custom_client->client_id, custom_client->owner_node->participant_id, (char *) custom_client->topic.topic_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(&custom_client->qos), UXR_REPLACE | UXR_REUSE); diff --git a/rmw_microxrcedds_c/src/rmw_microros_internal/utils.h b/rmw_microxrcedds_c/src/rmw_microros_internal/utils.h index eac326c3..3a1dace6 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 219fb146..2b372db0 100644 --- a/rmw_microxrcedds_c/src/rmw_node.c +++ b/rmw_microxrcedds_c/src/rmw_node.c @@ -89,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( @@ -102,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..67957d74 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) From e3e17ba5766f52e729bd1791997c33cf22c16d47 Mon Sep 17 00:00:00 2001 From: Pablo Garrido Date: Mon, 28 Mar 2022 16:05:11 +0200 Subject: [PATCH 5/6] Uncrustify Signed-off-by: Pablo Garrido --- rmw_microxrcedds_c/src/rmw_microros/ping.c | 106 ++++++++++++------ .../src/rmw_microros_internal/utils.h | 2 +- rmw_microxrcedds_c/src/utils.c | 2 +- 3 files changed, 73 insertions(+), 37 deletions(-) diff --git a/rmw_microxrcedds_c/src/rmw_microros/ping.c b/rmw_microxrcedds_c/src/rmw_microros/ping.c index 1138af01..58856158 100644 --- a/rmw_microxrcedds_c/src/rmw_microros/ping.c +++ b/rmw_microxrcedds_c/src/rmw_microros/ping.c @@ -120,15 +120,14 @@ rmw_ret_t rmw_uros_regenerate_entities() bool ping_success = uxr_ping_agent_attempts(context->session.comm, 1000, 1); - if (!ping_success) - { + if (!ping_success) { return RMW_RET_ERROR; } // Regenerate sessions { rmw_uxrce_mempool_item_t * item = session_memory.allocateditems; - while( NULL != item) { + while (NULL != item) { rmw_context_impl_t * context = (rmw_context_impl_t *)item->data; uxr_create_session(&context->session); @@ -140,14 +139,16 @@ rmw_ret_t rmw_uros_regenerate_entities() // Regenerate nodes { rmw_uxrce_mempool_item_t * item = node_memory.allocateditems; - while( NULL != item) { + 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); + snprintf( + node_name_buffer, sizeof(node_name_buffer), "%s/%s", custom_node->node_namespace, + custom_node->node_name); } req = uxr_buffer_create_participant_bin( @@ -158,7 +159,9 @@ rmw_ret_t rmw_uros_regenerate_entities() node_name_buffer, UXR_REPLACE | UXR_REUSE); - run_xrce_session(custom_node->context, custom_node->context->creation_stream, req,custom_node->context->creation_timeout); + run_xrce_session( + custom_node->context, custom_node->context->creation_stream, req, + custom_node->context->creation_timeout); item = item->next; } @@ -167,12 +170,16 @@ rmw_ret_t rmw_uros_regenerate_entities() // Regenerate publishers { rmw_uxrce_mempool_item_t * item = publisher_memory.allocateditems; - while( NULL != item) { + 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)); + 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, @@ -183,7 +190,10 @@ rmw_ret_t rmw_uros_regenerate_entities() 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); + 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, @@ -192,7 +202,10 @@ rmw_ret_t rmw_uros_regenerate_entities() 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); + 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, @@ -203,21 +216,28 @@ rmw_ret_t rmw_uros_regenerate_entities() 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); + 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) { + 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)); + 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, @@ -228,7 +248,10 @@ rmw_ret_t rmw_uros_regenerate_entities() 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); + 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, @@ -237,7 +260,10 @@ rmw_ret_t rmw_uros_regenerate_entities() 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); + 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, @@ -248,7 +274,10 @@ rmw_ret_t rmw_uros_regenerate_entities() 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); + 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; @@ -257,13 +286,14 @@ rmw_ret_t rmw_uros_regenerate_entities() 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; + (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, + *custom_subscription->owner_node->context->creation_stream, + custom_subscription->datareader_id, data_request_stream_id, &delivery_control); item = item->next; @@ -273,7 +303,7 @@ rmw_ret_t rmw_uros_regenerate_entities() // Regenerate requesters { rmw_uxrce_mempool_item_t * item = service_memory.allocateditems; - while( NULL != item) { + while (NULL != item) { rmw_uxrce_service_t * custom_service = (rmw_uxrce_service_t *)item->data; uint16_t req = UXR_INVALID_REQUEST_ID; @@ -298,7 +328,10 @@ rmw_ret_t rmw_uros_regenerate_entities() 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); + 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) ? @@ -306,10 +339,10 @@ rmw_ret_t rmw_uros_regenerate_entities() 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; + 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, @@ -322,7 +355,7 @@ rmw_ret_t rmw_uros_regenerate_entities() // Regenerate repliers { rmw_uxrce_mempool_item_t * item = client_memory.allocateditems; - while( NULL != item) { + while (NULL != item) { rmw_uxrce_client_t * custom_client = (rmw_uxrce_client_t *)item->data; uint16_t req = UXR_INVALID_REQUEST_ID; @@ -346,7 +379,10 @@ rmw_ret_t rmw_uros_regenerate_entities() 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); + 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) ? @@ -354,10 +390,10 @@ rmw_ret_t rmw_uros_regenerate_entities() 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; + 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, diff --git a/rmw_microxrcedds_c/src/rmw_microros_internal/utils.h b/rmw_microxrcedds_c/src/rmw_microros_internal/utils.h index 3a1dace6..09e237af 100644 --- a/rmw_microxrcedds_c/src/rmw_microros_internal/utils.h +++ b/rmw_microxrcedds_c/src/rmw_microros_internal/utils.h @@ -24,7 +24,7 @@ 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]; +extern char node_name_buffer[2 * RMW_UXRCE_NODE_NAME_MAX_LENGTH]; bool run_xrce_session( rmw_context_impl_t * context, diff --git a/rmw_microxrcedds_c/src/utils.c b/rmw_microxrcedds_c/src/utils.c index 67957d74..4aa0f655 100644 --- a/rmw_microxrcedds_c/src/utils.c +++ b/rmw_microxrcedds_c/src/utils.c @@ -24,7 +24,7 @@ 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]; +char node_name_buffer[2 * RMW_UXRCE_NODE_NAME_MAX_LENGTH]; // Prefix strings static const char ros_topic_prefix[] = "rt"; From 53559d86dde24a76af37238ef7b56a2dfdeda918 Mon Sep 17 00:00:00 2001 From: Pablo Garrido Date: Mon, 28 Mar 2022 16:17:15 +0200 Subject: [PATCH 6/6] Update Signed-off-by: Pablo Garrido --- rmw_microxrcedds_c/include/rmw_microros/ping.h | 7 +++++++ rmw_microxrcedds_c/src/rmw_microros/ping.c | 6 ++++-- 2 files changed, 11 insertions(+), 2 deletions(-) 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_microros/ping.c b/rmw_microxrcedds_c/src/rmw_microros/ping.c index 58856158..f328ba6d 100644 --- a/rmw_microxrcedds_c/src/rmw_microros/ping.c +++ b/rmw_microxrcedds_c/src/rmw_microros/ping.c @@ -107,7 +107,9 @@ rmw_ret_t rmw_uros_ping_agent_options( return success ? RMW_RET_OK : RMW_RET_ERROR; } -rmw_ret_t rmw_uros_regenerate_entities() +rmw_ret_t rmw_uros_regenerate_entities( + const int timeout_ms, + const uint8_t attempts) { bool success = true; @@ -118,7 +120,7 @@ rmw_ret_t rmw_uros_regenerate_entities() 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, 1000, 1); + bool ping_success = uxr_ping_agent_attempts(context->session.comm, timeout_ms, attempts); if (!ping_success) { return RMW_RET_ERROR;