diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 0b49267..aa12398 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -321,9 +321,13 @@ public: register_param_change_callback(CallbackT && callback); RCLCPP_PUBLIC - std::map + std::map> get_topic_names_and_types() const; + RCLCPP_PUBLIC + std::map> + get_service_names_and_types() 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 a8c463b..09863b4 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp @@ -56,8 +56,13 @@ public: RCLCPP_PUBLIC virtual - std::map - get_topic_names_and_types() const; + std::map> + get_topic_names_and_types(bool no_demangle = false) const; + + RCLCPP_PUBLIC + virtual + std::map> + get_service_names_and_types() const; RCLCPP_PUBLIC virtual diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp index a7f4c0a..18c978d 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp +++ b/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 - get_topic_names_and_types() const = 0; + std::map> + 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> + get_service_names_and_types() const = 0; /// Return a vector of existing node names (string). RCLCPP_PUBLIC diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index c6dee80..09d3084 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -144,12 +144,18 @@ Node::list_parameters( return node_parameters_->list_parameters(prefixes, depth); } -std::map +std::map> Node::get_topic_names_and_types() const { return node_graph_->get_topic_names_and_types(); } +std::map> +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 { diff --git a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp index d0b87bd..d9f940e 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp @@ -45,41 +45,86 @@ NodeGraph::~NodeGraph() } } -std::map -NodeGraph::get_topic_names_and_types() const +std::map> +NodeGraph::get_topic_names_and_types(bool no_demangle) const { - rcl_topic_names_and_types_t topic_names_and_types = - rcl_get_zero_initialized_topic_names_and_types(); + rcl_names_and_types_t topic_names_and_types = rcl_get_zero_initialized_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 topics; - for (size_t i = 0; i < topic_names_and_types.topic_count; ++i) { - topics[topic_names_and_types.topic_names[i]] = topic_names_and_types.type_names[i]; + std::map> topics_and_types; + for (size_t i = 0; i < topic_names_and_types.names.size; ++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); - if (ret != RMW_RET_OK) { + ret = rcl_names_and_types_fini(&topic_names_and_types); + 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> +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> 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 @@ -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 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 diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index a83cb03..28b38ac 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -281,8 +281,12 @@ public: register_param_change_callback(CallbackT && callback); RCLCPP_LIFECYCLE_PUBLIC - std::map - get_topic_names_and_types() const; + std::map> + get_topic_names_and_types(bool no_demangle = false) const; + + RCLCPP_LIFECYCLE_PUBLIC + std::map> + get_service_names_and_types() const; RCLCPP_LIFECYCLE_PUBLIC size_t diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 88e2524..44863fd 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -162,10 +162,16 @@ LifecycleNode::list_parameters( return node_parameters_->list_parameters(prefixes, depth); } -std::map -LifecycleNode::get_topic_names_and_types() const +std::map> +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> +LifecycleNode::get_service_names_and_types() const +{ + return node_graph_->get_service_names_and_types(); } size_t