Set error status when duplicate markers are in the same MarkerArray (r…

…os2#891)

* Set error status when duplicate markers are in the same MarkerArray

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Use std::set with id before ns

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Lock/Unlock for every message

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Output first offending namespace and id

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add benchmark

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

More benchmarks

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Revert "Add benchmark"

This reverts commit 8aeea4c.

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

rviz_default_plugins/src/rviz_default_plugins/displays/marker/marker_common.cpp

@@ -31,6 +31,8 @@
 #include "rviz_default_plugins/displays/marker/marker_common.hpp"
 
 #include <memory>
+#include <set>
+#include <sstream>
 #include <string>
 #include <utility>
 #include <vector>
@@ -153,9 +155,41 @@ void MarkerCommon::addMessage(const visualization_msgs::msg::Marker::ConstShared
 void MarkerCommon::addMessage(
   const visualization_msgs::msg::MarkerArray::ConstSharedPtr array)
 {
+  using ns_type = decltype(visualization_msgs::msg::Marker::ns);
+  using id_type = decltype(visualization_msgs::msg::Marker::id);
+  using pair_type = std::pair<id_type, const ns_type &>;
+
+  // Keep track of unique markers
+  std::set<pair_type> unique_markers;
+  bool found_duplicate = false;
+  std::string offending_ns;
+  id_type offending_id = 0;
+
   for (auto const & marker : array->markers) {
+    if (!found_duplicate) {
+      pair_type pair(marker.id, marker.ns);
+      found_duplicate = !unique_markers.insert(pair).second;
+      if (found_duplicate) {
+        offending_ns = marker.ns;
+        offending_id = marker.id;
+      }
+    }
     addMessage(std::make_shared<visualization_msgs::msg::Marker>(marker));
   }
+
+  // Can't use setMarkerStatus on individual markers because processAdd would clear it.
+  const char * kDuplicateStatus = "Duplicate Marker Check";
+  if (found_duplicate) {
+    std::stringstream error_stream;
+    error_stream << "Multiple Markers in the same MarkerArray message had the same ns and id: ";
+    error_stream << "(" << offending_ns << ", " << offending_id << ")";
+    display_->setStatusStd(
+      rviz_common::properties::StatusProperty::Error,
+      kDuplicateStatus,
+      error_stream.str());
+  } else {
+    display_->deleteStatusStd(kDuplicateStatus);
+  }
 }
 
 // TODO(greimela): Revisit after MessageFilter is available in ROS2