diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index 48f7d18ce67a5..e4584a58ef0df 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_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/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp index ee2c7e0d5c3b9..5493f1dd594ce 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp @@ -24,6 +24,7 @@ #include #include +#include #include #include #include @@ -112,6 +113,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay std::mutex mutex; std::condition_variable condition; std::vector markers; + std::set existing_marker_ids; }; } // namespace object_detection diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp index 870b8f2c77543..24e67a6f44e95 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp @@ -15,6 +15,7 @@ #include #include +#include namespace autoware { @@ -130,7 +131,7 @@ std::vector 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 lock(mutex); if (!markers.empty()) { - clear_markers(); - + std::set new_marker_ids = std::set(); 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(); }