Sync Jazzy Aug 19, 2025 1.3.8

.circleci/config.yml

@@ -532,7 +532,7 @@ jobs:
 
 _parameters:
   release_parameters: &release_parameters
-    packages_skip_regex: "nav2_system_tests"
+    packages_skip_regex: "'(nav2_system_tests|nav2_smac_planner|nav2_mppi_controller|nav2_route|nav2_rviz_plugins|nav2_rotation_shim_controller|nav2_waypoint_follower|nav2_smoother|opennav_docking|nav2_behaviors|nav2_bringup|navigation2)'"
 
 workflows:
   version: 2

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.7</version>
+  <version>1.3.8</version>
   <description>
     <p>
       amcl is a probabilistic localization system for a robot moving in

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.hpp

@@ -29,7 +29,7 @@ class WouldAPlannerRecoveryHelp : public AreErrorCodesPresent
   using Action = nav2_msgs::action::ComputePathToPose;
   using ActionResult = Action::Result;
   using ThroughAction = nav2_msgs::action::ComputePathThroughPoses;
-  using ThroughActionResult = Action::Result;
+  using ThroughActionResult = ThroughAction::Result;
 
 public:
   WouldAPlannerRecoveryHelp(

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.7</version>
+  <version>1.3.8</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_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.7</version>
+  <version>1.3.8</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.7</version>
+  <version>1.3.8</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.7</version>
+  <version>1.3.8</version>
   <description>Nav2 BT Navigator Server</description>
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <license>Apache-2.0</license>

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.7</version>
+  <version>1.3.8</version>
   <description>Collision Monitor</description>
   <maintainer email="alexey.merzlyakov@samsung.com">Alexey Merzlyakov</maintainer>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

nav2_collision_monitor/src/collision_monitor_node.cpp

@@ -548,13 +548,17 @@ bool CollisionMonitor::processStopSlowdownLimit(
       const double linear_vel = std::hypot(velocity.x, velocity.y);  // absolute
       Velocity safe_vel;
       double ratio = 1.0;
+
+      // Calculate the most restrictive ratio to preserve curvature
       if (linear_vel != 0.0) {
-        ratio = std::clamp(polygon->getLinearLimit() / linear_vel, 0.0, 1.0);
+        ratio = std::min(ratio, polygon->getLinearLimit() / linear_vel);
+      }
+      if (velocity.tw != 0.0) {
+        ratio = std::min(ratio, polygon->getAngularLimit() / std::abs(velocity.tw));
       }
-      safe_vel.x = velocity.x * ratio;
-      safe_vel.y = velocity.y * ratio;
-      safe_vel.tw = std::clamp(
-        velocity.tw, -polygon->getAngularLimit(), polygon->getAngularLimit());
+      ratio = std::clamp(ratio, 0.0, 1.0);
+      // Apply the same ratio to all components to preserve curvature
+      safe_vel = velocity * ratio;
       // Check that currently calculated velocity is safer than
       // chosen for previous shapes one
       if (safe_vel < robot_action.req_vel) {

nav2_collision_monitor/test/collision_monitor_node_test.cpp

@@ -820,10 +820,12 @@ TEST_F(Tester, testProcessStopSlowdownLimit)
   publishCmdVel(0.5, 0.2, 0.1);
   ASSERT_TRUE(waitCmdVel(500ms));
   const double speed = std::sqrt(0.5 * 0.5 + 0.2 * 0.2);
-  const double ratio = LINEAR_LIMIT / speed;
+  const double linear_ratio = LINEAR_LIMIT / speed;
+  const double angular_ratio = ANGULAR_LIMIT / 0.1;
+  const double ratio = std::min(linear_ratio, angular_ratio);
   ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5 * ratio, EPSILON);
   ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2 * ratio, EPSILON);
-  ASSERT_NEAR(cmd_vel_out_->angular.z, 0.09, EPSILON);
+  ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1 * ratio, EPSILON);
   ASSERT_TRUE(waitActionState(500ms));
   ASSERT_EQ(action_state_->action_type, LIMIT);
   ASSERT_EQ(action_state_->polygon_name, "Limit");
@@ -905,10 +907,12 @@ TEST_F(Tester, testPolygonSource)
   publishCmdVel(0.5, 0.2, 0.1);
   EXPECT_TRUE(waitCmdVel(500ms));
   const double speed = std::sqrt(0.5 * 0.5 + 0.2 * 0.2);
