adding tolerance back in for smac lattice and hybrid-A* planners (#3734)

ros-navigation · Aug 3, 2023 · 762c99a · 762c99a
1 parent f424900
commit 762c99a
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 2 deletions.
diff --git 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<std::vector<std::tuple<float, float>>>();
   }
   // 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<float>(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
@@ -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<float>(_costmap->getResolution())))
+  {
     if (num_iterations < _a_star->getMaxIterations()) {
       throw nav2_core::NoValidPathCouldBeFound("no valid path found");
     } else {