add expand_action_name APIs to action client and server

Signed-off-by: Alberto Soragna <[email protected]>

rclcpp_action/CMakeLists.txt

@@ -77,7 +77,7 @@ if(BUILD_TESTING)
   set(ament_cmake_cppcheck_ADDITIONAL_INCLUDE_DIRS ${rclcpp_INCLUDE_DIRS})
   ament_lint_auto_find_test_dependencies()
 
-  add_subdirectory(test/benchmark)
+  #add_subdirectory(test/benchmark)
 
   ament_add_gtest(test_client test/test_client.cpp TIMEOUT 180)
   ament_add_test_label(test_client mimick)

rclcpp_action/include/rclcpp_action/client.hpp

@@ -91,6 +91,12 @@ class ClientBase : public rclcpp::Waitable
     );
   }
 
+  /// Compute the name of the action by expanding the namespace.
+  /** \return The name of the action. */
+  RCLCPP_ACTION_PUBLIC
+  std::string
+  expand_action_name() const;
+
   // -------------
   // Waitables API
 

rclcpp_action/include/rclcpp_action/server.hpp

@@ -84,6 +84,12 @@ class ServerBase : public rclcpp::Waitable
   RCLCPP_ACTION_PUBLIC
   virtual ~ServerBase();
 
+  /// Compute the name of the action by expanding the namespace.
+  /** \return The name of the action. */
+  RCLCPP_ACTION_PUBLIC
+  std::string
+  expand_action_name() const;
+
   // -------------
   // Waitables API
 

rclcpp_action/src/client.cpp

@@ -222,6 +222,27 @@ ClientBase::action_server_is_ready() const
   return is_ready;
 }
 
+std::string
+ClientBase::expand_action_name() const
+{
+  char * output_cstr = NULL;
+  auto allocator = rcl_get_default_allocator();
+  // Call `rcl_node_resolve_name with `only_expand` mode: remapping action names is not supported
+  rcl_ret_t ret = rcl_node_resolve_name(
+    this->pimpl_->node_handle.get(),
+    rcl_action_client_get_action_name(this->pimpl_->client_handle.get()),
+    allocator,
+    /*is_service*/ true,
+    /*only_expand*/ true,
+    &output_cstr);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::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;
+}
+
 bool
 ClientBase::wait_for_action_server_nanoseconds(std::chrono::nanoseconds timeout)
 {

rclcpp_action/src/server.cpp

@@ -86,6 +86,8 @@ class ServerBaseImpl
 
   rclcpp::Clock::SharedPtr clock_;
 
+  // node_handle_ must be destroyed after client_handle to prevent memory leak
+  std::shared_ptr<rcl_node_t> node_handle_{nullptr};
   // Do not declare this before clock_ as this depends on clock_(see #1526)
   std::shared_ptr<rcl_action_server_t> action_server_;
 
@@ -138,6 +140,7 @@ ServerBase::ServerBase(
       }
     };
 
+  pimpl_->node_handle_ = node_base->get_shared_rcl_node_handle();
   pimpl_->action_server_.reset(new rcl_action_server_t, deleter);
   *(pimpl_->action_server_) = rcl_action_get_zero_initialized_server();
 
@@ -168,6 +171,27 @@ ServerBase::~ServerBase()
 {
 }
 
+std::string
+ServerBase::expand_action_name() const
+{
+  char * output_cstr = NULL;
+  auto allocator = rcl_get_default_allocator();
+  // Call `rcl_node_resolve_name with `only_expand` mode: remapping action names is not supported
+  rcl_ret_t ret = rcl_node_resolve_name(
+    this->pimpl_->node_handle_.get(),
+    rcl_action_server_get_action_name(this->pimpl_->action_server_.get()),
+    allocator,
+    /*is_service*/ true,
+    /*only_expand*/ true,
+    &output_cstr);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::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;
+}
+
 size_t
 ServerBase::get_number_of_ready_subscriptions()
 {

rclcpp_action/test/test_client.cpp

@@ -389,6 +389,30 @@ TEST_F(TestClient, wait_for_action_server_rcl_errors)
   TearDownServer();
 }
 
+TEST_F(TestClient, action_name)
+{
+  {
+    // Default namespace
+    auto test_node = std::make_shared<rclcpp::Node>("test_node");
+    auto action_client = rclcpp_action::create_client<ActionType>(test_node, "my_action");
+    EXPECT_EQ(action_client->expand_action_name(), "/my_action");
+  }
+
+  {
+    // Custom namespace
+    auto test_node = std::make_shared<rclcpp::Node>("test_node", "test_namespace");
+    auto action_client = rclcpp_action::create_client<ActionType>(test_node, "my_action");
+    EXPECT_EQ(action_client->expand_action_name(), "/test_namespace/my_action");
+  }
+
+  {
+    // Action with absolute (global) name
+    auto test_node = std::make_shared<rclcpp::Node>("test_node", "test_namespace");
+    auto action_client = rclcpp_action::create_client<ActionType>(test_node, "/my_action");
+    EXPECT_EQ(action_client->expand_action_name(), "/my_action");
+  }
+}
+
 TEST_F(TestClient, is_ready) {
   auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
   rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();

rclcpp_action/test/test_server.cpp

@@ -226,6 +226,45 @@ TEST_F(TestServer, construction_and_destruction_sub_node)
   });
 }
 
+TEST_F(TestServer, action_name)
+{
+  auto create_server_func = [](std::shared_ptr<rclcpp::Node> node, const std::string & action_name)
+    {
+      using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+      auto as = rclcpp_action::create_server<Fibonacci>(
+        node, action_name,
+        [](const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>) {
+          return rclcpp_action::GoalResponse::REJECT;
+        },
+        [](std::shared_ptr<GoalHandle>) {
+          return rclcpp_action::CancelResponse::REJECT;
+        },
+        [](std::shared_ptr<GoalHandle>) {});
+      return as;
+    };
+
+  {
+    // Default namespace
+    auto test_node = std::make_shared<rclcpp::Node>("test_node");
+    auto action_server = create_server_func(test_node, "my_action");
+    EXPECT_EQ(action_server->expand_action_name(), "/my_action");
+  }
+
+  {
+    // Custom namespace
+    auto test_node = std::make_shared<rclcpp::Node>("test_node", "test_namespace");
+    auto action_server = create_server_func(test_node, "my_action");
+    EXPECT_EQ(action_server->expand_action_name(), "/test_namespace/my_action");
+  }
+
+  {
+    // Action with absolute (global) name
+    auto test_node = std::make_shared<rclcpp::Node>("test_node", "test_namespace");
+    auto action_server = create_server_func(test_node, "/my_action");
+    EXPECT_EQ(action_server->expand_action_name(), "/my_action");
+  }
+}
+
 TEST_F(TestServer, handle_goal_called)
 {
   auto node = std::make_shared<rclcpp::Node>("handle_goal_node", "/rclcpp_action/handle_goal");