From 6f3437d1772d87ca231088f3f0eae1ff52303af1 Mon Sep 17 00:00:00 2001
From: Karl Schulz <karl@angsa-robotics.com>
Date: Tue, 20 Jul 2021 22:12:54 +0200
Subject: [PATCH] SMAC Planner: Treating "inscribed" costs not as collision
 anymore

---
 nav2_costmap_2d/src/footprint_collision_checker.cpp       | 8 ++------
 .../include/nav2_smac_planner/collision_checker.hpp       | 2 +-
 2 files changed, 3 insertions(+), 7 deletions(-)

diff --git a/nav2_costmap_2d/src/footprint_collision_checker.cpp b/nav2_costmap_2d/src/footprint_collision_checker.cpp
index 09876fb7c7..51c1fe7841 100644
--- 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
index 78f6c2e4ad..c38aeff101 100644
--- 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(