If not downsampling costmap, default factor to 1 (#2695)

ros-navigation · Nov 19, 2021 · a9c5502 · a9c5502
1 parent 3b9ebdb
commit a9c5502
Showing 1 changed file with 6 additions and 0 deletions.
diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp
@@ -134,6 +134,9 @@ void SmacPlannerHybrid::configure(
   }
 
   // convert to grid coordinates
+  if (!_downsample_costmap) {
+    _downsampling_factor = 1;
+  }
   const double minimum_turning_radius_global_coords = _search_info.minimum_turning_radius;
   _search_info.minimum_turning_radius =
     _search_info.minimum_turning_radius / (_costmap->getResolution() * _downsampling_factor);
@@ -448,6 +451,9 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector<rclcpp::Parameter> para
   // Re-init if needed with mutex lock (to avoid re-init while creating a plan)
   if (reinit_a_star || reinit_downsampler || reinit_collision_checker || reinit_smoother) {
     // convert to grid coordinates
+    if (!_downsample_costmap) {
+      _downsampling_factor = 1;
+    }
     const double minimum_turning_radius_global_coords = _search_info.minimum_turning_radius;
     _search_info.minimum_turning_radius =
       _search_info.minimum_turning_radius / (_costmap->getResolution() * _downsampling_factor);