Skip to content

Commit

Permalink
add expand_action_name APIs to action client and server
Browse files Browse the repository at this point in the history
Signed-off-by: Alberto Soragna <[email protected]>
  • Loading branch information
alsora committed Aug 11, 2024
1 parent 6da8363 commit e689f7b
Show file tree
Hide file tree
Showing 7 changed files with 121 additions and 1 deletion.
2 changes: 1 addition & 1 deletion rclcpp_action/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
6 changes: 6 additions & 0 deletions rclcpp_action/include/rclcpp_action/client.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
6 changes: 6 additions & 0 deletions rclcpp_action/include/rclcpp_action/server.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
21 changes: 21 additions & 0 deletions rclcpp_action/src/client.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down
24 changes: 24 additions & 0 deletions rclcpp_action/src/server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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_;

Expand Down Expand Up @@ -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();

Expand Down Expand Up @@ -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()
{
Expand Down
24 changes: 24 additions & 0 deletions rclcpp_action/test/test_client.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down
39 changes: 39 additions & 0 deletions rclcpp_action/test/test_server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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");
Expand Down

0 comments on commit e689f7b

Please sign in to comment.