diff --git a/utility/crane_visualization_aggregator/src/crane_visualization_aggregator_node.cpp b/utility/crane_visualization_aggregator/src/crane_visualization_aggregator_node.cpp index 78bec0adf..e6526fac6 100644 --- a/utility/crane_visualization_aggregator/src/crane_visualization_aggregator_node.cpp +++ b/utility/crane_visualization_aggregator/src/crane_visualization_aggregator_node.cpp @@ -15,10 +15,6 @@ string layer string[] svg_primitives */ - -/** - -*/ class VisualizationAggregator : public rclcpp::Node { public: @@ -27,8 +23,29 @@ class VisualizationAggregator : public rclcpp::Node subscriber = create_subscription( "/visualizer_svgs", 10, [&](const crane_visualization_interfaces::msg::SvgPrimitiveArray::ConstSharedPtr & msg) { - // store into + // store into layers + layers.try_emplace(msg->layer, msg->svg_primitives); }); + publisher = create_publisher("/aggregated_svgs", 10); + timer = create_wall_timer(std::chrono::milliseconds(100), [this]() { + std::stringstream svg; + svg << "\n"; + svg << "\n"; + + for (const auto & [layer, primitives] : layers) { + svg << "\n"; + for (const auto & primitive : primitives) { + svg << primitive << "\n"; + } + svg << "\n"; + } + + svg << "\n"; + + std_msgs::msg::String msg; + msg.data = svg.str(); + publisher->publish(msg); + }); } private: @@ -36,7 +53,9 @@ class VisualizationAggregator : public rclcpp::Node subscriber; rclcpp::Publisher::SharedPtr publisher; - std::unordered_map layers; + std::unordered_map> layers; + + rclcpp::TimerBase::SharedPtr timer; }; int main(int argc, char ** argv)