2828using rclcpp::node_interfaces::NodeGraph;
2929using rclcpp::exceptions::throw_from_rcl_error;
3030using rclcpp::graph_listener::GraphListener;
31- using rcl_get_info_by_topic_func_t = rcl_ret_t (* )(
32- const rcl_node_t *,
33- rcutils_allocator_t *,
34- const char *,
35- bool ,
36- rmw_topic_endpoint_info_array_t *);
3731
3832NodeGraph::NodeGraph (rclcpp::node_interfaces::NodeBaseInterface * node_base)
3933: node_base_(node_base),
@@ -387,13 +381,12 @@ convert_to_topic_info_list(const rmw_topic_endpoint_info_array_t & info_array)
387381 return topic_info_list;
388382}
389383
390- template <typename TypeT , typename FunctionT>
384+ template <const char * EndpointType , typename FunctionT>
391385static std::vector<rclcpp::TopicEndpointInfo>
392386get_info_by_topic (
393387 rclcpp::node_interfaces::NodeBaseInterface * node_base,
394388 const std::string & topic_name,
395389 bool no_mangle,
396- TypeT type,
397390 FunctionT rcl_get_info_by_topic)
398391{
399392 std::vector<rclcpp::TopicEndpointInfo> topic_info_list;
@@ -411,7 +404,7 @@ get_info_by_topic(
411404 rcl_get_info_by_topic (rcl_node_handle, &allocator, fqdn.c_str (), no_mangle, &info_array);
412405 if (ret != RCL_RET_OK) {
413406 auto error_msg =
414- std::string (" Failed to get information by topic for: " ) + type + std::string (" :" );
407+ std::string (" Failed to get information by topic for " ) + EndpointType + std::string (" :" );
415408 if (ret == RCL_RET_UNSUPPORTED) {
416409 error_msg += std::string (" function not supported by RMW_IMPLEMENTATION" );
417410 } else {
@@ -435,28 +428,28 @@ get_info_by_topic(
435428 return topic_info_list;
436429}
437430
431+ static const char kPublisherEndpointTypeName [] = " publishers" ;
438432std::vector<rclcpp::TopicEndpointInfo>
439433NodeGraph::get_publishers_info_by_topic (
440434 const std::string & topic_name,
441435 bool no_mangle) const
442436{
443- return get_info_by_topic<const char *, rcl_get_info_by_topic_func_t >(
437+ return get_info_by_topic<kPublisherEndpointTypeName >(
444438 node_base_,
445439 topic_name,
446440 no_mangle,
447- " publisher" ,
448441 rcl_get_publishers_info_by_topic);
449442}
450443
444+ static const char kSubscriptionEndpointTypeName [] = " subscriptions" ;
451445std::vector<rclcpp::TopicEndpointInfo>
452446NodeGraph::get_subscriptions_info_by_topic (
453447 const std::string & topic_name,
454448 bool no_mangle) const
455449{
456- return get_info_by_topic<const char *, rcl_get_info_by_topic_func_t >(
450+ return get_info_by_topic<kSubscriptionEndpointTypeName >(
457451 node_base_,
458452 topic_name,
459453 no_mangle,
460- " subscriptions" ,
461454 rcl_get_subscriptions_info_by_topic);
462455}
0 commit comments