Skip to content

Commit

Permalink
refactor(perception_rviz_plugin): only delete markers with unused Mar…
Browse files Browse the repository at this point in the history
…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>
  • Loading branch information
3 people authored and shmpwk committed Aug 31, 2023
1 parent e6b6e09 commit 21fd035
Show file tree
Hide file tree
Showing 3 changed files with 19 additions and 3 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@

#include <condition_variable>
#include <list>
#include <set>
#include <string>
#include <unordered_map>
#include <vector>
Expand Down Expand Up @@ -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
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
#include <object_detection/predicted_objects_display.hpp>

#include <memory>
#include <set>

namespace autoware
{
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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();
}
Expand Down

0 comments on commit 21fd035

Please sign in to comment.