Fix costmap update not being published on force updates (#93)

rapyuta-robotics · Feb 1, 2024 · 8f79b4f · 8f79b4f
1 parent a603882
commit 8f79b4f
Showing 1 changed file with 7 additions and 4 deletions.
diff --git 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()