From e1bfa26cea18161693484ce2ea3d7715157fc942 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 30 May 2017 18:24:48 -0700 Subject: [PATCH] Use namespaces (#137) * fix documentation of expand_topic_name() * use expand_topic_name() in pub/sub/client/service * update test expectations * use absolute topic names to avoid mismatch due to expansion * propagate new functions from rcutils error handling * convert given rcl allocator to a rcutils allocator --- rcl/include/rcl/client.h | 8 ++-- rcl/include/rcl/error_handling.h | 4 ++ rcl/include/rcl/expand_topic_name.h | 8 ++-- rcl/include/rcl/publisher.h | 8 ++-- rcl/include/rcl/service.h | 8 ++-- rcl/include/rcl/subscription.h | 8 ++-- rcl/include/rcl/types.h | 4 +- rcl/src/rcl/client.c | 66 ++++++++++++++++++++++++++- rcl/src/rcl/publisher.c | 66 ++++++++++++++++++++++++++- rcl/src/rcl/service.c | 65 ++++++++++++++++++++++++++- rcl/src/rcl/subscription.c | 69 ++++++++++++++++++++++++++++- rcl/test/rcl/test_client.cpp | 3 +- rcl/test/rcl/test_graph.cpp | 12 ++--- rcl/test/rcl/test_publisher.cpp | 3 +- rcl/test/rcl/test_service.cpp | 3 +- rcl/test/rcl/test_subscription.cpp | 3 +- 16 files changed, 306 insertions(+), 32 deletions(-) diff --git a/rcl/include/rcl/client.h b/rcl/include/rcl/client.h index c4899e0..ad8131b 100644 --- a/rcl/include/rcl/client.h +++ b/rcl/include/rcl/client.h @@ -96,9 +96,10 @@ rcl_get_zero_initialized_client(void); * The rosidl_service_type_support_t object contains service type specific * information used to send or take requests and responses. * - * \todo TODO(wjwwood) update this once we've come up with an official scheme. - * The service name must be a non-empty string which follows the topic/service - * name format conventions. + * The topic name must be a c string which follows the topic and service name + * format rules for unexpanded names, also known as non-fully qualified names: + * + * \see rcl_expand_topic_name * * The options struct allows the user to set the quality of service settings as * well as a custom allocator which is used when initializing/finalizing the @@ -145,6 +146,7 @@ rcl_get_zero_initialized_client(void); * \return `RCL_RET_ALREADY_INIT` if the client is already initialized, or * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or * \return `RCL_RET_BAD_ALLOC` if allocating memory fails, or + * \return `RCL_RET_SERVICE_NAME_INVALID` if the given service name is invalid, or * \return `RCL_RET_ERROR` if an unspecified error occurs. */ RCL_PUBLIC diff --git a/rcl/include/rcl/error_handling.h b/rcl/include/rcl/error_handling.h index f835c02..18f0e66 100644 --- a/rcl/include/rcl/error_handling.h +++ b/rcl/include/rcl/error_handling.h @@ -31,6 +31,10 @@ typedef rcutils_error_state_t rcl_error_state_t; +#define rcl_error_state_copy rcutils_error_state_copy + +#define rcl_error_state_fini rcutils_error_state_fini + #define rcl_set_error_state rcutils_set_error_state #define RCL_SET_ERROR_MSG(msg, allocator) RCUTILS_SET_ERROR_MSG(msg, allocator) diff --git a/rcl/include/rcl/expand_topic_name.h b/rcl/include/rcl/expand_topic_name.h index 2bd41a1..1a50eea 100644 --- a/rcl/include/rcl/expand_topic_name.h +++ b/rcl/include/rcl/expand_topic_name.h @@ -49,12 +49,12 @@ extern "C" * if (rcutils_ret != RCUTILS_RET_OK) { * // ... error handling * } - * rcutils_ret = rcl_get_default_topic_name_substitutions(&substitutions_map); - * if (rcutils_ret != RCUTILS_RET_OK) { + * rcl_ret_t ret = rcl_get_default_topic_name_substitutions(&substitutions_map); + * if (ret != RCL_RET_OK) { * // ... error handling * } * char * expanded_topic_name = NULL; - * rcl_ret_t ret = rcl_expand_topic_name( + * ret = rcl_expand_topic_name( * "some/topic", * "my_node", * "my_ns", @@ -108,7 +108,7 @@ extern "C" * \return `RCL_RET_TOPIC_NAME_INVALID` if the given topic name is invalid, or * \return `RCL_RET_NODE_INVALID_NAME` if the name is invalid, or * \return `RCL_RET_NODE_INVALID_NAMESPACE` if the namespace_ is invalid, or - * \return `RCL_RET_UNKNOWN_SUBSTITUTION` if the given topic name is invalid, or + * \return `RCL_RET_UNKNOWN_SUBSTITUTION` for unknown substitutions in name, or * \return `RCL_RET_ERROR` if an unspecified error occurs. */ RCL_PUBLIC diff --git a/rcl/include/rcl/publisher.h b/rcl/include/rcl/publisher.h index 7e5ef96..aba70c8 100644 --- a/rcl/include/rcl/publisher.h +++ b/rcl/include/rcl/publisher.h @@ -89,9 +89,10 @@ rcl_get_zero_initialized_publisher(void); * The rosidl_message_type_support_t object contains message type specific * information used to publish messages. * - * \todo TODO(wjwwood) update this once we've come up with an official scheme. - * The topic name must be a non-empty string which follows the topic naming - * format. + * The topic name must be a c string which follows the topic and service name + * format rules for unexpanded names, also known as non-fully qualified names: + * + * \see rcl_expand_topic_name * * The options struct allows the user to set the quality of service settings as * well as a custom allocator which is used when initializing/finalizing the @@ -137,6 +138,7 @@ rcl_get_zero_initialized_publisher(void); * \return `RCL_RET_ALREADY_INIT` if the publisher is already initialized, or * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or * \return `RCL_RET_BAD_ALLOC` if allocating memory fails, or + * \return `RCL_RET_TOPIC_NAME_INVALID` if the given topic name is invalid, or * \return `RCL_RET_ERROR` if an unspecified error occurs. */ RCL_PUBLIC diff --git a/rcl/include/rcl/service.h b/rcl/include/rcl/service.h index 9ec7db9..d4a6ac6 100644 --- a/rcl/include/rcl/service.h +++ b/rcl/include/rcl/service.h @@ -91,9 +91,10 @@ rcl_get_zero_initialized_service(void); * The rosidl_service_type_support_t object contains service type specific * information used to send or take requests and responses. * - * \todo TODO(wjwwood) update this once we've come up with an official scheme. - * The service name must be a non-empty string which follows the service/topic - * naming format. + * The topic name must be a c string which follows the topic and service name + * format rules for unexpanded names, also known as non-fully qualified names: + * + * \see rcl_expand_topic_name * * The options struct allows the user to set the quality of service settings as * well as a custom allocator which is used when initializing/finalizing the @@ -138,6 +139,7 @@ rcl_get_zero_initialized_service(void); * \return `RCL_RET_OK` if service was initialized successfully, or * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or * \return `RCL_RET_BAD_ALLOC` if allocating memory failed, or + * \return `RCL_RET_SERVICE_NAME_INVALID` if the given service name is invalid, or * \return `RCL_RET_ERROR` if an unspecified error occurs. */ RCL_PUBLIC diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index 4e2c222..dbcb175 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -92,9 +92,10 @@ rcl_get_zero_initialized_subscription(void); * The rosidl_message_type_support_t object contains message type specific * information used to publish messages. * - * \todo TODO(wjwwood) update this once we've come up with an official scheme. - * The topic name must be a non-empty string which follows the topic naming - * format. + * The topic name must be a c string which follows the topic and service name + * format rules for unexpanded names, also known as non-fully qualified names: + * + * \see rcl_expand_topic_name * * The options struct allows the user to set the quality of service settings as * well as a custom allocator which is used when (de)initializing the @@ -140,6 +141,7 @@ rcl_get_zero_initialized_subscription(void); * \return `RCL_RET_OK` if subscription was initialized successfully, or * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or * \return `RCL_RET_BAD_ALLOC` if allocating memory failed, or + * \return `RCL_RET_TOPIC_NAME_INVALID` if the given topic name is invalid, or * \return `RCL_RET_ERROR` if an unspecified error occurs. */ RCL_PUBLIC diff --git a/rcl/include/rcl/types.h b/rcl/include/rcl/types.h index c989444..3cdf54e 100644 --- a/rcl/include/rcl/types.h +++ b/rcl/include/rcl/types.h @@ -38,8 +38,10 @@ typedef rmw_ret_t rcl_ret_t; #define RCL_RET_MISMATCHED_RMW_ID 102 /// Topic name does not pass validation. #define RCL_RET_TOPIC_NAME_INVALID 103 +/// Service name (same as topic name) does not pass validation. +#define RCL_RET_SERVICE_NAME_INVALID 104 /// Topic name substitution is unknown. -#define RCL_RET_UNKNOWN_SUBSTITUTION 104 +#define RCL_RET_UNKNOWN_SUBSTITUTION 105 // rcl node specific ret codes in 2XX /// Invalid rcl_node_t given return code. diff --git a/rcl/src/rcl/client.c b/rcl/src/rcl/client.c index f7776ce..4c20764 100644 --- a/rcl/src/rcl/client.c +++ b/rcl/src/rcl/client.c @@ -19,10 +19,13 @@ extern "C" #include "rcl/client.h" +#include #include +#include "rcl/expand_topic_name.h" #include "rmw/error_handling.h" #include "rmw/rmw.h" +#include "rmw/validate_full_topic_name.h" #include "./common.h" #include "./stdatomic_helper.h" @@ -73,6 +76,66 @@ rcl_client_init( RCL_SET_ERROR_MSG("client already initialized, or memory was unintialized", *allocator); return RCL_RET_ALREADY_INIT; } + // Expand the given service name. + rcutils_allocator_t rcutils_allocator = *allocator; // implicit conversion to rcutils version + rcutils_string_map_t substitutions_map = rcutils_get_zero_initialized_string_map(); + rcutils_ret_t rcutils_ret = rcutils_string_map_init(&substitutions_map, 0, rcutils_allocator); + if (rcutils_ret != RCUTILS_RET_OK) { + RCL_SET_ERROR_MSG(rcutils_get_error_string_safe(), *allocator) + if (rcutils_ret == RCUTILS_RET_BAD_ALLOC) { + return RCL_RET_BAD_ALLOC; + } + return RCL_RET_ERROR; + } + rcl_ret_t ret = rcl_get_default_topic_name_substitutions(&substitutions_map); + if (ret != RCL_RET_OK) { + rcutils_ret = rcutils_string_map_fini(&substitutions_map); + if (rcutils_ret != RCUTILS_RET_OK) { + fprintf(stderr, + "[" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: " + "failed to fini string_map (%d) during error handling: %s\n", + rcutils_ret, + rcutils_get_error_string_safe()); + } + if (ret == RCL_RET_BAD_ALLOC) { + return ret; + } + return RCL_RET_ERROR; + } + char * expanded_service_name = NULL; + ret = rcl_expand_topic_name( + service_name, + rcl_node_get_name(node), + rcl_node_get_namespace(node), + &substitutions_map, + *allocator, + &expanded_service_name); + rcutils_ret = rcutils_string_map_fini(&substitutions_map); + if (rcutils_ret != RCUTILS_RET_OK) { + RCL_SET_ERROR_MSG(rcutils_get_error_string_safe(), *allocator) + allocator->deallocate(expanded_service_name, allocator->state); + return RCL_RET_ERROR; + } + if (ret != RCL_RET_OK) { + if (ret == RCL_RET_BAD_ALLOC) { + return ret; + } else if (ret == RCL_RET_TOPIC_NAME_INVALID || ret == RCL_RET_UNKNOWN_SUBSTITUTION) { + return RCL_RET_SERVICE_NAME_INVALID; + } else { + return RCL_RET_ERROR; + } + } + // Validate the expanded service name. + int validation_result; + rmw_ret_t rmw_ret = rmw_validate_full_topic_name(expanded_service_name, &validation_result, NULL); + if (rmw_ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), *allocator); + return RCL_RET_ERROR; + } + if (validation_result != RMW_TOPIC_VALID) { + RCL_SET_ERROR_MSG(rmw_full_topic_name_validation_result_string(validation_result), *allocator) + return RCL_RET_SERVICE_NAME_INVALID; + } // Allocate space for the implementation struct. client->impl = (rcl_client_impl_t *)allocator->allocate( sizeof(rcl_client_impl_t), allocator->state); @@ -84,8 +147,9 @@ rcl_client_init( client->impl->rmw_handle = rmw_create_client( rcl_node_get_rmw_handle(node), type_support, - service_name, + expanded_service_name, &options->qos); + allocator->deallocate(expanded_service_name, allocator->state); if (!client->impl->rmw_handle) { RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), *allocator); goto fail; diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index 9f7fa5b..0cd5314 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -19,11 +19,14 @@ extern "C" #include "rcl/publisher.h" +#include #include #include "./common.h" +#include "rcl/expand_topic_name.h" #include "rmw/error_handling.h" #include "rmw/rmw.h" +#include "rmw/validate_full_topic_name.h" typedef struct rcl_publisher_impl_t { @@ -71,6 +74,66 @@ rcl_publisher_init( } RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT, *allocator); RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT, *allocator); + // Expand the given topic name. + rcutils_allocator_t rcutils_allocator = *allocator; // implicit conversion to rcutils version + rcutils_string_map_t substitutions_map = rcutils_get_zero_initialized_string_map(); + rcutils_ret_t rcutils_ret = rcutils_string_map_init(&substitutions_map, 0, rcutils_allocator); + if (rcutils_ret != RCUTILS_RET_OK) { + RCL_SET_ERROR_MSG(rcutils_get_error_string_safe(), *allocator) + if (rcutils_ret == RCUTILS_RET_BAD_ALLOC) { + return RCL_RET_BAD_ALLOC; + } + return RCL_RET_ERROR; + } + rcl_ret_t ret = rcl_get_default_topic_name_substitutions(&substitutions_map); + if (ret != RCL_RET_OK) { + rcutils_ret = rcutils_string_map_fini(&substitutions_map); + if (rcutils_ret != RCUTILS_RET_OK) { + fprintf(stderr, + "[" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: " + "failed to fini string_map (%d) during error handling: %s\n", + rcutils_ret, + rcutils_get_error_string_safe()); + } + if (ret == RCL_RET_BAD_ALLOC) { + return ret; + } + return RCL_RET_ERROR; + } + char * expanded_topic_name = NULL; + ret = rcl_expand_topic_name( + topic_name, + rcl_node_get_name(node), + rcl_node_get_namespace(node), + &substitutions_map, + *allocator, + &expanded_topic_name); + rcutils_ret = rcutils_string_map_fini(&substitutions_map); + if (rcutils_ret != RCUTILS_RET_OK) { + RCL_SET_ERROR_MSG(rcutils_get_error_string_safe(), *allocator) + allocator->deallocate(expanded_topic_name, allocator->state); + return RCL_RET_ERROR; + } + if (ret != RCL_RET_OK) { + if (ret == RCL_RET_BAD_ALLOC) { + return ret; + } else if (ret == RCL_RET_TOPIC_NAME_INVALID || ret == RCL_RET_UNKNOWN_SUBSTITUTION) { + return RCL_RET_TOPIC_NAME_INVALID; + } else { + return RCL_RET_ERROR; + } + } + // Validate the expanded topic name. + int validation_result; + rmw_ret_t rmw_ret = rmw_validate_full_topic_name(expanded_topic_name, &validation_result, NULL); + if (rmw_ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), *allocator); + return RCL_RET_ERROR; + } + if (validation_result != RMW_TOPIC_VALID) { + RCL_SET_ERROR_MSG(rmw_full_topic_name_validation_result_string(validation_result), *allocator) + return RCL_RET_TOPIC_NAME_INVALID; + } // Allocate space for the implementation struct. publisher->impl = (rcl_publisher_impl_t *)allocator->allocate( sizeof(rcl_publisher_impl_t), allocator->state); @@ -82,8 +145,9 @@ rcl_publisher_init( publisher->impl->rmw_handle = rmw_create_publisher( rcl_node_get_rmw_handle(node), type_support, - topic_name, + expanded_topic_name, &(options->qos)); + allocator->deallocate(expanded_topic_name, allocator->state); if (!publisher->impl->rmw_handle) { RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), *allocator); goto fail; diff --git a/rcl/src/rcl/service.c b/rcl/src/rcl/service.c index 5faf8fe..9a14102 100644 --- a/rcl/src/rcl/service.c +++ b/rcl/src/rcl/service.c @@ -23,8 +23,10 @@ extern "C" #include #include "./common.h" +#include "rcl/expand_topic_name.h" #include "rmw/error_handling.h" #include "rmw/rmw.h" +#include "rmw/validate_full_topic_name.h" typedef struct rcl_service_impl_t { @@ -71,6 +73,66 @@ rcl_service_init( RCL_SET_ERROR_MSG("service already initialized, or memory was unintialized", *allocator); return RCL_RET_ALREADY_INIT; } + // Expand the given service name. + rcutils_allocator_t rcutils_allocator = *allocator; // implicit conversion to rcutils version + rcutils_string_map_t substitutions_map = rcutils_get_zero_initialized_string_map(); + rcutils_ret_t rcutils_ret = rcutils_string_map_init(&substitutions_map, 0, rcutils_allocator); + if (rcutils_ret != RCUTILS_RET_OK) { + RCL_SET_ERROR_MSG(rcutils_get_error_string_safe(), *allocator) + if (rcutils_ret == RCUTILS_RET_BAD_ALLOC) { + return RCL_RET_BAD_ALLOC; + } + return RCL_RET_ERROR; + } + rcl_ret_t ret = rcl_get_default_topic_name_substitutions(&substitutions_map); + if (ret != RCL_RET_OK) { + rcutils_ret = rcutils_string_map_fini(&substitutions_map); + if (rcutils_ret != RCUTILS_RET_OK) { + fprintf(stderr, + "[" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: " + "failed to fini string_map (%d) during error handling: %s\n", + rcutils_ret, + rcutils_get_error_string_safe()); + } + if (ret == RCL_RET_BAD_ALLOC) { + return ret; + } + return RCL_RET_ERROR; + } + char * expanded_service_name = NULL; + ret = rcl_expand_topic_name( + service_name, + rcl_node_get_name(node), + rcl_node_get_namespace(node), + &substitutions_map, + *allocator, + &expanded_service_name); + rcutils_ret = rcutils_string_map_fini(&substitutions_map); + if (rcutils_ret != RCUTILS_RET_OK) { + RCL_SET_ERROR_MSG(rcutils_get_error_string_safe(), *allocator) + allocator->deallocate(expanded_service_name, allocator->state); + return RCL_RET_ERROR; + } + if (ret != RCL_RET_OK) { + if (ret == RCL_RET_BAD_ALLOC) { + return ret; + } else if (ret == RCL_RET_TOPIC_NAME_INVALID || ret == RCL_RET_UNKNOWN_SUBSTITUTION) { + return RCL_RET_SERVICE_NAME_INVALID; + } else { + return RCL_RET_ERROR; + } + } + // Validate the expanded service name. + int validation_result; + rmw_ret_t rmw_ret = rmw_validate_full_topic_name(expanded_service_name, &validation_result, NULL); + if (rmw_ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), *allocator); + return RCL_RET_ERROR; + } + if (validation_result != RMW_TOPIC_VALID) { + RCL_SET_ERROR_MSG(rmw_full_topic_name_validation_result_string(validation_result), *allocator) + return RCL_RET_SERVICE_NAME_INVALID; + } // Allocate space for the implementation struct. service->impl = (rcl_service_impl_t *)allocator->allocate( sizeof(rcl_service_impl_t), allocator->state); @@ -87,8 +149,9 @@ rcl_service_init( service->impl->rmw_handle = rmw_create_service( rcl_node_get_rmw_handle(node), type_support, - service_name, + expanded_service_name, &options->qos); + allocator->deallocate(expanded_service_name, allocator->state); if (!service->impl->rmw_handle) { RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), *allocator); goto fail; diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 88b4790..213bec2 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -19,9 +19,13 @@ extern "C" #include "rcl/subscription.h" +#include + +#include "./common.h" +#include "rcl/expand_topic_name.h" #include "rmw/error_handling.h" #include "rmw/rmw.h" -#include "./common.h" +#include "rmw/validate_full_topic_name.h" typedef struct rcl_subscription_impl_t { @@ -64,6 +68,66 @@ rcl_subscription_init( RCL_SET_ERROR_MSG("subscription already initialized, or memory was uninitialized", *allocator); return RCL_RET_ALREADY_INIT; } + // Expand the given topic name. + rcutils_allocator_t rcutils_allocator = *allocator; // implicit conversion to rcutils version + rcutils_string_map_t substitutions_map = rcutils_get_zero_initialized_string_map(); + rcutils_ret_t rcutils_ret = rcutils_string_map_init(&substitutions_map, 0, rcutils_allocator); + if (rcutils_ret != RCUTILS_RET_OK) { + RCL_SET_ERROR_MSG(rcutils_get_error_string_safe(), *allocator) + if (rcutils_ret == RCUTILS_RET_BAD_ALLOC) { + return RCL_RET_BAD_ALLOC; + } + return RCL_RET_ERROR; + } + rcl_ret_t ret = rcl_get_default_topic_name_substitutions(&substitutions_map); + if (ret != RCL_RET_OK) { + rcutils_ret = rcutils_string_map_fini(&substitutions_map); + if (rcutils_ret != RCUTILS_RET_OK) { + fprintf(stderr, + "[" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: " + "failed to fini string_map (%d) during error handling: %s\n", + rcutils_ret, + rcutils_get_error_string_safe()); + } + if (ret == RCL_RET_BAD_ALLOC) { + return ret; + } + return RCL_RET_ERROR; + } + char * expanded_topic_name = NULL; + ret = rcl_expand_topic_name( + topic_name, + rcl_node_get_name(node), + rcl_node_get_namespace(node), + &substitutions_map, + *allocator, + &expanded_topic_name); + rcutils_ret = rcutils_string_map_fini(&substitutions_map); + if (rcutils_ret != RCUTILS_RET_OK) { + RCL_SET_ERROR_MSG(rcutils_get_error_string_safe(), *allocator) + allocator->deallocate(expanded_topic_name, allocator->state); + return RCL_RET_ERROR; + } + if (ret != RCL_RET_OK) { + if (ret == RCL_RET_BAD_ALLOC) { + return ret; + } else if (ret == RCL_RET_TOPIC_NAME_INVALID || ret == RCL_RET_UNKNOWN_SUBSTITUTION) { + return RCL_RET_TOPIC_NAME_INVALID; + } else { + return RCL_RET_ERROR; + } + } + // Validate the expanded topic name. + int validation_result; + rmw_ret_t rmw_ret = rmw_validate_full_topic_name(expanded_topic_name, &validation_result, NULL); + if (rmw_ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), *allocator); + return RCL_RET_ERROR; + } + if (validation_result != RMW_TOPIC_VALID) { + RCL_SET_ERROR_MSG(rmw_full_topic_name_validation_result_string(validation_result), *allocator) + return RCL_RET_TOPIC_NAME_INVALID; + } // Allocate memory for the implementation struct. subscription->impl = (rcl_subscription_impl_t *)allocator->allocate( sizeof(rcl_subscription_impl_t), allocator->state); @@ -75,9 +139,10 @@ rcl_subscription_init( subscription->impl->rmw_handle = rmw_create_subscription( rcl_node_get_rmw_handle(node), type_support, - topic_name, + expanded_topic_name, &(options->qos), options->ignore_local_publications); + allocator->deallocate(expanded_topic_name, allocator->state); if (!subscription->impl->rmw_handle) { RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), *allocator); goto fail; diff --git a/rcl/test/rcl/test_client.cpp b/rcl/test/rcl/test_client.cpp index 2d0b0c5..b5291d9 100644 --- a/rcl/test/rcl/test_client.cpp +++ b/rcl/test/rcl/test_client.cpp @@ -73,6 +73,7 @@ TEST_F(TestClientFixture, test_client_nominal) { // Initialize the client. const char * topic_name = "add_two_ints"; + const char * expected_topic_name = "/add_two_ints"; rcl_client_options_t client_options = rcl_client_get_default_options(); const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( @@ -81,7 +82,7 @@ TEST_F(TestClientFixture, test_client_nominal) { // Check the return code of initialization and that the service name matches what's expected ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); - EXPECT_EQ(strcmp(rcl_client_get_service_name(&client), topic_name), 0); + EXPECT_EQ(strcmp(rcl_client_get_service_name(&client), expected_topic_name), 0); auto client_exit = make_scope_exit([&client, this]() { stop_memory_checking(); diff --git a/rcl/test/rcl/test_graph.cpp b/rcl/test/rcl/test_graph.cpp index ad64b1f..ea8ebec 100644 --- a/rcl/test/rcl/test_graph.cpp +++ b/rcl/test/rcl/test_graph.cpp @@ -161,7 +161,7 @@ TEST_F( stop_memory_checking(); rcl_ret_t ret; rcl_node_t zero_node = rcl_get_zero_initialized_node(); - const char * topic_name = "topic_test_rcl_count_publishers"; + const char * topic_name = "/topic_test_rcl_count_publishers"; size_t count; // invalid node ret = rcl_count_publishers(nullptr, topic_name, &count); @@ -199,7 +199,7 @@ TEST_F( stop_memory_checking(); rcl_ret_t ret; rcl_node_t zero_node = rcl_get_zero_initialized_node(); - const char * topic_name = "topic_test_rcl_count_subscribers"; + const char * topic_name = "/topic_test_rcl_count_subscribers"; size_t count; // invalid node ret = rcl_count_subscribers(nullptr, topic_name, &count); @@ -323,7 +323,7 @@ check_graph_state( */ TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_graph_query_functions) { stop_memory_checking(); - std::string topic_name("test_graph_query_functions__"); + std::string topic_name("/test_graph_query_functions__"); std::chrono::nanoseconds now = std::chrono::system_clock::now().time_since_epoch(); topic_name += std::to_string(now.count()); printf("Using topic name: %s\n", topic_name.c_str()); @@ -421,7 +421,7 @@ TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_graph_guard_conditi rcl_publisher_options_t pub_ops = rcl_publisher_get_default_options(); rcl_ret_t ret = rcl_publisher_init( &pub, this->node_ptr, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String), - "chatter_test_graph_guard_condition_topics", &pub_ops); + "/chatter_test_graph_guard_condition_topics", &pub_ops); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); // sleep std::this_thread::sleep_for(std::chrono::milliseconds(100)); @@ -430,7 +430,7 @@ TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_graph_guard_conditi rcl_subscription_options_t sub_ops = rcl_subscription_get_default_options(); ret = rcl_subscription_init( &sub, this->node_ptr, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String), - "chatter_test_graph_guard_condition_topics", &sub_ops); + "/chatter_test_graph_guard_condition_topics", &sub_ops); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); // sleep std::this_thread::sleep_for(std::chrono::milliseconds(100)); @@ -481,7 +481,7 @@ TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_rcl_service_server_ // First create a client which will be used to call the function. rcl_client_t client = rcl_get_zero_initialized_client(); auto ts = ROSIDL_GET_SRV_TYPE_SUPPORT(example_interfaces, AddTwoInts); - const char * service_name = "service_test_rcl_service_server_is_available"; + const char * service_name = "/service_test_rcl_service_server_is_available"; rcl_client_options_t client_options = rcl_client_get_default_options(); ret = rcl_client_init(&client, this->node_ptr, ts, service_name, &client_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); diff --git a/rcl/test/rcl/test_publisher.cpp b/rcl/test/rcl/test_publisher.cpp index 4a15041..d62f152 100644 --- a/rcl/test/rcl/test_publisher.cpp +++ b/rcl/test/rcl/test_publisher.cpp @@ -89,6 +89,7 @@ TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_publisher_nomin // Thread : main thread 7fff7342d000 // Internals : V6.4.140407OSS///v_topicNew/v_topic.c/448/21/1455157023.781423000 const char * topic_name = "chatter_int64"; + const char * expected_topic_name = "/chatter_int64"; rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, &publisher_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); @@ -97,7 +98,7 @@ TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_publisher_nomin rcl_ret_t ret = rcl_publisher_fini(&publisher, this->node_ptr); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); }); - EXPECT_EQ(strcmp(rcl_publisher_get_topic_name(&publisher), topic_name), 0); + EXPECT_EQ(strcmp(rcl_publisher_get_topic_name(&publisher), expected_topic_name), 0); std_msgs__msg__Int64 msg; std_msgs__msg__Int64__init(&msg); msg.data = 42; diff --git a/rcl/test/rcl/test_service.cpp b/rcl/test/rcl/test_service.cpp index a8c8baa..91470ba 100644 --- a/rcl/test/rcl/test_service.cpp +++ b/rcl/test/rcl/test_service.cpp @@ -119,6 +119,7 @@ TEST_F(CLASSNAME(TestServiceFixture, RMW_IMPLEMENTATION), test_service_nominal) const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( example_interfaces, AddTwoInts); const char * topic = "add_two_ints"; + const char * expected_topic = "/add_two_ints"; rcl_service_t service = rcl_get_zero_initialized_service(); rcl_service_options_t service_options = rcl_service_get_default_options(); @@ -126,7 +127,7 @@ TEST_F(CLASSNAME(TestServiceFixture, RMW_IMPLEMENTATION), test_service_nominal) ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); // Check that the service name matches what we assigned. - EXPECT_EQ(strcmp(rcl_service_get_service_name(&service), topic), 0); + EXPECT_EQ(strcmp(rcl_service_get_service_name(&service), expected_topic), 0); auto service_exit = make_scope_exit([&service, this]() { stop_memory_checking(); rcl_ret_t ret = rcl_service_fini(&service, this->node_ptr); diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 0dc7484..2a3ea9e 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -130,6 +130,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription // Thread : main thread 7fff7342d000 // Internals : V6.4.140407OSS///v_topicNew/v_topic.c/448/21/1455157023.781423000 const char * topic = "rcl_test_subscription_nominal_chatter_int64"; + const char * expected_topic = "/rcl_test_subscription_nominal_chatter_int64"; rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic, &publisher_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); @@ -147,7 +148,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription rcl_ret_t ret = rcl_subscription_fini(&subscription, this->node_ptr); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); }); - EXPECT_EQ(strcmp(rcl_subscription_get_topic_name(&subscription), topic), 0); + EXPECT_EQ(strcmp(rcl_subscription_get_topic_name(&subscription), expected_topic), 0); // TODO(wjwwood): add logic to wait for the connection to be established // probably using the count_subscriptions busy wait mechanism // until then we will sleep for a short period of time