可視化の調整

crane_robot_skills/include/crane_robot_skills/skill_base.hpp

@@ -149,6 +149,8 @@ class SkillInterface
     command_base->latest_msg.skill_name = name;
   }
 
+  virtual ~SkillInterface() { visualizer->clearBuffer(); }
+
   const std::string name;
 
   virtual Status run(

crane_world_model_publisher/src/visualization_data_handler.cpp

@@ -27,24 +27,19 @@ struct SvgRobotBuilder : public SvgPathBuilder
 
   std::string getSvgString() const override
   {
-    SvgPathDefinitionBuilder path_builder;
-    path_builder
+    SvgPathBuilder path_builder;
+    path_builder.definition
       .moveTo(robot_position.x() + botRightX(theta), robot_position.y() + botRightY(theta))
       .arcTo(
         {radius, radius}, 0, true, true,
         {robot_position.x() + botLeftX(theta), robot_position.y() + botLeftY(theta)})
       .lineTo(robot_position.x() + botRightX(theta), robot_position.y() + botRightY(theta));
 
-    SvgPathBuilder builder = path_builder.build();
-    builder.fill(fill_color, fill_opacity)
+    path_builder.fill(fill_color, fill_opacity)
       .stroke(stroke_color, stroke_opacity)
       .strokeWidth(stroke_width);
 
-    std::ostringstream oss;
-    oss << "<path d=\"" << builder.path << "\" style=\"stroke-width: " << builder.stroke_width
-        << "; stroke: " << builder.stroke_color << "; fill-opacity: " << builder.fill_opacity
-        << "; fill: " << builder.fill_color << ";\"></path>";
-    return oss.str();
+    return path_builder.getSvgString();
   }
 
   SvgRobotBuilder & position(Point p, double theta)

crane_world_model_publisher/src/world_model_publisher.cpp

@@ -440,48 +440,62 @@ void WorldModelPublisherComponent::publishWorldModel()
   pub_world_model->publish(wm);
 
   constexpr int SAMPLING_NUM = 4;
-  for (const auto & history : friend_history) {
-    if (history.size() > SAMPLING_NUM + 1) {
-      for (int index = 0; index < history.size() - SAMPLING_NUM; index += SAMPLING_NUM) {
-        Point p1;
-        Point p2;
-        p1 << history.at(index).x, history.at(index).y;
-        p2 << history.at(index + SAMPLING_NUM).x, history.at(index + SAMPLING_NUM).y;
-        SvgLineBuilder line_builder;
-        line_builder.start(p1)
-          .end(p2)
-          .stroke("yellow", index / static_cast<double>(history.size()))
-          .strokeWidth(30);
-        visualizer->add(line_builder.getSvgString());
+  for (const auto & [robot_id, history] : friend_history | ranges::views::enumerate) {
+    if (
+      history.size() > SAMPLING_NUM + 1 &&
+      robot_info[static_cast<uint8_t>(our_color)].at(robot_id).detected) {
+      for (int i = 0; i < 10; i++) {
+        SvgPolyLineBuilder polyline_builder;
+        int start = static_cast<int>((history.size() / 10.) * i);
+        int end = static_cast<int>((history.size() / 10.) * (i + 1));
+        for (int index = start; index < end; index += SAMPLING_NUM) {
+          polyline_builder.addPoint(history.at(index).x, history.at(index).y);
+        }
+        if (i != 9) {
+          polyline_builder.addPoint(history.at(end).x, history.at(end).y);
+        }
+        polyline_builder.stroke("yellow", start / static_cast<double>(history.size()))
+          .strokeWidth(15);
+        visualizer->add(polyline_builder.getSvgString());
       }
     }
   }
 
-  for (const auto & history : enemy_history) {
-    if (history.size() > SAMPLING_NUM + 1) {
-      for (int index = 0; index < history.size() - SAMPLING_NUM; index += SAMPLING_NUM) {
-        Point p1;
-        Point p2;
-        p1 << history.at(index).x, history.at(index).y;
-        p2 << history.at(index + SAMPLING_NUM).x, history.at(index + SAMPLING_NUM).y;
-        SvgLineBuilder line_builder;
-        line_builder.start(p1)
-          .end(p2)
-          .stroke("blue", index / static_cast<double>(history.size()))
-          .strokeWidth(30);
-        visualizer->add(line_builder.getSvgString());
+  for (const auto & [robot_id, history] : enemy_history | ranges::views::enumerate) {
+    if (
+      history.size() > SAMPLING_NUM + 1 &&
+      robot_info[static_cast<uint8_t>(their_color)].at(robot_id).detected) {
+      for (int i = 0; i < 10; i++) {
+        SvgPolyLineBuilder polyline_builder;
+        int start = static_cast<int>((history.size() / 10.) * i);
+        int end = static_cast<int>((history.size() / 10.) * (i + 1));
+        for (int index = start; index < end; index += SAMPLING_NUM) {
+          polyline_builder.addPoint(history.at(index).x, history.at(index).y);
+        }
+        if (i != 9) {
+          polyline_builder.addPoint(history.at(end).x, history.at(end).y);
+        }
+        polyline_builder.stroke("blue", start / static_cast<double>(history.size()))
+          .strokeWidth(15);
+        visualizer->add(polyline_builder.getSvgString());
       }
     }
   }
 
   if (ball_history.size() > SAMPLING_NUM + 1) {
-    for (int index = 0; index < ball_history.size() - SAMPLING_NUM; index += SAMPLING_NUM) {
-      SvgLineBuilder line_builder;
-      line_builder.start(ball_history.at(index))
-        .end(ball_history.at(index + SAMPLING_NUM))
-        .stroke("orange", index / static_cast<double>(ball_history.size()))
+    for (int i = 0; i < 10; i++) {
+      SvgPolyLineBuilder polyline_builder;
+      int start = static_cast<int>((ball_history.size() / 10.) * i);
+      int end = static_cast<int>((ball_history.size() / 10.) * (i + 1));
+      for (int index = start; index < end; index += SAMPLING_NUM) {
+        polyline_builder.addPoint(ball_history.at(index));
+      }
+      if (i != 9) {
+        polyline_builder.addPoint(ball_history.at(end));
+      }
+      polyline_builder.stroke("orange", start / static_cast<double>(ball_history.size()))
         .strokeWidth(30);
-      visualizer->add(line_builder.getSvgString());
+      visualizer->add(polyline_builder.getSvgString());
     }
   }
   visualizer->flush();

session/crane_planner_plugins/include/crane_planner_plugins/planner_base.hpp

@@ -54,6 +54,8 @@ class PlannerBase
   {
   }
 
+  virtual ~PlannerBase() { visualizer->clearBuffer(); }
+
   crane_msgs::srv::RobotSelect::Response doRobotSelect(
     const crane_msgs::srv::RobotSelect::Request::SharedPtr request,
     const std::unordered_map<uint8_t, RobotRole> & prev_roles, PlannerContext & context)

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>
 
@@ -311,7 +312,11 @@ struct SvgPolyLineBuilder
     for (const auto & p : points) {
       oss << p.x() * 1000. << "," << p.y() * 1000. << " ";
     }
-    oss << "\" stroke=\"" << stroke_color << "\" stroke-width=\"" << stroke_width << "\" />";
+    oss << "\" stroke=\"" << stroke_color << "\" stroke-width=\"" << stroke_width;
+    if (stroke_opacity != 1.) {
+      oss << "\" stroke-opacity=\"" << stroke_opacity;
+    }
+    oss << "\" fill=\"none\" />";
     return oss.str();
   }
 
@@ -327,7 +332,7 @@ struct SvgPolyLineBuilder
     return *this;
   }
 
-  SvgPolyLineBuilder & strokeColor(const std::string & color, double alpha = 1.0)
+  SvgPolyLineBuilder & stroke(const std::string & color, double alpha = 1.0)
   {
     stroke_color = color;
     stroke_opacity = alpha;
@@ -404,8 +409,6 @@ struct SvgPolygonBuilder
 
 struct SvgPathBuilder
 {
-  std::string path;
-
   std::string fill_color = "none";
 
   double fill_opacity = 1.;
@@ -421,17 +424,11 @@ struct SvgPathBuilder
   virtual std::string getSvgString() const
   {
     std::ostringstream oss;
-    oss << "<path d=\"" << path << "\" fill=\"" << fill_color << "\" stroke=\"" << stroke_color
-        << "\" stroke-width=\"" << stroke_width << "\" />";
+    oss << "<path d=\"" << definition.path << "\" fill=\"" << fill_color << "\" stroke=\""
+        << stroke_color << "\" stroke-width=\"" << stroke_width << "\" />";
     return oss.str();
   }
 
-  SvgPathBuilder & pathString(const std::string & path)
-  {
-    this->path = path;
-    return *this;
-  }
-
   SvgPathBuilder & fill(const std::string & color, double alpha = 1.0)
   {
     fill_color = color;
@@ -555,29 +552,22 @@ struct SvgPathBuilder
     {
       return arcTo(r.x(), r.y(), x_axis_rotation, large_arc_flag, sweep_flag, p.x(), p.y());
     }
-
-    SvgPathBuilder build()
-    {
-      SvgPathBuilder builder;
-      builder.path = path;
-      return builder;
-    }
-  };
+  } definition;
 };
 
 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>
@@ -601,12 +591,29 @@ 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 {
+        for (auto & svg_layer : CraneVisualizerBuffer::buffer->message_buffer.svg_primitive_arrays |
+                                  ranges::views::filter([&](auto svg_primitive_array) {
+                                    return svg_primitive_array.layer == layer;
+                                  })) {
+          svg_layer.svg_primitives.clear();
+        }
+      }
     }
   }
 
   struct MessageBuilder
   {
+    using SvgPrimitiveArray = crane_visualization_interfaces::msg::SvgPrimitiveArray;
     using SharedPtr = std::shared_ptr<MessageBuilder>;
     using UniquePtr = std::unique_ptr<MessageBuilder>;
 
@@ -617,15 +624,23 @@ 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()
+    {
+      clear();
+      if (CraneVisualizerBuffer::active()) {
+        CraneVisualizerBuffer::clear(layer);
+      }
+    }
 
     std::vector<std::string> message_buffer;
 

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;