diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index 7f3b3b9536..9210f65a52 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include @@ -95,7 +96,7 @@ class ClocksState final } } std::lock_guard guard(clock_list_lock_); - associated_clocks_.push_back(clock); + associated_clocks_.insert(clock); // Set the clock to zero unless there's a recently received message set_clock(last_time_msg_, ros_time_active_, clock); } @@ -104,10 +105,8 @@ class ClocksState final void detachClock(rclcpp::Clock::SharedPtr clock) { std::lock_guard guard(clock_list_lock_); - auto result = std::find(associated_clocks_.begin(), associated_clocks_.end(), clock); - if (result != associated_clocks_.end()) { - associated_clocks_.erase(result); - } else { + auto removed = associated_clocks_.erase(clock); + if (removed == 0) { RCLCPP_ERROR(logger_, "failed to remove clock"); } } @@ -184,8 +183,8 @@ class ClocksState final // A lock to protect iterating the associated_clocks_ field. std::mutex clock_list_lock_; - // A vector to store references to associated clocks. - std::vector associated_clocks_; + // An unordered_set to store references to associated clocks. + std::unordered_set associated_clocks_; // Local storage of validity of ROS time // This is needed when new clocks are added.