From 8f79b4f0443051d66f31a597c38477af7dca95bb Mon Sep 17 00:00:00 2001
From: Tiago Silva <15384781+siferati@users.noreply.github.com>
Date: Thu, 1 Feb 2024 09:15:16 +0900
Subject: [PATCH] Fix costmap update not being published on force updates (#93)

---
 costmap_2d/src/costmap_2d_ros.cpp | 11 +++++++----
 1 file changed, 7 insertions(+), 4 deletions(-)

diff --git a/costmap_2d/src/costmap_2d_ros.cpp b/costmap_2d/src/costmap_2d_ros.cpp
index 2d30d89e9..0ffac66de 100644
--- a/costmap_2d/src/costmap_2d_ros.cpp
+++ b/costmap_2d/src/costmap_2d_ros.cpp
@@ -460,10 +460,6 @@ void Costmap2DROS::mapUpdateLoop(double frequency)
 
     if (publish_cycle.toSec() > 0 && layered_costmap_->isInitialized())
     {
-      unsigned int x0, y0, xn, yn;
-      layered_costmap_->getBounds(&x0, &xn, &y0, &yn);
-      publisher_->updateBounds(x0, xn, y0, yn);
-
       ros::Time now = ros::Time::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)
@@ -504,6 +500,13 @@ void Costmap2DROS::updateMap()
       initialized_ = true;
     }
   }
+
+  if (publish_cycle.toSec() > 0 && layered_costmap_->isInitialized())
+  {
+    unsigned int x0, y0, xn, yn;
+    layered_costmap_->getBounds(&x0, &xn, &y0, &yn);
+    publisher_->updateBounds(x0, xn, y0, yn);
+  }
 }
 
 void Costmap2DROS::start()