From da404d2e9771af3a5c3f34bd10641835f339c882 Mon Sep 17 00:00:00 2001 From: "Minju, Lee" Date: Tue, 25 Jun 2024 21:41:58 +0900 Subject: [PATCH] add : get clients, servers info Signed-off-by: Minju, Lee --- rclcpp/include/rclcpp/node.hpp | 52 +++++++ .../rclcpp/node_interfaces/node_graph.hpp | 12 ++ .../node_interfaces/node_graph_interface.hpp | 24 ++++ rclcpp/src/rclcpp/node.cpp | 13 ++ .../src/rclcpp/node_interfaces/node_graph.cpp | 47 ++++++- rclcpp/test/rclcpp/test_node.cpp | 129 ++++++++++++++++++ .../rclcpp_lifecycle/lifecycle_node.hpp | 16 +++ rclcpp_lifecycle/src/lifecycle_node.cpp | 12 ++ 8 files changed, 300 insertions(+), 5 deletions(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 35863abba4..11e068d45a 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -1405,6 +1405,58 @@ 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, service endpoint's GID, and its QoS + * profile. + * + * 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 (e.g. DDS) apps). + * When the `no_mangle` parameter is `false`, the provided `service_name` should follow + * ROS service name conventions. + * + * '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 TopicEndpointInfo representing all the clients on this service. + * \throws InvalidTopicNameError 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, service endpoint's GID, and its QoS + * profile. + * + * 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 (e.g. DDS) apps). + * When the `no_mangle` parameter is `false`, the provided `service_name` should follow + * ROS service name conventions. + * + * '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 TopicEndpointInfo representing all the servers on this service. + * \throws InvalidTopicNameError 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..ce75573a52 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp @@ -159,6 +159,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..1b45bd603b 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp @@ -408,6 +408,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 1a68c7f108..cb319346e5 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -547,6 +547,19 @@ 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..1c8bb6be81 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp @@ -664,7 +664,8 @@ get_info_by_topic( rclcpp::node_interfaces::NodeBaseInterface * node_base, const std::string & topic_name, bool no_mangle, - FunctionT rcl_get_info_by_topic) + bool is_service, + FunctionT rcl_get_info_by_topic_or_service) { std::string fqdn; auto rcl_node_handle = node_base->get_rcl_node_handle(); @@ -676,7 +677,7 @@ get_info_by_topic( topic_name, rcl_node_get_name(rcl_node_handle), rcl_node_get_namespace(rcl_node_handle), - false); // false = not a service + is_service); // Get the node options const rcl_node_options_t * node_options = rcl_node_get_options(rcl_node_handle); @@ -687,9 +688,9 @@ get_info_by_topic( if (node_options->use_global_arguments) { global_args = &(rcl_node_handle->context->global_arguments); } - + auto rcl_remap_name = is_service ? rcl_remap_service_name : rcl_remap_topic_name; char * remapped_topic_name = nullptr; - rcl_ret_t ret = rcl_remap_topic_name( + rcl_ret_t ret = rcl_remap_name( &(node_options->arguments), global_args, fqdn.c_str(), @@ -708,7 +709,13 @@ get_info_by_topic( rcutils_allocator_t allocator = rcutils_get_default_allocator(); rcl_topic_endpoint_info_array_t info_array = rcl_get_zero_initialized_topic_endpoint_info_array(); rcl_ret_t ret = - rcl_get_info_by_topic(rcl_node_handle, &allocator, fqdn.c_str(), no_mangle, &info_array); + rcl_get_info_by_topic_or_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 topic for ") + EndpointType + std::string(":"); @@ -745,6 +752,7 @@ NodeGraph::get_publishers_info_by_topic( node_base_, topic_name, no_mangle, + false, rcl_get_publishers_info_by_topic); } @@ -758,9 +766,38 @@ NodeGraph::get_subscriptions_info_by_topic( node_base_, topic_name, no_mangle, + false, rcl_get_subscriptions_info_by_topic); } +static constexpr char kClientsEndpointTypeName[] = "clients"; +std::vector +NodeGraph::get_clients_info_by_service( + const std::string & service_name, + bool no_mangle) const +{ + return get_info_by_topic( + node_base_, + service_name, + no_mangle, + true, + rcl_get_clients_info_by_service); +} + +static constexpr char kServersEndpointTypeName[] = "servers"; +std::vector +NodeGraph::get_servers_info_by_service( + const std::string & service_name, + bool no_mangle) const +{ + return get_info_by_topic( + node_base_, + service_name, + no_mangle, + true, + rcl_get_servers_info_by_service); +} + std::string & rclcpp::TopicEndpointInfo::node_name() { diff --git a/rclcpp/test/rclcpp/test_node.cpp b/rclcpp/test/rclcpp/test_node.cpp index ad73aadc2a..03f5a734f4 100644 --- a/rclcpp/test/rclcpp/test_node.cpp +++ b/rclcpp/test/rclcpp/test_node.cpp @@ -3263,6 +3263,135 @@ 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 publisher + 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() > 0u;}; + ASSERT_TRUE(wait_for_event(node, client_is_generated)); + // List should have at least one item + auto client_list = node->get_clients_info_by_service(fq_service_name); + ASSERT_GE(client_list.size(), (size_t)1); + // Server list should be empty + EXPECT_TRUE(node->get_servers_info_by_service(fq_service_name).empty()); + // Verify client list has the right data. + for(auto client_info : client_list) { + EXPECT_EQ(node->get_name(), client_info.node_name()); + EXPECT_EQ(node->get_namespace(), client_info.node_namespace()); + ASSERT_NE(client_info.topic_type().find("test_msgs/srv/Empty"), std::string::npos); + if(client_info.topic_type() == "test_msgs/srv/Empty_Request") { + EXPECT_EQ(rclcpp::EndpointType::Publisher, client_info.endpoint_type()); + auto actual_qos_profile = client_info.qos_profile().get_rmw_qos_profile(); + { + SCOPED_TRACE("Publisher QOS 1"); + expect_qos_profile_eq(qos.get_rmw_qos_profile(), actual_qos_profile, true); + } + } else if(client_info.topic_type() == "test_msgs/srv/Empty_Response") { + EXPECT_EQ(rclcpp::EndpointType::Subscription, client_info.endpoint_type()); + auto actual_qos_profile = client_info.qos_profile().get_rmw_qos_profile(); + { + SCOPED_TRACE("Publisher QOS 1"); + expect_qos_profile_eq(qos.get_rmw_qos_profile(), actual_qos_profile, false); + } + } + } + + // Add a subscription + 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() > 0u;}; + ASSERT_TRUE(wait_for_event(node, server_is_generated)); + // Both lists should have at least one item + client_list = node->get_clients_info_by_service(fq_service_name); + auto server_list = node->get_servers_info_by_service(fq_service_name); + ASSERT_GE(client_list.size(), (size_t)1); + ASSERT_GE(server_list.size(), (size_t)1); + // Verify server list has the right data. + for(auto server_info : server_list) { + EXPECT_EQ(node->get_name(), server_info.node_name()); + EXPECT_EQ(node->get_namespace(), server_info.node_namespace()); + ASSERT_NE(server_info.topic_type().find("test_msgs/srv/Empty"), std::string::npos); + if(server_info.topic_type() == "test_msgs/srv/Empty_Request") { + EXPECT_EQ(rclcpp::EndpointType::Subscription, server_info.endpoint_type()); + auto actual_qos_profile = server_info.qos_profile().get_rmw_qos_profile(); + { + SCOPED_TRACE("Publisher QOS 1"); + expect_qos_profile_eq(qos2.get_rmw_qos_profile(), actual_qos_profile, false); + } + } else if(server_info.topic_type() == "test_msgs/srv/Empty_Response") { + EXPECT_EQ(rclcpp::EndpointType::Publisher, server_info.endpoint_type()); + auto actual_qos_profile = server_info.qos_profile().get_rmw_qos_profile(); + { + SCOPED_TRACE("Publisher QOS 1"); + expect_qos_profile_eq(qos2.get_rmw_qos_profile(), actual_qos_profile, true); + } + } + } + + // Error cases + EXPECT_THROW( + { + client_list = node->get_clients_info_by_service("13"); + }, rclcpp::exceptions::InvalidServiceNameError); + EXPECT_THROW( + { + server_list = 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 657ddddc34..d0c9844f53 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -722,6 +722,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 0c448cf5e6..6fb0b32746 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -410,6 +410,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)