feat: 可視化レイヤーのlifetimeベース管理機能を実装

crane_robot_skills/include/crane_robot_skills/skill_base.hpp

@@ -93,12 +93,16 @@ class SkillInterface
   : command(std::make_shared<PositionCommandWrapper>(name, id, wm)),
     visualizer(std::make_unique<crane::VisualizerMessageBuilder>("skill/" + name))
   {
+    // スキル用ビジュアライザのデフォルトduration: 0.5秒
+    visualizer->withDuration(0.5);
   }
 
   explicit SkillInterface(std::shared_ptr<PositionCommandWrapper> & command)
   : command(command),
     visualizer(std::make_unique<crane::VisualizerMessageBuilder>("skill/" + command->name))
   {
+    // スキル用ビジュアライザのデフォルトduration: 0.5秒
+    visualizer->withDuration(0.5);
   }
 
   virtual ~SkillInterface() { visualizer->clearBuffer(); }

crane_sessions/include/crane_sessions/session_base.hpp

@@ -55,6 +55,8 @@ class SessionBase
     world_model(world_model),
     visualizer(std::make_shared<VisualizerMessageBuilder>("session_coordinator/" + name))
   {
+    // セッション用ビジュアライザのデフォルトduration: 0.5秒
+    visualizer->withDuration(0.5);
   }
 
   virtual ~SessionBase() { visualizer->clearBuffer(); }

crane_visualization_interfaces/msg/SvgLayerUpdate.msg

@@ -1,3 +1,4 @@
 string layer
 string operation # "append" | "replace" | "clear"
 string[] svg_primitives
+float64 duration  # 有効期限（秒）。0 = 無限（デフォルト）

utility/crane_msg_wrappers/include/crane_msg_wrappers/crane_visualizer_wrapper.hpp

@@ -55,6 +55,7 @@ struct VisualizerMessageBuilder : public std::enable_shared_from_this<Visualizer
 
   std::string layer;
   std::string operation = "replace";  // default operation
+  double duration = 0.0;              // 有効期限（秒）。0 = 無限（デフォルト）
 
   explicit VisualizerMessageBuilder(const std::string & layer) : layer(layer) {}
 
@@ -85,6 +86,13 @@ struct VisualizerMessageBuilder : public std::enable_shared_from_this<Visualizer
     return *this;
   }
 
+  // Duration modifier
+  [[nodiscard]] auto withDuration(double seconds) -> VisualizerMessageBuilder &
+  {
+    duration = seconds;
+    return *this;
+  }
+
   SvgCircleBuilder circle();
 
   auto line() -> SvgLineBuilder;

utility/crane_msg_wrappers/src/crane_visualizer_wrapper.cpp

@@ -15,6 +15,7 @@ auto VisualizerMessageBuilder::flush() -> void
     update_msg.layer = layer;
     update_msg.operation = operation;
     update_msg.svg_primitives = message_buffer;
+    update_msg.duration = duration;
     CraneVisualizerBuffer::buffer->message_buffer.updates.push_back(update_msg);
     message_buffer.clear();
   }

utility/crane_visualization_aggregator/src/crane_visualization_aggregator_node.cpp

@@ -17,6 +17,13 @@
 string layer
 string[] svg_primitives
  */
+
+struct LayerState
+{
+  std::vector<std::string> svg_primitives;
+  std::optional<rclcpp::Time> expiration;  // 期限（nullopt = 無限）
+};
+
 class VisualizationAggregator : public rclcpp::Node
 {
 public:
@@ -27,19 +34,34 @@ class VisualizationAggregator : public rclcpp::Node
       [&](const crane_visualization_interfaces::msg::SvgUpdates::ConstSharedPtr & msg) {
         for (const auto & update : msg->updates) {
           auto & current = layers[update.layer];
+
+          // durationから期限を計算
+          if (update.duration > 0) {
+            current.expiration = this->now() + rclcpp::Duration::from_seconds(update.duration);
+          } else {
+            current.expiration = std::nullopt;  // 無限
+          }
+
+          // 既存の処理
           if (update.operation == "replace") {
-            current = update.svg_primitives;
+            current.svg_primitives = update.svg_primitives;
           } else if (update.operation == "append") {
-            current.insert(
-              current.end(), update.svg_primitives.begin(), update.svg_primitives.end());
+            current.svg_primitives.insert(
+              current.svg_primitives.end(), update.svg_primitives.begin(),
+              update.svg_primitives.end());
           } else if (update.operation == "clear") {
-            current.clear();
+            current.svg_primitives.clear();
           }
         }
       });
     publisher =
       create_publisher<crane_visualization_interfaces::msg::SvgSnapshot>("/aggregated_svgs", 10);
     timer = create_wall_timer(std::chrono::milliseconds(5000), [this]() { publishSnapshot(); });
+
+    // 期限切れレイヤーをクリーンアップするタイマー（100ms周期）
+    cleanup_timer =
+      create_wall_timer(std::chrono::milliseconds(100), [this]() { cleanupExpiredLayers(); });
+
     publishSnapshot();
   }
 
@@ -50,22 +72,35 @@ class VisualizationAggregator : public rclcpp::Node
     msg.header.stamp = this->now();
     msg.epoch = epoch_;
     msg.seq = seq_++;
-    for (const auto & [layer, primitives] : layers) {
+    for (const auto & [layer, state] : layers) {
       crane_visualization_interfaces::msg::SvgLayerSnapshot layer_msg;
       layer_msg.layer = layer;
-      layer_msg.svg_primitives = primitives;
+      layer_msg.svg_primitives = state.svg_primitives;
       msg.layers.push_back(layer_msg);
     }
     publisher->publish(msg);
   }
 
+  void cleanupExpiredLayers()
+  {
+    auto now = this->now();
+    for (auto it = layers.begin(); it != layers.end();) {
+      if (it->second.expiration && now > *it->second.expiration) {
+        it = layers.erase(it);  // 期限切れ: 削除
+      } else {
+        ++it;
+      }
+    }
+  }
+
   rclcpp::Subscription<crane_visualization_interfaces::msg::SvgUpdates>::SharedPtr updates_sub_;
 
   rclcpp::Publisher<crane_visualization_interfaces::msg::SvgSnapshot>::SharedPtr publisher;
 
-  std::unordered_map<std::string, std::vector<std::string>> layers;
+  std::unordered_map<std::string, LayerState> layers;
 
   rclcpp::TimerBase::SharedPtr timer;
+  rclcpp::TimerBase::SharedPtr cleanup_timer;
 
   uint32_t epoch_ = 0;
   uint32_t seq_ = 0;