fix(smac_planner): detect oriented footprint collisions in SMAC smoother for non-circular robots

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")};

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);
+    nav2_costmap_2d::Costmap2D * costmap,
+    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);
+    nav2_costmap_2d::Costmap2D * costmap,
+    const double & max_time,
+    const nav2_costmap_2d::Footprint & footprint = {});
 
   /**
    * @brief Get the field value for a given dimension
@@ -145,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);
 
   /**
@@ -159,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);
 
   /**
@@ -184,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

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();
 
@@ -320,7 +321,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

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

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

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"
 
@@ -51,8 +52,9 @@ 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)
+  nav2_costmap_2d::Costmap2D * costmap,
+  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 +88,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
@@ -109,8 +111,9 @@ bool Smoother::smooth(
 bool Smoother::smoothImpl(
   nav_msgs::msg::Path & path,
   bool & reversing_segment,
-  const nav2_costmap_2d::Costmap2D * costmap,
-  const double & max_time)
+  nav2_costmap_2d::Costmap2D * costmap,
+  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);
@@ -119,11 +122,15 @@ 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;
 
+  // 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);
+
   while (change >= tolerance_) {
     its += 1;
     change = 0.0;
@@ -164,24 +171,55 @@ 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;
+    // 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(
+        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_);
+          return false;
+        }
       }
     }
 
@@ -192,7 +230,7 @@ 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_);
@@ -251,7 +289,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_);
 
@@ -347,7 +385,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
@@ -393,7 +431,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

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,50 @@ 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;};
+  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;
+  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, no_viapoints, 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);

nav2_smac_planner/test/test_smac_lattice.cpp

@@ -398,6 +398,50 @@ 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;};
+  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;
+  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, no_viapoints, 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);