diff --git a/rclcpp_action/CMakeLists.txt b/rclcpp_action/CMakeLists.txt index f5537d4f30..8753add537 100644 --- a/rclcpp_action/CMakeLists.txt +++ b/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) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 2993f28bc3..32347cb2d8 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/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 diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index dc64991c46..7b586ded7d 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/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 diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index 3ea9b1fb1a..a7472c01be 100644 --- a/rclcpp_action/src/client.cpp +++ b/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) { diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index fefc02d6ad..e343ebcc6b 100644 --- a/rclcpp_action/src/server.cpp +++ b/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 node_handle_{nullptr}; // Do not declare this before clock_ as this depends on clock_(see #1526) std::shared_ptr 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() { diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index 08093cb873..209d13cacf 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/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("test_node"); + auto action_client = rclcpp_action::create_client(test_node, "my_action"); + EXPECT_EQ(action_client->expand_action_name(), "/my_action"); + } + + { + // Custom namespace + auto test_node = std::make_shared("test_node", "test_namespace"); + auto action_client = rclcpp_action::create_client(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("test_node", "test_namespace"); + auto action_client = rclcpp_action::create_client(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(client_node, action_name); rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index c63d1dfdfc..23b20b99c1 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/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 node, const std::string & action_name) + { + using GoalHandle = rclcpp_action::ServerGoalHandle; + auto as = rclcpp_action::create_server( + node, action_name, + [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::REJECT; + }, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }, + [](std::shared_ptr) {}); + return as; + }; + + { + // Default namespace + auto test_node = std::make_shared("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("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("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("handle_goal_node", "/rclcpp_action/handle_goal");