From cf96dbb77e49f2490ef1fee72cbedafad3505c95 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 2 Aug 2023 17:24:38 -0700 Subject: [PATCH] adding tolerance back in for smac lattice and hybrid-A* planners (#3734) Signed-off-by: enricosutera --- nav2_smac_planner/src/smac_planner_hybrid.cpp | 5 ++++- nav2_smac_planner/src/smac_planner_lattice.cpp | 4 +++- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 949b3cd73bb..acf518100b7 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -356,7 +356,10 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( expansions = std::make_unique>>(); } // Note: All exceptions thrown are handled by the planner server and returned to the action - if (!_a_star->createPath(path, num_iterations, 0, expansions.get())) { + if (!_a_star->createPath( + path, num_iterations, + _tolerance / static_cast(costmap->getResolution()), expansions.get())) + { if (num_iterations < _a_star->getMaxIterations()) { throw nav2_core::NoValidPathCouldBeFound("no valid path found"); } else { diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 66ddd95d743..8b46fc3ae74 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -284,7 +284,9 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( std::string error; // Note: All exceptions thrown are handled by the planner server and returned to the action - if (!_a_star->createPath(path, num_iterations, 0 /*no tolerance*/)) { + if (!_a_star->createPath( + path, num_iterations, _tolerance / static_cast(_costmap->getResolution()))) + { if (num_iterations < _a_star->getMaxIterations()) { throw nav2_core::NoValidPathCouldBeFound("no valid path found"); } else {