From a166f6417163b67f8392460608990ed5b9243f77 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Thu, 25 May 2017 17:02:01 -0700 Subject: [PATCH] catch and report name issues for node names, namespaces, and topic/service names --- rclcpp/CMakeLists.txt | 50 +++++++++ rclcpp/include/rclcpp/client.hpp | 39 ++++--- rclcpp/include/rclcpp/exceptions.hpp | 74 ++++++++++++- rclcpp/include/rclcpp/service.hpp | 37 +++++-- rclcpp/include/rclcpp/subscription.hpp | 31 ++++-- rclcpp/src/rclcpp/exceptions.cpp | 28 ++++- .../src/rclcpp/node_interfaces/node_base.cpp | 38 +++++++ rclcpp/src/rclcpp/publisher.cpp | 102 +++++++++++------- rclcpp/src/rclcpp/service.cpp | 6 ++ rclcpp/src/rclcpp/subscription.cpp | 16 ++- rclcpp/test/test_client.cpp | 60 +++++++++++ rclcpp/test/test_node.cpp | 52 +++++++++ rclcpp/test/test_publisher.cpp | 60 +++++++++++ rclcpp/test/test_service.cpp | 63 +++++++++++ rclcpp/test/test_subscription.cpp | 63 +++++++++++ 15 files changed, 641 insertions(+), 78 deletions(-) create mode 100644 rclcpp/test/test_client.cpp create mode 100644 rclcpp/test/test_node.cpp create mode 100644 rclcpp/test/test_publisher.cpp create mode 100644 rclcpp/test/test_service.cpp create mode 100644 rclcpp/test/test_subscription.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index f13bb2d7f6..7e5fd70a7d 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -93,6 +93,16 @@ if(BUILD_TESTING) find_package(rmw_implementation_cmake REQUIRED) + ament_add_gtest(test_client test/test_client.cpp) + if(TARGET test_client) + target_include_directories(test_client PUBLIC + ${rcl_interfaces_INCLUDE_DIRS} + ${rmw_INCLUDE_DIRS} + ${rosidl_generator_cpp_INCLUDE_DIRS} + ${rosidl_typesupport_cpp_INCLUDE_DIRS} + ) + target_link_libraries(test_client ${PROJECT_NAME}) + endif() ament_add_gtest(test_expand_topic_or_service_name test/test_expand_topic_or_service_name.cpp) if(TARGET test_expand_topic_or_service_name) target_include_directories(test_expand_topic_or_service_name PUBLIC @@ -132,6 +142,26 @@ if(BUILD_TESTING) ${rosidl_typesupport_cpp_INCLUDE_DIRS} ) endif() + ament_add_gtest(test_node test/test_node.cpp) + if(TARGET test_node) + target_include_directories(test_node PUBLIC + ${rcl_interfaces_INCLUDE_DIRS} + ${rmw_INCLUDE_DIRS} + ${rosidl_generator_cpp_INCLUDE_DIRS} + ${rosidl_typesupport_cpp_INCLUDE_DIRS} + ) + target_link_libraries(test_node ${PROJECT_NAME}) + endif() + ament_add_gtest(test_publisher test/test_publisher.cpp) + if(TARGET test_publisher) + target_include_directories(test_publisher PUBLIC + ${rcl_interfaces_INCLUDE_DIRS} + ${rmw_INCLUDE_DIRS} + ${rosidl_generator_cpp_INCLUDE_DIRS} + ${rosidl_typesupport_cpp_INCLUDE_DIRS} + ) + target_link_libraries(test_publisher ${PROJECT_NAME}) + endif() ament_add_gtest(test_rate test/test_rate.cpp ENV RCL_ASSERT_RMW_ID_MATCHES=${rmw_implementation}) if(TARGET test_rate) @@ -145,6 +175,26 @@ if(BUILD_TESTING) ${PROJECT_NAME} ) endif() + ament_add_gtest(test_service test/test_service.cpp) + if(TARGET test_service) + target_include_directories(test_service PUBLIC + ${rcl_interfaces_INCLUDE_DIRS} + ${rmw_INCLUDE_DIRS} + ${rosidl_generator_cpp_INCLUDE_DIRS} + ${rosidl_typesupport_cpp_INCLUDE_DIRS} + ) + target_link_libraries(test_service ${PROJECT_NAME}) + endif() + ament_add_gtest(test_subscription test/test_subscription.cpp) + if(TARGET test_subscription) + target_include_directories(test_subscription PUBLIC + ${rcl_interfaces_INCLUDE_DIRS} + ${rmw_INCLUDE_DIRS} + ${rosidl_generator_cpp_INCLUDE_DIRS} + ${rosidl_typesupport_cpp_INCLUDE_DIRS} + ) + target_link_libraries(test_subscription ${PROJECT_NAME}) + endif() ament_add_gtest(test_find_weak_nodes test/test_find_weak_nodes.cpp) if(TARGET test_find_weak_nodes) target_include_directories(test_find_weak_nodes PUBLIC diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index f6ea530c32..6160f7a379 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -27,11 +27,13 @@ #include "rcl/error_handling.h" #include "rcl/wait.h" +#include "rclcpp/exceptions.hpp" #include "rclcpp/function_traits.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_graph_interface.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/utilities.hpp" +#include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/visibility_control.hpp" #include "rmw/error_handling.h" @@ -138,14 +140,24 @@ class Client : public ClientBase using rosidl_typesupport_cpp::get_service_type_support_handle; auto service_type_support_handle = get_service_type_support_handle(); - if (rcl_client_init(&client_handle_, this->get_rcl_node_handle(), - service_type_support_handle, service_name.c_str(), &client_options) != RCL_RET_OK) - { - // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) - throw std::runtime_error( - std::string("could not create client: ") + - rcl_get_error_string_safe()); - // *INDENT-ON* + rcl_ret_t ret = rcl_client_init( + &client_handle_, + this->get_rcl_node_handle(), + service_type_support_handle, + service_name.c_str(), + &client_options); + if (ret != RCL_RET_OK) { + if (ret == RCL_RET_SERVICE_NAME_INVALID) { + auto rcl_node_handle = this->get_rcl_node_handle(); + // this will throw on any validation problem + rcl_reset_error(); + expand_topic_or_service_name( + service_name, + rcl_node_get_name(rcl_node_handle), + rcl_node_get_namespace(rcl_node_handle), + true); + } + rclcpp::exceptions::throw_from_rcl_error(ret, "could not create client"); } } @@ -153,7 +165,8 @@ class Client : public ClientBase { if (rcl_client_fini(&client_handle_, this->get_rcl_node_handle()) != RCL_RET_OK) { fprintf(stderr, - "Error in destruction of rmw client handle: %s\n", rmw_get_error_string_safe()); + "Error in destruction of rcl client handle: %s\n", rcl_get_error_string_safe()); + rcl_reset_error(); } } @@ -212,11 +225,9 @@ class Client : public ClientBase { std::lock_guard lock(pending_requests_mutex_); int64_t sequence_number; - if (RCL_RET_OK != rcl_send_request(get_client_handle(), request.get(), &sequence_number)) { - // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) - throw std::runtime_error( - std::string("failed to send request: ") + rcl_get_error_string_safe()); - // *INDENT-ON* + rcl_ret_t ret = rcl_send_request(get_client_handle(), request.get(), &sequence_number); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); } SharedPromise call_promise = std::make_shared(); diff --git a/rclcpp/include/rclcpp/exceptions.hpp b/rclcpp/include/rclcpp/exceptions.hpp index d650ba4238..9b6eb11a70 100644 --- a/rclcpp/include/rclcpp/exceptions.hpp +++ b/rclcpp/include/rclcpp/exceptions.hpp @@ -35,18 +35,88 @@ class InvalidNodeError : public std::runtime_error : std::runtime_error("node is invalid") {} }; +/// Thrown when a any kind of name (node, namespace, topic, etc.) is invalid. +class NameValidationError : public std::invalid_argument +{ +public: + NameValidationError( + const char * name_type_, + const char * name_, + const char * error_msg_, + size_t invalid_index_) + : std::invalid_argument(format_error(name_type_, name_, error_msg_, invalid_index_)), + name_type(name_type_), name(name_), error_msg(error_msg_), invalid_index(invalid_index_) + {} + + static std::string + format_error( + const char * name_type, + const char * name, + const char * error_msg, + size_t invalid_index); + + const std::string name_type; + const std::string name; + const std::string error_msg; + const size_t invalid_index; +}; + +/// Thrown when a node name is invalid. +class InvalidNodeNameError : public NameValidationError +{ +public: + InvalidNodeNameError(const char * node_name, const char * error_msg, size_t invalid_index) + : NameValidationError("node name", node_name, error_msg, invalid_index) + {} +}; + +/// Thrown when a node namespace is invalid. +class InvalidNamespaceError : public NameValidationError +{ +public: + InvalidNamespaceError(const char * namespace_, const char * error_msg, size_t invalid_index) + : NameValidationError("namespace", namespace_, error_msg, invalid_index) + {} +}; + +/// Thrown when a topic name is invalid. +class InvalidTopicNameError : public NameValidationError +{ +public: + InvalidTopicNameError(const char * namespace_, const char * error_msg, size_t invalid_index) + : NameValidationError("topic name", namespace_, error_msg, invalid_index) + {} +}; + +/// Thrown when a service name is invalid. +class InvalidServiceNameError : public NameValidationError +{ +public: + InvalidServiceNameError(const char * namespace_, const char * error_msg, size_t invalid_index) + : NameValidationError("service name", namespace_, error_msg, invalid_index) + {} +}; + /// Throw a C++ std::exception which was created based on an rcl error. /** + * Passing nullptr for reset_error is safe and will avoid calling any function + * to reset the error. + * * \param ret the return code for the current error state * \param prefix string to prefix to the error if applicable (not all errors have custom messages) - * \param reset_error if true rcl_reset_error() is called before returning + * \param error_state error state to create exception from, if nullptr rcl_get_error_state is used + * \param reset_error function to be called before throwing which whill clear the error state * \throws std::invalid_argument if ret is RCL_RET_OK * \throws std::runtime_error if the rcl_get_error_state returns 0 * \throws RCLErrorBase some child class exception based on ret */ RCLCPP_PUBLIC void -throw_from_rcl_error(rcl_ret_t ret, const std::string & prefix = "", bool reset_error = true); +throw_from_rcl_error( + rcl_ret_t ret, + const std::string & prefix = "", + const rcl_error_state_t * error_state = nullptr, + void (* reset_error)() = rcl_reset_error); class RCLErrorBase { diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 9d32870e5d..621e900a19 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -25,8 +25,10 @@ #include "rcl/service.h" #include "rclcpp/any_service_callback.hpp" +#include "rclcpp/exceptions.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/type_support_decl.hpp" +#include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/visibility_control.hpp" #include "rmw/error_handling.h" #include "rmw/rmw.h" @@ -70,6 +72,10 @@ class ServiceBase protected: RCLCPP_DISABLE_COPY(ServiceBase) + RCLCPP_PUBLIC + rcl_node_t * + get_rcl_node_handle() const; + std::shared_ptr node_handle_; rcl_service_t * service_handle_ = nullptr; @@ -109,12 +115,25 @@ class Service : public ServiceBase service_handle_ = new rcl_service_t; *service_handle_ = rcl_get_zero_initialized_service(); - if (rcl_service_init( - service_handle_, node_handle.get(), service_type_support_handle, service_name.c_str(), - &service_options) != RCL_RET_OK) - { - throw std::runtime_error(std::string("could not create service: ") + - rcl_get_error_string_safe()); + rcl_ret_t ret = rcl_service_init( + service_handle_, + node_handle.get(), + service_type_support_handle, + service_name.c_str(), + &service_options); + if (ret != RCL_RET_OK) { + if (ret == RCL_RET_SERVICE_NAME_INVALID) { + auto rcl_node_handle = get_rcl_node_handle(); + // this will throw on any validation problem + rcl_reset_error(); + expand_topic_or_service_name( + service_name, + rcl_node_get_name(rcl_node_handle), + rcl_node_get_namespace(rcl_node_handle), + true); + } + + rclcpp::exceptions::throw_from_rcl_error(ret, "could not create service"); } } @@ -151,6 +170,7 @@ class Service : public ServiceBase ss << "Error in destruction of rcl service_handle_ handle: " << rcl_get_error_string_safe() << '\n'; (std::cerr << ss.str()).flush(); + rcl_reset_error(); } } } @@ -183,10 +203,7 @@ class Service : public ServiceBase rcl_ret_t status = rcl_send_response(get_service_handle(), req_id.get(), response.get()); if (status != RCL_RET_OK) { - // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) - throw std::runtime_error( - std::string("failed to send response: ") + rcl_get_error_string_safe()); - // *INDENT-ON* + rclcpp::exceptions::throw_from_rcl_error(status, "failed to send response"); } } diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index ef7d288784..38d53bf3c9 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -29,10 +29,12 @@ #include "rcl_interfaces/msg/intra_process_message.hpp" +#include "rclcpp/exceptions.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/message_memory_strategy.hpp" #include "rclcpp/any_subscription_callback.hpp" #include "rclcpp/type_support_decl.hpp" +#include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp @@ -238,16 +240,25 @@ class Subscription : public SubscriptionBase MatchesAnyPublishersCallbackType matches_any_publisher_callback, const rcl_subscription_options_t & intra_process_options) { - if (rcl_subscription_init( - &intra_process_subscription_handle_, node_handle_.get(), - rclcpp::type_support::get_intra_process_message_msg_type_support(), - (std::string(get_topic_name()) + "__intra").c_str(), - &intra_process_options) != RCL_RET_OK) - { - // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) - throw std::runtime_error( - std::string("could not create intra process subscription: ") + rcl_get_error_string_safe()); - // *INDENT-ON* + std::string intra_process_topic_name = std::string(get_topic_name()) + "/_intra"; + rcl_ret_t ret = rcl_subscription_init( + &intra_process_subscription_handle_, + node_handle_.get(), + rclcpp::type_support::get_intra_process_message_msg_type_support(), + intra_process_topic_name.c_str(), + &intra_process_options); + if (ret != RCL_RET_OK) { + if (ret == RCL_RET_TOPIC_NAME_INVALID) { + auto rcl_node_handle = node_handle_.get(); + // this will throw on any validation problem + rcl_reset_error(); + expand_topic_or_service_name( + intra_process_topic_name, + rcl_node_get_name(rcl_node_handle), + rcl_node_get_namespace(rcl_node_handle)); + } + + rclcpp::exceptions::throw_from_rcl_error(ret, "could not create intra process subscription"); } intra_process_subscription_id_ = intra_process_subscription_id; diff --git a/rclcpp/src/rclcpp/exceptions.cpp b/rclcpp/src/rclcpp/exceptions.cpp index 175226c39b..b10523c2c0 100644 --- a/rclcpp/src/rclcpp/exceptions.cpp +++ b/rclcpp/src/rclcpp/exceptions.cpp @@ -18,18 +18,40 @@ #include #include +using namespace std::string_literals; + namespace rclcpp { namespace exceptions { +std::string +NameValidationError::format_error( + const char * name_type, + const char * name, + const char * error_msg, + size_t invalid_index) +{ + std::string msg = ""; + msg += "Invalid "s + name_type + ": " + error_msg + ":\n"; + msg += " '"s + name + "'\n"; + msg += " "s + std::string(invalid_index, ' ') + "^\n"; + return msg; +} + void -throw_from_rcl_error(rcl_ret_t ret, const std::string & prefix, bool reset_error) +throw_from_rcl_error( + rcl_ret_t ret, + const std::string & prefix, + const rcl_error_state_t * error_state, + void (* reset_error)()) { if (RCL_RET_OK == ret) { throw std::invalid_argument("ret is RCL_RET_OK"); } - const rcl_error_state_t * error_state = rcl_get_error_state(); + if (!error_state) { + error_state = rcl_get_error_state(); + } if (!error_state) { throw std::runtime_error("rcl error state is not set"); } @@ -39,7 +61,7 @@ throw_from_rcl_error(rcl_ret_t ret, const std::string & prefix, bool reset_error } RCLErrorBase base_exc(ret, error_state); if (reset_error) { - rcl_reset_error(); + reset_error(); } switch (ret) { case RCL_RET_BAD_ALLOC: diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index d4160c47ba..177576931e 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -20,6 +20,8 @@ #include "rclcpp/node_interfaces/node_base.hpp" #include "rclcpp/exceptions.hpp" +#include "rmw/validate_node_name.h" +#include "rmw/validate_namespace.h" using rclcpp::exceptions::throw_from_rcl_error; @@ -90,6 +92,42 @@ NodeBase::NodeBase( // Finalize the interrupt guard condition. finalize_notify_guard_condition(); + if (ret == RCL_RET_NODE_INVALID_NAME) { + rcl_reset_error(); // discard rcl_node_init error + int validation_result; + size_t invalid_index; + rmw_ret_t rmw_ret = + rmw_validate_node_name(node_name.c_str(), &validation_result, &invalid_index); + if (rmw_ret != RMW_RET_OK) { + if (rmw_ret == RMW_RET_INVALID_ARGUMENT) { + throw_from_rcl_error(RCL_RET_INVALID_ARGUMENT, "failed to validate node name"); + } + throw_from_rcl_error(RCL_RET_ERROR, "failed to validate node name"); + } + throw rclcpp::exceptions::InvalidNodeNameError( + node_name.c_str(), + rmw_node_name_validation_result_string(validation_result), + invalid_index); + } + + if (ret == RCL_RET_NODE_INVALID_NAMESPACE) { + rcl_reset_error(); // discard rcl_node_init error + int validation_result; + size_t invalid_index; + rmw_ret_t rmw_ret = + rmw_validate_namespace(namespace_.c_str(), &validation_result, &invalid_index); + if (rmw_ret != RMW_RET_OK) { + if (rmw_ret == RMW_RET_INVALID_ARGUMENT) { + throw_from_rcl_error(RCL_RET_INVALID_ARGUMENT, "failed to validate namespace"); + } + throw_from_rcl_error(RCL_RET_ERROR, "failed to validate namespace"); + } + throw rclcpp::exceptions::InvalidNamespaceError( + namespace_.c_str(), + rmw_namespace_validation_result_string(validation_result), + invalid_index); + } + throw_from_rcl_error(ret, "failed to initialize rcl node"); } diff --git a/rclcpp/src/rclcpp/publisher.cpp b/rclcpp/src/rclcpp/publisher.cpp index 98ca8e8e22..5ebfd2a40e 100644 --- a/rclcpp/src/rclcpp/publisher.cpp +++ b/rclcpp/src/rclcpp/publisher.cpp @@ -28,8 +28,10 @@ #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/allocator/allocator_deleter.hpp" +#include "rclcpp/exceptions.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node.hpp" +#include "rclcpp/expand_topic_or_service_name.hpp" using rclcpp::publisher::PublisherBase; @@ -41,25 +43,36 @@ PublisherBase::PublisherBase( : rcl_node_handle_(node_base->get_shared_rcl_node_handle()), intra_process_publisher_id_(0), store_intra_process_message_(nullptr) { - if (rcl_publisher_init( - &publisher_handle_, rcl_node_handle_.get(), &type_support, - topic.c_str(), &publisher_options) != RCL_RET_OK) - { - throw std::runtime_error( - std::string("could not create publisher: ") + - rcl_get_error_string_safe()); + rcl_ret_t ret = rcl_publisher_init( + &publisher_handle_, + rcl_node_handle_.get(), + &type_support, + topic.c_str(), + &publisher_options); + if (ret != RCL_RET_OK) { + if (ret == RCL_RET_TOPIC_NAME_INVALID) { + auto rcl_node_handle = rcl_node_handle_.get(); + // this will throw on any validation problem + rcl_reset_error(); + expand_topic_or_service_name( + topic, + rcl_node_get_name(rcl_node_handle), + rcl_node_get_namespace(rcl_node_handle)); + } + + rclcpp::exceptions::throw_from_rcl_error(ret, "could not create publisher"); } // Life time of this object is tied to the publisher handle. rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle(&publisher_handle_); if (!publisher_rmw_handle) { - throw std::runtime_error( - std::string("failed to get rmw handle: ") + rcl_get_error_string_safe()); + auto msg = std::string("failed to get rmw handle: ") + rcl_get_error_string_safe(); + rcl_reset_error(); + throw std::runtime_error(msg); } if (rmw_get_gid_for_publisher(publisher_rmw_handle, &rmw_gid_) != RMW_RET_OK) { - // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) - throw std::runtime_error( - std::string("failed to get publisher gid: ") + rmw_get_error_string_safe()); - // *INDENT-ON* + auto msg = std::string("failed to get publisher gid: ") + rmw_get_error_string_safe(); + rmw_reset_error(); + throw std::runtime_error(msg); } } @@ -70,6 +83,7 @@ PublisherBase::~PublisherBase() stderr, "Error in destruction of intra process rcl publisher handle: %s\n", rcl_get_error_string_safe()); + rcl_reset_error(); } if (rcl_publisher_fini(&publisher_handle_, rcl_node_handle_.get()) != RCL_RET_OK) { @@ -77,6 +91,7 @@ PublisherBase::~PublisherBase() stderr, "Error in destruction of rcl publisher handle: %s\n", rcl_get_error_string_safe()); + rcl_reset_error(); } } @@ -91,8 +106,9 @@ PublisherBase::get_queue_size() const { const rcl_publisher_options_t * publisher_options = rcl_publisher_get_options(&publisher_handle_); if (!publisher_options) { - throw std::runtime_error( - std::string("failed to get publisher options: ") + rcl_get_error_string_safe()); + auto msg = std::string("failed to get publisher options: ") + rcl_get_error_string_safe(); + rcl_reset_error(); + throw std::runtime_error(msg); } return publisher_options->qos.depth; } @@ -121,14 +137,16 @@ PublisherBase::operator==(const rmw_gid_t * gid) const bool result = false; auto ret = rmw_compare_gids_equal(gid, &this->get_gid(), &result); if (ret != RMW_RET_OK) { - throw std::runtime_error( - std::string("failed to compare gids: ") + rmw_get_error_string_safe()); + auto msg = std::string("failed to compare gids: ") + rmw_get_error_string_safe(); + rmw_reset_error(); + throw std::runtime_error(msg); } if (!result) { ret = rmw_compare_gids_equal(gid, &this->get_intra_process_gid(), &result); if (ret != RMW_RET_OK) { - throw std::runtime_error( - std::string("failed to compare gids: ") + rmw_get_error_string_safe()); + auto msg = std::string("failed to compare gids: ") + rmw_get_error_string_safe(); + rmw_reset_error(); + throw std::runtime_error(msg); } } return result; @@ -140,15 +158,25 @@ PublisherBase::setup_intra_process( StoreMessageCallbackT callback, const rcl_publisher_options_t & intra_process_options) { - auto intra_process_topic_name = std::string(this->get_topic_name()) + "__intra"; - if (rcl_publisher_init( - &intra_process_publisher_handle_, rcl_node_handle_.get(), - rclcpp::type_support::get_intra_process_message_msg_type_support(), - intra_process_topic_name.c_str(), &intra_process_options) != RCL_RET_OK) - { - throw std::runtime_error( - std::string("could not create intra process publisher: ") + - rcl_get_error_string_safe()); + auto intra_process_topic_name = std::string(this->get_topic_name()) + "/_intra"; + rcl_ret_t ret = rcl_publisher_init( + &intra_process_publisher_handle_, + rcl_node_handle_.get(), + rclcpp::type_support::get_intra_process_message_msg_type_support(), + intra_process_topic_name.c_str(), + &intra_process_options); + if (ret != RCL_RET_OK) { + if (ret == RCL_RET_TOPIC_NAME_INVALID) { + auto rcl_node_handle = rcl_node_handle_.get(); + // this will throw on any validation problem + rcl_reset_error(); + expand_topic_or_service_name( + intra_process_topic_name, + rcl_node_get_name(rcl_node_handle), + rcl_node_get_namespace(rcl_node_handle)); + } + + rclcpp::exceptions::throw_from_rcl_error(ret, "could not create intra process publisher"); } intra_process_publisher_id_ = intra_process_publisher_id; @@ -157,16 +185,16 @@ PublisherBase::setup_intra_process( rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle( &intra_process_publisher_handle_); if (publisher_rmw_handle == nullptr) { - throw std::runtime_error(std::string( - "Failed to get rmw publisher handle") + rcl_get_error_string_safe()); + auto msg = std::string("Failed to get rmw publisher handle") + rcl_get_error_string_safe(); + rcl_reset_error(); + throw std::runtime_error(msg); } - auto ret = rmw_get_gid_for_publisher( + auto rmw_ret = rmw_get_gid_for_publisher( publisher_rmw_handle, &intra_process_rmw_gid_); - if (ret != RMW_RET_OK) { - // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) - throw std::runtime_error( - std::string("failed to create intra process publisher gid: ") + - rmw_get_error_string_safe()); - // *INDENT-ON* + if (rmw_ret != RMW_RET_OK) { + auto msg = + std::string("failed to create intra process publisher gid: ") + rmw_get_error_string_safe(); + rmw_reset_error(); + throw std::runtime_error(msg); } } diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index 136d6883a0..4adcc73c85 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -51,3 +51,9 @@ ServiceBase::get_service_handle() { return service_handle_; } + +rcl_node_t * +ServiceBase::get_rcl_node_handle() const +{ + return node_handle_.get(); +} diff --git a/rclcpp/src/rclcpp/subscription.cpp b/rclcpp/src/rclcpp/subscription.cpp index 5d5ba174c9..81f08d8271 100644 --- a/rclcpp/src/rclcpp/subscription.cpp +++ b/rclcpp/src/rclcpp/subscription.cpp @@ -18,6 +18,9 @@ #include #include +#include "rclcpp/exceptions.hpp" +#include "rclcpp/expand_topic_or_service_name.hpp" + #include "rmw/error_handling.h" #include "rmw/rmw.h" @@ -38,8 +41,17 @@ SubscriptionBase::SubscriptionBase( topic_name.c_str(), &subscription_options); if (ret != RCL_RET_OK) { - throw std::runtime_error( - std::string("could not create subscription: ") + rcl_get_error_string_safe()); + if (ret == RCL_RET_TOPIC_NAME_INVALID) { + auto rcl_node_handle = node_handle_.get(); + // this will throw on any validation problem + rcl_reset_error(); + expand_topic_or_service_name( + topic_name, + rcl_node_get_name(rcl_node_handle), + rcl_node_get_namespace(rcl_node_handle)); + } + + rclcpp::exceptions::throw_from_rcl_error(ret, "could not create subscription"); } } diff --git a/rclcpp/test/test_client.cpp b/rclcpp/test/test_client.cpp new file mode 100644 index 0000000000..52fca58715 --- /dev/null +++ b/rclcpp/test/test_client.cpp @@ -0,0 +1,60 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "rcl_interfaces/srv/list_parameters.hpp" + +class TestClient : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + void SetUp() + { + node = std::make_shared("my_node", "/ns"); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::node::Node::SharedPtr node; +}; + +/* + Testing client construction and destruction. + */ +TEST_F(TestClient, construction_and_destruction) { + using rcl_interfaces::srv::ListParameters; + { + auto client = node->create_client("service"); + } + + { + ASSERT_THROW({ + auto client = node->create_client("invalid_service?"); + }, rclcpp::exceptions::InvalidServiceNameError); + } +} diff --git a/rclcpp/test/test_node.cpp b/rclcpp/test/test_node.cpp new file mode 100644 index 0000000000..34d0e110df --- /dev/null +++ b/rclcpp/test/test_node.cpp @@ -0,0 +1,52 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/rclcpp.hpp" + +class TestNode : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } +}; + +/* + Testing node construction and destruction. + */ +TEST_F(TestNode, construction_and_destruction) { + { + auto node = std::make_shared("my_node", "/ns"); + } + + { + ASSERT_THROW({ + auto node = std::make_shared("invalid_node?", "/ns"); + }, rclcpp::exceptions::InvalidNodeNameError); + } + + { + ASSERT_THROW({ + auto node = std::make_shared("my_node", "/invalid_ns?"); + }, rclcpp::exceptions::InvalidNamespaceError); + } +} diff --git a/rclcpp/test/test_publisher.cpp b/rclcpp/test/test_publisher.cpp new file mode 100644 index 0000000000..e7cb7f7cef --- /dev/null +++ b/rclcpp/test/test_publisher.cpp @@ -0,0 +1,60 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "rcl_interfaces/msg/intra_process_message.hpp" + +class TestPublisher : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + void SetUp() + { + node = std::make_shared("my_node", "/ns"); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::node::Node::SharedPtr node; +}; + +/* + Testing publisher construction and destruction. + */ +TEST_F(TestPublisher, construction_and_destruction) { + using rcl_interfaces::msg::IntraProcessMessage; + { + auto publisher = node->create_publisher("topic"); + } + + { + ASSERT_THROW({ + auto publisher = node->create_publisher("invalid_topic?"); + }, rclcpp::exceptions::InvalidTopicNameError); + } +} diff --git a/rclcpp/test/test_service.cpp b/rclcpp/test/test_service.cpp new file mode 100644 index 0000000000..02752ba3a3 --- /dev/null +++ b/rclcpp/test/test_service.cpp @@ -0,0 +1,63 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "rcl_interfaces/srv/list_parameters.hpp" + +class TestService : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + void SetUp() + { + node = std::make_shared("my_node", "/ns"); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::node::Node::SharedPtr node; +}; + +/* + Testing service construction and destruction. + */ +TEST_F(TestService, construction_and_destruction) { + using rcl_interfaces::srv::ListParameters; + auto callback = + [](const ListParameters::Request::SharedPtr, ListParameters::Response::SharedPtr) { + }; + { + auto service = node->create_service("service", callback); + } + + { + ASSERT_THROW({ + auto service = node->create_service("invalid_service?", callback); + }, rclcpp::exceptions::InvalidServiceNameError); + } +} diff --git a/rclcpp/test/test_subscription.cpp b/rclcpp/test/test_subscription.cpp new file mode 100644 index 0000000000..44f9c991c2 --- /dev/null +++ b/rclcpp/test/test_subscription.cpp @@ -0,0 +1,63 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "rcl_interfaces/msg/intra_process_message.hpp" + +class TestSubscription : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + void SetUp() + { + node = std::make_shared("my_node", "/ns"); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::node::Node::SharedPtr node; +}; + +/* + Testing subscription construction and destruction. + */ +TEST_F(TestSubscription, construction_and_destruction) { + using rcl_interfaces::msg::IntraProcessMessage; + auto callback = [](const IntraProcessMessage::SharedPtr msg) { + (void)msg; + }; + { + auto sub = node->create_subscription("topic", callback); + } + + { + ASSERT_THROW({ + auto sub = node->create_subscription("invalid_topic?", callback); + }, rclcpp::exceptions::InvalidTopicNameError); + } +}