Jazzy sync Jan 23, 2026

nav2_amcl/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_amcl</name>
-  <version>1.3.10</version>
+  <version>1.3.11</version>
   <description>
     <p>
       amcl is a probabilistic localization system for a robot moving in

nav2_behavior_tree/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_behavior_tree</name>
-  <version>1.3.10</version>
+  <version>1.3.11</version>
   <description>Nav2 behavior tree wrappers, nodes, and utilities</description>
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>

nav2_behavior_tree/src/behavior_tree_engine.cpp

@@ -64,7 +64,9 @@ BehaviorTreeEngine::run(
 
       result = tree->tickOnce();
 
-      onLoop();
+      if (result == BT::NodeStatus::RUNNING || result == BT::NodeStatus::IDLE) {
+        onLoop();
+      }
 
       if (!loopRate.sleep()) {
         RCLCPP_DEBUG_THROTTLE(

nav2_behaviors/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_behaviors</name>
-  <version>1.3.10</version>
+  <version>1.3.11</version>
   <description>Nav2 behavior server</description>
   <maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

nav2_bringup/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_bringup</name>
-  <version>1.3.10</version>
+  <version>1.3.11</version>
   <description>Bringup scripts and configurations for the Nav2 stack</description>
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

nav2_bt_navigator/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_bt_navigator</name>
-  <version>1.3.10</version>
+  <version>1.3.11</version>
   <description>Nav2 BT Navigator Server</description>
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <license>Apache-2.0</license>

nav2_bt_navigator/src/navigators/navigate_through_poses.cpp

@@ -17,6 +17,7 @@
 #include <set>
 #include <memory>
 #include <limits>
+#include <stdexcept>
 #include "nav2_bt_navigator/navigators/navigate_through_poses.hpp"
 
 namespace nav2_bt_navigator
@@ -203,11 +204,9 @@ NavigateThroughPosesNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal)
     // if pending goal has an empty behavior_tree field, it requests the default BT file
     // accept the pending goal if the current goal is running the default BT file
     if (!initializeGoalPoses(bt_action_server_->acceptPendingGoal())) {
-      RCLCPP_WARN(
-        logger_,
+      throw std::runtime_error(
         "Preemption request was rejected since the goal poses could not be "
-        "transformed. For now, continuing to track the last goal until completion.");
-      bt_action_server_->terminatePendingGoal();
+        "transformed.");
     }
   } else {
     RCLCPP_WARN(

nav2_collision_monitor/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_collision_monitor</name>
-  <version>1.3.10</version>
+  <version>1.3.11</version>
   <description>Collision Monitor</description>
   <maintainer email="alexey.merzlyakov@samsung.com">Alexey Merzlyakov</maintainer>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

nav2_common/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_common</name>
-  <version>1.3.10</version>
+  <version>1.3.11</version>
   <description>Common support functionality used throughout the navigation 2 stack</description>
   <maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>
   <license>Apache-2.0</license>

nav2_constrained_smoother/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_constrained_smoother</name>
-  <version>1.3.10</version>
+  <version>1.3.11</version>
   <description>Ceres constrained smoother</description>
   <maintainer email="vargovcik@robotechvision.com">Matej Vargovcik</maintainer>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

nav2_controller/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_controller</name>
-  <version>1.3.10</version>
+  <version>1.3.11</version>
   <description>Controller action interface</description>
   <maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>
   <license>Apache-2.0</license>

nav2_controller/src/controller_server.cpp

@@ -159,6 +159,9 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
   for (size_t i = 0; i != progress_checker_ids_.size(); i++) {
     progress_checker_ids_concat_ += progress_checker_ids_[i] + std::string(" ");
   }
+  if (progress_checker_ids_concat_.empty()) {
+    progress_checker_ids_concat_ = "(none)";
+  }
 
   RCLCPP_INFO(
     get_logger(),
@@ -417,6 +420,22 @@ bool ControllerServer::findProgressCheckerId(
   const std::string & c_name,
   std::string & current_progress_checker)
 {
+  if (progress_checkers_.size() == 0) {
+    if (c_name.empty()) {
+      RCLCPP_DEBUG(
+        get_logger(),
+        "No progress checker configured and none requested. Progress checking will be bypassed.");
+      current_progress_checker = "";
+      return true;
+    } else {
+      RCLCPP_ERROR(
+        get_logger(), "FollowPath called with progress_checker name %s in parameter"
+        " 'current_progress_checker', but no progress checkers are configured.",
+        c_name.c_str());
+      return false;
+    }
+  }
+
   if (progress_checkers_.find(c_name) == progress_checkers_.end()) {
     if (progress_checkers_.size() == 1 && c_name.empty()) {
       RCLCPP_WARN_ONCE(
@@ -476,7 +495,9 @@ void ControllerServer::computeControl()
     }
 
     setPlannerPath(goal->path);
-    progress_checkers_[current_progress_checker_]->reset();
+    if (!current_progress_checker_.empty()) {
+      progress_checkers_[current_progress_checker_]->reset();
+    }
 
     last_valid_cmd_time_ = now();
     rclcpp::WallRate loop_rate(controller_frequency_);
@@ -525,6 +546,7 @@ void ControllerServer::computeControl()
           get_logger(),
           "Control loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz.",
           controller_frequency_, 1 / cycle_duration.seconds());
+        loop_rate.reset();
       }
     }
   } catch (nav2_core::InvalidController & e) {
@@ -629,8 +651,10 @@ void ControllerServer::computeAndPublishVelocity()
     throw nav2_core::ControllerTFError("Failed to obtain robot pose");
   }
 
-  if (!progress_checkers_[current_progress_checker_]->check(pose)) {
-    throw nav2_core::FailedToMakeProgress("Failed to make progress");
+  if (!current_progress_checker_.empty()) {
+    if (!progress_checkers_[current_progress_checker_]->check(pose)) {
+      throw nav2_core::FailedToMakeProgress("Failed to make progress");
+    }
   }
 
   nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist());
@@ -729,7 +753,9 @@ void ControllerServer::updateGlobalPath()
           get_logger(), "Change of progress checker %s requested, resetting it",
           goal->progress_checker_id.c_str());
         current_progress_checker_ = current_progress_checker;
-        progress_checkers_[current_progress_checker_]->reset();
+        if (!current_progress_checker_.empty()) {
+          progress_checkers_[current_progress_checker_]->reset();
+        }
       }
     } else {
       RCLCPP_INFO(

nav2_core/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_core</name>
-  <version>1.3.10</version>
+  <version>1.3.11</version>
   <description>A set of headers for plugins core to the Nav2 stack</description>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
   <maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp

@@ -122,19 +122,11 @@ class Costmap2DPublisher
   }
 
   /**
-   * @brief  Publishes the visualization data over ROS
+   * @brief Publishes the visualization data over ROS
+   * @note Only publishes when the associated layer is enabled
    */
   void publishCostmap();
 
-  /**
-   * @brief Check if the publisher is active
-   * @return True if the frequency for the publisher is non-zero, false otherwise
-   */
-  bool active()
-  {
-    return active_;
-  }
-
 private:
   /** @brief Prepare grid_ message for publication. */
   void prepareGrid();
@@ -165,7 +157,6 @@ class Costmap2DPublisher
   unsigned int x0_, xn_, y0_, yn_;
   double saved_origin_x_;
   double saved_origin_y_;
-  bool active_;
   bool always_send_full_costmap_;
   double map_vis_z_;
 

nav2_costmap_2d/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format ="3">
   <name>nav2_costmap_2d</name>
-  <version>1.3.10</version>
+  <version>1.3.11</version>
   <description>
     This package provides an implementation of a 2D costmap that takes in sensor
     data from the world, builds a 2D or 3D occupancy grid of the data (depending

nav2_costmap_2d/src/costmap_2d_publisher.cpp

@@ -37,6 +37,7 @@
  *         David V. Lu!!
  *********************************************************************/
 #include "nav2_costmap_2d/costmap_2d_publisher.hpp"
+#include "nav2_costmap_2d/costmap_layer.hpp"
 
 #include <string>
 #include <memory>
@@ -59,7 +60,6 @@ Costmap2DPublisher::Costmap2DPublisher(
 : costmap_(costmap),
   global_frame_(global_frame),
   topic_name_(topic_name),
-  active_(false),
   always_send_full_costmap_(always_send_full_costmap),
   map_vis_z_(map_vis_z)
 {
@@ -232,6 +232,11 @@ std::unique_ptr<nav2_msgs::msg::CostmapUpdate> Costmap2DPublisher::createCostmap
 
 void Costmap2DPublisher::publishCostmap()
 {
+  auto const costmap_layer = dynamic_cast<CostmapLayer *>(costmap_);
+  if (costmap_layer != nullptr && !costmap_layer->isEnabled()) {
+    return;
+  }
+
   float resolution = costmap_->getResolution();
   if (always_send_full_costmap_ || grid_resolution_ != resolution ||
     grid_width_ != costmap_->getSizeInCellsX() ||

nav2_costmap_2d/src/costmap_layer.cpp

@@ -57,6 +57,12 @@ void CostmapLayer::matchSize()
 {
   std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
   Costmap2D * master = layered_costmap_->getCostmap();
+  if (!master) {
+    RCLCPP_WARN(
+      rclcpp::get_logger("nav2_costmap_2d"),
+      "Cannot match size for layer, master costmap is not initialized yet.");
+    return;
+  }
   resizeMap(
     master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
     master->getOriginX(), master->getOriginY());

nav2_costmap_2d/src/layered_costmap.cpp

@@ -44,6 +44,8 @@
 #include <vector>
 #include <limits>
 
+#include <rclcpp/clock.hpp>
+
 #include "nav2_costmap_2d/footprint.hpp"
 
 
@@ -115,7 +117,9 @@ void LayeredCostmap::resizeMap(
   for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin();
     plugin != plugins_.end(); ++plugin)
   {
-    (*plugin)->matchSize();
+    if (*plugin) {
+      (*plugin)->matchSize();
+    }
   }
   for (vector<std::shared_ptr<Layer>>::iterator filter = filters_.begin();
     filter != filters_.end(); ++filter)
@@ -146,9 +150,11 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw)
   }
 
   if (isOutofBounds(robot_x, robot_y)) {
-    RCLCPP_WARN(
+    rclcpp::Clock clock{RCL_ROS_TIME};
+    RCLCPP_WARN_THROTTLE(
       rclcpp::get_logger("nav2_costmap_2d"),
-      "Robot is out of bounds of the costmap!");
+      clock, 5000,
+      "Robot is out of bounds of the costmap");
   }
 
   if (plugins_.size() == 0 && filters_.size() == 0) {

nav2_costmap_2d/test/unit/CMakeLists.txt

@@ -66,4 +66,4 @@ ament_add_gtest(obstacle_layer_test obstacle_layer_test.cpp)
 target_link_libraries(obstacle_layer_test
   ${PROJECT_NAME}::nav2_costmap_2d_core
   layers
-)
+)

nav2_docking/opennav_docking/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>opennav_docking</name>
-  <version>1.3.10</version>
+  <version>1.3.11</version>
   <description>A Task Server for robot charger docking</description>
   <maintainer email="steve@opennav.org">Steve Macenski</maintainer>
   <license>Apache-2.0</license>

nav2_docking/opennav_docking_bt/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>opennav_docking_bt</name>
-  <version>1.3.10</version>
+  <version>1.3.11</version>
   <description>A set of BT nodes and XMLs for docking</description>
   <maintainer email="steve@opennav.org">Steve Macenski</maintainer>
   <license>Apache-2.0</license>

nav2_docking/opennav_docking_core/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>opennav_docking_core</name>
-  <version>1.3.10</version>
+  <version>1.3.11</version>
   <description>A set of headers for plugins core to the opennav docking framework</description>
   <maintainer email="steve@opennav.org">Steve Macenski</maintainer>
   <license>Apache-2.0</license>

nav2_dwb_controller/costmap_queue/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>costmap_queue</name>
-  <version>1.3.10</version>
+  <version>1.3.11</version>
   <description>The costmap_queue package</description>
   <maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

nav2_dwb_controller/dwb_core/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>dwb_core</name>
-  <version>1.3.10</version>
+  <version>1.3.11</version>
   <description>DWB core interfaces package</description>
   <maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>
   <license>BSD-3-Clause</license>

nav2_dwb_controller/dwb_critics/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>dwb_critics</name>
-  <version>1.3.10</version>
+  <version>1.3.11</version>
   <description>The dwb_critics package</description>
   <maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

nav2_dwb_controller/dwb_msgs/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>dwb_msgs</name>
-  <version>1.3.10</version>
+  <version>1.3.11</version>
   <description>Message/Service definitions specifically for the dwb_core</description>
   <maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

nav2_dwb_controller/dwb_plugins/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>dwb_plugins</name>
-  <version>1.3.10</version>
+  <version>1.3.11</version>
   <description>
       Standard implementations of the GoalChecker
       and TrajectoryGenerators for dwb_core