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; nav2_costmap_2d::Costmap2D * _costmap; + std::shared_ptr _costmap_ros; std::unique_ptr _costmap_downsampler; rclcpp::Clock::SharedPtr _clock; rclcpp::Logger _logger{rclcpp::get_logger("SmacPlanner2D")}; diff --git a/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp index b4a65be8e48..ad559d59db1 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 #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 diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index a62641da414..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(); @@ -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 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..a249dc45f0f 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" @@ -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 + 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(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(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(oriented_cost) >= + static_cast(nav2_costmap_2d::LETHAL_OBSTACLE) && + static_cast(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 diff --git a/nav2_smac_planner/test/test_smac_hybrid.cpp b/nav2_smac_planner/test/test_smac_hybrid.cpp index 6497677243d..11efb8b8ec7 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,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("SmacSE2SmootherCovTest", options); + + std::shared_ptr costmap_ros = + std::make_shared("global_costmap"); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + + node->configure(); + node->activate(); + + auto planner = std::make_unique(); + EXPECT_NO_THROW(planner->configure(node, "test", nullptr, costmap_ros)); + planner->activate(); + EXPECT_TRUE(planner->hasSmootherInitialized()); + + auto dummy_cancel_checker = []() {return false;}; + std::vector 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); diff --git a/nav2_smac_planner/test/test_smac_lattice.cpp b/nav2_smac_planner/test/test_smac_lattice.cpp index 612b47a0b66..86e14fcab4e 100644 --- a/nav2_smac_planner/test/test_smac_lattice.cpp +++ b/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("SmacLatticeSmootherCovTest", options); + + std::shared_ptr costmap_ros = + std::make_shared("global_costmap"); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + + node->configure(); + node->activate(); + + auto planner = std::make_unique(); + EXPECT_NO_THROW(planner->configure(node, "test", nullptr, costmap_ros)); + planner->activate(); + EXPECT_TRUE(planner->hasSmootherInitialized()); + + auto dummy_cancel_checker = []() {return false;}; + std::vector 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); diff --git a/nav2_smac_planner/test/test_smoother.cpp b/nav2_smac_planner/test/test_smoother.cpp index 45d2f629421..e334bb52e8f 100644 --- a/nav2_smac_planner/test/test_smoother.cpp +++ b/nav2_smac_planner/test/test_smoother.cpp @@ -180,6 +180,83 @@ 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("SmacSmootherFootprintTest"); + nav2_smac_planner::SmootherParams params; + params.get(node, "test"); + double maxtime = 1.0; + + auto smoother = std::make_unique(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(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): 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); 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); + 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; + const size_t original_size = plan_copy.poses.size(); + 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; +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv);