Bug report

Required Info:

• Operating System: Ubuntu 22.04
• Installation type: binaries
• Version or commit hash: upgrade from 0.25.3-1jammy.20230721.203549 to 0.25.6-1jammy.20240217.053651 (Debian)
• DDS implementation: rmw_cyclonedds_cpp
• Client library (if applicable): rclcpp

Steps to reproduce issue

This is observed performance involving sim, control, and camera publishers / subscribers.
We (@calderpg-tri and I) observed this by looking at delays for image receipt, and noticed that #636 (specifically the backport in bda8c49) causes an observable (significant?) delay from when an image is published to when it is received, while tf is concurrently being used.

If I use 0.25.6 but revert the given commit, I see good performance "expected behavior" in terms of timing.

I do not yet have a minimal reproduction case. See benchmark in #679, and results posted below in this issue.

Expected behavior

Negligible performance impact

Actual behavior

Observable (significant?) performance impact

Additional information

we should consider using something more performant than std::find() on a std::list. Not immediately sure what that'll be.
From initial glance at ::insertData and ::pruneList, I assume storage_ is sorted, so perhaps that can simple be changed to

bool should_insert = true;
// Search along all data with same timestamp (sorted), and only insert if we did not find the exact same data.
while (storage_it != storage_.end() && storage_it->stamp_ == new_data->stamp_) {
  if (*storage_it == new_data) {
    should_insert = false;
    break;
  }
  storage_it++;
}
if (should_insert) {
  storage_.insert(storage_it, new_data);
}

fyi @nachovizzo (\cc @clalancette @ahcorde)