expose get_service_names_and_types_by_node from rcl in rclcpp (ros2#1131

) * expose get_service_names_and_types_by_node from rcl in rclcpp Signed-off-by: Dirk Thomas <dirk-thomas@users.noreply.github.com> * fix spelling Signed-off-by: Dirk Thomas <dirk-thomas@users.noreply.github.com> * zero initialize Signed-off-by: Dirk Thomas <dirk-thomas@users.noreply.github.com> * check return value and cleanup Signed-off-by: Dirk Thomas <dirk-thomas@users.noreply.github.com> * use throw_from_rcl_error Signed-off-by: Dirk Thomas <dirk-thomas@users.noreply.github.com> * cleanup error handling Signed-off-by: Dirk Thomas <dirk-thomas@users.noreply.github.com>
DensoADAS · Jul 7, 2020 · 11cb640 · 11cb640
1 parent db772b5
commit 11cb640
Show file tree

Hide file tree

Showing 7 changed files with 103 additions and 2 deletions.
diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp
@@ -872,6 +872,12 @@ class Node : public std::enable_shared_from_this<Node>
   std::map<std::string, std::vector<std::string>>
   get_service_names_and_types() const;
 
+  RCLCPP_PUBLIC
+  std::map<std::string, std::vector<std::string>>
+  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
@@ -64,6 +64,12 @@ class NodeGraph : public NodeGraphInterface
   std::map<std::string, std::vector<std::string>>
   get_service_names_and_types() const override;
 
+  RCLCPP_PUBLIC
+  std::map<std::string, std::vector<std::string>>
+  get_service_names_and_types_by_node(
+    const std::string & node_name,
+    const std::string & namespace_) const override;
+
   RCLCPP_PUBLIC
   std::vector<std::string>
   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
@@ -165,6 +165,20 @@ class NodeGraphInterface
   std::map<std::string, std::vector<std::string>>
   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<std::string, std::vector<std::string>>
+  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
@@ -352,6 +352,15 @@ Node::get_service_names_and_types() const
   return node_graph_->get_service_names_and_types();
 }
 
+std::map<std::string, std::vector<std::string>>
+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
@@ -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<std::string, std::vector<std::string>> 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<std::string, std::vector<std::string>> services_and_types;
@@ -134,6 +136,48 @@ NodeGraph::get_service_names_and_types() const
   return services_and_types;
 }
 
+std::map<std::string, std::vector<std::string>>
+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<std::string, std::vector<std::string>> 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<std::string>
 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
@@ -483,6 +483,19 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface,
   std::map<std::string, std::vector<std::string>>
   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<std::string, std::vector<std::string>>
+  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
@@ -279,6 +279,15 @@ LifecycleNode::get_service_names_and_types() const
   return node_graph_->get_service_names_and_types();
 }
 
+std::map<std::string, std::vector<std::string>>
+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
 {