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

nav2_controller/include/nav2_controller/controller_server.hpp

@@ -193,6 +193,12 @@ class ControllerServer : public nav2::LifecycleNode
    * @brief Called on goal exit
    */
   void onGoalExit(bool force_stop);
+  /**
+   * @brief Wait for costmap to become current, with timeout
+   * @return Duration in seconds spent waiting for the costmap (0.0 if already current)
+   * @throw nav2_core::ControllerTimedOut if costmap update times out
+   */
+  double waitForCostmap();
   /**
    * @brief Checks if goal is reached
    * @return true or false

nav2_controller/src/controller_server.cpp

@@ -516,14 +516,7 @@ void ControllerServer::computeControl()
       }
 
       // Don't compute a trajectory until costmap is valid (after clear costmap)
-      rclcpp::Rate r(100);
-      auto waiting_start = now();
-      while (!costmap_ros_->isCurrent()) {
-        if (now() - waiting_start > params_->costmap_update_timeout) {
-          throw nav2_core::ControllerTimedOut("Costmap timed out waiting for update");
-        }
-        r.sleep();
-      }
+      double costmap_wait = waitForCostmap();
 
       updateGlobalPath();
 
@@ -538,8 +531,11 @@ void ControllerServer::computeControl()
       if (!loop_rate.sleep()) {
         RCLCPP_WARN(
           get_logger(),
-          "Control loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz.",
-          params_->controller_frequency, 1 / cycle_duration.seconds());
+          "Control loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz."
+          "%s",
+          params_->controller_frequency, 1 / cycle_duration.seconds(),
+          costmap_wait > 0.0 ?
+          (" Waited " + std::to_string(costmap_wait) + "s for costmap update.").c_str() : "");
         loop_rate.reset();
       }
     }
@@ -625,6 +621,23 @@ void ControllerServer::computeControl()
   action_server_->succeeded_current();
 }
 
+double ControllerServer::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::ControllerTimedOut(ex.what());
+    }
+    if (was_waiting) {
+      return (now() - waiting_start).seconds();
+    }
+  }
+  return 0.0;
+}
+
 void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path)
 {
   RCLCPP_DEBUG(

nav2_costmap_2d/plugins/static_layer.cpp

@@ -298,6 +298,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/include/nav2_planner/planner_server.hpp

@@ -137,8 +137,9 @@ class PlannerServer : public nav2::LifecycleNode
   /**
    * @brief Wait for costmap to be valid with updated sensor data or repopulate after a
    * clearing recovery. Blocks until true without timeout.
+   * @return Duration in seconds spent waiting for the costmap (0.0 if already current)
    */
-  void waitForCostmap();
+  double waitForCostmap();
 
   /**
    * @brief Check if an action server has a preemption request and replaces the goal

nav2_planner/src/planner_server.cpp

@@ -243,15 +243,21 @@ bool PlannerServer::isServerInactive(
   return false;
 }
 
-void PlannerServer::waitForCostmap()
+double 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) {
+      return (now() - waiting_start).seconds();
+    }
   }
+  return 0.0;
 }
 
 template<typename T>
@@ -348,7 +354,7 @@ void PlannerServer::computePlanThroughPoses()
       return;
     }
 
-    waitForCostmap();
+    double costmap_wait = waitForCostmap();
 
     getPreemptedGoalIfRequested<ActionThroughPoses>(action_server_poses_, goal);
 
@@ -443,8 +449,11 @@ void PlannerServer::computePlanThroughPoses()
     if (params_->max_planner_duration && cycle_duration.seconds() > params_->max_planner_duration) {
       RCLCPP_WARN(
         get_logger(),
-        "Planner loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz",
-        1 / params_->max_planner_duration, 1 / cycle_duration.seconds());
+        "Planner loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz"
+        "%s",
+        1 / params_->max_planner_duration, 1 / cycle_duration.seconds(),
+        costmap_wait > 0.0 ?
+        (" Waited " + std::to_string(costmap_wait) + "s for costmap update.").c_str() : "");
     }
 
     action_server_poses_->succeeded_current(result);
@@ -516,7 +525,7 @@ PlannerServer::computePlan()
       return;
     }
 
-    waitForCostmap();
+    double costmap_wait = waitForCostmap();
 
     getPreemptedGoalIfRequested<ActionToPose>(action_server_pose_, goal);
 
@@ -550,8 +559,11 @@ PlannerServer::computePlan()
     if (params_->max_planner_duration && cycle_duration.seconds() > params_->max_planner_duration) {
       RCLCPP_WARN(
         get_logger(),
-        "Planner loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz",
-        1 / params_->max_planner_duration, 1 / cycle_duration.seconds());
+        "Planner loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz"
+        "%s",
+        1 / params_->max_planner_duration, 1 / cycle_duration.seconds(),
+        costmap_wait > 0.0 ?
+        (" Waited " + std::to_string(costmap_wait) + "s for costmap update.").c_str() : "");
     }
     action_server_pose_->succeeded_current(result);
   } catch (nav2_core::InvalidPlanner & ex) {