smac checking collision footprint only when in possibly inscribed situation (#2286)

* smac checking collision footprint only when in possibly inscribed situation

* adding intermediate check

* testing based on possibly inscribed conditions

* fix tests

nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp

@@ -69,7 +69,26 @@ public:
     costmap_->mapToWorld(static_cast<double>(x), static_cast<double>(y), wx, wy);
 
     if (!footprint_is_radius_) {
-      // if footprint, then we check for the footprint's points
+      // if footprint, then we check for the footprint's points, but first see
+      // if the robot is even potentially in an inscribed collision
+      footprint_cost_ = costmap_->getCost(
+        static_cast<unsigned int>(x), static_cast<unsigned int>(y));
+
+      if (footprint_cost_ < POSSIBLY_INSCRIBED) {
+        return false;
+      }
+
+      // If its inscribed, in collision, or unknown in the middle,
+      // no need to even check the footprint, its invalid
+      if (footprint_cost_ == UNKNOWN && !traverse_unknown) {
+        return true;
+      }
+
+      if (footprint_cost_ == INSCRIBED || footprint_cost_ == OCCUPIED) {
+        return true;
+      }
+
+      // if possible inscribed, need to check actual footprint pose
       footprint_cost_ = footprintCostAtPose(
         wx, wy, static_cast<double>(theta), unoriented_footprint_);
       if (footprint_cost_ == UNKNOWN && traverse_unknown) {

nav2_smac_planner/include/nav2_smac_planner/constants.hpp

@@ -63,6 +63,7 @@ const float UNKNOWN = 255;
 const float OCCUPIED = 254;
 const float INSCRIBED = 253;
 const float MAX_NON_OBSTACLE = 252;
+const float POSSIBLY_INSCRIBED = 128;
 const float FREE = 0;
 
 }  // namespace nav2_smac_planner

nav2_smac_planner/test/test_collision_checker.cpp

@@ -94,7 +94,7 @@ TEST(collision_footprint, test_footprint_at_pose_with_movement)
 
   for (unsigned int i = 40; i <= 60; ++i) {
     for (unsigned int j = 40; j <= 60; ++j) {
-      costmap_->setCost(i, j, 0);
+      costmap_->setCost(i, j, 128);
     }
   }
 
@@ -118,7 +118,7 @@ TEST(collision_footprint, test_footprint_at_pose_with_movement)
 
   collision_checker.inCollision(50, 50, 0.0, false);
   float cost = collision_checker.getCost();
-  EXPECT_NEAR(cost, 0.0, 0.001);
+  EXPECT_NEAR(cost, 128.0, 0.001);
 
   collision_checker.inCollision(50, 49, 0.0, false);
   float up_value = collision_checker.getCost();
@@ -133,8 +133,7 @@ TEST(collision_footprint, test_footprint_at_pose_with_movement)
 TEST(collision_footprint, test_point_and_line_cost)
 {
   nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(
-    100, 100, 0.10000, 0, 0.0,
-    0.0);
+    100, 100, 0.10000, 0, 0.0, 128.0);
 
   costmap_->setCost(62, 50, 254);
   costmap_->setCost(39, 60, 254);
@@ -159,7 +158,7 @@ TEST(collision_footprint, test_point_and_line_cost)
 
   collision_checker.inCollision(50, 50, 0.0, false);
   float value = collision_checker.getCost();
-  EXPECT_NEAR(value, 0.0, 0.001);
+  EXPECT_NEAR(value, 128.0, 0.001);
 
   collision_checker.inCollision(49, 50, 0.0, false);
   float left_value = collision_checker.getCost();