SMAC Planner: Treating "inscribed" costs not as collision anymore

ros-navigation · Jul 20, 2021 · 6f3437d · 6f3437d
1 parent 21a44bc
commit 6f3437d
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 7 deletions.
diff --git a/nav2_costmap_2d/src/footprint_collision_checker.cpp b/nav2_costmap_2d/src/footprint_collision_checker.cpp
@@ -75,9 +75,7 @@ double FootprintCollisionChecker<CostmapT>::footprintCost(const Footprint footpr
     y0 = y1;
 
     // if in collision, no need to continue
-    if (footprint_cost == static_cast<double>(INSCRIBED_INFLATED_OBSTACLE) ||
-      footprint_cost == static_cast<double>(LETHAL_OBSTACLE))
-    {
+    if (footprint_cost == static_cast<double>(LETHAL_OBSTACLE)) {
       return footprint_cost;
     }
   }
@@ -101,9 +99,7 @@ double FootprintCollisionChecker<CostmapT>::lineCost(int x0, int x1, int y0, int
     }
 
     // if in collision, no need to continue
-    if (line_cost == static_cast<double>(INSCRIBED_INFLATED_OBSTACLE) ||
-      line_cost == static_cast<double>(LETHAL_OBSTACLE))
-    {
+    if (line_cost == static_cast<double>(LETHAL_OBSTACLE)) {
       return line_cost;
     }
   }

diff --git a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp
@@ -150,7 +150,7 @@ class GridCollisionChecker
       }
 
       // if occupied or unknown and not to traverse unknown space
-      return footprint_cost_ >= INSCRIBED;
+      return footprint_cost_ >= OCCUPIED;
     } else {
       // if radius, then we can check the center of the cost assuming inflation is used
       footprint_cost_ = costmap_->getCost(