diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 968ada6de7..a9355d5ea4 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -1388,6 +1388,66 @@ class Node : public std::enable_shared_from_this std::vector get_subscriptions_info_by_topic(const std::string & topic_name, bool no_mangle = false) const; + /// Return the service endpoint information about clients on a given service. + /** + * The returned parameter is a list of service endpoint information, where each item will contain + * the node name, node namespace, service type, endpoint type, endpoint count, + * service endpoint's GIDs, and its QoS profiles. + * + * When the `no_mangle` parameter is `true`, the provided `service_name` should be a valid service + * name for the middleware (useful when combining ROS with native middleware apps). + * When the `no_mangle` parameter is `false`, the provided `service_name` should follow + * ROS service name conventions. In DDS-based RMWs, services are implemented as topics with mangled + * names (e.g., `rq/my_serviceRequest` and `rp/my_serviceReply`), so `no_mangle = true` is not + * supported and will result in an error. Use `get_publishers_info_by_topic` or + * `get_subscriptions_info_by_topic` for unmangled topic queries in such cases. Other RMWs + * (e.g., Zenoh) may support `no_mangle = true` if they natively handle services without topic-based + * + * 'service_name` may be a relative, private, or fully qualified service name. + * A relative or private service will be expanded using this node's namespace and name. + * The queried `service_name` is not remapped. + * + * \param[in] service_name the actual service name used; it will not be automatically remapped. + * \param[in] no_mangle if `true`, `service_name` needs to be a valid middleware service name, + * otherwise it should be a valid ROS service name. Defaults to `false`. + * \return a list of SeviceEndpointInfo representing all the clients on this service. + * \throws InvalidServiceNameError if the given service_name is invalid. + * \throws std::runtime_error if internal error happens. + */ + RCLCPP_PUBLIC + std::vector + get_clients_info_by_service(const std::string & service_name, bool no_mangle = false) const; + + /// Return the service endpoint information about servers on a given service. + /** + * The returned parameter is a list of service endpoint information, where each item will contain + * the node name, node namespace, service type, endpoint type, endpoint count, + * service endpoint's GIDs, and its QoS profiles. + * + * When the `no_mangle` parameter is `true`, the provided `service_name` should be a valid service + * name for the middleware (useful when combining ROS with native middleware apps). + * When the `no_mangle` parameter is `false`, the provided `service_name` should follow + * ROS service name conventions. In DDS-based RMWs, services are implemented as topics with mangled + * names (e.g., `rq/my_serviceRequest` and `rp/my_serviceReply`), so `no_mangle = true` is not + * supported and will result in an error. Use `rcl_get_publishers_info_by_topic` or + * `rcl_get_subscriptions_info_by_topic` for unmangled topic queries in such cases. Other RMWs + * (e.g., Zenoh) may support `no_mangle = true` if they natively handle services without topic-based + * + * 'service_name` may be a relative, private, or fully qualified service name. + * A relative or private service will be expanded using this node's namespace and name. + * The queried `service_name` is not remapped. + * + * \param[in] service_name the actual service name used; it will not be automatically remapped. + * \param[in] no_mangle if `true`, `service_name` needs to be a valid middleware service name, + * otherwise it should be a valid ROS service name. Defaults to `false`. + * \return a list of SeviceEndpointInfo representing all the servers on this service. + * \throws InvalidServiceNameError if the given service_name is invalid. + * \throws std::runtime_error if internal error happens. + */ + RCLCPP_PUBLIC + std::vector + get_servers_info_by_service(const std::string & service_name, bool no_mangle = false) const; + /// Return a graph event, which will be set anytime a graph change occurs. /* The graph Event object is a loan which must be returned. * The Event object is scoped and therefore to return the loan just let it go diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp index 863dbee1bf..2a204476a8 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp @@ -33,6 +33,7 @@ #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/node_interfaces/node_graph_interface.hpp" #include "rclcpp/visibility_control.hpp" +#include "rmw/service_endpoint_info_array.h" #include "rmw/topic_endpoint_info_array.h" namespace rclcpp @@ -159,6 +160,18 @@ class NodeGraph : public NodeGraphInterface const std::string & topic_name, bool no_mangle = false) const override; + RCLCPP_PUBLIC + std::vector + get_clients_info_by_service( + const std::string & service_name, + bool no_mangle = false) const override; + + RCLCPP_PUBLIC + std::vector + get_servers_info_by_service( + const std::string & service_name, + bool no_mangle = false) const override; + private: RCLCPP_DISABLE_COPY(NodeGraph) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp index 80abc308c1..117f298707 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp @@ -18,6 +18,8 @@ #include #include #include +#include +#include #include #include #include @@ -40,7 +42,9 @@ enum class EndpointType { Invalid = RMW_ENDPOINT_INVALID, Publisher = RMW_ENDPOINT_PUBLISHER, - Subscription = RMW_ENDPOINT_SUBSCRIPTION + Subscription = RMW_ENDPOINT_SUBSCRIPTION, + Client = RMW_ENDPOINT_CLIENT, + Server = RMW_ENDPOINT_SERVER }; /** @@ -143,6 +147,125 @@ class TopicEndpointInfo rosidl_type_hash_t topic_type_hash_; }; +/** + * Struct that contains service endpoint information like the associated node name, node namespace, + * service type, endpoint type, endpoint count, endpoint GIDs, and its QoS profiles. + */ +class ServiceEndpointInfo +{ +public: + /// Construct a ServiceEndpointInfo from a rcl_service_endpoint_info_t. + RCLCPP_PUBLIC + explicit ServiceEndpointInfo(const rcl_service_endpoint_info_t & info) + : node_name_(info.node_name), + node_namespace_(info.node_namespace), + service_type_(info.service_type), + endpoint_type_(static_cast(info.endpoint_type)), + service_type_hash_(info.service_type_hash), + endpoint_count_(info.endpoint_count) + { + for(size_t i = 0; i < endpoint_count_; i++) { + std::array gid; + std::copy(info.endpoint_gids[i], info.endpoint_gids[i] + RMW_GID_STORAGE_SIZE, gid.begin()); + endpoint_gids_.push_back(gid); + + rclcpp::QoS qos( + {info.qos_profiles[i].history, info.qos_profiles[i].depth}, info.qos_profiles[i]); + qos_profiles_.push_back(qos); + } + } + + /// Get a mutable reference to the node name. + RCLCPP_PUBLIC + std::string & + node_name(); + + /// Get a const reference to the node name. + RCLCPP_PUBLIC + const std::string & + node_name() const; + + /// Get a mutable reference to the node namespace. + RCLCPP_PUBLIC + std::string & + node_namespace(); + + /// Get a const reference to the node namespace. + RCLCPP_PUBLIC + const std::string & + node_namespace() const; + + /// Get a mutable reference to the service type string. + RCLCPP_PUBLIC + std::string & + service_type(); + + /// Get a const reference to the service type string. + RCLCPP_PUBLIC + const std::string & + service_type() const; + + /// Get a mutable reference to the service endpoint type. + RCLCPP_PUBLIC + rclcpp::EndpointType & + endpoint_type(); + + /// Get a const reference to the service endpoint type. + RCLCPP_PUBLIC + const rclcpp::EndpointType & + endpoint_type() const; + + /// Get a mutable reference to the endpoint count. + RCLCPP_PUBLIC + size_t & + endpoint_count(); + + /// Get a const reference to the endpoint count. + RCLCPP_PUBLIC + const size_t & + endpoint_count() const; + + /// Get a mutable reference to the GID of the service endpoint. + RCLCPP_PUBLIC + std::vector> & + endpoint_gids(); + + /// Get a const reference to the GID of the service endpoint. + RCLCPP_PUBLIC + const std::vector> & + endpoint_gids() const; + + /// Get a mutable reference to the QoS profile of the service endpoint. + RCLCPP_PUBLIC + std::vector & + qos_profiles(); + + /// Get a const reference to the QoS profile of the service endpoint. + RCLCPP_PUBLIC + const std::vector & + qos_profiles() const; + + /// Get a mutable reference to the type hash of the service endpoint. + RCLCPP_PUBLIC + rosidl_type_hash_t & + service_type_hash(); + + /// Get a const reference to the type hash of the service endpoint. + RCLCPP_PUBLIC + const rosidl_type_hash_t & + service_type_hash() const; + +private: + std::string node_name_; + std::string node_namespace_; + std::string service_type_; + rclcpp::EndpointType endpoint_type_; + std::vector> endpoint_gids_; + std::vector qos_profiles_; + rosidl_type_hash_t service_type_hash_; + size_t endpoint_count_; +}; + namespace node_interfaces { @@ -408,6 +531,30 @@ class NodeGraphInterface virtual std::vector get_subscriptions_info_by_topic(const std::string & topic_name, bool no_mangle = false) const = 0; + + /// Return the service endpoint information about clients on a given service. + /** + * \param[in] service_name the actual service name used; it will not be automatically remapped. + * \param[in] no_mangle if `true`, `service_name` needs to be a valid middleware service name, + * otherwise it should be a valid ROS service name. + * \sa rclcpp::Node::get_clients_info_by_service + */ + RCLCPP_PUBLIC + virtual + std::vector + get_clients_info_by_service(const std::string & service_name, bool no_mangle = false) const = 0; + + /// Return the service endpoint information about servers on a given service. + /** + * \param[in] service_name the actual service name used; it will not be automatically remapped. + * \param[in] no_mangle if `true`, `service_name` needs to be a valid middleware service name, + * otherwise it should be a valid ROS service name. + * \sa rclcpp::Node::get_servers_info_by_service + */ + RCLCPP_PUBLIC + virtual + std::vector + get_servers_info_by_service(const std::string & service_name, bool no_mangle = false) const = 0; }; } // namespace node_interfaces diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 72461125b9..123065a796 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -547,6 +547,18 @@ Node::get_subscriptions_info_by_topic(const std::string & topic_name, bool no_ma return node_graph_->get_subscriptions_info_by_topic(topic_name, no_mangle); } +std::vector +Node::get_clients_info_by_service(const std::string & service_name, bool no_mangle) const +{ + return node_graph_->get_clients_info_by_service(service_name, no_mangle); +} + +std::vector +Node::get_servers_info_by_service(const std::string & service_name, bool no_mangle) const +{ + return node_graph_->get_servers_info_by_service(service_name, no_mangle); +} + void Node::for_each_callback_group( const node_interfaces::NodeBaseInterface::CallbackGroupFunction & func) diff --git a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp index f6e9e1fda0..40e7efbf37 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp @@ -15,6 +15,7 @@ #include "rclcpp/node_interfaces/node_graph.hpp" #include +#include #include #include #include @@ -761,6 +762,122 @@ NodeGraph::get_subscriptions_info_by_topic( rcl_get_subscriptions_info_by_topic); } +static +std::vector +convert_to_service_info_list(const rcl_service_endpoint_info_array_t & info_array) +{ + std::vector service_info_list; + for (size_t i = 0; i < info_array.size; ++i) { + service_info_list.push_back(rclcpp::ServiceEndpointInfo(info_array.info_array[i])); + } + return service_info_list; +} + +template +static std::vector +get_info_by_service( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const std::string & service_name, + bool no_mangle, + FunctionT rcl_get_info_by_service) +{ + std::string fqdn; + auto rcl_node_handle = node_base->get_rcl_node_handle(); + + if (no_mangle) { + fqdn = service_name; + } else { + fqdn = rclcpp::expand_topic_or_service_name( + service_name, + rcl_node_get_name(rcl_node_handle), + rcl_node_get_namespace(rcl_node_handle), + true); + + // Get the node options + const rcl_node_options_t * node_options = rcl_node_get_options(rcl_node_handle); + if (nullptr == node_options) { + throw std::runtime_error("Need valid node options in get_info_by_service()"); + } + const rcl_arguments_t * global_args = nullptr; + if (node_options->use_global_arguments) { + global_args = &(rcl_node_handle->context->global_arguments); + } + + char * remapped_service_name = nullptr; + rcl_ret_t ret = rcl_remap_service_name( + &(node_options->arguments), + global_args, + fqdn.c_str(), + rcl_node_get_name(rcl_node_handle), + rcl_node_get_namespace(rcl_node_handle), + node_options->allocator, + &remapped_service_name); + if (RCL_RET_OK != ret) { + throw_from_rcl_error(ret, std::string("Failed to remap service name ") + fqdn); + } else if (nullptr != remapped_service_name) { + fqdn = remapped_service_name; + node_options->allocator.deallocate(remapped_service_name, node_options->allocator.state); + } + } + + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rcl_service_endpoint_info_array_t info_array = + rcl_get_zero_initialized_service_endpoint_info_array(); + rcl_ret_t ret = + rcl_get_info_by_service(rcl_node_handle, &allocator, fqdn.c_str(), no_mangle, &info_array); + if (RCL_RET_OK != ret) { + auto error_msg = + std::string("Failed to get information by service 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_service_endpoint_info_array_fini(&info_array, &allocator)) { + error_msg += std::string(", failed also to cleanup service info array, leaking memory: ") + + rcl_get_error_string().str; + rcl_reset_error(); + } + throw_from_rcl_error(ret, error_msg); + } + + std::vector service_info_list = + convert_to_service_info_list(info_array); + ret = rcl_service_endpoint_info_array_fini(&info_array, &allocator); + if (RCL_RET_OK != ret) { + throw_from_rcl_error(ret, "rcl_service_info_array_fini failed."); + } + + return service_info_list; +} + +static constexpr char kClientEndpointTypeName[] = "clients"; +std::vector +NodeGraph::get_clients_info_by_service( + const std::string & service_name, + bool no_mangle) const +{ + return get_info_by_service( + node_base_, + service_name, + no_mangle, + rcl_get_clients_info_by_service); +} + +static constexpr char kServerEndpointTypeName[] = "servers"; +std::vector +NodeGraph::get_servers_info_by_service( + const std::string & service_name, + bool no_mangle) const +{ + return get_info_by_service( + node_base_, + service_name, + no_mangle, + rcl_get_servers_info_by_service); +} + std::string & rclcpp::TopicEndpointInfo::node_name() { @@ -844,3 +961,99 @@ rclcpp::TopicEndpointInfo::topic_type_hash() const { return topic_type_hash_; } + +std::string & +rclcpp::ServiceEndpointInfo::node_name() +{ + return node_name_; +} + +const std::string & +rclcpp::ServiceEndpointInfo::node_name() const +{ + return node_name_; +} + +std::string & +rclcpp::ServiceEndpointInfo::node_namespace() +{ + return node_namespace_; +} + +const std::string & +rclcpp::ServiceEndpointInfo::node_namespace() const +{ + return node_namespace_; +} + +std::string & +rclcpp::ServiceEndpointInfo::service_type() +{ + return service_type_; +} + +const std::string & +rclcpp::ServiceEndpointInfo::service_type() const +{ + return service_type_; +} + +rclcpp::EndpointType & +rclcpp::ServiceEndpointInfo::endpoint_type() +{ + return endpoint_type_; +} + +const rclcpp::EndpointType & +rclcpp::ServiceEndpointInfo::endpoint_type() const +{ + return endpoint_type_; +} + +size_t & +rclcpp::ServiceEndpointInfo::endpoint_count() +{ + return endpoint_count_; +} + +const size_t & +rclcpp::ServiceEndpointInfo::endpoint_count() const +{ + return endpoint_count_; +} + +std::vector> & +rclcpp::ServiceEndpointInfo::endpoint_gids() +{ + return endpoint_gids_; +} + +const std::vector> & +rclcpp::ServiceEndpointInfo::endpoint_gids() const +{ + return endpoint_gids_; +} + +std::vector & +rclcpp::ServiceEndpointInfo::qos_profiles() +{ + return qos_profiles_; +} + +const std::vector & +rclcpp::ServiceEndpointInfo::qos_profiles() const +{ + return qos_profiles_; +} + +rosidl_type_hash_t & +rclcpp::ServiceEndpointInfo::service_type_hash() +{ + return service_type_hash_; +} + +const rosidl_type_hash_t & +rclcpp::ServiceEndpointInfo::service_type_hash() const +{ + return service_type_hash_; +} diff --git a/rclcpp/test/rclcpp/test_node.cpp b/rclcpp/test/rclcpp/test_node.cpp index dc8eedb64e..da467cce16 100644 --- a/rclcpp/test/rclcpp/test_node.cpp +++ b/rclcpp/test/rclcpp/test_node.cpp @@ -3689,6 +3689,115 @@ TEST_F(TestNode, get_publishers_subscriptions_info_by_topic) { }, rclcpp::exceptions::InvalidTopicNameError); } +// test that calling get_clients_info_by_service and get_servers_info_by_service +TEST_F(TestNode, get_clients_servers_info_by_service) { + auto node = std::make_shared("my_node", "/ns"); + std::string service_name = "test_service_info"; + std::string fq_service_name = rclcpp::expand_topic_or_service_name( + service_name, node->get_name(), node->get_namespace(), true); + + // Lists should be empty + EXPECT_TRUE(node->get_clients_info_by_service(fq_service_name).empty()); + EXPECT_TRUE(node->get_servers_info_by_service(fq_service_name).empty()); + + // Add a client + rclcpp::QoSInitialization qos_initialization = + { + RMW_QOS_POLICY_HISTORY_KEEP_ALL, + 10 + }; + rmw_qos_profile_t rmw_qos_profile_default = + { + RMW_QOS_POLICY_HISTORY_KEEP_ALL, + 10, + RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, + RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, + {1, 12345}, + {20, 9887665}, + RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC, + {5, 23456}, + false + }; + rclcpp::QoS qos = rclcpp::QoS(qos_initialization, rmw_qos_profile_default); + auto client = node->create_client(service_name, qos); + + // Wait for the underlying RMW implementation to catch up with graph changes + auto client_is_generated = + [&]() {return node->get_clients_info_by_service(fq_service_name).size() == 1u;}; + ASSERT_TRUE(wait_for_event(node, client_is_generated)); + + // List should have one item + auto clients_info = node->get_clients_info_by_service(fq_service_name); + ASSERT_EQ(clients_info.size(), (size_t) 1); + // Server list should be empty + EXPECT_TRUE(node->get_servers_info_by_service(fq_service_name).empty()); + + // Verify client info has the right data. + EXPECT_EQ(node->get_name(), clients_info[0].node_name()); + EXPECT_EQ(node->get_namespace(), clients_info[0].node_namespace()); + EXPECT_EQ("test_msgs/srv/Empty", clients_info[0].service_type()); + EXPECT_EQ(rclcpp::EndpointType::Client, clients_info[0].endpoint_type()); + EXPECT_TRUE(clients_info[0].endpoint_count() == 1 || clients_info[0].endpoint_count() == 2); + for(size_t i = 0; i < clients_info[0].endpoint_count(); i++) { + auto qos_profile = clients_info[0].qos_profiles()[i].get_rmw_qos_profile(); + expect_qos_profile_eq(qos.get_rmw_qos_profile(), qos_profile, false); + } + + // Add a service server + rclcpp::QoSInitialization qos_initialization2 = + { + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 0 + }; + rmw_qos_profile_t rmw_qos_profile_default2 = + { + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 0, + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + {15, 1678}, + {29, 2345}, + RMW_QOS_POLICY_LIVELINESS_AUTOMATIC, + {5, 23456}, + false + }; + rclcpp::QoS qos2 = rclcpp::QoS(qos_initialization2, rmw_qos_profile_default2); + auto callback = [](test_msgs::srv::Empty_Request::ConstSharedPtr req, + test_msgs::srv::Empty_Response::ConstSharedPtr resp) { + (void)req; + (void)resp; + }; + auto server = node->create_service(service_name, callback, qos2); + // Wait for the underlying RMW implementation to catch up with graph changes + auto server_is_generated = + [&]() {return node->get_servers_info_by_service(fq_service_name).size() == 1u;}; + ASSERT_TRUE(wait_for_event(node, server_is_generated)); + + // List should have one item + auto servers_info = node->get_servers_info_by_service(fq_service_name); + ASSERT_EQ(servers_info.size(), (size_t) 1); + // Verify server info has the right data. + EXPECT_EQ(node->get_name(), servers_info[0].node_name()); + EXPECT_EQ(node->get_namespace(), servers_info[0].node_namespace()); + EXPECT_EQ("test_msgs/srv/Empty", servers_info[0].service_type()); + EXPECT_EQ(rclcpp::EndpointType::Server, servers_info[0].endpoint_type()); + EXPECT_TRUE(servers_info[0].endpoint_count() == 1 || servers_info[0].endpoint_count() == 2); + for(size_t i = 0; i < servers_info[0].endpoint_count(); i++) { + auto qos_profile = servers_info[0].qos_profiles()[i].get_rmw_qos_profile(); + expect_qos_profile_eq(qos2.get_rmw_qos_profile(), qos_profile, false); + } + + // Error cases + EXPECT_THROW( + { + clients_info = node->get_clients_info_by_service("13"); + }, rclcpp::exceptions::InvalidServiceNameError); + EXPECT_THROW( + { + servers_info = node->get_servers_info_by_service("13"); + }, rclcpp::exceptions::InvalidServiceNameError); +} + TEST_F(TestNode, callback_groups) { auto node = std::make_shared("node", "ns"); size_t num_callback_groups_in_basic_node = 0; diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 6e196c887b..0f8e8dce8e 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -693,6 +693,22 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, std::vector get_subscriptions_info_by_topic(const std::string & topic_name, bool no_mangle = false) const; + /// Return the service endpoint information about clients on a given service. + /** + * \sa rclcpp::Node::get_clients_info_by_service + */ + RCLCPP_LIFECYCLE_PUBLIC + std::vector + get_clients_info_by_service(const std::string & service_name, bool no_mangle = false) const; + + /// Return the service endpoint information about server on a given service. + /** + * \sa rclcpp::Node::get_servers_info_by_service + */ + RCLCPP_LIFECYCLE_PUBLIC + std::vector + get_servers_info_by_service(const std::string & service_name, bool no_mangle = false) const; + /// Return a graph event, which will be set anytime a graph change occurs. /* The graph Event object is a loan which must be returned. * The Event object is scoped and therefore to return the load just let it go diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 03ece2e58b..5a55acf0a6 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -422,6 +422,18 @@ LifecycleNode::get_subscriptions_info_by_topic(const std::string & topic_name, b return node_graph_->get_subscriptions_info_by_topic(topic_name, no_mangle); } +std::vector +LifecycleNode::get_clients_info_by_service(const std::string & service_name, bool no_mangle) const +{ + return node_graph_->get_clients_info_by_service(service_name, no_mangle); +} + +std::vector +LifecycleNode::get_servers_info_by_service(const std::string & service_name, bool no_mangle) const +{ + return node_graph_->get_servers_info_by_service(service_name, no_mangle); +} + void LifecycleNode::for_each_callback_group( const rclcpp::node_interfaces::NodeBaseInterface::CallbackGroupFunction & func) diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp index c31d408a71..adbd49261d 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -922,6 +922,12 @@ TEST_F(TestDefaultStateMachine, test_graph_services) { EXPECT_EQ(1u, test_node->count_services("/testnode/get_available_transitions")); EXPECT_EQ(1u, test_node->count_services("/testnode/get_state")); EXPECT_EQ(1u, test_node->count_services("/testnode/get_transition_graph")); + + auto clients_info = + test_node->get_clients_info_by_service("/testnode/change_state"); + EXPECT_EQ(0u, clients_info.size()); + auto servers_info = test_node->get_servers_info_by_service("/testnode/change_state"); + EXPECT_EQ(1u, servers_info.size()); } TEST_F(TestDefaultStateMachine, test_graph_services_by_node) {