-  const double ratio = LINEAR_LIMIT / speed;
+  const double linear_ratio = LINEAR_LIMIT / speed;
+  const double angular_ratio = ANGULAR_LIMIT / 0.1;
+  const double ratio = std::min(linear_ratio, angular_ratio);
   EXPECT_NEAR(cmd_vel_out_->linear.x, 0.5 * ratio, EPSILON);
   EXPECT_NEAR(cmd_vel_out_->linear.y, 0.2 * ratio, EPSILON);
-  EXPECT_NEAR(cmd_vel_out_->angular.z, 0.09, EPSILON);
+  EXPECT_NEAR(cmd_vel_out_->angular.z, 0.1 * ratio, EPSILON);
   EXPECT_TRUE(waitActionState(500ms));
   EXPECT_EQ(action_state_->action_type, LIMIT);
   EXPECT_EQ(action_state_->polygon_name, "Limit");

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.7</version>
+  <version>1.3.8</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.7</version>
+  <version>1.3.8</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.7</version>
+  <version>1.3.8</version>
   <description>Controller action interface</description>
   <maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>
   <license>Apache-2.0</license>

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.7</version>
+  <version>1.3.8</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/static_layer.hpp

@@ -154,6 +154,15 @@ class StaticLayer : public CostmapLayer
    */
   unsigned char interpretValue(unsigned char value);
 
