refactor to support multiple types and no_demangle (#339)

* refactor to support multiple types and no_demangle

* add get_service_names_and_types

rclcpp/include/rclcpp/node.hpp

@@ -321,9 +321,13 @@ public:
   register_param_change_callback(CallbackT && callback);
 
   RCLCPP_PUBLIC
-  std::map<std::string, std::string>
+  std::map<std::string, std::vector<std::string>>
   get_topic_names_and_types() const;
 
+  RCLCPP_PUBLIC
+  std::map<std::string, std::vector<std::string>>
+  get_service_names_and_types() const;
+
   RCLCPP_PUBLIC
   size_t
   count_publishers(const std::string & topic_name) const;

rclcpp/include/rclcpp/node_interfaces/node_graph.hpp

@@ -56,8 +56,13 @@ public:
 
   RCLCPP_PUBLIC
   virtual
-  std::map<std::string, std::string>
+  std::map<std::string, std::vector<std::string>>
-  get_topic_names_and_types() const;
+  get_topic_names_and_types(bool no_demangle = false) const;
+
+  RCLCPP_PUBLIC
+  virtual
+  std::map<std::string, std::vector<std::string>>
+  get_service_names_and_types() const;
 
   RCLCPP_PUBLIC
   virtual

rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp

@@ -37,15 +37,28 @@ class NodeGraphInterface
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeGraphInterface)
 
-  /// Return a map of existing topic names (string) to topic types (string).
+  /// Return a map of existing topic names to list of topic types.
   /**
    * A topic is considered to exist when at least one publisher or subscriber
    * exists for it, whether they be local or remote to this process.
+   *
+   * \param[in] no_demangle if true, topic names and types are not demangled
    */
   RCLCPP_PUBLIC
   virtual
-  std::map<std::string, std::string>
+  std::map<std::string, std::vector<std::string>>
-  get_topic_names_and_types() const = 0;
+  get_topic_names_and_types(bool no_demangle = false) const = 0;
+
+  /// Return a map of existing service names to list of service types.
+  /**
+   * A service is considered to exist when at least one service server or
+   * service client exists for it, whether they be local or remote to this
+   * process.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::map<std::string, std::vector<std::string>>
+  get_service_names_and_types() const = 0;
 
   /// Return a vector of existing node names (string).
   RCLCPP_PUBLIC

rclcpp/src/rclcpp/node.cpp

@@ -144,12 +144,18 @@ Node::list_parameters(
   return node_parameters_->list_parameters(prefixes, depth);
 }
 
-std::map<std::string, std::string>
+std::map<std::string, std::vector<std::string>>
 Node::get_topic_names_and_types() const
 {
   return node_graph_->get_topic_names_and_types();
 }
 
+std::map<std::string, std::vector<std::string>>
+Node::get_service_names_and_types() const
+{
+  return node_graph_->get_service_names_and_types();
+}
+
 size_t
 Node::count_publishers(const std::string & topic_name) const
 {

rclcpp/src/rclcpp/node_interfaces/node_graph.cpp

@@ -45,41 +45,86 @@ NodeGraph::~NodeGraph()
   }
 }
 
-std::map<std::string, std::string>
+std::map<std::string, std::vector<std::string>>
-NodeGraph::get_topic_names_and_types() const
+NodeGraph::get_topic_names_and_types(bool no_demangle) const
 {
-  rcl_topic_names_and_types_t topic_names_and_types =
+  rcl_names_and_types_t topic_names_and_types = rcl_get_zero_initialized_names_and_types();
-    rcl_get_zero_initialized_topic_names_and_types();
 
+  rcl_allocator_t allocator = rcl_get_default_allocator();
   auto ret = rcl_get_topic_names_and_types(
     node_base_->get_rcl_node_handle(),
-    rcl_get_default_allocator(),
+    &allocator,
+    no_demangle,
     &topic_names_and_types);
-  if (ret != RMW_RET_OK) {
+  if (ret != RCL_RET_OK) {
     auto error_msg = std::string("failed to get topic names and types: ") +
       rcl_get_error_string_safe();
     rcl_reset_error();
-    if (rmw_destroy_topic_names_and_types(&topic_names_and_types) != RMW_RET_OK) {
+    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_safe();
     }
-    throw std::runtime_error(error_msg + rmw_get_error_string_safe());
+    throw std::runtime_error(error_msg + rcl_get_error_string_safe());
   }
 
-  std::map<std::string, std::string> topics;
+  std::map<std::string, std::vector<std::string>> topics_and_types;
-  for (size_t i = 0; i < topic_names_and_types.topic_count; ++i) {
+  for (size_t i = 0; i < topic_names_and_types.names.size; ++i) {
-    topics[topic_names_and_types.topic_names[i]] = topic_names_and_types.type_names[i];
+    std::string topic_name = topic_names_and_types.names.data[i];
+    for (size_t j = 0; j < topic_names_and_types.types[i].size; ++j) {
+      topics_and_types[topic_name].emplace_back(topic_names_and_types.types[i].data[j]);
+    }
   }
 
-  ret = rmw_destroy_topic_names_and_types(&topic_names_and_types);
+  ret = rcl_names_and_types_fini(&topic_names_and_types);
-  if (ret != RMW_RET_OK) {
+  if (ret != RCL_RET_OK) {
     // *INDENT-OFF*
     throw std::runtime_error(
-      std::string("could not destroy topic names and types: ") + rmw_get_error_string_safe());
+      std::string("could not destroy topic names and types: ") + rcl_get_error_string_safe());
     // *INDENT-ON*
   }
 
-  return topics;
+  return topics_and_types;
+}
+
+std::map<std::string, std::vector<std::string>>
+NodeGraph::get_service_names_and_types() 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();
+  auto ret = rcl_get_service_names_and_types(
+    node_base_->get_rcl_node_handle(),
+    &allocator,
+    &service_names_and_types);
+  if (ret != RCL_RET_OK) {
+    auto error_msg = std::string("failed to get service names and types: ") +
+      rcl_get_error_string_safe();
+    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_safe();
+    }
+    throw std::runtime_error(error_msg + rcl_get_error_string_safe());
+  }
+
+  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) {
+    // *INDENT-OFF*
+    throw std::runtime_error(
+      std::string("could not destroy service names and types: ") + rcl_get_error_string_safe());
+    // *INDENT-ON*
+  }
+
+  return services_and_types;
 }
 
 std::vector<std::string>
@@ -96,7 +141,7 @@ NodeGraph::get_node_names() const
   if (ret != RCL_RET_OK) {
     auto error_msg = std::string("failed to get node names: ") + rcl_get_error_string_safe();
     rcl_reset_error();
-    if (rcutils_string_array_fini(&node_names_c, &allocator) != RCUTILS_RET_OK) {
+    if (rcutils_string_array_fini(&node_names_c) != RCUTILS_RET_OK) {
       error_msg += std::string(", failed also to cleanup node names, leaking memory: ") +
         rcl_get_error_string_safe();
     }
@@ -106,7 +151,7 @@ NodeGraph::get_node_names() const
 
   std::vector<std::string> node_names(&node_names_c.data[0],
     &node_names_c.data[0 + node_names_c.size]);
-  ret = rcutils_string_array_fini(&node_names_c, &allocator);
+  ret = rcutils_string_array_fini(&node_names_c);
   if (ret != RCUTILS_RET_OK) {
     // *INDENT-OFF*
     // TODO(karsten1987): Append rcutils_error_message once it's in master

rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp

@@ -281,8 +281,12 @@ public:
   register_param_change_callback(CallbackT && callback);
 
   RCLCPP_LIFECYCLE_PUBLIC
-  std::map<std::string, std::string>
+  std::map<std::string, std::vector<std::string>>
-  get_topic_names_and_types() const;
+  get_topic_names_and_types(bool no_demangle = false) const;
+
+  RCLCPP_LIFECYCLE_PUBLIC
+  std::map<std::string, std::vector<std::string>>
+  get_service_names_and_types() const;
 
   RCLCPP_LIFECYCLE_PUBLIC
   size_t

rclcpp_lifecycle/src/lifecycle_node.cpp

@@ -162,10 +162,16 @@ LifecycleNode::list_parameters(
   return node_parameters_->list_parameters(prefixes, depth);
 }
 
-std::map<std::string, std::string>
+std::map<std::string, std::vector<std::string>>
-LifecycleNode::get_topic_names_and_types() const
+LifecycleNode::get_topic_names_and_types(bool no_demangle) const
 {
-  return node_graph_->get_topic_names_and_types();
+  return node_graph_->get_topic_names_and_types(no_demangle);
+}
+
+std::map<std::string, std::vector<std::string>>
+LifecycleNode::get_service_names_and_types() const
+{
+  return node_graph_->get_service_names_and_types();
 }
 
 size_t