Add methods in topic and service interface to resolve a name (#1410)

Signed-off-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>

rclcpp/include/rclcpp/node_interfaces/node_base.hpp

@@ -118,6 +118,10 @@ class NodeBase : public NodeBaseInterface
   bool
   get_enable_topic_statistics_default() const override;
 
+  std::string
+  resolve_topic_or_service_name(
+    const std::string & name, bool is_service, bool only_expand = false) const override;
+
 private:
   RCLCPP_DISABLE_COPY(NodeBase)
 

rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp

@@ -163,6 +163,13 @@ class NodeBaseInterface
   virtual
   bool
   get_enable_topic_statistics_default() const = 0;
+
+  /// Expand and remap a given topic or service name.
+  RCLCPP_PUBLIC
+  virtual
+  std::string
+  resolve_topic_or_service_name(
+    const std::string & name, bool is_service, bool only_expand = false) const = 0;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_services.hpp

@@ -15,6 +15,8 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_SERVICES_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_SERVICES_HPP_
 
+#include <string>
+
 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/client.hpp"
 #include "rclcpp/macros.hpp"
@@ -53,6 +55,10 @@ class NodeServices : public NodeServicesInterface
     rclcpp::ServiceBase::SharedPtr service_base_ptr,
     rclcpp::CallbackGroup::SharedPtr group) override;
 
+  RCLCPP_PUBLIC
+  std::string
+  resolve_service_name(const std::string & name, bool only_expand = false) const override;
+
 private:
   RCLCPP_DISABLE_COPY(NodeServices)
 

rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp

@@ -15,6 +15,8 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_SERVICES_INTERFACE_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_SERVICES_INTERFACE_HPP_
 
+#include <string>
+
 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/client.hpp"
 #include "rclcpp/macros.hpp"
@@ -49,6 +51,12 @@ class NodeServicesInterface
   add_service(
     rclcpp::ServiceBase::SharedPtr service_base_ptr,
     rclcpp::CallbackGroup::SharedPtr group) = 0;
+
+  /// Get the remapped and expanded service name given a input name.
+  RCLCPP_PUBLIC
+  virtual
+  std::string
+  resolve_service_name(const std::string & name, bool only_expand = false) const = 0;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_topics.hpp

@@ -81,6 +81,10 @@ class NodeTopics : public NodeTopicsInterface
   rclcpp::node_interfaces::NodeTimersInterface *
   get_node_timers_interface() const override;
 
+  RCLCPP_PUBLIC
+  std::string
+  resolve_topic_name(const std::string & name, bool only_expand = false) const override;
+
 private:
   RCLCPP_DISABLE_COPY(NodeTopics)
 

rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp

@@ -86,6 +86,12 @@ class NodeTopicsInterface
   virtual
   rclcpp::node_interfaces::NodeTimersInterface *
   get_node_timers_interface() const = 0;
+
+  /// Get a remapped and expanded topic name given an input name.
+  RCLCPP_PUBLIC
+  virtual
+  std::string
+  resolve_topic_name(const std::string & name, bool only_expand = false) const = 0;
 };
 
 }  // namespace node_interfaces

rclcpp/src/rclcpp/node_interfaces/node_base.cpp

@@ -288,3 +288,24 @@ NodeBase::get_enable_topic_statistics_default() const
 {
   return enable_topic_statistics_default_;
 }
+
+std::string
+NodeBase::resolve_topic_or_service_name(
+  const std::string & name, bool is_service, bool only_expand) const
+{
+  char * output_cstr = NULL;
+  auto allocator = rcl_get_default_allocator();
+  rcl_ret_t ret = rcl_node_resolve_name(
+    node_handle_.get(),
+    name.c_str(),
+    allocator,
+    is_service,
+    only_expand,
+    &output_cstr);
+  if (RCL_RET_OK != ret) {
+    throw_from_rcl_error(ret, "failed to resolve name", rcl_get_error_state());
+  }
+  std::string output{output_cstr};
+  allocator.deallocate(output_cstr, allocator.state);
+  return output;
+}

rclcpp/src/rclcpp/node_interfaces/node_services.cpp

@@ -78,3 +78,9 @@ NodeServices::add_client(
     }
   }
 }
+
+std::string
+NodeServices::resolve_service_name(const std::string & name, bool only_expand) const
+{
+  return node_base_->resolve_topic_or_service_name(name, true, only_expand);
+}

rclcpp/src/rclcpp/node_interfaces/node_topics.cpp

@@ -129,3 +129,9 @@ NodeTopics::get_node_timers_interface() const
 {
   return node_timers_;
 }
+
+std::string
+NodeTopics::resolve_topic_name(const std::string & name, bool only_expand) const
+{
+  return node_base_->resolve_topic_or_service_name(name, false, only_expand);
+}

rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp

@@ -16,6 +16,7 @@
 
 #include <memory>
 #include <string>
+#include <vector>
 
 #include "rcl/node_options.h"
 #include "rclcpp/node.hpp"
@@ -55,7 +56,9 @@ class TestNodeService : public ::testing::Test
   {
     rclcpp::init(0, nullptr);
 
-    node = std::make_shared<rclcpp::Node>("node", "ns");
+    rclcpp::NodeOptions options{};
+    options.arguments(std::vector<std::string>{"-r", "foo:=bar"});
+    node = std::make_shared<rclcpp::Node>("node", "ns", options);
 
     // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure
     // the proper type is being tested and covered.
@@ -129,3 +132,13 @@ TEST_F(TestNodeService, add_client_rcl_trigger_guard_condition_error)
     node_services->add_client(client, callback_group),
     std::runtime_error("Failed to notify wait set on client creation: error not set"));
 }
+
+TEST_F(TestNodeService, resolve_service_name)
+{
+  EXPECT_EQ("/ns/bar", node_services->resolve_service_name("foo", false));
+  EXPECT_EQ("/ns/foo", node_services->resolve_service_name("foo", true));
+  EXPECT_EQ("/foo", node_services->resolve_service_name("/foo", true));
+  EXPECT_THROW(
+    node_services->resolve_service_name("this is not a valid name!~>", true),
+    rclcpp::exceptions::RCLError);
+}

rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp

@@ -17,6 +17,7 @@
 #include <memory>
 #include <string>
 #include <type_traits>
+#include <vector>
 
 #include "rcl/node_options.h"
 #include "rclcpp/node.hpp"
@@ -80,7 +81,9 @@ class TestNodeTopics : public ::testing::Test
   void SetUp()
   {
     rclcpp::init(0, nullptr);
-    node = std::make_shared<rclcpp::Node>("node", "ns");
+    rclcpp::NodeOptions options{};
+    options.arguments(std::vector<std::string>{"-r", "foo:=bar"});
+    node = std::make_shared<rclcpp::Node>("node", "ns", options);
 
     // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure
     // the proper type is being tested and covered.
@@ -154,3 +157,13 @@ TEST_F(TestNodeTopics, add_subscription_rcl_trigger_guard_condition_error)
     node_topics->add_subscription(subscription, callback_group),
     std::runtime_error("failed to notify wait set on subscription creation: error not set"));
 }
+
+TEST_F(TestNodeTopics, resolve_topic_name)
+{
+  EXPECT_EQ("/ns/bar", node_topics->resolve_topic_name("foo", false));
+  EXPECT_EQ("/ns/foo", node_topics->resolve_topic_name("foo", true));
+  EXPECT_EQ("/foo", node_topics->resolve_topic_name("/foo", true));
+  EXPECT_THROW(
+    node_topics->resolve_topic_name("this is not a valid name!~>", true),
+    rclcpp::exceptions::RCLError);
+}