From 7426d737edcef06b510eaccc1f07d757776bf96f Mon Sep 17 00:00:00 2001 From: Alex Youngs Date: Wed, 25 May 2022 04:26:59 +0000 Subject: [PATCH] Check topic for type to determine which subscription callback to trigger --- mapviz_plugins/src/marker_plugin.cpp | 44 +++++++++++++++++++++++----- 1 file changed, 36 insertions(+), 8 deletions(-) diff --git a/mapviz_plugins/src/marker_plugin.cpp b/mapviz_plugins/src/marker_plugin.cpp index 17b763ee..ddf9e2cd 100644 --- a/mapviz_plugins/src/marker_plugin.cpp +++ b/mapviz_plugins/src/marker_plugin.cpp @@ -123,14 +123,42 @@ namespace mapviz_plugins marker_array_sub_.reset(); if (!topic_.empty()) { - marker_sub_ = node_->create_subscription( - topic_, - rclcpp::QoS(100), - std::bind(&MarkerPlugin::handleMarker, this, std::placeholders::_1)); - marker_array_sub_ = node_->create_subscription( - topic_, - rclcpp::QoS(100), - std::bind(&MarkerPlugin::handleMarkerArray, this, std::placeholders::_1)); + // ROS 2 does not allow for a simple way to subscribe to a general type of message (i.e. Marker and MarkerArray) + // That would require a way to de-serialize the data for mapviz to consume (based on message type) + // The code below checks for the topic type and subscribes in the appropriate manner + + auto known_topics = node_->get_topic_names_and_types(); + if (known_topics.count(topic_) > 0) + { + std::string topic_type = known_topics[topic_][0]; + if (topic_type == "visualization_msgs/msg/Marker") + { + marker_sub_ = node_->create_subscription( + topic_, + rclcpp::QoS(100), + std::bind(&MarkerPlugin::handleMarker, this, std::placeholders::_1)); + } + else if (topic_type == "visualization_msgs/msg/MarkerArray") + { + marker_array_sub_ = node_->create_subscription( + topic_, + rclcpp::QoS(100), + std::bind(&MarkerPlugin::handleMarkerArray, this, std::placeholders::_1)); + } + else + { + RCLCPP_ERROR(node_->get_logger(), + "Unable to subscribe to topic %s (unsupported type %s).", + topic_.c_str(), topic_type.c_str()); + return; + } + } + else + { + RCLCPP_ERROR(node_->get_logger(), + "Unable to subscribe to topic %s (does not exist).", topic_.c_str()); + return; + } RCLCPP_INFO(node_->get_logger(), "Subscribing to %s", topic_.c_str()); }