From 894f8f4ad708710d65b92c79b14f98ab3d17e432 Mon Sep 17 00:00:00 2001
From: leoniegadner <62353963+leoniegadner@users.noreply.github.com>
Date: Fri, 2 Jun 2023 10:59:46 +0900
Subject: [PATCH] Visualize Costmaps when jumping back in ROS time (#81)

* Publish costmaps even when ROS time jumps backwards

* Remove prev variable

* shorten to (last_publish_ - now).toSec()
---
 costmap_2d/src/costmap_2d_ros.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/costmap_2d/src/costmap_2d_ros.cpp b/costmap_2d/src/costmap_2d_ros.cpp
index 41b6c44b88..59196412e9 100644
--- a/costmap_2d/src/costmap_2d_ros.cpp
+++ b/costmap_2d/src/costmap_2d_ros.cpp
@@ -465,7 +465,8 @@ void Costmap2DROS::mapUpdateLoop(double frequency)
       publisher_->updateBounds(x0, xn, y0, yn);
 
       ros::Time now = ros::Time::now();
-      if (last_publish_ + publish_cycle < now)
+      ROS_WARN_COND(now < last_publish_, "ROS Time jumped backwards by %.3f s. Publishing costmaps anyway.", (last_publish_ - now).toSec());
+      if (now < last_publish_ || last_publish_ + publish_cycle < now)
       {
         publisher_->publishCostmap();
         last_publish_ = now;