refactor(perception_rviz_plugin): only delete markers with unused MarkersIDs during updates.

common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp

@@ -146,6 +146,11 @@
     m_marker_common.addMessage(markers_ptr);
   }
 
+  void deleteMarker(rviz_default_plugins::displays::MarkerID marker_id)
+  {
+    m_marker_common.deleteMarker(marker_id);
+  }
+
 protected:
   /// \brief Convert given shape msg into a Marker
   /// \tparam ClassificationContainerT List type with ObjectClassificationMsg

common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp

@@ -24,6 +24,7 @@
 
 #include <condition_variable>
 #include <list>
+#include <set>
 #include <string>
 #include <unordered_map>
 #include <vector>
@@ -112,6 +113,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay
   std::mutex mutex;
   std::condition_variable condition;
   std::vector<visualization_msgs::msg::Marker::SharedPtr> markers;
+  std::set<rviz_default_plugins::displays::MarkerID> existing_marker_ids;
 };
 
 }  // namespace object_detection

common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp

@@ -15,6 +15,7 @@
 #include <object_detection/predicted_objects_display.hpp>
 
 #include <memory>
+#include <set>
 
 namespace autoware
 {
@@ -130,7 +131,7 @@
       auto acceleration_text_marker_ptr = acceleration_text_marker.value();
       acceleration_text_marker_ptr->header = msg->header;
       acceleration_text_marker_ptr->id = uuid_to_marker_id(object.object_id);
-      add_marker(acceleration_text_marker_ptr);
+      markers.push_back(acceleration_text_marker_ptr);
     }
 
     // Get marker for twist
@@ -195,11 +196,19 @@
   std::unique_lock<std::mutex> lock(mutex);
 
   if (!markers.empty()) {
-    clear_markers();
-
+    std::set new_marker_ids = std::set<rviz_default_plugins::displays::MarkerID>();
     for (const auto & marker : markers) {
+      rviz_default_plugins::displays::MarkerID marker_id =
+        rviz_default_plugins::displays::MarkerID(marker->ns, marker->id);
       add_marker(marker);
+      new_marker_ids.insert(marker_id);
+    }
+    for (auto itr = existing_marker_ids.begin(); itr != existing_marker_ids.end(); itr++) {
+      if (new_marker_ids.find(*itr) == new_marker_ids.end()) {
+        deleteMarker(*itr);
+      }
     }
+    existing_marker_ids = new_marker_ids;
 
     markers.clear();
   }