From c2549482ff775fd6c57348a47c67db1ef32c6693 Mon Sep 17 00:00:00 2001
From: Akihiko Komada <aki1770@gmail.com>
Date: Mon, 13 Apr 2026 01:27:18 +0900
Subject: [PATCH 01/15] fix(smac_planner): check oriented footprint collision
 in smoother for non-circular robots

The SMAC path smoother was checking only the centre-point cost of each pose
after gradient descent, which allowed smoothed paths to produce lethal oriented
footprint collisions for non-circular robots (wide rectangular footprints
passing diagonally through narrow corridors pass the centre-cost check but
fail a proper footprint check).

Add an optional `footprint` parameter (default empty = circular / no change in
behaviour) to `Smoother::smooth()` and `Smoother::smoothImpl()`.  When a
footprint is provided, a `FootprintCollisionChecker` pass runs over the
smoothed path after `updateApproximatePathOrientations()`; any pose whose
oriented cost exceeds MAX_NON_OBSTACLE_COST triggers a warning and returns
false, leaving the original path unchanged.

The three planner plugins (Hybrid, 2D, Lattice) forward the costmap robot
footprint to the smoother so non-circular robots benefit automatically.

Regression test added: `test_footprint_collision_detection` sets up a corridor
with lethal walls, confirms a narrow footprint passes and a wide footprint
(1.2 m) correctly fails.

Fixes #5330

Co-Authored-By: Claude and aki1770-del <aki1770@gmail.com>
Signed-off-by: Akihiko Komada <aki1770@gmail.com>
---
 .../include/nav2_smac_planner/smoother.hpp    | 11 ++-
 nav2_smac_planner/src/smac_planner_2d.cpp     |  2 +-
 nav2_smac_planner/src/smac_planner_hybrid.cpp |  2 +-
 .../src/smac_planner_lattice.cpp              |  2 +-
 nav2_smac_planner/src/smoother.cpp            | 38 ++++++++--
 nav2_smac_planner/test/test_smoother.cpp      | 72 +++++++++++++++++++
 6 files changed, 118 insertions(+), 9 deletions(-)

diff --git a/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp
index b4a65be8e48..d01ee9f01b3 100644
--- a/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp
@@ -18,6 +18,7 @@
 #include <vector>
 
 #include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_costmap_2d/footprint_collision_checker.hpp"
 #include "nav2_smac_planner/types.hpp"
 #include "nav2_smac_planner/constants.hpp"
 #include "nav2_util/geometry_utils.hpp"
@@ -92,12 +93,16 @@ class Smoother
    * @param path Reference to path
    * @param costmap Pointer to minimal costmap
    * @param max_time Maximum time to compute, stop early if over limit
