Set StaticLayer to non-current as soon as an update is received

nav2_controller/src/controller_server.cpp

@@ -518,12 +518,18 @@ void ControllerServer::computeControl()
       // Don't compute a trajectory until costmap is valid (after clear costmap)
       rclcpp::Rate r(100);
       auto waiting_start = now();
+      bool was_waiting = !costmap_ros_->isCurrent();
       while (!costmap_ros_->isCurrent()) {
         if (now() - waiting_start > params_->costmap_update_timeout) {
           throw nav2_core::ControllerTimedOut("Costmap timed out waiting for update");
         }
         r.sleep();
       }
+      if (was_waiting) {
+        RCLCPP_INFO(
+          get_logger(), "Local costmap became current after %.3f s",
+          (now() - waiting_start).seconds());
+      }
 
       updateGlobalPath();
 

nav2_costmap_2d/plugins/static_layer.cpp

@@ -290,6 +290,7 @@ StaticLayer::incomingMap(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & ne
   }
   std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
   map_buffer_ = new_map;
+  current_ = false;
 }
 
 void

nav2_planner/src/planner_server.cpp

@@ -246,11 +246,18 @@ bool PlannerServer::isServerInactive(
 void PlannerServer::waitForCostmap()
 {
   if (params_->costmap_update_timeout > rclcpp::Duration(0, 0)) {
+    auto waiting_start = now();
+    bool was_waiting = !costmap_ros_->isCurrent();
     try {
       costmap_ros_->waitUntilCurrent(params_->costmap_update_timeout);
     } catch (const std::runtime_error & ex) {
       throw nav2_core::PlannerTimedOut(ex.what());
     }
+    if (was_waiting) {
+      RCLCPP_INFO(
+        get_logger(), "Global costmap became current after %.3f s",
+        (now() - waiting_start).seconds());
+    }
   }
 }