refactor(perception_rviz_plugin): only delete markers with unused Mar…

…kersIDs during updates. (autowarefoundation#4783) * refactor(perception_rviz_plugin): only delete markers with unused MarkersIDs during updates. Signed-off-by: Owen-Liuyuxuan <[email protected]> * style(pre-commit): autofix * fix: include <set> for linting test Signed-off-by: Owen-Liuyuxuan <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: Owen-Liuyuxuan <[email protected]> Co-authored-by: Owen-Liuyuxuan <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
tier4 · Aug 29, 2023 · 0cbfc3d · 0cbfc3d
1 parent 84107d0
commit 0cbfc3d
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 3 deletions.
diff --git a/...ware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/...ware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp
@@ -146,6 +146,11 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
     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

diff --git a/...toware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp b/...toware_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

diff --git a/...n/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp b/...n/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 @@ std::vector<visualization_msgs::msg::Marker::SharedPtr> PredictedObjectsDisplay:
       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 @@ void PredictedObjectsDisplay::update(float wall_dt, float ros_dt)
   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();
   }