Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Set error status when duplicate markers are in the same MarkerArray #891

Merged
merged 6 commits into from
Sep 6, 2022
Merged

Set error status when duplicate markers are in the same MarkerArray #891

Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
e48c005
Set error status when duplicate markers are in the same MarkerArray
sloretz Aug 29, 2022
6d59b56
Use std::set with id before ns
sloretz Aug 30, 2022
9cfd488
Lock/Unlock for every message
sloretz Sep 1, 2022
ba16715
Output first offending namespace and id
sloretz Sep 2, 2022
8aeea4c
Add benchmark
sloretz Aug 30, 2022
9bc888b
Revert "Add benchmark"
sloretz Sep 2, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
34 changes: 34 additions & 0 deletions rviz_default_plugins/src/rviz_default_plugins/displays/marker/marker_common.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -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;
Comment on lines +170 to +171
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Consider using emplace if performance is a concern. I'm not sure if moves the needle on your benchmark though:

Suggested change
pair_type pair(marker.id, marker.ns);
found_duplicate = !unique_markers.insert(pair).second;
found_duplicate = !unique_markers.emplace(marker.id, marker.ns).second;

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I tried out in a benchmark, but I can't see a difference between emplace and insert within the noise.

One of the runs 
56: 2022-09-02T13:58:15-07:00                      
56: Running /home/osrf/ws/ros2/build/rviz_default_plugins/bm891
56: Run on (24 X 4950.19 MHz CPU s)
56: CPU Caches:
56:   L1 Data 32 KiB (x12)
56:   L1 Instruction 32 KiB (x12)
56:   L2 Unified 512 KiB (x12)
56:   L3 Unified 32768 KiB (x2)
56: Load Average: 1.67, 2.49, 1.99
56: ***WARNING*** CPU scaling is enabled, the benchmark real time measurements may be noisy and will incur extra overhead.
56: ------------------------------------------------------------------------------------------------------------
56: Benchmark                                                                  Time             CPU   Iterations
56: ------------------------------------------------------------------------------------------------------------
56: no_check_10                                                              494 ns          494 ns      1390157
56: no_check_100                                                            5035 ns         5034 ns       139124
56: no_check_1000                                                          52140 ns        52134 ns        13403
56: no_check_10000                                                        522287 ns       522195 ns         1330
56: increasing_ids__vector_of_pairs__10                                      540 ns          540 ns      1293151
56: increasing_ids__vector_of_pairs__100                                    5126 ns         5124 ns       136431
56: increasing_ids__vector_of_pairs__1000                                  53451 ns        53444 ns        13273
56: increasing_ids__vector_of_pairs__10000                                526744 ns       526664 ns         1291
56: decreasing_ids__vector_of_pairs__10                                      552 ns          552 ns      1266562
56: decreasing_ids__vector_of_pairs__100                                    7073 ns         7072 ns        97768
56: decreasing_ids__vector_of_pairs__1000                                 187947 ns       187922 ns         3712
56: decreasing_ids__vector_of_pairs__10000                              11610110 ns     11608107 ns           60
56: increasing_ids__set_insertion__10                                        730 ns          730 ns       954514
56: increasing_ids__set_insertion__100                                     10345 ns        10341 ns        67748
56: increasing_ids__set_insertion__1000                                   121661 ns       121645 ns         5752
56: increasing_ids__set_insertion__10000                                 1360934 ns      1360740 ns          516
56: decreasing_ids__set_insertion__10                                        738 ns          738 ns       947607
56: decreasing_ids__set_insertion__100                                      9665 ns         9664 ns        72394
56: decreasing_ids__set_insertion__1000                                   120601 ns       120586 ns         5779
56: decreasing_ids__set_insertion__10000                                 1340419 ns      1340184 ns          526
56: increasing_ids__set_insertion_id_first__10                               708 ns          708 ns       987930
56: increasing_ids__set_insertion_id_first__100                             8838 ns         8837 ns        79525
56: increasing_ids__set_insertion_id_first__1000                          105766 ns       105751 ns         6617
56: increasing_ids__set_insertion_id_first__10000                        1141860 ns      1141685 ns          615
56: decreasing_ids__set_insertion_id_first__10                               724 ns          724 ns       967446
56: decreasing_ids__set_insertion_id_first__100                             8674 ns         8673 ns        80745
56: decreasing_ids__set_insertion_id_first__1000                          105557 ns       105542 ns         6615
56: decreasing_ids__set_insertion_id_first__10000                        1128819 ns      1128658 ns          631
56: increasing_ids__set_insertion_id_first_always_lock__10                   752 ns          752 ns       930742
56: increasing_ids__set_insertion_id_first_always_lock__100                 9304 ns         9303 ns        75296
56: increasing_ids__set_insertion_id_first_always_lock__1000              109530 ns       109515 ns         6393
56: increasing_ids__set_insertion_id_first_always_lock__10000            1175368 ns      1175185 ns          596
56: decreasing_ids__set_insertion_id_first_always_lock__10                   722 ns          722 ns       968217
56: decreasing_ids__set_insertion_id_first_always_lock__100                 9042 ns         9041 ns        77217
56: decreasing_ids__set_insertion_id_first_always_lock__1000              108964 ns       108949 ns         6395
56: decreasing_ids__set_insertion_id_first_always_lock__10000            1185452 ns      1185020 ns          598
56: increasing_ids__set_insertion_id_first_always_lock_emplace__10           757 ns          757 ns       921752                 
56: increasing_ids__set_insertion_id_first_always_lock_emplace__100         9250 ns         9249 ns        75718
56: increasing_ids__set_insertion_id_first_always_lock_emplace__1000      109513 ns       109499 ns         6318
56: increasing_ids__set_insertion_id_first_always_lock_emplace__10000    1190149 ns      1189968 ns          591
56: decreasing_ids__set_insertion_id_first_always_lock_emplace__10           759 ns          759 ns       920237
56: decreasing_ids__set_insertion_id_first_always_lock_emplace__100         9133 ns         9132 ns        76482
56: decreasing_ids__set_insertion_id_first_always_lock_emplace__1000      109207 ns       109191 ns         6436
56: decreasing_ids__set_insertion_id_first_always_lock_emplace__10000    1139710 ns      1139538 ns          613

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
Expand Down