From c848c9a8710130d5adfaa9746c6939b21699c5de Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Mon, 20 Jul 2026 08:21:36 +0900 Subject: [PATCH 1/3] introduce ActionEndpointInfo from action graph. Signed-off-by: Tomoya Fujita --- rclcpp_action/CMakeLists.txt | 12 + rclcpp_action/include/rclcpp_action/graph.hpp | 234 +++++++++++++++ .../include/rclcpp_action/rclcpp_action.hpp | 1 + rclcpp_action/src/graph.cpp | 283 ++++++++++++++++++ rclcpp_action/test/test_graph.cpp | 248 +++++++++++++++ 5 files changed, 778 insertions(+) create mode 100644 rclcpp_action/include/rclcpp_action/graph.hpp create mode 100644 rclcpp_action/src/graph.cpp create mode 100644 rclcpp_action/test/test_graph.cpp diff --git a/rclcpp_action/CMakeLists.txt b/rclcpp_action/CMakeLists.txt index ece59d7726..4eefd6af2d 100644 --- a/rclcpp_action/CMakeLists.txt +++ b/rclcpp_action/CMakeLists.txt @@ -23,6 +23,7 @@ add_library(${PROJECT_NAME} src/create_generic_client.cpp src/generic_client.cpp src/generic_client_goal_handle.cpp + src/graph.cpp src/qos.cpp src/server.cpp src/server_goal_handle.cpp @@ -109,6 +110,17 @@ if(BUILD_TESTING) ) endif() + ament_add_gtest(test_graph test/test_graph.cpp TIMEOUT 180) + if(TARGET test_graph) + target_link_libraries(test_graph + ${PROJECT_NAME} + rcl::rcl + rcl_action::rcl_action + rclcpp::rclcpp + test_msgs::test_msgs + ) + endif() + ament_add_gtest(test_server test/test_server.cpp TIMEOUT 180) ament_add_test_label(test_server mimick) if(TARGET test_server) diff --git a/rclcpp_action/include/rclcpp_action/graph.hpp b/rclcpp_action/include/rclcpp_action/graph.hpp new file mode 100644 index 0000000000..92ad86d938 --- /dev/null +++ b/rclcpp_action/include/rclcpp_action/graph.hpp @@ -0,0 +1,234 @@ +// Copyright 2026 TriOrb, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP_ACTION__GRAPH_HPP_ +#define RCLCPP_ACTION__GRAPH_HPP_ + +#include +#include +#include + +#include "rcl_action/graph.h" + +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_graph_interface.hpp" + +#include "rclcpp_action/visibility_control.hpp" + +namespace rclcpp_action +{ + +/// Endpoint information of an action client or an action server. +/** + * An action is built on top of three services and two topics: + * + * - the goal service (`/_action/send_goal`) + * - the cancel service (`/_action/cancel_goal`) + * - the result service (`/_action/get_result`) + * - the feedback topic (`/_action/feedback`) + * - the status topic (`/_action/status`) + * + * This class aggregates the endpoint information of all the underlying + * entities of one action client or one action server. + * The goal service endpoint is used as the canonical identity of an action + * client or an action server, so the goal service endpoint information is + * always populated and the node name, node namespace, action type, and + * endpoint type are derived from it. + * The remaining endpoint information is correlated to the goal service + * endpoint by the node name and node namespace, and is `std::nullopt` if the + * underlying entity has not been discovered. + */ +class ActionEndpointInfo +{ +public: + /// Construct from a rcl_action_endpoint_info_t. + /** + * \param[in] info the rcl endpoint information to construct from, its + * `goal_service_info` member must be populated. + * \throws std::invalid_argument if the goal service info is not populated. + */ + RCLCPP_ACTION_PUBLIC + explicit ActionEndpointInfo(const rcl_action_endpoint_info_t & info); + + /// Get the node name of the action endpoint. + RCLCPP_ACTION_PUBLIC + const std::string & + node_name() const; + + /// Get the node namespace of the action endpoint. + RCLCPP_ACTION_PUBLIC + const std::string & + node_namespace() const; + + /// Get the action type, derived from the goal service type. + RCLCPP_ACTION_PUBLIC + std::string + action_type() const; + + /// Get the endpoint type (client or server) of the action endpoint. + RCLCPP_ACTION_PUBLIC + rclcpp::EndpointType + endpoint_type() const; + + /// Get a mutable reference to the endpoint information of the goal service. + RCLCPP_ACTION_PUBLIC + rclcpp::ServiceEndpointInfo & + goal_service_info(); + + /// Get a const reference to the endpoint information of the goal service. + RCLCPP_ACTION_PUBLIC + const rclcpp::ServiceEndpointInfo & + goal_service_info() const; + + /// Get a mutable reference to the endpoint information of the cancel service. + RCLCPP_ACTION_PUBLIC + std::optional & + cancel_service_info(); + + /// Get a const reference to the endpoint information of the cancel service. + RCLCPP_ACTION_PUBLIC + const std::optional & + cancel_service_info() const; + + /// Get a mutable reference to the endpoint information of the result service. + RCLCPP_ACTION_PUBLIC + std::optional & + result_service_info(); + + /// Get a const reference to the endpoint information of the result service. + RCLCPP_ACTION_PUBLIC + const std::optional & + result_service_info() const; + + /// Get a mutable reference to the endpoint information of the feedback topic. + RCLCPP_ACTION_PUBLIC + std::optional & + feedback_topic_info(); + + /// Get a const reference to the endpoint information of the feedback topic. + RCLCPP_ACTION_PUBLIC + const std::optional & + feedback_topic_info() const; + + /// Get a mutable reference to the endpoint information of the status topic. + RCLCPP_ACTION_PUBLIC + std::optional & + status_topic_info(); + + /// Get a const reference to the endpoint information of the status topic. + RCLCPP_ACTION_PUBLIC + const std::optional & + status_topic_info() const; + +private: + rclcpp::ServiceEndpointInfo goal_service_info_; + std::optional cancel_service_info_; + std::optional result_service_info_; + std::optional feedback_topic_info_; + std::optional status_topic_info_; +}; + +/// Return a list of action clients on a given action. +/** + * The returned parameter is a list of ActionEndpointInfo objects, where each + * aggregates the endpoint information of all the underlying entities of one + * action client, i.e. the clients of the goal, cancel, and result services + * and the subscriptions on the feedback and status topics. + * + * The `action_name` may be a relative, private, or fully qualified action + * name. + * A relative or private action name will be expanded using the node's + * namespace and name. + * The queried `action_name` is not remapped. + * + * \param[in] node_base the node base interface of the node used to query the ROS graph. + * \param[in] action_name name of the action to query. + * \return a list of ActionEndpointInfo representing all the action clients on this action. + * \throws std::invalid_argument if the action name is invalid. + * \throws std::runtime_error if the list cannot be retrieved. + */ +RCLCPP_ACTION_PUBLIC +std::vector +get_action_clients_info_by_action( + const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr & node_base, + const std::string & action_name); + +/// Return a list of action servers on a given action. +/** + * The returned parameter is a list of ActionEndpointInfo objects, where each + * aggregates the endpoint information of all the underlying entities of one + * action server, i.e. the servers of the goal, cancel, and result services + * and the publishers on the feedback and status topics. + * + * The `action_name` may be a relative, private, or fully qualified action + * name. + * A relative or private action name will be expanded using the node's + * namespace and name. + * The queried `action_name` is not remapped. + * + * \param[in] node_base the node base interface of the node used to query the ROS graph. + * \param[in] action_name name of the action to query. + * \return a list of ActionEndpointInfo representing all the action servers on this action. + * \throws std::invalid_argument if the action name is invalid. + * \throws std::runtime_error if the list cannot be retrieved. + */ +RCLCPP_ACTION_PUBLIC +std::vector +get_action_servers_info_by_action( + const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr & node_base, + const std::string & action_name); + +/// Return the number of action clients on a given action. +/** + * The `action_name` may be a relative, private, or fully qualified action + * name. + * A relative or private action name will be expanded using the node's + * namespace and name. + * The queried `action_name` is not remapped. + * + * \param[in] node_base the node base interface of the node used to query the ROS graph. + * \param[in] action_name name of the action to count the number of action clients. + * \return the number of action clients on the action. + * \throws std::invalid_argument if the action name is invalid. + * \throws std::runtime_error if the count cannot be retrieved. + */ +RCLCPP_ACTION_PUBLIC +size_t +count_action_clients( + const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr & node_base, + const std::string & action_name); + +/// Return the number of action servers on a given action. +/** + * The `action_name` may be a relative, private, or fully qualified action + * name. + * A relative or private action name will be expanded using the node's + * namespace and name. + * The queried `action_name` is not remapped. + * + * \param[in] node_base the node base interface of the node used to query the ROS graph. + * \param[in] action_name name of the action to count the number of action servers. + * \return the number of action servers on the action. + * \throws std::invalid_argument if the action name is invalid. + * \throws std::runtime_error if the count cannot be retrieved. + */ +RCLCPP_ACTION_PUBLIC +size_t +count_action_servers( + const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr & node_base, + const std::string & action_name); + +} // namespace rclcpp_action + +#endif // RCLCPP_ACTION__GRAPH_HPP_ diff --git a/rclcpp_action/include/rclcpp_action/rclcpp_action.hpp b/rclcpp_action/include/rclcpp_action/rclcpp_action.hpp index 61122e9ff5..9dbd3f5683 100644 --- a/rclcpp_action/include/rclcpp_action/rclcpp_action.hpp +++ b/rclcpp_action/include/rclcpp_action/rclcpp_action.hpp @@ -37,6 +37,7 @@ #include "rclcpp_action/client_goal_handle.hpp" #include "rclcpp_action/create_client.hpp" #include "rclcpp_action/create_server.hpp" +#include "rclcpp_action/graph.hpp" #include "rclcpp_action/server.hpp" #include "rclcpp_action/server_goal_handle.hpp" #include "rclcpp_action/visibility_control.hpp" diff --git a/rclcpp_action/src/graph.cpp b/rclcpp_action/src/graph.cpp new file mode 100644 index 0000000000..b247a5d726 --- /dev/null +++ b/rclcpp_action/src/graph.cpp @@ -0,0 +1,283 @@ +// Copyright 2026 TriOrb, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "rcl/error_handling.h" +#include "rcl/graph.h" +#include "rcl_action/graph.h" +#include "rcutils/allocator.h" + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/expand_topic_or_service_name.hpp" + +#include "rclcpp_action/graph.hpp" + +using rclcpp_action::ActionEndpointInfo; + +namespace +{ + +const rcl_action_endpoint_info_t & +validate_endpoint_info(const rcl_action_endpoint_info_t & info) +{ + if (!info.goal_service_info.node_name || + !info.goal_service_info.node_namespace || + !info.goal_service_info.service_type) + { + throw std::invalid_argument( + "Constructing ActionEndpointInfo with invalid goal service endpoint info"); + } + return info; +} + +std::vector +convert_to_action_info_list(const rcl_action_endpoint_info_array_t & info_array) +{ + std::vector action_info_list; + action_info_list.reserve(info_array.size); + for (size_t i = 0; i < info_array.size; ++i) { + action_info_list.emplace_back(info_array.info_array[i]); + } + return action_info_list; +} + +template +std::vector +get_info_by_action( + const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr & node_base, + const std::string & action_name, + FunctionT rcl_action_get_info_by_action) +{ + auto rcl_node_handle = node_base->get_rcl_node_handle(); + + std::string fqdn = rclcpp::expand_topic_or_service_name( + action_name, + rcl_node_get_name(rcl_node_handle), + rcl_node_get_namespace(rcl_node_handle), + false); // false = not a service + + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rcl_action_endpoint_info_array_t info_array = + rcl_action_get_zero_initialized_endpoint_info_array(); + rcl_ret_t ret = + rcl_action_get_info_by_action(rcl_node_handle, &allocator, fqdn.c_str(), &info_array); + if (RCL_RET_OK != ret) { + auto error_msg = + std::string("Failed to get information by action for ") + EndpointType + std::string(":"); + if (RCL_RET_UNSUPPORTED == ret) { + error_msg += std::string("function not supported by RMW_IMPLEMENTATION"); + } else { + error_msg += rcl_get_error_string().str; + } + rcl_reset_error(); + if (RCL_RET_OK != rcl_action_endpoint_info_array_fini(&info_array, &allocator)) { + error_msg += std::string(", failed also to cleanup endpoint info array, leaking memory: ") + + rcl_get_error_string().str; + rcl_reset_error(); + } + rclcpp::exceptions::throw_from_rcl_error(ret, error_msg); + } + + std::vector action_info_list = convert_to_action_info_list(info_array); + ret = rcl_action_endpoint_info_array_fini(&info_array, &allocator); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "rcl_action_endpoint_info_array_fini failed."); + } + + return action_info_list; +} + +size_t +count_action_entities( + const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr & node_base, + const std::string & action_name, + bool count_clients) +{ + auto rcl_node_handle = node_base->get_rcl_node_handle(); + + std::string fqdn = rclcpp::expand_topic_or_service_name( + action_name, + rcl_node_get_name(rcl_node_handle), + rcl_node_get_namespace(rcl_node_handle), + false); // false = not a service + + size_t count = 0; + rcl_ret_t ret; + if (count_clients) { + ret = rcl_action_count_clients(rcl_node_handle, fqdn.c_str(), &count); + } else { + ret = rcl_action_count_servers(rcl_node_handle, fqdn.c_str(), &count); + } + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error( + ret, std::string("could not count ") + (count_clients ? "action clients" : "action servers")); + } + return count; +} + +} // namespace + +namespace rclcpp_action +{ + +ActionEndpointInfo::ActionEndpointInfo(const rcl_action_endpoint_info_t & info) +: goal_service_info_(validate_endpoint_info(info).goal_service_info) +{ + if (info.cancel_service_info.node_name) { + cancel_service_info_.emplace(info.cancel_service_info); + } + if (info.result_service_info.node_name) { + result_service_info_.emplace(info.result_service_info); + } + if (info.feedback_topic_info.node_name) { + feedback_topic_info_.emplace(info.feedback_topic_info); + } + if (info.status_topic_info.node_name) { + status_topic_info_.emplace(info.status_topic_info); + } +} + +const std::string & +ActionEndpointInfo::node_name() const +{ + return goal_service_info_.node_name(); +} + +const std::string & +ActionEndpointInfo::node_namespace() const +{ + return goal_service_info_.node_namespace(); +} + +std::string +ActionEndpointInfo::action_type() const +{ + const std::string & goal_service_type = goal_service_info_.service_type(); + const std::string suffix = "_SendGoal"; + if (goal_service_type.size() > suffix.size() && + 0 == goal_service_type.compare( + goal_service_type.size() - suffix.size(), suffix.size(), suffix)) + { + return goal_service_type.substr(0, goal_service_type.size() - suffix.size()); + } + return goal_service_type; +} + +rclcpp::EndpointType +ActionEndpointInfo::endpoint_type() const +{ + return goal_service_info_.endpoint_type(); +} + +rclcpp::ServiceEndpointInfo & +ActionEndpointInfo::goal_service_info() +{ + return goal_service_info_; +} + +const rclcpp::ServiceEndpointInfo & +ActionEndpointInfo::goal_service_info() const +{ + return goal_service_info_; +} + +std::optional & +ActionEndpointInfo::cancel_service_info() +{ + return cancel_service_info_; +} + +const std::optional & +ActionEndpointInfo::cancel_service_info() const +{ + return cancel_service_info_; +} + +std::optional & +ActionEndpointInfo::result_service_info() +{ + return result_service_info_; +} + +const std::optional & +ActionEndpointInfo::result_service_info() const +{ + return result_service_info_; +} + +std::optional & +ActionEndpointInfo::feedback_topic_info() +{ + return feedback_topic_info_; +} + +const std::optional & +ActionEndpointInfo::feedback_topic_info() const +{ + return feedback_topic_info_; +} + +std::optional & +ActionEndpointInfo::status_topic_info() +{ + return status_topic_info_; +} + +const std::optional & +ActionEndpointInfo::status_topic_info() const +{ + return status_topic_info_; +} + +static constexpr char kActionClientEndpointTypeName[] = "action clients"; +std::vector +get_action_clients_info_by_action( + const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr & node_base, + const std::string & action_name) +{ + return get_info_by_action( + node_base, action_name, rcl_action_get_clients_info_by_action); +} + +static constexpr char kActionServerEndpointTypeName[] = "action servers"; +std::vector +get_action_servers_info_by_action( + const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr & node_base, + const std::string & action_name) +{ + return get_info_by_action( + node_base, action_name, rcl_action_get_servers_info_by_action); +} + +size_t +count_action_clients( + const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr & node_base, + const std::string & action_name) +{ + return count_action_entities(node_base, action_name, true); +} + +size_t +count_action_servers( + const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr & node_base, + const std::string & action_name) +{ + return count_action_entities(node_base, action_name, false); +} + +} // namespace rclcpp_action diff --git a/rclcpp_action/test/test_graph.cpp b/rclcpp_action/test/test_graph.cpp new file mode 100644 index 0000000000..b0dadbf189 --- /dev/null +++ b/rclcpp_action/test/test_graph.cpp @@ -0,0 +1,248 @@ +// Copyright 2026 TriOrb, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "rclcpp_action/create_client.hpp" +#include "rclcpp_action/create_server.hpp" +#include "rclcpp_action/graph.hpp" + +#include "test_msgs/action/fibonacci.hpp" + +using namespace std::chrono_literals; + +using Fibonacci = test_msgs::action::Fibonacci; +using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle; + +namespace +{ + +constexpr char kActionName[] = "fibonacci"; +constexpr char kFqActionName[] = "/ns/fibonacci"; + +bool wait_for_event( + std::shared_ptr node, + std::function predicate, + std::chrono::milliseconds timeout = 3s, + std::chrono::milliseconds sleep_period = 100ms) +{ + auto start = std::chrono::steady_clock::now(); + std::chrono::milliseconds time_slept(0); + + bool predicate_result; + while (!(predicate_result = predicate()) && + time_slept < std::chrono::duration_cast(timeout)) + { + rclcpp::Event::SharedPtr graph_event = node->get_graph_event(); + node->wait_for_graph_change(graph_event, sleep_period); + time_slept = std::chrono::duration_cast( + std::chrono::steady_clock::now() - start); + } + return predicate_result; +} + +} // namespace + +class TestActionGraph : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("my_node", "/ns"); + } + + void TearDown() + { + node.reset(); + } + + std::shared_ptr node; +}; + +TEST_F(TestActionGraph, get_action_clients_servers_info_by_action) +{ + auto node_base = node->get_node_base_interface(); + // Lists should be empty + EXPECT_TRUE( + rclcpp_action::get_action_clients_info_by_action(node_base, kFqActionName).empty()); + EXPECT_TRUE( + rclcpp_action::get_action_servers_info_by_action(node_base, kFqActionName).empty()); + + // Add an action client + auto action_client = rclcpp_action::create_client(node, kActionName); + // Wait for the action client to appear in the graph + ASSERT_TRUE( + wait_for_event( + node, + [&]() { + return rclcpp_action::get_action_clients_info_by_action( + node_base, kFqActionName).size() == 1u; + })); + // Server list should still be empty + EXPECT_TRUE( + rclcpp_action::get_action_servers_info_by_action(node_base, kFqActionName).empty()); + + // Verify the client list has the right data + auto client_list = rclcpp_action::get_action_clients_info_by_action(node_base, kFqActionName); + ASSERT_EQ(1u, client_list.size()); + EXPECT_EQ(node->get_name(), client_list[0].node_name()); + EXPECT_EQ(node->get_namespace(), client_list[0].node_namespace()); + EXPECT_EQ("test_msgs/action/Fibonacci", client_list[0].action_type()); + EXPECT_EQ(rclcpp::EndpointType::Client, client_list[0].endpoint_type()); + // Verify the underlying entities of the action client + const rclcpp::ServiceEndpointInfo & goal_info = client_list[0].goal_service_info(); + EXPECT_EQ("test_msgs/action/Fibonacci_SendGoal", goal_info.service_type()); + EXPECT_EQ(rclcpp::EndpointType::Client, goal_info.endpoint_type()); + EXPECT_TRUE(goal_info.endpoint_count() == 1u || goal_info.endpoint_count() == 2u); + ASSERT_TRUE(client_list[0].cancel_service_info().has_value()); + EXPECT_EQ( + "action_msgs/srv/CancelGoal", client_list[0].cancel_service_info()->service_type()); + ASSERT_TRUE(client_list[0].result_service_info().has_value()); + EXPECT_EQ( + "test_msgs/action/Fibonacci_GetResult", client_list[0].result_service_info()->service_type()); + ASSERT_TRUE(client_list[0].feedback_topic_info().has_value()); + EXPECT_EQ( + "test_msgs/action/Fibonacci_FeedbackMessage", + client_list[0].feedback_topic_info()->topic_type()); + EXPECT_EQ( + rclcpp::EndpointType::Subscription, client_list[0].feedback_topic_info()->endpoint_type()); + ASSERT_TRUE(client_list[0].status_topic_info().has_value()); + EXPECT_EQ( + "action_msgs/msg/GoalStatusArray", client_list[0].status_topic_info()->topic_type()); + EXPECT_EQ( + rclcpp::EndpointType::Subscription, client_list[0].status_topic_info()->endpoint_type()); + + // Add an action server + auto handle_goal = + [](const rclcpp_action::GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + auto handle_cancel = + [](const std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }; + auto handle_accepted = [](const std::shared_ptr) {}; + auto action_server = rclcpp_action::create_server( + node, kActionName, handle_goal, handle_cancel, handle_accepted); + // Wait for the action server to appear in the graph + ASSERT_TRUE( + wait_for_event( + node, + [&]() { + return rclcpp_action::get_action_servers_info_by_action( + node_base, kFqActionName).size() == 1u; + })); + + // Verify the server list has the right data + auto server_list = rclcpp_action::get_action_servers_info_by_action(node_base, kFqActionName); + ASSERT_EQ(1u, server_list.size()); + EXPECT_EQ(node->get_name(), server_list[0].node_name()); + EXPECT_EQ(node->get_namespace(), server_list[0].node_namespace()); + EXPECT_EQ("test_msgs/action/Fibonacci", server_list[0].action_type()); + EXPECT_EQ(rclcpp::EndpointType::Server, server_list[0].endpoint_type()); + // Verify the underlying entities of the action server + const rclcpp::ServiceEndpointInfo & server_goal_info = server_list[0].goal_service_info(); + EXPECT_EQ("test_msgs/action/Fibonacci_SendGoal", server_goal_info.service_type()); + EXPECT_EQ(rclcpp::EndpointType::Server, server_goal_info.endpoint_type()); + EXPECT_TRUE( + server_goal_info.endpoint_count() == 1u || server_goal_info.endpoint_count() == 2u); + ASSERT_TRUE(server_list[0].cancel_service_info().has_value()); + EXPECT_EQ( + "action_msgs/srv/CancelGoal", server_list[0].cancel_service_info()->service_type()); + ASSERT_TRUE(server_list[0].result_service_info().has_value()); + EXPECT_EQ( + "test_msgs/action/Fibonacci_GetResult", server_list[0].result_service_info()->service_type()); + ASSERT_TRUE(server_list[0].feedback_topic_info().has_value()); + EXPECT_EQ( + "test_msgs/action/Fibonacci_FeedbackMessage", + server_list[0].feedback_topic_info()->topic_type()); + EXPECT_EQ( + rclcpp::EndpointType::Publisher, server_list[0].feedback_topic_info()->endpoint_type()); + ASSERT_TRUE(server_list[0].status_topic_info().has_value()); + EXPECT_EQ( + "action_msgs/msg/GoalStatusArray", server_list[0].status_topic_info()->topic_type()); + EXPECT_EQ( + rclcpp::EndpointType::Publisher, server_list[0].status_topic_info()->endpoint_type()); + + // Error case: invalid action name + EXPECT_THROW( + rclcpp_action::get_action_clients_info_by_action(node_base, "13"), + rclcpp::exceptions::InvalidTopicNameError); + EXPECT_THROW( + rclcpp_action::get_action_servers_info_by_action(node_base, "13"), + rclcpp::exceptions::InvalidTopicNameError); +} + +TEST_F(TestActionGraph, count_action_clients_servers) +{ + auto node_base = node->get_node_base_interface(); + EXPECT_EQ(0u, rclcpp_action::count_action_clients(node_base, kActionName)); + EXPECT_EQ(0u, rclcpp_action::count_action_servers(node_base, kActionName)); + + auto action_client = rclcpp_action::create_client(node, kActionName); + ASSERT_TRUE( + wait_for_event( + node, + [&]() { + return rclcpp_action::count_action_clients(node_base, kActionName) == 1u; + })); + EXPECT_EQ(1u, rclcpp_action::count_action_clients(node_base, kFqActionName)); + EXPECT_EQ(0u, rclcpp_action::count_action_servers(node_base, kActionName)); + + auto handle_goal = + [](const rclcpp_action::GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + auto handle_cancel = + [](const std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }; + auto handle_accepted = [](const std::shared_ptr) {}; + auto action_server = rclcpp_action::create_server( + node, kActionName, handle_goal, handle_cancel, handle_accepted); + ASSERT_TRUE( + wait_for_event( + node, + [&]() { + return rclcpp_action::count_action_servers(node_base, kActionName) == 1u; + })); + EXPECT_EQ(1u, rclcpp_action::count_action_clients(node_base, kFqActionName)); + EXPECT_EQ(1u, rclcpp_action::count_action_servers(node_base, kFqActionName)); + + // Error case: invalid action name + EXPECT_THROW( + rclcpp_action::count_action_clients(node_base, "13"), + rclcpp::exceptions::InvalidTopicNameError); + EXPECT_THROW( + rclcpp_action::count_action_servers(node_base, "13"), + rclcpp::exceptions::InvalidTopicNameError); +} From 8cdd21adf3b9226e8aaff64edd3507c8ac8ab9ab Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Mon, 20 Jul 2026 08:44:09 +0900 Subject: [PATCH 2/3] avoid the per-call allocation by using a constexpr suffix and length. Signed-off-by: Tomoya Fujita --- rclcpp_action/src/graph.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/rclcpp_action/src/graph.cpp b/rclcpp_action/src/graph.cpp index b247a5d726..733c0af9f4 100644 --- a/rclcpp_action/src/graph.cpp +++ b/rclcpp_action/src/graph.cpp @@ -168,12 +168,13 @@ std::string ActionEndpointInfo::action_type() const { const std::string & goal_service_type = goal_service_info_.service_type(); - const std::string suffix = "_SendGoal"; - if (goal_service_type.size() > suffix.size() && + static constexpr char suffix[] = "_SendGoal"; + static constexpr size_t suffix_len = sizeof(suffix) - 1; + if (goal_service_type.size() > suffix_len && 0 == goal_service_type.compare( - goal_service_type.size() - suffix.size(), suffix.size(), suffix)) + goal_service_type.size() - suffix_len, suffix_len, suffix)) { - return goal_service_type.substr(0, goal_service_type.size() - suffix.size()); + return goal_service_type.substr(0, goal_service_type.size() - suffix_len); } return goal_service_type; } From f0f9113e3a9b14cf3eb10685ccb2e5717f38f086 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Wed, 22 Jul 2026 09:22:24 +0900 Subject: [PATCH 3/3] add defensive check to make sure all the contents are available. Signed-off-by: Tomoya Fujita --- rclcpp_action/src/graph.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/rclcpp_action/src/graph.cpp b/rclcpp_action/src/graph.cpp index 733c0af9f4..c0d2013929 100644 --- a/rclcpp_action/src/graph.cpp +++ b/rclcpp_action/src/graph.cpp @@ -51,7 +51,16 @@ convert_to_action_info_list(const rcl_action_endpoint_info_array_t & info_array) std::vector action_info_list; action_info_list.reserve(info_array.size); for (size_t i = 0; i < info_array.size; ++i) { - action_info_list.emplace_back(info_array.info_array[i]); + const rcl_action_endpoint_info_t & info = info_array.info_array[i]; + // rcl_action guarantees the goal service info of each entry is populated, + // skip malformed entries defensively instead of failing the whole query. + if (!info.goal_service_info.node_name || + !info.goal_service_info.node_namespace || + !info.goal_service_info.service_type) + { + continue; + } + action_info_list.emplace_back(info); } return action_info_list; }