diff --git a/swri_roscpp/include/swri_roscpp/subscriber_impl.h b/swri_roscpp/include/swri_roscpp/subscriber_impl.h index 0c7c9fa5..6d32cd58 100644 --- a/swri_roscpp/include/swri_roscpp/subscriber_impl.h +++ b/swri_roscpp/include/swri_roscpp/subscriber_impl.h @@ -76,7 +76,8 @@ namespace swri message_count_++; - if (stamp.nanoseconds() != 0) { + // Check for out of order messages and messages that appear to come from the future + if ((stamp.nanoseconds() != 0) && (now >= stamp)) { rclcpp::Duration latency = now - stamp; if (message_count_ == 1) { min_latency_ = latency;