catch and report name issues

for node names, namespaces, and topic/service names

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

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,22 +140,33 @@ class Client : public ClientBase
     using rosidl_typesupport_cpp::get_service_type_support_handle;
     auto service_type_support_handle =
       get_service_type_support_handle<ServiceT>();
-    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");
     }
   }
 
   virtual ~Client()
   {
     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<std::mutex> 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<Promise>();

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
 {

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<rcl_node_t> 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");
     }
   }
 

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;