+   * @param footprint Robot footprint for oriented collision checking (non-circular robots).
+   *        When non-empty, an oriented footprint collision check is performed on the
+   *        smoothed path after orientation assignment. Empty footprint skips the check.
    * @return If smoothing was successful
    */
   bool smooth(
     nav_msgs::msg::Path & path,
     const nav2_costmap_2d::Costmap2D * costmap,
-    const double & max_time);
+    const double & max_time,
+    const nav2_costmap_2d::Footprint & footprint = {});
 
 protected:
   /**
@@ -106,13 +111,15 @@ class Smoother
    * @param reversing_segment Return if this is a reversing segment
    * @param costmap Pointer to minimal costmap
    * @param max_time Maximum time to compute, stop early if over limit
+   * @param footprint Robot footprint for oriented collision checking (non-circular robots)
    * @return If smoothing was successful
    */
   bool smoothImpl(
     nav_msgs::msg::Path & path,
     bool & reversing_segment,
     const nav2_costmap_2d::Costmap2D * costmap,
-    const double & max_time);
+    const double & max_time,
+    const nav2_costmap_2d::Footprint & footprint = {});
 
   /**
    * @brief Get the field value for a given dimension
diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp
index a62641da414..72f9c39e432 100644
--- a/nav2_smac_planner/src/smac_planner_2d.cpp
+++ b/nav2_smac_planner/src/smac_planner_2d.cpp
@@ -320,7 +320,7 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan(
 #endif
 
   // Smooth plan
-  _smoother->smooth(plan, costmap, time_remaining);
+  _smoother->smooth(plan, costmap, time_remaining, _costmap_ros->getRobotFootprint());
 
   // If use_final_approach_orientation=true, interpolate the last pose orientation from the
   // previous pose to set the orientation to the 'final approach' orientation of the robot so
diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp
index c369bfceb37..b2d00b9ca6a 100644
--- a/nav2_smac_planner/src/smac_planner_hybrid.cpp
+++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp
@@ -545,7 +545,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
 
   // Smooth plan
   if (_smoother && num_iterations > 1) {
-    _smoother->smooth(plan, costmap, time_remaining);
+    _smoother->smooth(plan, costmap, time_remaining, _costmap_ros->getRobotFootprint());
   }
 
 #ifdef BENCHMARK_TESTING
diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp
index 64360a8be64..04cf635e7b0 100644
--- a/nav2_smac_planner/src/smac_planner_lattice.cpp
+++ b/nav2_smac_planner/src/smac_planner_lattice.cpp
@@ -496,7 +496,7 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan(
 
   // Smooth plan
   if (_smoother && num_iterations > 1) {
-    _smoother->smooth(plan, _costmap, time_remaining);
+    _smoother->smooth(plan, _costmap, time_remaining, _costmap_ros->getRobotFootprint());
   }
 
 #ifdef BENCHMARK_TESTING
diff --git a/nav2_smac_planner/src/smoother.cpp b/nav2_smac_planner/src/smoother.cpp
index 6e722f61e3e..e5db1c51829 100644
--- a/nav2_smac_planner/src/smoother.cpp
+++ b/nav2_smac_planner/src/smoother.cpp
@@ -52,7 +52,8 @@ void Smoother::initialize(const double & min_turning_radius)
 bool Smoother::smooth(
   nav_msgs::msg::Path & path,
   const nav2_costmap_2d::Costmap2D * costmap,
-  const double & max_time)
+  const double & max_time,
+  const nav2_costmap_2d::Footprint & footprint)
 {
   // by-pass path orientations approximation when skipping smac smoother
   if (max_its_ == 0) {
@@ -86,7 +87,7 @@ bool Smoother::smooth(
       const geometry_msgs::msg::Pose start_pose = curr_path_segment.poses.front().pose;
       const geometry_msgs::msg::Pose goal_pose = curr_path_segment.poses.back().pose;
       bool local_success =
-        smoothImpl(curr_path_segment, reversing_segment, costmap, time_remaining);
+        smoothImpl(curr_path_segment, reversing_segment, costmap, time_remaining, footprint);
       success = success && local_success;
 
       // Enforce boundary conditions
@@ -110,7 +111,8 @@ bool Smoother::smoothImpl(
   nav_msgs::msg::Path & path,
   bool & reversing_segment,
   const nav2_costmap_2d::Costmap2D * costmap,
-  const double & max_time)
+  const double & max_time,
+  const nav2_costmap_2d::Footprint & footprint)
 {
   steady_clock::time_point a = steady_clock::now();
   rclcpp::Duration max_dur = rclcpp::Duration::from_seconds(max_time);
@@ -192,10 +194,38 @@ bool Smoother::smoothImpl(
   // but really puts the path quality over the top.
   if (do_refinement_ && refinement_ctr_ < refinement_num_) {
     refinement_ctr_++;
-    smoothImpl(new_path, reversing_segment, costmap, max_time);
+    smoothImpl(new_path, reversing_segment, costmap, max_time, footprint);
   }
 
   nav2_util::updateApproximatePathOrientations(new_path, reversing_segment, is_holonomic_);
+
+  // Oriented footprint collision check for non-circular robots (fix for #5330).
+  // The per-iteration cost check above uses center-point cost only, which is
+  // sufficient for circular robots (costmap inflation captures the radius) but
+  // misses orientation-dependent footprint extensions for non-circular robots.
+  // After orientations are assigned we validate each smoothed pose with the full
+  // oriented footprint and revert to the original path if a collision is found.
+  if (!footprint.empty() && costmap) {
+    nav2_costmap_2d::FootprintCollisionChecker<const nav2_costmap_2d::Costmap2D *>
+      checker(costmap);
+    for (const auto & pose_stamped : new_path.poses) {
+      const double yaw = tf2::getYaw(pose_stamped.pose.orientation);
+      const double cost = checker.footprintCostAtPose(
+        pose_stamped.pose.position.x, pose_stamped.pose.position.y,
+        yaw, footprint);
+      if (static_cast<float>(cost) > MAX_NON_OBSTACLE_COST &&
+        static_cast<float>(cost) != UNKNOWN_COST)
+      {
+        RCLCPP_WARN(
+          rclcpp::get_logger("SmacPlannerSmoother"),
+          "Smoothed path produces an oriented footprint collision for a non-circular robot. "
+          "Returning original path.");
+        nav2_util::updateApproximatePathOrientations(path, reversing_segment, is_holonomic_);
+        return false;
+      }
+    }
+  }
+
   path = new_path;
   return true;
 }
diff --git a/nav2_smac_planner/test/test_smoother.cpp b/nav2_smac_planner/test/test_smoother.cpp
index 45d2f629421..9c3c5fb7856 100644
--- a/nav2_smac_planner/test/test_smoother.cpp
+++ b/nav2_smac_planner/test/test_smoother.cpp
@@ -180,6 +180,78 @@ TEST(SmootherTest, test_full_smoother)
   delete costmap;
 }
 
+// Regression test for #5330: SMAC smoother must detect oriented footprint collisions
+// for non-circular robots. A wide rectangular robot travelling diagonally through a
+// narrow corridor may have a clear center-cost path but a colliding oriented footprint.
+TEST(SmootherTest, test_footprint_collision_detection)
+{
+  nav2::LifecycleNode::SharedPtr node =
+    std::make_shared<nav2::LifecycleNode>("SmacSmootherFootprintTest");
+  nav2_smac_planner::SmootherParams params;
+  params.get(node, "test");
+  double maxtime = 1.0;
+
+  auto smoother = std::make_unique<SmootherWrapper>(params);
+  smoother->initialize(0.4);
+
+  // 100x100 costmap, 0.1m/cell → 10m x 10m
+  nav2_costmap_2d::Costmap2D * costmap =
+    new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0);
+
+  // Obstacle walls at y=40..44 and y=56..60 (cells), leaving a 1-cell wide gap at y=45..55.
+  // A circular robot (radius ≤ 0.5m) passes through the gap safely via center-cost check.
+  // A wide rectangular robot (width 1.2m) cannot fit without footprint collision.
+  for (unsigned int i = 0; i < 100; ++i) {
+    for (unsigned int j = 40; j <= 44; ++j) {
+      costmap->setCost(i, j, 254);  // lethal obstacle — lower wall
+    }
+    for (unsigned int j = 56; j <= 60; ++j) {
+      costmap->setCost(i, j, 254);  // lethal obstacle — upper wall
+    }
+  }
+
+  // Path: straight line from (0.5, 5.0) to (9.5, 5.0) — centre of the gap (y=5.0m = cell 50).
+  // Center-cost check passes because cell 50 is obstacle-free.
+  nav_msgs::msg::Path plan;
+  plan.header.frame_id = "map";
+  for (int i = 5; i <= 90; i += 5) {
+    geometry_msgs::msg::PoseStamped p;
+    p.header = plan.header;
+    p.pose.position.x = static_cast<double>(i) * 0.1;
+    p.pose.position.y = 5.0;
+    p.pose.orientation.w = 1.0;
+    plan.poses.push_back(p);
+  }
+
+  // Without footprint: smoothing succeeds (center-cost only).
+  auto plan_copy = plan;
+  EXPECT_TRUE(smoother->smooth(plan_copy, costmap, maxtime));
+
+  // With a narrow footprint (0.4m wide): also succeeds — fits in the gap.
+  nav2_costmap_2d::Footprint narrow_footprint;
+  geometry_msgs::msg::Point pt;
+  pt.x = 0.4; pt.y = 0.2; narrow_footprint.push_back(pt);
+  pt.x = 0.4; pt.y = -0.2; narrow_footprint.push_back(pt);
+  pt.x = -0.4; pt.y = -0.2; narrow_footprint.push_back(pt);
+  pt.x = -0.4; pt.y = 0.2; narrow_footprint.push_back(pt);
+
+  plan_copy = plan;
+  EXPECT_TRUE(smoother->smooth(plan_copy, costmap, maxtime, narrow_footprint));
+
+  // With a wide footprint (1.2m wide): oriented footprint check must detect collision.
+  // The path travels along y=5.0 but the footprint extends ±0.6m in y — hitting the walls.
+  nav2_costmap_2d::Footprint wide_footprint;
+  pt.x = 0.4; pt.y = 0.6; wide_footprint.push_back(pt);
+  pt.x = 0.4; pt.y = -0.6; wide_footprint.push_back(pt);
+  pt.x = -0.4; pt.y = -0.6; wide_footprint.push_back(pt);
+  pt.x = -0.4; pt.y = 0.6; wide_footprint.push_back(pt);
+
+  plan_copy = plan;
+  EXPECT_FALSE(smoother->smooth(plan_copy, costmap, maxtime, wide_footprint));
+
+  delete costmap;
+}
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);

From c0a782b93be23394cb77e0551a088fdf6c1d8c2f Mon Sep 17 00:00:00 2001
From: Akihiko Komada <aki1770@gmail.com>
Date: Mon, 13 Apr 2026 01:41:30 +0900
Subject: [PATCH 02/15] style: fix uncrustify indent on
 FootprintCollisionChecker constructor line

ament_uncrustify requires 4-space indent on continuation of the type declaration,
not 6.  Fixes CI lint failure.

Co-Authored-By: Claude and aki1770-del <aki1770@gmail.com>
Signed-off-by: Akihiko Komada <aki1770@gmail.com>
---
 nav2_smac_planner/src/smoother.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/nav2_smac_planner/src/smoother.cpp b/nav2_smac_planner/src/smoother.cpp
index e5db1c51829..ec9a56d84a7 100644
--- a/nav2_smac_planner/src/smoother.cpp
+++ b/nav2_smac_planner/src/smoother.cpp
@@ -207,7 +207,7 @@ bool Smoother::smoothImpl(
   // oriented footprint and revert to the original path if a collision is found.
   if (!footprint.empty() && costmap) {
     nav2_costmap_2d::FootprintCollisionChecker<const nav2_costmap_2d::Costmap2D *>
-      checker(costmap);
+    checker(costmap);
     for (const auto & pose_stamped : new_path.poses) {
       const double yaw = tf2::getYaw(pose_stamped.pose.orientation);
       const double cost = checker.footprintCostAtPose(

From 2d647c059b79c1df4ada0e0b1a06b7e376b070d6 Mon Sep 17 00:00:00 2001
From: Akihiko Komada <aki1770@gmail.com>
Date: Mon, 13 Apr 2026 12:10:24 +0900
Subject: [PATCH 03/15] fix(smac_planner_2d): store costmap_ros member to fix
 build failure

SmacPlanner2D discarded the costmap_ros shared_ptr after configure(),
storing only the inner Costmap2D pointer.  The footprint collision check
added in the smoother path called _costmap_ros->getRobotFootprint() which
did not exist in this class, causing a compile error in both jazzy and
kilted CI builds.

Store _costmap_ros as a class member (matching SmacPlannerHybrid's
pattern) and assign it in configure() so createPlan() can use it.

Co-Authored-By: Claude and aki1770-del <aki1770@gmail.com>
Signed-off-by: Akihiko Komada <aki1770@gmail.com>
---
 nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp | 1 +
 nav2_smac_planner/src/smac_planner_2d.cpp                       | 1 +
 2 files changed, 2 insertions(+)

diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp
index 5edf22acbf6..1f29ad2c3f3 100644
--- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp
@@ -115,6 +115,7 @@ class SmacPlanner2D : public nav2_core::GlobalPlanner
   GridCollisionChecker _collision_checker;
   std::unique_ptr<Smoother> _smoother;
   nav2_costmap_2d::Costmap2D * _costmap;
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> _costmap_ros;
   std::unique_ptr<CostmapDownsampler> _costmap_downsampler;
   rclcpp::Clock::SharedPtr _clock;
   rclcpp::Logger _logger{rclcpp::get_logger("SmacPlanner2D")};
diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp
index 72f9c39e432..25e8501ea0a 100644
--- a/nav2_smac_planner/src/smac_planner_2d.cpp
+++ b/nav2_smac_planner/src/smac_planner_2d.cpp
@@ -55,6 +55,7 @@ void SmacPlanner2D::configure(
   _logger = node->get_logger();
   _clock = node->get_clock();
   _costmap = costmap_ros->getCostmap();
+  _costmap_ros = costmap_ros;
   _name = name;
   _global_frame = costmap_ros->getGlobalFrameID();
 

From 487ef27bb2d350b63f8d147e1287669f74f2017e Mon Sep 17 00:00:00 2001
From: Akihiko Komada <aki1770@gmail.com>
Date: Mon, 13 Apr 2026 19:11:44 +0900
Subject: [PATCH 04/15] fix(smac_planner): use Costmap2D* instantiation in
 FootprintCollisionChecker
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

FootprintCollisionChecker is only explicitly instantiated for Costmap2D* and
shared_ptr<Costmap2D> in footprint_collision_checker.cpp. Using the const*
variant caused an undefined reference linker error in test_nodehybrid.

Switch to the existing Costmap2D* instantiation with const_cast — safe because
footprintCostAtPose is read-only on the costmap.

Signed-off-by: Akihiko Komada <aki1770@gmail.com>
---
 nav2_smac_planner/src/smoother.cpp | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/nav2_smac_planner/src/smoother.cpp b/nav2_smac_planner/src/smoother.cpp
index ec9a56d84a7..b28eb56e4c9 100644
--- a/nav2_smac_planner/src/smoother.cpp
+++ b/nav2_smac_planner/src/smoother.cpp
@@ -206,8 +206,10 @@ bool Smoother::smoothImpl(
   // After orientations are assigned we validate each smoothed pose with the full
   // oriented footprint and revert to the original path if a collision is found.
   if (!footprint.empty() && costmap) {
-    nav2_costmap_2d::FootprintCollisionChecker<const nav2_costmap_2d::Costmap2D *>
-    checker(costmap);
+    // FootprintCollisionChecker is only instantiated for Costmap2D* (non-const).
+    // const_cast is safe here: footprintCostAtPose only reads the costmap.
+    nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>
+    checker(const_cast<nav2_costmap_2d::Costmap2D *>(costmap));
     for (const auto & pose_stamped : new_path.poses) {
       const double yaw = tf2::getYaw(pose_stamped.pose.orientation);
       const double cost = checker.footprintCostAtPose(

From b76c2bbef369fb6841a0566b4302c565abea9740 Mon Sep 17 00:00:00 2001
From: Akihiko Komada <aki1770@gmail.com>
Date: Mon, 13 Apr 2026 20:14:47 +0900
Subject: [PATCH 05/15] test(smac_planner): add smoother coverage tests for
 hybrid and lattice

Both uncovered lines are inside `if (_smoother && num_iterations > 1)`.
The existing tests never reach smoother creation because their first
configure() calls fail with invalid parameters before line 228/215 where
`_smoother` is allocated.

Add `test_smac_se2_smoother_coverage` and `test_smac_lattice_smoother_coverage`
using fresh unique nodes with `smooth_path: true` via parameter_overrides so
the first (and only) configure() succeeds, creates the smoother, and
createPlan() exercises the `_smoother->smooth(...)` call at lines 542/493.

Also expose `hasSmootherInitialized()` in HybridWrap to assert the smoother
was actually created before planning.

Co-Authored-By: Claude and aki1770-del <aki1770@gmail.com>
Signed-off-by: Akihiko Komada <aki1770@gmail.com>
---
 nav2_smac_planner/test/test_smac_hybrid.cpp  | 48 ++++++++++++++++++++
 nav2_smac_planner/test/test_smac_lattice.cpp | 43 ++++++++++++++++++
 2 files changed, 91 insertions(+)

diff --git a/nav2_smac_planner/test/test_smac_hybrid.cpp b/nav2_smac_planner/test/test_smac_hybrid.cpp
index 6497677243d..da19ede4233 100644
--- a/nav2_smac_planner/test/test_smac_hybrid.cpp
+++ b/nav2_smac_planner/test/test_smac_hybrid.cpp
@@ -60,6 +60,11 @@ class HybridWrap : public nav2_smac_planner::SmacPlannerHybrid
   {
     return _goal_heading_mode;
   }
+
+  bool hasSmootherInitialized()
+  {
+    return _smoother != nullptr;
+  }
 };
 
 // SMAC smoke tests for plugin-level issues rather than algorithms
@@ -383,6 +388,49 @@ TEST(SmacTest, test_smac_se2_reconfigure)
   EXPECT_EQ(nodeSE2->get_parameter("test.downsampling_factor").as_int(), 2);
 }
 
+TEST(SmacTest, test_smac_se2_smoother_coverage)
+{
+  rclcpp::NodeOptions options;
+  options.parameter_overrides(
+    {rclcpp::Parameter("test.smooth_path", true)});
+  nav2::LifecycleNode::SharedPtr node =
+    std::make_shared<nav2::LifecycleNode>("SmacSE2SmootherCovTest", options);
+
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros =
+    std::make_shared<nav2_costmap_2d::Costmap2DROS>("global_costmap");
+  costmap_ros->on_configure(rclcpp_lifecycle::State());
+
+  node->configure();
+  node->activate();
+
+  auto planner = std::make_unique<HybridWrap>();
+  EXPECT_NO_THROW(planner->configure(node, "test", nullptr, costmap_ros));
+  planner->activate();
+  EXPECT_TRUE(planner->hasSmootherInitialized());
+
+  auto dummy_cancel_checker = []() { return false; };
+  geometry_msgs::msg::PoseStamped start, goal;
+  start.pose.position.x = 0.0;
+  start.pose.position.y = 0.0;
+  start.pose.orientation.w = 1.0;
+  goal.pose.position.x = 1.0;
+  goal.pose.position.y = 1.0;
+  goal.pose.orientation.w = 1.0;
+
+  nav_msgs::msg::Path plan;
+  EXPECT_NO_THROW(plan = planner->createPlan(start, goal, dummy_cancel_checker));
+  EXPECT_FALSE(plan.poses.empty());
+
+  planner->deactivate();
+  planner->cleanup();
+  planner.reset();
+  costmap_ros->on_cleanup(rclcpp_lifecycle::State());
+  costmap_ros.reset();
+  node->deactivate();
+  node->cleanup();
+  node.reset();
+}
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
diff --git a/nav2_smac_planner/test/test_smac_lattice.cpp b/nav2_smac_planner/test/test_smac_lattice.cpp
index 612b47a0b66..e7bd358df4c 100644
--- a/nav2_smac_planner/test/test_smac_lattice.cpp
+++ b/nav2_smac_planner/test/test_smac_lattice.cpp
@@ -398,6 +398,49 @@ TEST(SmacTest, test_smac_lattice_omni_reconfigure)
   nodeLattice.reset();
 }
 
+TEST(SmacTest, test_smac_lattice_smoother_coverage)
+{
+  rclcpp::NodeOptions options;
+  options.parameter_overrides(
+    {rclcpp::Parameter("test.smooth_path", true)});
+  nav2::LifecycleNode::SharedPtr node =
+    std::make_shared<nav2::LifecycleNode>("SmacLatticeSmootherCovTest", options);
+
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros =
+    std::make_shared<nav2_costmap_2d::Costmap2DROS>("global_costmap");
+  costmap_ros->on_configure(rclcpp_lifecycle::State());
+
+  node->configure();
+  node->activate();
+
+  auto planner = std::make_unique<LatticeWrap>();
+  EXPECT_NO_THROW(planner->configure(node, "test", nullptr, costmap_ros));
+  planner->activate();
+  EXPECT_TRUE(planner->hasSmootherInitialized());
+
+  auto dummy_cancel_checker = []() { return false; };
+  geometry_msgs::msg::PoseStamped start, goal;
+  start.pose.position.x = 0.0;
+  start.pose.position.y = 0.0;
+  start.pose.orientation.w = 1.0;
+  goal.pose.position.x = 1.0;
+  goal.pose.position.y = 1.0;
+  goal.pose.orientation.w = 1.0;
+
+  nav_msgs::msg::Path plan;
+  EXPECT_NO_THROW(plan = planner->createPlan(start, goal, dummy_cancel_checker));
+  EXPECT_FALSE(plan.poses.empty());
+
+  planner->deactivate();
+  planner->cleanup();
+  planner.reset();
+  costmap_ros->on_cleanup(rclcpp_lifecycle::State());
+  costmap_ros.reset();
+  node->deactivate();
+  node->cleanup();
+  node.reset();
+}
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);

From 575289c0e1313eaf4ff282987bc4f8ab68fc9ecb Mon Sep 17 00:00:00 2001
From: Akihiko Komada <aki1770@gmail.com>
Date: Mon, 13 Apr 2026 20:22:19 +0900
Subject: [PATCH 06/15] fix(smac_planner): stop smoothed path at oriented
 footprint collision boundary
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Per mini-1235 review: instead of discarding the entire smoothed result when an
oriented footprint collision is detected, stop the check loop at the first
colliding pose and truncate the smoothed path there.

Smoother::smooth() copies only curr_path_segment.poses.size() elements back,
so the truncated smoothed prefix is used and the original planner poses are
retained from the collision point onward — no full revert needed.

Co-Authored-By: Claude and aki1770-del <aki1770@gmail.com>
Signed-off-by: Akihiko Komada <aki1770@gmail.com>
---
 nav2_smac_planner/src/smoother.cpp       | 17 +++++++++++------
 nav2_smac_planner/test/test_smoother.cpp |  6 ++++--
 2 files changed, 15 insertions(+), 8 deletions(-)

diff --git a/nav2_smac_planner/src/smoother.cpp b/nav2_smac_planner/src/smoother.cpp
index b28eb56e4c9..5d42335e699 100644
--- a/nav2_smac_planner/src/smoother.cpp
+++ b/nav2_smac_planner/src/smoother.cpp
@@ -204,16 +204,20 @@ bool Smoother::smoothImpl(
   // sufficient for circular robots (costmap inflation captures the radius) but
   // misses orientation-dependent footprint extensions for non-circular robots.
   // After orientations are assigned we validate each smoothed pose with the full
-  // oriented footprint and revert to the original path if a collision is found.
+  // oriented footprint. When a collision is detected the loop stops at that pose
+  // so the maximum collision-free smoothed prefix is used. The caller's path
+  // retains the original planner poses from the collision point onward (via the
+  // partial std::copy in Smoother::smooth).
   if (!footprint.empty() && costmap) {
     // FootprintCollisionChecker is only instantiated for Costmap2D* (non-const).
     // const_cast is safe here: footprintCostAtPose only reads the costmap.
     nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>
     checker(const_cast<nav2_costmap_2d::Costmap2D *>(costmap));
-    for (const auto & pose_stamped : new_path.poses) {
-      const double yaw = tf2::getYaw(pose_stamped.pose.orientation);
+    for (size_t idx = 0; idx < new_path.poses.size(); ++idx) {
+      const double yaw = tf2::getYaw(new_path.poses[idx].pose.orientation);
       const double cost = checker.footprintCostAtPose(
-        pose_stamped.pose.position.x, pose_stamped.pose.position.y,
+        new_path.poses[idx].pose.position.x,
+        new_path.poses[idx].pose.position.y,
         yaw, footprint);
       if (static_cast<float>(cost) > MAX_NON_OBSTACLE_COST &&
         static_cast<float>(cost) != UNKNOWN_COST)
@@ -221,8 +225,9 @@ bool Smoother::smoothImpl(
         RCLCPP_WARN(
           rclcpp::get_logger("SmacPlannerSmoother"),
           "Smoothed path produces an oriented footprint collision for a non-circular robot. "
-          "Returning original path.");
-        nav2_util::updateApproximatePathOrientations(path, reversing_segment, is_holonomic_);
+          "Stopping smoothed path at collision-free boundary.");
+        new_path.poses.resize(idx);
+        path = new_path;
         return false;
       }
     }
diff --git a/nav2_smac_planner/test/test_smoother.cpp b/nav2_smac_planner/test/test_smoother.cpp
index 9c3c5fb7856..e3eb0e0be39 100644
--- a/nav2_smac_planner/test/test_smoother.cpp
+++ b/nav2_smac_planner/test/test_smoother.cpp
@@ -238,8 +238,10 @@ TEST(SmootherTest, test_footprint_collision_detection)
   plan_copy = plan;
   EXPECT_TRUE(smoother->smooth(plan_copy, costmap, maxtime, narrow_footprint));
 
-  // With a wide footprint (1.2m wide): oriented footprint check must detect collision.
-  // The path travels along y=5.0 but the footprint extends ±0.6m in y — hitting the walls.
+  // With a wide footprint (1.2m wide): oriented footprint check detects collision at
+  // the very first pose (footprint extends ±0.6m in y, touching the lethal walls at
+  // y=4.4 and y=5.6). The smoothed path is truncated at idx=0 (empty prefix) and
+  // false is returned; the caller's path segment is left with the original planner poses.
   nav2_costmap_2d::Footprint wide_footprint;
   pt.x = 0.4; pt.y = 0.6; wide_footprint.push_back(pt);
   pt.x = 0.4; pt.y = -0.6; wide_footprint.push_back(pt);

From 2504cd44b8aa8071594c54aa05fe3d26a65d1945 Mon Sep 17 00:00:00 2001
From: Akihiko Komada <aki1770@gmail.com>
Date: Mon, 13 Apr 2026 21:19:39 +0900
Subject: [PATCH 07/15] style: fix uncrustify lambda spacing in smoother
 coverage tests

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
Signed-off-by: Akihiko Komada <aki1770@gmail.com>
---
 nav2_smac_planner/test/test_smac_hybrid.cpp  | 2 +-
 nav2_smac_planner/test/test_smac_lattice.cpp | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/nav2_smac_planner/test/test_smac_hybrid.cpp b/nav2_smac_planner/test/test_smac_hybrid.cpp
index da19ede4233..d03f2ba1169 100644
--- a/nav2_smac_planner/test/test_smac_hybrid.cpp
+++ b/nav2_smac_planner/test/test_smac_hybrid.cpp
@@ -408,7 +408,7 @@ TEST(SmacTest, test_smac_se2_smoother_coverage)
   planner->activate();
   EXPECT_TRUE(planner->hasSmootherInitialized());
 
-  auto dummy_cancel_checker = []() { return false; };
+  auto dummy_cancel_checker = []() {return false;};
   geometry_msgs::msg::PoseStamped start, goal;
   start.pose.position.x = 0.0;
   start.pose.position.y = 0.0;
diff --git a/nav2_smac_planner/test/test_smac_lattice.cpp b/nav2_smac_planner/test/test_smac_lattice.cpp
index e7bd358df4c..ed0f5f8d1ee 100644
--- a/nav2_smac_planner/test/test_smac_lattice.cpp
+++ b/nav2_smac_planner/test/test_smac_lattice.cpp
@@ -418,7 +418,7 @@ TEST(SmacTest, test_smac_lattice_smoother_coverage)
   planner->activate();
   EXPECT_TRUE(planner->hasSmootherInitialized());
 
-  auto dummy_cancel_checker = []() { return false; };
+  auto dummy_cancel_checker = []() {return false;};
   geometry_msgs::msg::PoseStamped start, goal;
   start.pose.position.x = 0.0;
   start.pose.position.y = 0.0;

From 0deb9902a76b5e2f87b6528173cea3eb2c88c001 Mon Sep 17 00:00:00 2001
From: Akihiko Komada <aki1770@gmail.com>
Date: Tue, 14 Apr 2026 05:22:24 +0900
Subject: [PATCH 08/15] fix(smac_planner): use LETHAL_OBSTACLE threshold for
 footprint collision check
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

FootprintCollisionChecker::footprintCostAtPose returns raw cell costs along
the footprint perimeter. For full perimeter checking, only LETHAL_OBSTACLE
(254) represents an actual obstacle cell. INSCRIBED_INFLATED_OBSTACLE (253)
means the robot center (for circular approximation) is within the inflation
radius — not a collision for an oriented footprint check.

Corrects the threshold from MAX_NON_OBSTACLE_COST (252) to
nav2_costmap_2d::LETHAL_OBSTACLE (254) per SteveMacenski review feedback.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
Signed-off-by: Akihiko Komada <aki1770@gmail.com>
---
 nav2_smac_planner/src/smoother.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/nav2_smac_planner/src/smoother.cpp b/nav2_smac_planner/src/smoother.cpp
index 5d42335e699..850c081d302 100644
--- a/nav2_smac_planner/src/smoother.cpp
+++ b/nav2_smac_planner/src/smoother.cpp
@@ -23,6 +23,7 @@
 
 #include "tf2/utils.hpp"
 
+#include "nav2_costmap_2d/cost_values.hpp"
 #include "nav2_smac_planner/smoother.hpp"
 #include "nav2_util/smoother_utils.hpp"
 
@@ -219,7 +220,7 @@ bool Smoother::smoothImpl(
         new_path.poses[idx].pose.position.x,
         new_path.poses[idx].pose.position.y,
         yaw, footprint);
-      if (static_cast<float>(cost) > MAX_NON_OBSTACLE_COST &&
+      if (static_cast<float>(cost) >= static_cast<float>(nav2_costmap_2d::LETHAL_OBSTACLE) &&
         static_cast<float>(cost) != UNKNOWN_COST)
       {
         RCLCPP_WARN(

From 27511e292c8d9af5f4c4847f4a3052a81bc3b98d Mon Sep 17 00:00:00 2001
From: Akihiko Komada <aki1770@gmail.com>
Date: Wed, 29 Apr 2026 22:44:17 +0900
Subject: [PATCH 09/15] fix(smac_planner): per-iteration revert +
 SmoothedPathInCollision throw
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Address Maurice's CHANGES_REQUESTED on PR #6081:

1. Move oriented-footprint check INTO the gradient-descent while loop
   (per r3072469939). Previously the check ran once after the loop and
   the post-loop block truncated new_path then propagated an empty path
   through the recursive smoothImpl refinement self-call, causing the
   regression test to crash on the path_size-1 underflow.

2. Per-iteration revert (path = last_path) on collision — matches the
   nav2_smoother::SimpleSmoother idiom (simple_smoother.cpp:171-178).
   Path length is preserved end-to-end. Truncation removed.

3. Throw nav2_core::SmoothedPathInCollision on oriented-footprint
   collision — matches the canonical fail-loud signal already used by
   nav2_smoother::Nav2Smoother (nav2_smoother.cpp:285-303). Caller
   receives an explicit collision exception instead of a silently
   truncated path. This closes the controller-tracking hazard for
   wide-rectangular robots in confined corridors that motivated #5330.

Test updated: wide-footprint case now expects SmoothedPathInCollision
throw and verifies path length preservation (EXPECT_EQ poses.size()).

Closes the empty-path crash that broke CI on commit 1c5feb6.
Refs #5330.

Signed-off-by: Akihiko Komada <aki1770@gmail.com>
---
 nav2_smac_planner/src/smoother.cpp       | 93 +++++++++++++++---------
 nav2_smac_planner/test/test_smoother.cpp | 17 +++--
 2 files changed, 70 insertions(+), 40 deletions(-)

diff --git a/nav2_smac_planner/src/smoother.cpp b/nav2_smac_planner/src/smoother.cpp
index 850c081d302..155ca0fda02 100644
--- a/nav2_smac_planner/src/smoother.cpp
+++ b/nav2_smac_planner/src/smoother.cpp
@@ -23,6 +23,7 @@
 
 #include "tf2/utils.hpp"
 
+#include "nav2_core/smoother_exceptions.hpp"
 #include "nav2_costmap_2d/cost_values.hpp"
 #include "nav2_smac_planner/smoother.hpp"
 #include "nav2_util/smoother_utils.hpp"
@@ -127,6 +128,21 @@ bool Smoother::smoothImpl(
   nav_msgs::msg::Path new_path = path;
   nav_msgs::msg::Path last_path = path;
 
+  // Oriented footprint collision checker for non-circular robots (fix for #5330).
+  // Built once and reused across iterations. The center-cost check inside the
+  // inner pose loop is sufficient for circular robots (costmap inflation captures
+  // the radius) but misses orientation-dependent footprint extensions for
+  // non-circular robots. The oriented-footprint check below is evaluated per
+  // gradient-descent iteration, after orientation assignment, on the new_path.
+  // On collision, the iteration is reverted (path = last_path) — matching the
+  // nav2_smoother::SimpleSmoother per-iteration revert idiom — so the smoother
+  // never publishes a smoothed path it knows to be in oriented collision.
+  const bool check_oriented_footprint = !footprint.empty() && costmap;
+  // FootprintCollisionChecker is only instantiated for Costmap2D* (non-const).
+  // const_cast is safe here: footprintCostAtPose only reads the costmap.
+  nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>
+  oriented_checker(const_cast<nav2_costmap_2d::Costmap2D *>(costmap));
+
   while (change >= tolerance_) {
     its += 1;
     change = 0.0;
@@ -188,6 +204,48 @@ bool Smoother::smoothImpl(
       }
     }
 
+    // Oriented footprint collision check (fix for #5330).
+    // Evaluated per iteration so the smoother stops smoothing as soon as a step
+    // would pull the oriented footprint into a lethal cell. On collision, the
+    // path is reverted to last_path (the last collision-free iteration) and
+    // nav2_core::SmoothedPathInCollision is thrown — matching the canonical
+    // fail-loud signal used by nav2_smoother::Nav2Smoother (collision_checker
+    // catch site at nav2_smoother.cpp:285-303). The caller's input path is
+    // length-preserved (std::copy back-into-segment is bypassed by the throw)
+    // so downstream consumers receive either a fully-smoothed-and-clean path
+    // or an explicit collision exception, never a silently-truncated path.
+    // Combines Maurice's r3072469939 in-loop guidance (UPA reading-α) with the
+    // nav2_core SmoothedPathInCollision idiom (AAA reading-β safety class).
+    if (check_oriented_footprint) {
+      nav_msgs::msg::Path oriented_candidate = new_path;
+      nav2_util::updateApproximatePathOrientations(
+        oriented_candidate, reversing_segment, is_holonomic_);
+      for (size_t idx = 0; idx < oriented_candidate.poses.size(); ++idx) {
+        const double yaw = tf2::getYaw(oriented_candidate.poses[idx].pose.orientation);
+        const double oriented_cost = oriented_checker.footprintCostAtPose(
+          oriented_candidate.poses[idx].pose.position.x,
+          oriented_candidate.poses[idx].pose.position.y,
+          yaw, footprint);
+        if (
+          static_cast<float>(oriented_cost) >=
+          static_cast<float>(nav2_costmap_2d::LETHAL_OBSTACLE) &&
+          static_cast<float>(oriented_cost) != UNKNOWN_COST)
+        {
+          RCLCPP_DEBUG(
+            rclcpp::get_logger("SmacPlannerSmoother"),
+            "Smoothing iteration produced an oriented footprint collision for a "
+            "non-circular robot. Reverting to the last collision-free iteration.");
+          path = last_path;
+          nav2_util::updateApproximatePathOrientations(path, reversing_segment, is_holonomic_);
+          throw nav2_core::SmoothedPathInCollision(
+                  "Smoothed path collides with the oriented robot footprint at "
+                  "X: " + std::to_string(oriented_candidate.poses[idx].pose.position.x) +
+                  " Y: " + std::to_string(oriented_candidate.poses[idx].pose.position.y) +
+                  " Theta: " + std::to_string(yaw));
+        }
+      }
+    }
+
     last_path = new_path;
   }
 
@@ -199,41 +257,6 @@ bool Smoother::smoothImpl(
   }
 
   nav2_util::updateApproximatePathOrientations(new_path, reversing_segment, is_holonomic_);
-
-  // Oriented footprint collision check for non-circular robots (fix for #5330).
-  // The per-iteration cost check above uses center-point cost only, which is
-  // sufficient for circular robots (costmap inflation captures the radius) but
-  // misses orientation-dependent footprint extensions for non-circular robots.
-  // After orientations are assigned we validate each smoothed pose with the full
-  // oriented footprint. When a collision is detected the loop stops at that pose
-  // so the maximum collision-free smoothed prefix is used. The caller's path
-  // retains the original planner poses from the collision point onward (via the
-  // partial std::copy in Smoother::smooth).
-  if (!footprint.empty() && costmap) {
-    // FootprintCollisionChecker is only instantiated for Costmap2D* (non-const).
-    // const_cast is safe here: footprintCostAtPose only reads the costmap.
-    nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>
-    checker(const_cast<nav2_costmap_2d::Costmap2D *>(costmap));
-    for (size_t idx = 0; idx < new_path.poses.size(); ++idx) {
-      const double yaw = tf2::getYaw(new_path.poses[idx].pose.orientation);
-      const double cost = checker.footprintCostAtPose(
-        new_path.poses[idx].pose.position.x,
-        new_path.poses[idx].pose.position.y,
-        yaw, footprint);
-      if (static_cast<float>(cost) >= static_cast<float>(nav2_costmap_2d::LETHAL_OBSTACLE) &&
-        static_cast<float>(cost) != UNKNOWN_COST)
-      {
-        RCLCPP_WARN(
-          rclcpp::get_logger("SmacPlannerSmoother"),
-          "Smoothed path produces an oriented footprint collision for a non-circular robot. "
-          "Stopping smoothed path at collision-free boundary.");
-        new_path.poses.resize(idx);
-        path = new_path;
-        return false;
-      }
-    }
-  }
-
   path = new_path;
   return true;
 }
diff --git a/nav2_smac_planner/test/test_smoother.cpp b/nav2_smac_planner/test/test_smoother.cpp
index e3eb0e0be39..9ab1a070883 100644
--- a/nav2_smac_planner/test/test_smoother.cpp
+++ b/nav2_smac_planner/test/test_smoother.cpp
@@ -20,6 +20,7 @@
 
 #include "gtest/gtest.h"
 #include "rclcpp/rclcpp.hpp"
+#include "nav2_core/smoother_exceptions.hpp"
 #include "nav2_costmap_2d/costmap_2d.hpp"
 #include "nav2_costmap_2d/costmap_subscriber.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
@@ -238,10 +239,11 @@ TEST(SmootherTest, test_footprint_collision_detection)
   plan_copy = plan;
   EXPECT_TRUE(smoother->smooth(plan_copy, costmap, maxtime, narrow_footprint));
 
-  // With a wide footprint (1.2m wide): oriented footprint check detects collision at
-  // the very first pose (footprint extends ±0.6m in y, touching the lethal walls at
-  // y=4.4 and y=5.6). The smoothed path is truncated at idx=0 (empty prefix) and
-  // false is returned; the caller's path segment is left with the original planner poses.
+  // With a wide footprint (1.2m wide): the per-iteration oriented footprint check
+  // detects collision (footprint extends ±0.6m in y, touching the lethal walls at
+  // y=4.4 and y=5.6) and throws nav2_core::SmoothedPathInCollision — matching the
+  // canonical fail-loud idiom used by nav2_smoother::Nav2Smoother. The caller's
+  // path is left unchanged (full length preserved); no truncated path is returned.
   nav2_costmap_2d::Footprint wide_footprint;
   pt.x = 0.4; pt.y = 0.6; wide_footprint.push_back(pt);
   pt.x = 0.4; pt.y = -0.6; wide_footprint.push_back(pt);
@@ -249,7 +251,12 @@ TEST(SmootherTest, test_footprint_collision_detection)
   pt.x = -0.4; pt.y = 0.6; wide_footprint.push_back(pt);
 
   plan_copy = plan;
-  EXPECT_FALSE(smoother->smooth(plan_copy, costmap, maxtime, wide_footprint));
+  const size_t original_size = plan_copy.poses.size();
+  EXPECT_THROW(
+    smoother->smooth(plan_copy, costmap, maxtime, wide_footprint),
+    nav2_core::SmoothedPathInCollision);
+  // Path length is preserved on collision-throw — matches simple_smoother revert idiom.
+  EXPECT_EQ(plan_copy.poses.size(), original_size);
 
   delete costmap;
 }

From e77e7968dbde30a96422ea519d5a9eb8504160b7 Mon Sep 17 00:00:00 2001
From: Akihiko Komada <aki1770@gmail.com>
Date: Wed, 29 Apr 2026 23:54:04 +0900
Subject: [PATCH 10/15] fix(smac_planner): drop non-substantive comment lines
 on in-loop check

Strip two trailing lines from the in-loop oriented-footprint check
comment that referenced internal review meta-context (a specific
inline-review ID and internal team-vocabulary tags) instead of
helping a future nav2 reader understand the code.

Self-review pass on the surrounding diff: substantive WHY comments
(rationale for per-iteration evaluation, revert behavior, length
preservation, nav2_smoother.cpp:285-303 fail-loud parallel,
const_cast safety note) are kept. No code or behavior changes.

Signed-off-by: Akihiko Komada <aki1770@gmail.com>
---
 nav2_smac_planner/src/smoother.cpp | 2 --
 1 file changed, 2 deletions(-)

diff --git a/nav2_smac_planner/src/smoother.cpp b/nav2_smac_planner/src/smoother.cpp
index 155ca0fda02..cb5aaa1dae0 100644
--- a/nav2_smac_planner/src/smoother.cpp
+++ b/nav2_smac_planner/src/smoother.cpp
@@ -214,8 +214,6 @@ bool Smoother::smoothImpl(
     // length-preserved (std::copy back-into-segment is bypassed by the throw)
     // so downstream consumers receive either a fully-smoothed-and-clean path
     // or an explicit collision exception, never a silently-truncated path.
-    // Combines Maurice's r3072469939 in-loop guidance (UPA reading-α) with the
-    // nav2_core SmoothedPathInCollision idiom (AAA reading-β safety class).
     if (check_oriented_footprint) {
       nav_msgs::msg::Path oriented_candidate = new_path;
       nav2_util::updateApproximatePathOrientations(

From 16cecfd242828f613d22b6750ef41f49cbb6a16a Mon Sep 17 00:00:00 2001
From: Akihiko Komada <aki1770@gmail.com>
Date: Thu, 30 Apr 2026 00:49:03 +0900
Subject: [PATCH 11/15] test(smac_planner): update test createPlan calls to
 4-arg signature

The SmacPlannerHybrid and SmacPlannerLattice test files in this branch
still called the 3-arg createPlan(start, goal, cancel_checker) signature.
On main, BaseGlobalPlanner::createPlan gained a viapoints parameter
(commit 51e3125, PR #5995), and main's test files were updated to the
4-arg form. Build Against Released Distributions CI fails on jazzy and
kilted because this branch's test files diverged.

Mirror main's pattern: declare a per-TEST
std::vector<geometry_msgs::msg::PoseStamped> no_viapoints{} alongside
dummy_cancel_checker, and pass it as the third argument to all 8
createPlan call sites in test_smac_hybrid.cpp and test_smac_lattice.cpp.

No production-code changes. Smoother behaviour under test is unchanged.

Signed-off-by: Akihiko Komada <aki1770@gmail.com>
---
 nav2_smac_planner/test/test_smac_hybrid.cpp  | 3 ++-
 nav2_smac_planner/test/test_smac_lattice.cpp | 3 ++-
 2 files changed, 4 insertions(+), 2 deletions(-)

diff --git a/nav2_smac_planner/test/test_smac_hybrid.cpp b/nav2_smac_planner/test/test_smac_hybrid.cpp
index d03f2ba1169..11efb8b8ec7 100644
--- a/nav2_smac_planner/test/test_smac_hybrid.cpp
+++ b/nav2_smac_planner/test/test_smac_hybrid.cpp
@@ -409,6 +409,7 @@ TEST(SmacTest, test_smac_se2_smoother_coverage)
   EXPECT_TRUE(planner->hasSmootherInitialized());
 
   auto dummy_cancel_checker = []() {return false;};
+  std::vector<geometry_msgs::msg::PoseStamped> no_viapoints{};
   geometry_msgs::msg::PoseStamped start, goal;
   start.pose.position.x = 0.0;
   start.pose.position.y = 0.0;
@@ -418,7 +419,7 @@ TEST(SmacTest, test_smac_se2_smoother_coverage)
   goal.pose.orientation.w = 1.0;
 
   nav_msgs::msg::Path plan;
-  EXPECT_NO_THROW(plan = planner->createPlan(start, goal, dummy_cancel_checker));
+  EXPECT_NO_THROW(plan = planner->createPlan(start, goal, no_viapoints, dummy_cancel_checker));
   EXPECT_FALSE(plan.poses.empty());
 
   planner->deactivate();
diff --git a/nav2_smac_planner/test/test_smac_lattice.cpp b/nav2_smac_planner/test/test_smac_lattice.cpp
index ed0f5f8d1ee..86e14fcab4e 100644
--- a/nav2_smac_planner/test/test_smac_lattice.cpp
+++ b/nav2_smac_planner/test/test_smac_lattice.cpp
@@ -419,6 +419,7 @@ TEST(SmacTest, test_smac_lattice_smoother_coverage)
   EXPECT_TRUE(planner->hasSmootherInitialized());
 
   auto dummy_cancel_checker = []() {return false;};
+  std::vector<geometry_msgs::msg::PoseStamped> no_viapoints{};
   geometry_msgs::msg::PoseStamped start, goal;
   start.pose.position.x = 0.0;
   start.pose.position.y = 0.0;
@@ -428,7 +429,7 @@ TEST(SmacTest, test_smac_lattice_smoother_coverage)
   goal.pose.orientation.w = 1.0;
 
   nav_msgs::msg::Path plan;
-  EXPECT_NO_THROW(plan = planner->createPlan(start, goal, dummy_cancel_checker));
+  EXPECT_NO_THROW(plan = planner->createPlan(start, goal, no_viapoints, dummy_cancel_checker));
   EXPECT_FALSE(plan.poses.empty());
 
   planner->deactivate();

From a936a2045d0f2413b5cd3b515714b9bb58678100 Mon Sep 17 00:00:00 2001
From: Akihiko Komada <aki1770@gmail.com>
Date: Fri, 1 May 2026 07:50:02 +0900
Subject: [PATCH 12/15] fix(smac_planner): drop const_cast on costmap pointer
 in Smoother

Take a non-const Costmap2D* through Smoother::smooth, smoothImpl,
enforceStartBoundaryConditions, enforceEndBoundaryConditions, and
findBoundaryExpansion so the FootprintCollisionChecker<Costmap2D*>
can be constructed directly. All three production callers
(smac_planner_2d, smac_planner_hybrid, smac_planner_lattice) already
hold non-const Costmap2D*, so this is a clean signature change with
no const-cast left behind.

Co-Authored-By: Claude and aki1770-del <aki1770@gmail.com>
Signed-off-by: Akihiko Komada <aki1770@gmail.com>
---
 .../include/nav2_smac_planner/smoother.hpp         | 10 +++++-----
 nav2_smac_planner/src/smoother.cpp                 | 14 ++++++--------
 2 files changed, 11 insertions(+), 13 deletions(-)

diff --git a/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp
index d01ee9f01b3..ad559d59db1 100644
--- a/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp
@@ -100,7 +100,7 @@ class Smoother
    */
   bool smooth(
     nav_msgs::msg::Path & path,
-    const nav2_costmap_2d::Costmap2D * costmap,
+    nav2_costmap_2d::Costmap2D * costmap,
     const double & max_time,
     const nav2_costmap_2d::Footprint & footprint = {});
 