+  /**
+   * @brief Check if two double values are equal within a given epsilon
+   * @param a First double value
+   * @param b Second double value
+   * @param epsilon The tolerance for equality check
+   * @return True if the values are equal within the tolerance, false otherwise
+   */
+  bool isEqual(double a, double b, double epsilon);
+
   /**
    * @brief Callback executed when a parameter change is detected
    * @param event ParameterEvent message

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.7</version>
+  <version>1.3.8</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/plugins/costmap_filters/costmap_filter.cpp

@@ -218,7 +218,7 @@ unsigned char CostmapFilter::getMaskCost(
 {
   const unsigned int index = my * filter_mask->info.width + mx;
 
-  const char data = filter_mask->data[index];
+  const signed char data = filter_mask->data[index];
   if (data == nav2_util::OCC_GRID_UNKNOWN) {
     return NO_INFORMATION;
   } else {

nav2_costmap_2d/plugins/static_layer.cpp

@@ -47,6 +47,8 @@
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "nav2_util/validate_messages.hpp"
 
+#define EPSILON 1e-5
+
 PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::StaticLayer, nav2_costmap_2d::Layer)
 
 using nav2_costmap_2d::NO_INFORMATION;
@@ -196,9 +198,9 @@ StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map)
   Costmap2D * master = layered_costmap_->getCostmap();
   if (!layered_costmap_->isRolling() && (master->getSizeInCellsX() != size_x ||
     master->getSizeInCellsY() != size_y ||
-    master->getResolution() != new_map.info.resolution ||
-    master->getOriginX() != new_map.info.origin.position.x ||
-    master->getOriginY() != new_map.info.origin.position.y ||
+    !isEqual(master->getResolution(), new_map.info.resolution, EPSILON) ||
+    !isEqual(master->getOriginX(), new_map.info.origin.position.x, EPSILON) ||
+    !isEqual(master->getOriginY(), new_map.info.origin.position.y, EPSILON) ||
     !layered_costmap_->isSizeLocked()))
   {
     // Update the size of the layered costmap (and all layers, including this one)
@@ -212,9 +214,9 @@ StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map)
       new_map.info.origin.position.y,
       true);
   } else if (size_x_ != size_x || size_y_ != size_y ||  // NOLINT
-    resolution_ != new_map.info.resolution ||
-    origin_x_ != new_map.info.origin.position.x ||
-    origin_y_ != new_map.info.origin.position.y)
+    !isEqual(resolution_, new_map.info.resolution, EPSILON) ||
+    !isEqual(origin_x_, new_map.info.origin.position.x, EPSILON) ||
+    !isEqual(origin_y_, new_map.info.origin.position.y, EPSILON))
   {
     // only update the size of the costmap stored locally in this layer
     RCLCPP_INFO(
@@ -467,6 +469,18 @@ StaticLayer::updateCosts(
   current_ = true;
 }
 
+/**
+  * @brief Check if two floating point numbers are equal within a given epsilon
+  * @param a First number
+  * @param b Second number
+  * @param epsilon Tolerance for equality check
+  * @return True if numbers are equal within the tolerance, false otherwise
+  */
+bool StaticLayer::isEqual(double a, double b, double epsilon)
+{
+  return std::abs(a - b) < epsilon;
+}
+
 /**
   * @brief Callback executed when a parameter change is detected
   * @param event ParameterEvent message

nav2_costmap_2d/plugins/voxel_layer.cpp

@@ -64,6 +64,7 @@ void VoxelLayer::onInitialize()
 
   declareParameter("enabled", rclcpp::ParameterValue(true));
   declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(true));
+  declareParameter("min_obstacle_height", rclcpp::ParameterValue(0.0));
   declareParameter("max_obstacle_height", rclcpp::ParameterValue(2.0));
   declareParameter("z_voxels", rclcpp::ParameterValue(10));
   declareParameter("origin_z", rclcpp::ParameterValue(0.0));
@@ -80,6 +81,7 @@ void VoxelLayer::onInitialize()
 
   node->get_parameter(name_ + "." + "enabled", enabled_);
   node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_);
+  node->get_parameter(name_ + "." + "min_obstacle_height", min_obstacle_height_);
   node->get_parameter(name_ + "." + "max_obstacle_height", max_obstacle_height_);
   node->get_parameter(name_ + "." + "z_voxels", size_z_);
   node->get_parameter(name_ + "." + "origin_z", origin_z_);
@@ -195,6 +197,11 @@ void VoxelLayer::updateBounds(
     sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
 
     for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
+      // if the obstacle is too low, we won't add it
+      if (*iter_z < min_obstacle_height_) {
+        continue;
+      }
+
       // if the obstacle is too high or too far away from the robot we won't add it
       if (*iter_z > max_obstacle_height_) {
         continue;
@@ -344,31 +351,47 @@ void VoxelLayer::raytraceFreespace(
     double b = wpy - oy;
     double c = wpz - oz;
     double t = 1.0;
+    bool wp_outside = false;
 
     // we can only raytrace to a maximum z height
     if (wpz > map_end_z) {
       // we know we want the vector's z value to be max_z
       t = std::max(0.0, std::min(t, (map_end_z - 0.01 - oz) / c));
+      wp_outside = true;
     } else if (wpz < origin_z_) {
       // and we can only raytrace down to the floor
       // we know we want the vector's z value to be 0.0
       t = std::min(t, (origin_z_ - oz) / c);
+      wp_outside = true;
     }
 
     // the minimum value to raytrace from is the origin
     if (wpx < origin_x_) {
       t = std::min(t, (origin_x_ - ox) / a);
+      wp_outside = true;
     }
     if (wpy < origin_y_) {
       t = std::min(t, (origin_y_ - oy) / b);
+      wp_outside = true;
     }
 
     // the maximum value to raytrace to is the end of the map
     if (wpx > map_end_x) {
       t = std::min(t, (map_end_x - ox) / a);
+      wp_outside = true;
     }
     if (wpy > map_end_y) {
       t = std::min(t, (map_end_y - oy) / b);
+      wp_outside = true;
+    }
+
+    constexpr double wp_epsilon = 1e-5;
+    if (wp_outside) {
+      if (t > 0.0) {
+        t -= wp_epsilon;
+      } else if (t < 0.0) {
+        t += wp_epsilon;
+      }
     }
 
     wpx = ox + a * t;
@@ -498,7 +521,9 @@ VoxelLayer::dynamicParametersCallback(
     const auto & param_name = parameter.get_name();
 
     if (param_type == ParameterType::PARAMETER_DOUBLE) {
-      if (param_name == name_ + "." + "max_obstacle_height") {
+      if (param_name == name_ + "." + "min_obstacle_height") {
+        min_obstacle_height_ = parameter.as_double();
+      } else if (param_name == name_ + "." + "max_obstacle_height") {
         max_obstacle_height_ = parameter.as_double();
       } else if (param_name == name_ + "." + "origin_z") {
         origin_z_ = parameter.as_double();

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.7</version>
+  <version>1.3.8</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/src/docking_server.cpp

@@ -308,7 +308,7 @@ void DockingServer::dockRobot()
       } catch (opennav_docking_core::DockingException & e) {
         if (++num_retries_ > max_retries_) {
           RCLCPP_ERROR(get_logger(), "Failed to dock, all retries have been used");
-          throw e;
+          throw;
         }
         RCLCPP_WARN(get_logger(), "Docking failed, will retry: %s", e.what());
       }

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.7</version>
+  <version>1.3.8</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.7</version>
+  <version>1.3.8</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.7</version>
+  <version>1.3.8</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.7</version>
+  <version>1.3.8</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.7</version>
+  <version>1.3.8</version>
   <description>The dwb_critics package</description>
   <maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>