Sanity checking time before performing duration calculation (#735)

swri-robotics · Jun 3, 2024 · a608051 · a608051
1 parent 31e3525
commit a608051
Showing 1 changed file with 2 additions and 1 deletion.
diff --git 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;