From fb845acd7663072b972fd53b4e4d80806adbb993 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 30 May 2017 19:36:54 -0700 Subject: [PATCH 1/4] implement avoid_ros_namespace_conventions QoS option --- rmw_connext_cpp/src/functions.cpp | 323 +++++++++++----------- rmw_connext_dynamic_cpp/src/functions.cpp | 20 ++ 2 files changed, 188 insertions(+), 155 deletions(-) diff --git a/rmw_connext_cpp/src/functions.cpp b/rmw_connext_cpp/src/functions.cpp index 23059e37..3e8c982f 100644 --- a/rmw_connext_cpp/src/functions.cpp +++ b/rmw_connext_cpp/src/functions.cpp @@ -40,8 +40,7 @@ # pragma GCC diagnostic pop #endif -// TODO(karsten1987): Introduce rcutils.h -#include "rcutils/concat.h" +#include "rcutils/format_string.h" #include "rcutils/types.h" #include "rcutils/split.h" @@ -214,6 +213,57 @@ rmw_destroy_node(rmw_node_t * node) return destroy_node(rti_connext_identifier, node); } +bool +__process_topic_name( + const char * topic_name, + bool avoid_ros_namespace_conventions, + char ** topic_str, + char ** partition_str) +{ + bool success = true; + rcutils_string_array_t name_tokens; + + // allocates memory, but doesn't have to be freed. + // partition operater takes ownership of it. + name_tokens = rcutils_split_last(topic_name, '/'); + if (name_tokens.size == 1) { + if (!avoid_ros_namespace_conventions) { + *partition_str = DDS_String_dup(ros_topics_prefix); + } + *topic_str = DDS_String_dup(name_tokens.data[0]); + } else if (name_tokens.size == 2) { + if (avoid_ros_namespace_conventions) { + // no ros_topics_prefix, so store the user's namespace directly + *partition_str = DDS_String_dup(name_tokens.data[0]); + } else { + // concat the ros_topics_prefix with the user's namespace + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + char * concat_str = + rcutils_format_string(allocator, "%s/%s", ros_topics_prefix, name_tokens.data[0]); + if (!concat_str) { + RMW_SET_ERROR_MSG("could not allocate memory for partition string") + success = false; + goto end; + } + *partition_str = DDS_String_dup(concat_str); + allocator.deallocate(concat_str, allocator.state); + } + // Connext will call deallocate on this, passing ownership to connext + *topic_str = DDS_String_dup(name_tokens.data[1]); + } else { + RMW_SET_ERROR_MSG("Illformated topic name") + success = false; + } + +end: + // all necessary strings are copied into connext + // free that memory + if (rcutils_string_array_fini(&name_tokens) != RCUTILS_RET_OK) { + fprintf(stderr, "Failed to destroy the token string array\n"); + } + return success; +} + rmw_publisher_t * rmw_create_publisher( const rmw_node_t * node, @@ -274,7 +324,6 @@ rmw_create_publisher( rmw_publisher_t * publisher = nullptr; // memory allocations for namespacing - rcutils_string_array_t name_tokens; char * partition_str = nullptr; char * topic_str = nullptr; @@ -297,36 +346,21 @@ rmw_create_publisher( goto fail; } - // allocates memory, but doesn't have to be freed. - // partition operater takes ownership of it. - name_tokens = rcutils_split_last(topic_name, '/'); - if (name_tokens.size == 1) { - partition_str = DDS_String_dup(ros_topics_prefix); - topic_str = DDS_String_dup(name_tokens.data[0]); - } else if (name_tokens.size == 2) { - // TODO(Karsten1987): Fix utility function for this - size_t partition_length = strlen(ros_topics_prefix) + strlen(name_tokens.data[0]) + 2; - char * concat_str = reinterpret_cast(rmw_allocate(partition_length * sizeof(char))); - snprintf(concat_str, partition_length, "%s/%s", ros_topics_prefix, name_tokens.data[0]); - // Connext will call deallocate on this, passing ownership to connext - partition_str = DDS_String_dup(concat_str); - topic_str = DDS_String_dup(name_tokens.data[1]); - // free temporary memory - rmw_free(concat_str); - } else { - RMW_SET_ERROR_MSG("Illformated topic name"); + if (!__process_topic_name( + topic_name, + qos_profile->avoid_ros_namespace_conventions, + &topic_str, + &partition_str)) + { goto fail; } - // all necessary strings are copied into connext - // free that memory - if (rcutils_string_array_fini(&name_tokens) != RCUTILS_RET_OK) { - fprintf(stderr, "Failed to destroy the token string array\n"); - } // we have to set the partition array to length 1 // and then set the partition_str in it - publisher_qos.partition.name.ensure_length(1, 1); - publisher_qos.partition.name[0] = partition_str; + if (partition_str && strlen(partition_str) != 0) { // only set if not empty + publisher_qos.partition.name.ensure_length(1, 1); + publisher_qos.partition.name[0] = partition_str; + } dds_publisher = participant->create_publisher( publisher_qos, NULL, DDS_STATUS_MASK_NONE); @@ -451,11 +485,6 @@ rmw_create_publisher( rmw_free(buf); } - // cleanup namespacing - if (rcutils_string_array_fini(&name_tokens) != RCUTILS_RET_OK) { - fprintf(stderr, "Failed to destroy the token string array\n"); - } - return NULL; } @@ -649,36 +678,21 @@ rmw_create_subscription(const rmw_node_t * node, goto fail; } - // allocates memory, but doesn't have to be freed. - // partition operater takes ownership of it. - name_tokens = rcutils_split_last(topic_name, '/'); - if (name_tokens.size == 1) { - partition_str = DDS_String_dup(ros_topics_prefix); - topic_str = DDS_String_dup(name_tokens.data[0]); - } else if (name_tokens.size == 2) { - // TODO(Karsten1987): Fix utility function for this - size_t partition_length = strlen(ros_topics_prefix) + strlen(name_tokens.data[0]) + 2; - char * concat_str = reinterpret_cast(rmw_allocate(partition_length * sizeof(char))); - snprintf(concat_str, partition_length, "%s/%s", ros_topics_prefix, name_tokens.data[0]); - // Connext will call deallocate on this, passing ownership to connext - partition_str = DDS_String_dup(concat_str); - topic_str = DDS_String_dup(name_tokens.data[1]); - // free temporary memory - rmw_free(concat_str); - } else { - RMW_SET_ERROR_MSG("Illformated topic name"); + if (!__process_topic_name( + topic_name, + qos_profile->avoid_ros_namespace_conventions, + &topic_str, + &partition_str)) + { goto fail; } - // all necessary strings are copied into connext - // free that memory - if (rcutils_string_array_fini(&name_tokens) != RCUTILS_RET_OK) { - fprintf(stderr, "Failed to destroy the token string array\n"); - } // we have to set the partition array to length 1 // and then set the partition_str in it - subscriber_qos.partition.name.ensure_length(1, 1); - subscriber_qos.partition.name[0] = partition_str; + if (partition_str && strlen(partition_str) != 0) { // only set if not empty + subscriber_qos.partition.name.ensure_length(1, 1); + subscriber_qos.partition.name[0] = partition_str; + } dds_subscriber = participant->create_subscriber( subscriber_qos, NULL, DDS_STATUS_MASK_NONE); @@ -1023,6 +1037,71 @@ rmw_wait(rmw_subscriptions_t * subscriptions, wait_timeout); } +bool +__process_service_name( + const char * service_name, + bool avoid_ros_namespace_conventions, + char ** request_partition_str, + char ** response_partition_str, + char ** service_str) +{ + bool success = true; + rcutils_string_array_t name_tokens = rcutils_get_zero_initialized_string_array(); + + // get actual data subsription object + // allocates memory, but doesn't have to be freed. + // partition operater takes ownership of it + name_tokens = rcutils_split_last(service_name, '/'); + if (name_tokens.size == 1) { + if (!avoid_ros_namespace_conventions) { + *request_partition_str = DDS_String_dup(ros_service_requester_prefix); + *response_partition_str = DDS_String_dup(ros_service_response_prefix); + } + *service_str = DDS_String_dup(name_tokens.data[0]); + } else if (name_tokens.size == 2) { + if (avoid_ros_namespace_conventions) { + // no ros_service_*_prefix, so store the user's namespace directly + *request_partition_str = DDS_String_dup(name_tokens.data[0]); + *response_partition_str = DDS_String_dup(name_tokens.data[0]); + } else { + // concat the ros_service_*_prefix with the user's namespace + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + char * request_concat_str = rcutils_format_string( + allocator, + "%s/%s", ros_service_requester_prefix, name_tokens.data[0]); + if (!request_concat_str) { + RMW_SET_ERROR_MSG("could not allocate memory for partition string") + success = false; + goto end; + } + char * response_concat_str = rcutils_format_string( + allocator, + "%s/%s", ros_service_response_prefix, name_tokens.data[0]); + if (!response_concat_str) { + allocator.deallocate(request_concat_str, allocator.state); + RMW_SET_ERROR_MSG("could not allocate memory for partition string") + success = false; + goto end; + } + *request_partition_str = DDS_String_dup(request_concat_str); + *response_partition_str = DDS_String_dup(response_concat_str); + allocator.deallocate(request_concat_str, allocator.state); + allocator.deallocate(response_concat_str, allocator.state); + } + *service_str = DDS_String_dup(name_tokens.data[1]); + } else { + RMW_SET_ERROR_MSG("Illformated service name") + } + +end: + // free that memory + if (rcutils_string_array_fini(&name_tokens) != RCUTILS_RET_OK) { + fprintf(stderr, "Failed to destroy the token string array\n"); + } + + return success; +} + rmw_client_t * rmw_create_client( const rmw_node_t * node, @@ -1080,7 +1159,6 @@ rmw_create_client( rmw_client_t * client = nullptr; // memory allocations for namespacing - rcutils_string_array_t name_tokens = rcutils_get_zero_initialized_string_array(); char * request_partition_str = nullptr; char * response_partition_str = nullptr; char * service_str = nullptr; @@ -1102,46 +1180,15 @@ rmw_create_client( goto fail; } - // get actual data subsription object - // allocates memory, but doesn't have to be freed. - // partition operater takes ownership of it. - name_tokens = rcutils_split_last(service_name, '/'); - if (name_tokens.size == 1) { - request_partition_str = DDS_String_dup(ros_service_requester_prefix); - response_partition_str = DDS_String_dup(ros_service_response_prefix); - service_str = DDS_String_dup(name_tokens.data[0]); - } else if (name_tokens.size == 2) { - // TODO(Karsten1987): Fix utility function for this - size_t request_partition_length = - strlen(ros_service_requester_prefix) + strlen(name_tokens.data[0]) + 2; - char * request_concat_str = reinterpret_cast(rmw_allocate( - request_partition_length * sizeof(char))); - snprintf(request_concat_str, request_partition_length, - "%s/%s", ros_service_requester_prefix, name_tokens.data[0]); - - size_t response_partition_length = - strlen(ros_service_response_prefix) + strlen(name_tokens.data[0]) + 2; - char * response_concat_str = reinterpret_cast(rmw_allocate( - response_partition_length * sizeof(char))); - snprintf(response_concat_str, response_partition_length, - "%s/%s", ros_service_response_prefix, name_tokens.data[0]); - - // Connext will call deallocate on this, passing ownership to connext - request_partition_str = DDS_String_dup(request_concat_str); - response_partition_str = DDS_String_dup(response_concat_str); - service_str = DDS_String_dup(name_tokens.data[1]); - // free temporary memory - rmw_free(request_concat_str); - rmw_free(response_concat_str); - } else { - RMW_SET_ERROR_MSG("Illformated service name"); + if (!__process_service_name( + service_name, + qos_profile->avoid_ros_namespace_conventions, + &request_partition_str, + &response_partition_str, + &service_str)) + { goto fail; } - // all necessary strings are copied into connext - // free that memory - if (rcutils_string_array_fini(&name_tokens) != RCUTILS_RET_OK) { - fprintf(stderr, "Failed to destroy the token string array\n"); - } // TODO(karsten1987): For now, I'll expose the datawriter // to access the respective DDSPublisher object. @@ -1181,15 +1228,19 @@ rmw_create_client( // we have to set the partition array to length 1 // and then set the partition_str in it - subscriber_qos.partition.name.ensure_length(1, 1); - subscriber_qos.partition.name[0] = response_partition_str; + if (response_partition_str && strlen(response_partition_str) != 0) { + subscriber_qos.partition.name.ensure_length(1, 1); + subscriber_qos.partition.name[0] = response_partition_str; + } // update attached subscriber dds_subscriber->set_qos(subscriber_qos); // we cannot assign the partition_ptr again, // as rti takes ownership over it. - publisher_qos.partition.name.ensure_length(1, 1); - publisher_qos.partition.name[0] = request_partition_str; + if (request_partition_str && strlen(response_partition_str) != 0) { + publisher_qos.partition.name.ensure_length(1, 1); + publisher_qos.partition.name[0] = request_partition_str; + } // update attached publisher dds_publisher->set_qos(publisher_qos); @@ -1272,11 +1323,6 @@ rmw_create_client( rmw_free(buf); } - // cleanup namespacing - if (rcutils_string_array_fini(&name_tokens) != RCUTILS_RET_OK) { - fprintf(stderr, "Failed to destroy the token string array\n"); - } - return NULL; } @@ -1447,10 +1493,9 @@ rmw_create_service( rmw_service_t * service = nullptr; // memory allocations for namespacing - rcutils_string_array_t name_tokens = rcutils_get_zero_initialized_string_array(); char * request_partition_str = nullptr; char * response_partition_str = nullptr; - const char * service_str = nullptr; + char * service_str = nullptr; // Begin initializing elements. service = rmw_service_allocate(); @@ -1469,46 +1514,15 @@ rmw_create_service( goto fail; } - // get actual data subsription object - // allocates memory, but doesn't have to be freed. - // partition operater takes ownership of it. - name_tokens = rcutils_split_last(service_name, '/'); - if (name_tokens.size == 1) { - request_partition_str = DDS_String_dup(ros_service_requester_prefix); - response_partition_str = DDS_String_dup(ros_service_response_prefix); - service_str = DDS_String_dup(name_tokens.data[0]); - } else if (name_tokens.size == 2) { - // TODO(Karsten1987): Fix utility function for this - size_t request_partition_length = - strlen(ros_service_requester_prefix) + strlen(name_tokens.data[0]) + 2; - char * request_concat_str = reinterpret_cast(rmw_allocate( - request_partition_length * sizeof(char))); - snprintf(request_concat_str, request_partition_length, - "%s/%s", ros_service_requester_prefix, name_tokens.data[0]); - - size_t response_partition_length = - strlen(ros_service_requester_prefix) + strlen(name_tokens.data[0]) + 2; - char * response_concat_str = reinterpret_cast(rmw_allocate( - response_partition_length * sizeof(char))); - snprintf(response_concat_str, response_partition_length, - "%s/%s", ros_service_response_prefix, name_tokens.data[0]); - - // Connext will call deallocate on this, passing ownership to connext - request_partition_str = DDS_String_dup(request_concat_str); - response_partition_str = DDS_String_dup(response_concat_str); - service_str = DDS_String_dup(name_tokens.data[1]); - // free temporary memory - rmw_free(request_concat_str); - rmw_free(response_concat_str); - } else { - RMW_SET_ERROR_MSG("Illformated service name"); + if (!__process_service_name( + service_name, + qos_profile->avoid_ros_namespace_conventions, + &request_partition_str, + &response_partition_str, + &service_str)) + { goto fail; } - // all necessary strings are copied into connext - // free that memory - if (rcutils_string_array_fini(&name_tokens) != RCUTILS_RET_OK) { - fprintf(stderr, "Failed to destroy the token string array\n"); - } replier = callbacks->create_replier( participant, service_str, &datareader_qos, &datawriter_qos, @@ -1551,13 +1565,17 @@ rmw_create_service( // we have to set the partition array to length 1 // and then set the partition_str in it - subscriber_qos.partition.name.ensure_length(1, 1); - subscriber_qos.partition.name[0] = request_partition_str; + if (request_partition_str && strlen(request_partition_str) != 0) { + subscriber_qos.partition.name.ensure_length(1, 1); + subscriber_qos.partition.name[0] = request_partition_str; + } // update attached subscriber dds_subscriber->set_qos(subscriber_qos); - publisher_qos.partition.name.ensure_length(1, 1); - publisher_qos.partition.name[0] = response_partition_str; + if (response_partition_str && strlen(response_partition_str) != 0) { + publisher_qos.partition.name.ensure_length(1, 1); + publisher_qos.partition.name[0] = response_partition_str; + } // update attached publisher dds_publisher->set_qos(publisher_qos); @@ -1639,11 +1657,6 @@ rmw_create_service( rmw_free(buf); } - // cleanup namespacing - if (rcutils_string_array_fini(&name_tokens) != RCUTILS_RET_OK) { - fprintf(stderr, "Failed to destroy the token string array\n"); - } - return NULL; } diff --git a/rmw_connext_dynamic_cpp/src/functions.cpp b/rmw_connext_dynamic_cpp/src/functions.cpp index 9743b67d..de632fb5 100644 --- a/rmw_connext_dynamic_cpp/src/functions.cpp +++ b/rmw_connext_dynamic_cpp/src/functions.cpp @@ -267,6 +267,11 @@ rmw_create_publisher( return nullptr; } + if (qos_profile->avoid_ros_namespace_conventions) { + RMW_SET_ERROR_MSG("QoS 'avoid_ros_namespace_conventions' is not implemented"); + return NULL; + } + auto node_info = static_cast(node->data); if (!node_info) { RMW_SET_ERROR_MSG("node info handle is null"); @@ -758,6 +763,11 @@ rmw_create_subscription( return nullptr; } + if (qos_profile->avoid_ros_namespace_conventions) { + RMW_SET_ERROR_MSG("QoS 'avoid_ros_namespace_conventions' is not implemented"); + return NULL; + } + auto node_info = static_cast(node->data); if (!node_info) { RMW_SET_ERROR_MSG("node info handle is null"); @@ -1340,6 +1350,11 @@ rmw_create_client( return nullptr; } + if (qos_profile->avoid_ros_namespace_conventions) { + RMW_SET_ERROR_MSG("QoS 'avoid_ros_namespace_conventions' is not implemented"); + return NULL; + } + auto node_info = static_cast(node->data); if (!node_info) { RMW_SET_ERROR_MSG("node info handle is null"); @@ -1749,6 +1764,11 @@ rmw_create_service( return nullptr; } + if (qos_profile->avoid_ros_namespace_conventions) { + RMW_SET_ERROR_MSG("QoS 'avoid_ros_namespace_conventions' is not implemented"); + return NULL; + } + auto node_info = static_cast(node->data); if (!node_info) { RMW_SET_ERROR_MSG("node info handle is null"); From a1e652e6714d6a9c98cf553a6b561681c60c3e34 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 2 Jun 2017 16:19:50 -0700 Subject: [PATCH 2/4] address comments --- rmw_connext_cpp/src/functions.cpp | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/rmw_connext_cpp/src/functions.cpp b/rmw_connext_cpp/src/functions.cpp index 3e8c982f..76b8f5b8 100644 --- a/rmw_connext_cpp/src/functions.cpp +++ b/rmw_connext_cpp/src/functions.cpp @@ -214,7 +214,7 @@ rmw_destroy_node(rmw_node_t * node) } bool -__process_topic_name( +_process_topic_name( const char * topic_name, bool avoid_ros_namespace_conventions, char ** topic_str, @@ -346,7 +346,7 @@ rmw_create_publisher( goto fail; } - if (!__process_topic_name( + if (!_process_topic_name( topic_name, qos_profile->avoid_ros_namespace_conventions, &topic_str, @@ -678,7 +678,7 @@ rmw_create_subscription(const rmw_node_t * node, goto fail; } - if (!__process_topic_name( + if (!_process_topic_name( topic_name, qos_profile->avoid_ros_namespace_conventions, &topic_str, @@ -1038,12 +1038,12 @@ rmw_wait(rmw_subscriptions_t * subscriptions, } bool -__process_service_name( +_process_service_name( const char * service_name, bool avoid_ros_namespace_conventions, + char ** service_str, char ** request_partition_str, - char ** response_partition_str, - char ** service_str) + char ** response_partition_str) { bool success = true; rcutils_string_array_t name_tokens = rcutils_get_zero_initialized_string_array(); @@ -1180,12 +1180,12 @@ rmw_create_client( goto fail; } - if (!__process_service_name( + if (!_process_service_name( service_name, qos_profile->avoid_ros_namespace_conventions, + &service_str, &request_partition_str, - &response_partition_str, - &service_str)) + &response_partition_str)) { goto fail; } @@ -1514,12 +1514,12 @@ rmw_create_service( goto fail; } - if (!__process_service_name( + if (!_process_service_name( service_name, qos_profile->avoid_ros_namespace_conventions, + &service_str, &request_partition_str, - &response_partition_str, - &service_str)) + &response_partition_str)) { goto fail; } From 128fec2d21047f72efd18a47d60a9d30216ef701 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 2 Jun 2017 16:24:16 -0700 Subject: [PATCH 3/4] address comments --- rmw_connext_cpp/src/functions.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rmw_connext_cpp/src/functions.cpp b/rmw_connext_cpp/src/functions.cpp index 76b8f5b8..94187e42 100644 --- a/rmw_connext_cpp/src/functions.cpp +++ b/rmw_connext_cpp/src/functions.cpp @@ -221,7 +221,7 @@ _process_topic_name( char ** partition_str) { bool success = true; - rcutils_string_array_t name_tokens; + rcutils_string_array_t name_tokens = rcutils_get_zero_initialized_string_array(); // allocates memory, but doesn't have to be freed. // partition operater takes ownership of it. @@ -251,7 +251,7 @@ _process_topic_name( // Connext will call deallocate on this, passing ownership to connext *topic_str = DDS_String_dup(name_tokens.data[1]); } else { - RMW_SET_ERROR_MSG("Illformated topic name") + RMW_SET_ERROR_MSG("incorrectly formatted topic name") success = false; } From 3996c1393f96c7275609d3d3b0034befd11fcb37 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 7 Jun 2017 17:53:13 -0700 Subject: [PATCH 4/4] fix style --- rmw_connext_cpp/src/functions.cpp | 36 +++++++++++++++---------------- 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/rmw_connext_cpp/src/functions.cpp b/rmw_connext_cpp/src/functions.cpp index 94187e42..a16b0e7a 100644 --- a/rmw_connext_cpp/src/functions.cpp +++ b/rmw_connext_cpp/src/functions.cpp @@ -347,10 +347,10 @@ rmw_create_publisher( } if (!_process_topic_name( - topic_name, - qos_profile->avoid_ros_namespace_conventions, - &topic_str, - &partition_str)) + topic_name, + qos_profile->avoid_ros_namespace_conventions, + &topic_str, + &partition_str)) { goto fail; } @@ -679,10 +679,10 @@ rmw_create_subscription(const rmw_node_t * node, } if (!_process_topic_name( - topic_name, - qos_profile->avoid_ros_namespace_conventions, - &topic_str, - &partition_str)) + topic_name, + qos_profile->avoid_ros_namespace_conventions, + &topic_str, + &partition_str)) { goto fail; } @@ -1181,11 +1181,11 @@ rmw_create_client( } if (!_process_service_name( - service_name, - qos_profile->avoid_ros_namespace_conventions, - &service_str, - &request_partition_str, - &response_partition_str)) + service_name, + qos_profile->avoid_ros_namespace_conventions, + &service_str, + &request_partition_str, + &response_partition_str)) { goto fail; } @@ -1515,11 +1515,11 @@ rmw_create_service( } if (!_process_service_name( - service_name, - qos_profile->avoid_ros_namespace_conventions, - &service_str, - &request_partition_str, - &response_partition_str)) + service_name, + qos_profile->avoid_ros_namespace_conventions, + &service_str, + &request_partition_str, + &response_partition_str)) { goto fail; }