SvgLayerArrayを全面的に採用

ibis-ssl · Jan 31, 2025 · ef36d46 · ef36d46
1 parent b333f4c
commit ef36d46
Show file tree

Hide file tree

Showing 2 changed files with 39 additions and 16 deletions.
diff --git a/utility/crane_msg_wrappers/include/crane_msg_wrappers/crane_visualizer_wrapper.hpp b/utility/crane_msg_wrappers/include/crane_msg_wrappers/crane_visualizer_wrapper.hpp
@@ -8,8 +8,9 @@
 #define CRANE_MSG_WRAPPERS__CRANE_VISUALIZER_WRAPPER_HPP_
 
 #include <crane_basics/boost_geometry.hpp>
-#include <crane_visualization_interfaces/msg/svg_primitive_array.hpp>
+#include <crane_visualization_interfaces/msg/svg_layer_array.hpp>
 #include <memory>
+#include <range/v3/all.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <string>
 
@@ -556,17 +557,17 @@ struct SvgPathBuilder
 
 struct CraneVisualizerBuffer
 {
-  using SvgPrimitiveArray = crane_visualization_interfaces::msg::SvgPrimitiveArray;
+  using SvgLayerArray = crane_visualization_interfaces::msg::SvgLayerArray;
   static inline std::unique_ptr<CraneVisualizerBuffer> buffer = nullptr;
 
-  rclcpp::Publisher<SvgPrimitiveArray>::SharedPtr publisher;
+  rclcpp::Publisher<SvgLayerArray>::SharedPtr publisher;
 
-  SvgPrimitiveArray message_buffer;
+  SvgLayerArray message_buffer;
 
   template <typename Node>
   CraneVisualizerBuffer(Node & node, const std::string topic)
   {
-    publisher = node.template create_publisher<SvgPrimitiveArray>(topic, rclcpp::SensorDataQoS());
+    publisher = node.template create_publisher<SvgLayerArray>(topic, rclcpp::SensorDataQoS());
   }
 
   template <typename Node>
@@ -590,12 +591,26 @@ struct CraneVisualizerBuffer
   {
     if (active()) {
       buffer->publisher->publish(buffer->message_buffer);
-      buffer->message_buffer.svg_primitives.clear();
+      buffer->message_buffer.svg_primitive_arrays.clear();
+    }
+  }
+
+  static auto clear(std::string layer = "") -> void
+  {
+    if (CraneVisualizerBuffer::active()) {
+      if (layer == "") {
+        CraneVisualizerBuffer::buffer->message_buffer.svg_primitive_arrays.clear();
+      } else {
+        ranges::actions::remove_if(
+          CraneVisualizerBuffer::buffer->message_buffer.svg_primitive_arrays,
+          [&layer](const auto & layer_array) { return layer_array.layer == layer; });
+      }
     }
   }
 
   struct MessageBuilder
   {
+    using SvgPrimitiveArray = crane_visualization_interfaces::msg::SvgPrimitiveArray;
     using SharedPtr = std::shared_ptr<MessageBuilder>;
     using UniquePtr = std::unique_ptr<MessageBuilder>;
 
@@ -606,15 +621,22 @@ struct CraneVisualizerBuffer
     void flush()
     {
       if (CraneVisualizerBuffer::active()) {
-        CraneVisualizerBuffer::buffer->message_buffer.layer = layer;
-        CraneVisualizerBuffer::buffer->message_buffer.svg_primitives.insert(
-          CraneVisualizerBuffer::buffer->message_buffer.svg_primitives.end(),
-          message_buffer.begin(), message_buffer.end());
+        SvgPrimitiveArray layer_msg;
+        layer_msg.layer = layer;
+        layer_msg.svg_primitives = message_buffer;
+        CraneVisualizerBuffer::buffer->message_buffer.svg_primitive_arrays.push_back(layer_msg);
         message_buffer.clear();
       }
     }
 
-    using SvgPrimitiveArray = crane_visualization_interfaces::msg::SvgPrimitiveArray;
+    void clear() { message_buffer.clear(); }
+
+    void clearBuffer()
+    {
+      if (CraneVisualizerBuffer::active()) {
+        CraneVisualizerBuffer::clear(layer);
+      }
+    }
 
     std::vector<std::string> message_buffer;
 

diff --git a/utility/crane_visualization_aggregator/src/crane_visualization_aggregator_node.cpp b/utility/crane_visualization_aggregator/src/crane_visualization_aggregator_node.cpp
@@ -21,11 +21,13 @@ class VisualizationAggregator : public rclcpp::Node
 public:
   VisualizationAggregator() : Node("visualization_aggregator")
   {
-    subscriber = create_subscription<crane_visualization_interfaces::msg::SvgPrimitiveArray>(
+    subscriber = create_subscription<crane_visualization_interfaces::msg::SvgLayerArray>(
       "/visualizer_svgs", rclcpp::SensorDataQoS(),
-      [&](const crane_visualization_interfaces::msg::SvgPrimitiveArray::ConstSharedPtr & msg) {
+      [&](const crane_visualization_interfaces::msg::SvgLayerArray::ConstSharedPtr & msg) {
         // store into　layers
-        layers[msg->layer] = msg->svg_primitives;
+        for (const auto & layer_msg : msg->svg_primitive_arrays) {
+          layers[layer_msg.layer] = layer_msg.svg_primitives;
+        }
       });
     publisher =
       create_publisher<crane_visualization_interfaces::msg::SvgLayerArray>("/aggregated_svgs", 10);
@@ -43,8 +45,7 @@ class VisualizationAggregator : public rclcpp::Node
   }
 
 private:
-  rclcpp::Subscription<crane_visualization_interfaces::msg::SvgPrimitiveArray>::SharedPtr
-    subscriber;
+  rclcpp::Subscription<crane_visualization_interfaces::msg::SvgLayerArray>::SharedPtr subscriber;
   rclcpp::Publisher<crane_visualization_interfaces::msg::SvgLayerArray>::SharedPtr publisher;
 
   std::unordered_map<std::string, std::vector<std::string>> layers;