minor changes in coarse search resolution

Signed-off-by: stevedan <[email protected]>
ros-navigation · Feb 5, 2025 · 009a8a9 · 009a8a9
1 parent b81f52e
commit 009a8a9
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 5 deletions.
diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp
@@ -286,7 +286,10 @@ void AStarAlgorithm<NodeT>::setGoal(
     _goals_vector.push_back(goals[i]);
   }
   // Configure the coarse search resolution
-  if(goal_heading_mode != GoalHeadingMode::ALL_DIRECTION || coarse_search_resolution == 0) {
+  if (_coarse_search_resolution <= 0) {
+    throw nav2_core::PlannerException("Invalid coarse search resolution, Cannot be <= 0");
+  }
+  else if(goal_heading_mode != GoalHeadingMode::ALL_DIRECTION) {
     _coarse_search_resolution = 1;
   } else {
     _coarse_search_resolution = coarse_search_resolution;

diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp
@@ -210,7 +210,8 @@ void SmacPlannerHybrid::configure(
       _logger, "coarse iteration resolution selected as <= 0, "
       "disabling coarse iteration resolution search for goal heading"
     );
-    _coarse_search_resolution = 0;
+
+    _coarse_search_resolution = 1;
   }
 
   if (_minimum_turning_radius_global_coords < _costmap->getResolution() * _downsampling_factor) {
@@ -723,7 +724,7 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector<rclcpp::Parameter> para
             _logger, "coarse iteration resolution selected as <= 0, "
             "disabling coarse iteration resolution search for goal heading"
           );
-          _coarse_search_resolution = 0;
+          _coarse_search_resolution = 1;
         }
       }
     }

diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp
@@ -181,7 +181,8 @@ void SmacPlannerLattice::configure(
       _logger, "coarse iteration resolution selected as <= 0, "
       "disabling coarse iteration resolution search for goal heading"
     );
-    _coarse_search_resolution = 0;
+    throw nav2_core::PlannerException("Invalid coarse search resolution");
+    _coarse_search_resolution = 1;
   }
   float lookup_table_dim =
     static_cast<float>(_lookup_table_size) /
@@ -615,7 +616,8 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector<rclcpp::Parameter> par
             _logger, "coarse iteration resolution selected as <= 0, "
             "disabling coarse iteration resolution search for goal heading"
           );
-          _coarse_search_resolution = 0;
+          throw nav2_core::PlannerException("Invalid coarse search resolution");
+          _coarse_search_resolution = 1;
         }
       }
     }