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 <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>

rclcpp/include/rclcpp/node.hpp

@@ -872,6 +872,12 @@ public:
   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;

rclcpp/include/rclcpp/node_interfaces/node_graph.hpp

@@ -64,6 +64,12 @@ public:
   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;

rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp

@@ -165,6 +165,20 @@ public:
   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

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
 {

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
 {