From 11cb640844b3fc32b54b1b9e5d18ce79ecfa1745 Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Tue, 26 May 2020 10:26:51 -0700 Subject: [PATCH] expose get_service_names_and_types_by_node from rcl in rclcpp (#1131) * expose get_service_names_and_types_by_node from rcl in rclcpp Signed-off-by: Dirk Thomas * fix spelling Signed-off-by: Dirk Thomas * zero initialize Signed-off-by: Dirk Thomas * check return value and cleanup Signed-off-by: Dirk Thomas * use throw_from_rcl_error Signed-off-by: Dirk Thomas * cleanup error handling Signed-off-by: Dirk Thomas --- rclcpp/include/rclcpp/node.hpp | 6 +++ .../rclcpp/node_interfaces/node_graph.hpp | 6 +++ .../node_interfaces/node_graph_interface.hpp | 14 ++++++ rclcpp/src/rclcpp/node.cpp | 9 ++++ .../src/rclcpp/node_interfaces/node_graph.cpp | 48 ++++++++++++++++++- .../rclcpp_lifecycle/lifecycle_node.hpp | 13 +++++ rclcpp_lifecycle/src/lifecycle_node.cpp | 9 ++++ 7 files changed, 103 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index d54ea21c5e..b73b62d720 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -872,6 +872,12 @@ class Node : public std::enable_shared_from_this std::map> get_service_names_and_types() const; + RCLCPP_PUBLIC + std::map> + get_service_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_) const; + RCLCPP_PUBLIC size_t count_publishers(const std::string & topic_name) const; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp index 5882c727af..538958fc50 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp @@ -64,6 +64,12 @@ class NodeGraph : public NodeGraphInterface std::map> get_service_names_and_types() const override; + RCLCPP_PUBLIC + std::map> + get_service_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_) const override; + RCLCPP_PUBLIC std::vector get_node_names() const override; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp index 26ab3118ef..34d864fe21 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp @@ -165,6 +165,20 @@ class NodeGraphInterface std::map> get_service_names_and_types() const = 0; + /// Return a map of existing service names to list of service types for a specific node. + /** + * This function only considers services - not clients. + * + * \param[in] node_name name of the node + * \param[in] namespace_ namespace of the node + */ + RCLCPP_PUBLIC + virtual + std::map> + get_service_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_) const = 0; + /// Return a vector of existing node names (string). RCLCPP_PUBLIC virtual diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 30804809a4..b1f9e66f6a 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -352,6 +352,15 @@ Node::get_service_names_and_types() const return node_graph_->get_service_names_and_types(); } +std::map> +Node::get_service_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_) const +{ + return node_graph_->get_service_names_and_types_by_node( + node_name, namespace_); +} + size_t Node::count_publishers(const std::string & topic_name) const { diff --git a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp index 2e4d73440b..0aa361e38d 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp @@ -70,8 +70,9 @@ NodeGraph::get_topic_names_and_types(bool no_demangle) const if (rcl_names_and_types_fini(&topic_names_and_types) != RCL_RET_OK) { error_msg += std::string(", failed also to cleanup topic names and types, leaking memory: ") + rcl_get_error_string().str; + rcl_reset_error(); } - throw std::runtime_error(error_msg + rcl_get_error_string().str); + throw std::runtime_error(error_msg); } std::map> topics_and_types; @@ -111,8 +112,9 @@ NodeGraph::get_service_names_and_types() const error_msg += std::string(", failed also to cleanup service names and types, leaking memory: ") + rcl_get_error_string().str; + rcl_reset_error(); } - throw std::runtime_error(error_msg + rcl_get_error_string().str); + throw std::runtime_error(error_msg); } std::map> services_and_types; @@ -134,6 +136,48 @@ NodeGraph::get_service_names_and_types() const return services_and_types; } +std::map> +NodeGraph::get_service_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_) const +{ + rcl_names_and_types_t service_names_and_types = rcl_get_zero_initialized_names_and_types(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret = rcl_get_service_names_and_types_by_node( + node_base_->get_rcl_node_handle(), + &allocator, + node_name.c_str(), + namespace_.c_str(), + &service_names_and_types); + if (ret != RCL_RET_OK) { + auto error_msg = std::string("failed to get service names and types by node: ") + + rcl_get_error_string().str; + rcl_reset_error(); + if (rcl_names_and_types_fini(&service_names_and_types) != RCL_RET_OK) { + error_msg += + std::string(", failed also to cleanup service names and types, leaking memory: ") + + rcl_get_error_string().str; + rcl_reset_error(); + } + throw std::runtime_error(error_msg); + } + + std::map> services_and_types; + for (size_t i = 0; i < service_names_and_types.names.size; ++i) { + std::string service_name = service_names_and_types.names.data[i]; + for (size_t j = 0; j < service_names_and_types.types[i].size; ++j) { + services_and_types[service_name].emplace_back(service_names_and_types.types[i].data[j]); + } + } + + ret = rcl_names_and_types_fini(&service_names_and_types); + if (ret != RCL_RET_OK) { + throw_from_rcl_error(ret, "could not destroy service names and types"); + } + + return services_and_types; +} + std::vector NodeGraph::get_node_names() const { diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 7ebf833fb1..5eed721191 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -483,6 +483,19 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, std::map> get_service_names_and_types() const; + /// Return a map of existing service names to list of service types for a specific node. + /** + * This function only considers services - not clients. + * + * \param[in] node_name name of the node + * \param[in] namespace_ namespace of the node + */ + RCLCPP_LIFECYCLE_PUBLIC + std::map> + get_service_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_) const; + /// Return the number of publishers that are advertised on a given topic. /** * \sa rclcpp::Node::count_publishers diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 7b36f1f753..bf784195a4 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -279,6 +279,15 @@ LifecycleNode::get_service_names_and_types() const return node_graph_->get_service_names_and_types(); } +std::map> +LifecycleNode::get_service_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_) const +{ + return node_graph_->get_service_names_and_types_by_node( + node_name, namespace_); +} + size_t LifecycleNode::count_publishers(const std::string & topic_name) const {