From 5d9db5de748e936c102352121f7910c7d64ba2ad Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 21 Oct 2020 18:30:55 -0300 Subject: [PATCH] Add methods in topic and service interface to resolve a name (#1410) Signed-off-by: Ivan Santiago Paunovic --- .../rclcpp/node_interfaces/node_base.hpp | 4 ++++ .../node_interfaces/node_base_interface.hpp | 7 +++++++ .../rclcpp/node_interfaces/node_services.hpp | 6 ++++++ .../node_services_interface.hpp | 8 +++++++ .../rclcpp/node_interfaces/node_topics.hpp | 4 ++++ .../node_interfaces/node_topics_interface.hpp | 6 ++++++ .../src/rclcpp/node_interfaces/node_base.cpp | 21 +++++++++++++++++++ .../rclcpp/node_interfaces/node_services.cpp | 6 ++++++ .../rclcpp/node_interfaces/node_topics.cpp | 6 ++++++ .../node_interfaces/test_node_services.cpp | 15 ++++++++++++- .../node_interfaces/test_node_topics.cpp | 15 ++++++++++++- 11 files changed, 96 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp index 5e7f8b7d35..3f8b0021a0 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp +++ b/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) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp index 840159aaba..a9c30dd09a 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp +++ b/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 diff --git a/rclcpp/include/rclcpp/node_interfaces/node_services.hpp b/rclcpp/include/rclcpp/node_interfaces/node_services.hpp index 1eecf69eee..b538ab4ef2 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_services.hpp +++ b/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 + #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) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp index 53a2e3fc4c..ab6cdab42e 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp +++ b/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 + #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 diff --git a/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp b/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp index 38402eff6e..b4d8c5b2f6 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp +++ b/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) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp index 4d9edd0b18..ca69e86e73 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp +++ b/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 diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index 05779331fb..fba6e84f9d 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/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; +} diff --git a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp index ebb0de9bfb..dee39cb403 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp +++ b/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); +} diff --git a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp index fced51c3f5..c57fbcee5d 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp +++ b/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); +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp index a48c50254a..a7607f5d76 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp @@ -16,6 +16,7 @@ #include #include +#include #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("node", "ns"); + rclcpp::NodeOptions options{}; + options.arguments(std::vector{"-r", "foo:=bar"}); + node = std::make_shared("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); +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp index 9b1d4f374e..16f1d92b86 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #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("node", "ns"); + rclcpp::NodeOptions options{}; + options.arguments(std::vector{"-r", "foo:=bar"}); + node = std::make_shared("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); +}