@@ -117,7 +117,7 @@ class Smoother
   bool smoothImpl(
     nav_msgs::msg::Path & path,
     bool & reversing_segment,
-    const nav2_costmap_2d::Costmap2D * costmap,
+    nav2_costmap_2d::Costmap2D * costmap,
     const double & max_time,
     const nav2_costmap_2d::Footprint & footprint = {});
 
@@ -152,7 +152,7 @@ class Smoother
   void enforceStartBoundaryConditions(
     const geometry_msgs::msg::Pose & start_pose,
     nav_msgs::msg::Path & path,
-    const nav2_costmap_2d::Costmap2D * costmap,
+    nav2_costmap_2d::Costmap2D * costmap,
     const bool & reversing_segment);
 
   /**
@@ -166,7 +166,7 @@ class Smoother
   void enforceEndBoundaryConditions(
     const geometry_msgs::msg::Pose & end_pose,
     nav_msgs::msg::Path & path,
-    const nav2_costmap_2d::Costmap2D * costmap,
+    nav2_costmap_2d::Costmap2D * costmap,
     const bool & reversing_segment);
 
   /**
@@ -191,7 +191,7 @@ class Smoother
     const geometry_msgs::msg::Pose & start,
     const geometry_msgs::msg::Pose & end,
     BoundaryExpansion & expansion,
-    const nav2_costmap_2d::Costmap2D * costmap);
+    nav2_costmap_2d::Costmap2D * costmap);
 
   /**
    * @brief Generates boundary expansions with end idx at least strategic
diff --git a/nav2_smac_planner/src/smoother.cpp b/nav2_smac_planner/src/smoother.cpp
index cb5aaa1dae0..35ccb5a64d9 100644
--- a/nav2_smac_planner/src/smoother.cpp
+++ b/nav2_smac_planner/src/smoother.cpp
@@ -53,7 +53,7 @@ void Smoother::initialize(const double & min_turning_radius)
 
 bool Smoother::smooth(
   nav_msgs::msg::Path & path,
-  const nav2_costmap_2d::Costmap2D * costmap,
+  nav2_costmap_2d::Costmap2D * costmap,
   const double & max_time,
   const nav2_costmap_2d::Footprint & footprint)
 {
@@ -112,7 +112,7 @@ bool Smoother::smooth(
 bool Smoother::smoothImpl(
   nav_msgs::msg::Path & path,
   bool & reversing_segment,
-  const nav2_costmap_2d::Costmap2D * costmap,
+  nav2_costmap_2d::Costmap2D * costmap,
   const double & max_time,
   const nav2_costmap_2d::Footprint & footprint)
 {
@@ -138,10 +138,8 @@ bool Smoother::smoothImpl(
   // nav2_smoother::SimpleSmoother per-iteration revert idiom — so the smoother
   // never publishes a smoothed path it knows to be in oriented collision.
   const bool check_oriented_footprint = !footprint.empty() && costmap;
-  // FootprintCollisionChecker is only instantiated for Costmap2D* (non-const).
-  // const_cast is safe here: footprintCostAtPose only reads the costmap.
   nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>
-  oriented_checker(const_cast<nav2_costmap_2d::Costmap2D *>(costmap));
+  oriented_checker(costmap);
 
   while (change >= tolerance_) {
     its += 1;
@@ -310,7 +308,7 @@ void Smoother::findBoundaryExpansion(
   const geometry_msgs::msg::Pose & start,
   const geometry_msgs::msg::Pose & end,
   BoundaryExpansion & expansion,
-  const nav2_costmap_2d::Costmap2D * costmap)
+  nav2_costmap_2d::Costmap2D * costmap)
 {
   ompl::base::ScopedState<> from(state_space_), to(state_space_), s(state_space_);
 
@@ -406,7 +404,7 @@ BoundaryExpansions Smoother::generateBoundaryExpansionPoints(IteratorT start, It
 void Smoother::enforceStartBoundaryConditions(
   const geometry_msgs::msg::Pose & start_pose,
   nav_msgs::msg::Path & path,
-  const nav2_costmap_2d::Costmap2D * costmap,
+  nav2_costmap_2d::Costmap2D * costmap,
   const bool & reversing_segment)
 {
   // Find range of points for testing
@@ -452,7 +450,7 @@ void Smoother::enforceStartBoundaryConditions(
 void Smoother::enforceEndBoundaryConditions(
   const geometry_msgs::msg::Pose & end_pose,
   nav_msgs::msg::Path & path,
-  const nav2_costmap_2d::Costmap2D * costmap,
+  nav2_costmap_2d::Costmap2D * costmap,
   const bool & reversing_segment)
 {
   // Find range of points for testing

From bf9b7249cd07fec3f4fefde290447bdacecf7ca5 Mon Sep 17 00:00:00 2001
From: Akihiko Komada <aki1770@gmail.com>
Date: Fri, 1 May 2026 07:50:27 +0900
Subject: [PATCH 13/15] fix(smac_planner): trim verbose comments in
 Smoother::smoothImpl

Drop two long comment blocks in favor of one-line markers.

Co-Authored-By: Claude and aki1770-del <aki1770@gmail.com>
Signed-off-by: Akihiko Komada <aki1770@gmail.com>
---
 nav2_smac_planner/src/smoother.cpp | 21 ++-------------------
 1 file changed, 2 insertions(+), 19 deletions(-)

diff --git a/nav2_smac_planner/src/smoother.cpp b/nav2_smac_planner/src/smoother.cpp
index 35ccb5a64d9..c8ec840582e 100644
--- a/nav2_smac_planner/src/smoother.cpp
+++ b/nav2_smac_planner/src/smoother.cpp
@@ -128,15 +128,7 @@ bool Smoother::smoothImpl(
   nav_msgs::msg::Path new_path = path;
   nav_msgs::msg::Path last_path = path;
 
-  // Oriented footprint collision checker for non-circular robots (fix for #5330).
-  // Built once and reused across iterations. The center-cost check inside the
-  // inner pose loop is sufficient for circular robots (costmap inflation captures
-  // the radius) but misses orientation-dependent footprint extensions for
-  // non-circular robots. The oriented-footprint check below is evaluated per
-  // gradient-descent iteration, after orientation assignment, on the new_path.
-  // On collision, the iteration is reverted (path = last_path) — matching the
-  // nav2_smoother::SimpleSmoother per-iteration revert idiom — so the smoother
-  // never publishes a smoothed path it knows to be in oriented collision.
+  // Oriented footprint collision checker for non-circular robots (#5330).
   const bool check_oriented_footprint = !footprint.empty() && costmap;
   nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>
   oriented_checker(costmap);
@@ -202,16 +194,7 @@ bool Smoother::smoothImpl(
       }
     }
 
-    // Oriented footprint collision check (fix for #5330).
-    // Evaluated per iteration so the smoother stops smoothing as soon as a step
-    // would pull the oriented footprint into a lethal cell. On collision, the
-    // path is reverted to last_path (the last collision-free iteration) and
-    // nav2_core::SmoothedPathInCollision is thrown — matching the canonical
-    // fail-loud signal used by nav2_smoother::Nav2Smoother (collision_checker
-    // catch site at nav2_smoother.cpp:285-303). The caller's input path is
-    // length-preserved (std::copy back-into-segment is bypassed by the throw)
-    // so downstream consumers receive either a fully-smoothed-and-clean path
-    // or an explicit collision exception, never a silently-truncated path.
+    // Per-iteration oriented footprint check; revert to last_path on collision.
     if (check_oriented_footprint) {
       nav_msgs::msg::Path oriented_candidate = new_path;
       nav2_util::updateApproximatePathOrientations(

From 11b6382222093421e19d19d94833c1117880906f Mon Sep 17 00:00:00 2001
From: Akihiko Komada <aki1770@gmail.com>
Date: Fri, 1 May 2026 07:51:14 +0900
Subject: [PATCH 14/15] fix(smac_planner): revert path on oriented footprint
 collision instead of throwing

Replace the throw of nav2_core::SmoothedPathInCollision with a revert
to the last collision-free iteration and a return of false, matching
the existing per-iteration revert idiom in nav2_smoother::SimpleSmoother.
The center-cost check inside the same loop already returns false on
collision; oriented-footprint collisions now follow the same shape.

Update test_footprint_collision_detection accordingly: the wide-footprint
case now expects EXPECT_FALSE rather than EXPECT_THROW. The unused
smoother_exceptions.hpp include is dropped from both files.

Co-Authored-By: Claude and aki1770-del <aki1770@gmail.com>
Signed-off-by: Akihiko Komada <aki1770@gmail.com>
---
 nav2_smac_planner/src/smoother.cpp       |  7 +------
 nav2_smac_planner/test/test_smoother.cpp | 12 ++++--------
 2 files changed, 5 insertions(+), 14 deletions(-)

diff --git a/nav2_smac_planner/src/smoother.cpp b/nav2_smac_planner/src/smoother.cpp
index c8ec840582e..e8a4c22e337 100644
--- a/nav2_smac_planner/src/smoother.cpp
+++ b/nav2_smac_planner/src/smoother.cpp
@@ -23,7 +23,6 @@
 
 #include "tf2/utils.hpp"
 
-#include "nav2_core/smoother_exceptions.hpp"
 #include "nav2_costmap_2d/cost_values.hpp"
 #include "nav2_smac_planner/smoother.hpp"
 #include "nav2_util/smoother_utils.hpp"
@@ -216,11 +215,7 @@ bool Smoother::smoothImpl(
             "non-circular robot. Reverting to the last collision-free iteration.");
           path = last_path;
           nav2_util::updateApproximatePathOrientations(path, reversing_segment, is_holonomic_);
-          throw nav2_core::SmoothedPathInCollision(
-                  "Smoothed path collides with the oriented robot footprint at "
-                  "X: " + std::to_string(oriented_candidate.poses[idx].pose.position.x) +
-                  " Y: " + std::to_string(oriented_candidate.poses[idx].pose.position.y) +
-                  " Theta: " + std::to_string(yaw));
+          return false;
         }
       }
     }
diff --git a/nav2_smac_planner/test/test_smoother.cpp b/nav2_smac_planner/test/test_smoother.cpp
index 9ab1a070883..e334bb52e8f 100644
--- a/nav2_smac_planner/test/test_smoother.cpp
+++ b/nav2_smac_planner/test/test_smoother.cpp
@@ -20,7 +20,6 @@
 
 #include "gtest/gtest.h"
 #include "rclcpp/rclcpp.hpp"
-#include "nav2_core/smoother_exceptions.hpp"
 #include "nav2_costmap_2d/costmap_2d.hpp"
 #include "nav2_costmap_2d/costmap_subscriber.hpp"
 #include "nav2_ros_common/lifecycle_node.hpp"
@@ -241,9 +240,8 @@ TEST(SmootherTest, test_footprint_collision_detection)
 
   // With a wide footprint (1.2m wide): the per-iteration oriented footprint check
   // detects collision (footprint extends ±0.6m in y, touching the lethal walls at
-  // y=4.4 and y=5.6) and throws nav2_core::SmoothedPathInCollision — matching the
-  // canonical fail-loud idiom used by nav2_smoother::Nav2Smoother. The caller's
-  // path is left unchanged (full length preserved); no truncated path is returned.
+  // y=4.4 and y=5.6); smoother returns false and reverts to the last collision-free
+  // path, matching the simple_smoother per-iteration revert idiom.
   nav2_costmap_2d::Footprint wide_footprint;
   pt.x = 0.4; pt.y = 0.6; wide_footprint.push_back(pt);
   pt.x = 0.4; pt.y = -0.6; wide_footprint.push_back(pt);
@@ -252,10 +250,8 @@ TEST(SmootherTest, test_footprint_collision_detection)
 
   plan_copy = plan;
   const size_t original_size = plan_copy.poses.size();
-  EXPECT_THROW(
-    smoother->smooth(plan_copy, costmap, maxtime, wide_footprint),
-    nav2_core::SmoothedPathInCollision);
-  // Path length is preserved on collision-throw — matches simple_smoother revert idiom.
+  EXPECT_FALSE(smoother->smooth(plan_copy, costmap, maxtime, wide_footprint));
+  // Path length is preserved on revert — matches simple_smoother idiom.
   EXPECT_EQ(plan_copy.poses.size(), original_size);
 
   delete costmap;

From edfdd2977055469e062126a9719ee6440c74bce6 Mon Sep 17 00:00:00 2001
From: Akihiko Komada <aki1770@gmail.com>
Date: Fri, 1 May 2026 07:51:54 +0900
Subject: [PATCH 15/15] fix(smac_planner): skip center-point cost lookup when
 oriented checker active
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

The center-point cost lookup inside the inner pose loop is duplicate
work whenever the oriented-footprint check at the bottom of the same
iteration is enabled — that check already covers the center cell.
Guard the center-point block on !check_oriented_footprint so it only
runs when there is no footprint configured. Move mx/my into the
narrowest scope that uses them.

Co-Authored-By: Claude and aki1770-del <aki1770@gmail.com>
Signed-off-by: Akihiko Komada <aki1770@gmail.com>
---
 nav2_smac_planner/src/smoother.cpp | 39 ++++++++++++++++--------------
 1 file changed, 21 insertions(+), 18 deletions(-)

diff --git a/nav2_smac_planner/src/smoother.cpp b/nav2_smac_planner/src/smoother.cpp
index e8a4c22e337..a249dc45f0f 100644
--- a/nav2_smac_planner/src/smoother.cpp
+++ b/nav2_smac_planner/src/smoother.cpp
@@ -122,7 +122,6 @@ bool Smoother::smoothImpl(
   double change = tolerance_;
   const unsigned int & path_size = path.poses.size();
   double x_i, y_i, y_m1, y_ip1, y_i_org;
-  unsigned int mx, my;
 
   nav_msgs::msg::Path new_path = path;
   nav_msgs::msg::Path last_path = path;
@@ -172,24 +171,28 @@ bool Smoother::smoothImpl(
         change += abs(y_i - y_i_org);
       }
 
-      // validate update is admissible, only checks cost if a valid costmap pointer is provided
-      float cost = 0.0;
-      if (costmap) {
-        costmap->worldToMap(
-          getFieldByDim(new_path.poses[i], 0),
-          getFieldByDim(new_path.poses[i], 1),
-          mx, my);
-        cost = static_cast<float>(costmap->getCost(mx, my));
-      }
+      // Center-point cost check: only run when no oriented footprint check is
+      // configured. The oriented check below subsumes the center-point lookup.
+      if (!check_oriented_footprint) {
+        float cost = 0.0;
+        if (costmap) {
+          unsigned int mx, my;
+          costmap->worldToMap(
+            getFieldByDim(new_path.poses[i], 0),
+            getFieldByDim(new_path.poses[i], 1),
+            mx, my);
+          cost = static_cast<float>(costmap->getCost(mx, my));
+        }
 
-      if (cost > MAX_NON_OBSTACLE_COST && cost != UNKNOWN_COST) {
-        RCLCPP_DEBUG(
-          rclcpp::get_logger("SmacPlannerSmoother"),
-          "Smoothing process resulted in an infeasible collision. "
-          "Returning the last path before the infeasibility was introduced.");
-        path = last_path;
-        nav2_util::updateApproximatePathOrientations(path, reversing_segment, is_holonomic_);
-        return false;
+        if (cost > MAX_NON_OBSTACLE_COST && cost != UNKNOWN_COST) {
+          RCLCPP_DEBUG(
+            rclcpp::get_logger("SmacPlannerSmoother"),
+            "Smoothing process resulted in an infeasible collision. "
+            "Returning the last path before the infeasibility was introduced.");
+          path = last_path;
+          nav2_util::updateApproximatePathOrientations(path, reversing_segment, is_holonomic_);
+          return false;
+        }
       }
     }