Fixes smac planner non-circular footprint search issue (backport #4851)…

… (#4852)

* Fixes smac planner non-circular footprint search issue (#4851)

* initial prototype to resolve smac planner issue

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

* fix test

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

---------

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
(cherry picked from commit bc9f2bc)

# Conflicts:
#	nav2_smac_planner/src/collision_checker.cpp
#	nav2_smac_planner/src/node_hybrid.cpp

* fix mergify issue

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

---------

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp

@@ -127,7 +127,7 @@ class GridCollisionChecker
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
   std::vector<nav2_costmap_2d::Footprint> oriented_footprints_;
   nav2_costmap_2d::Footprint unoriented_footprint_;
-  float footprint_cost_;
+  float center_cost_;
   bool footprint_is_radius_;
   std::vector<float> angles_;
   float possible_collision_cost_{-1};

nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp

@@ -441,14 +441,6 @@ class NodeHybrid
     const unsigned int & start_x, const unsigned int & start_y,
     const unsigned int & goal_x, const unsigned int & goal_y);
 
-  /**
-   * @brief Using the inflation layer, find the footprint's adjusted cost
-   * if the robot is non-circular
-   * @param cost Cost to adjust
-   * @return float Cost adjusted
-   */
-  static float adjustedFootprintCost(const float & cost);
-
   /**
    * @brief Retrieve all valid neighbors of a node.
    * @param validity_checker Functor for state validity checking
@@ -476,7 +468,6 @@ class NodeHybrid
     */
   static void destroyStaticAssets()
   {
-    inflation_layer.reset();
     costmap_ros.reset();
   }
 
@@ -491,7 +482,6 @@ class NodeHybrid
   static ObstacleHeuristicQueue obstacle_heuristic_queue;
 
   static std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros;
-  static std::shared_ptr<nav2_costmap_2d::InflationLayer> inflation_layer;
   // Dubin / Reeds-Shepp lookup and size for dereferencing
   static LookupTable dist_heuristic_lookup_table;
   static float size_lookup;

nav2_smac_planner/src/collision_checker.cpp

@@ -115,32 +115,31 @@ bool GridCollisionChecker::inCollision(
   }
 
   // Assumes setFootprint already set
-  double wx, wy;
-  costmap_->mapToWorld(static_cast<double>(x), static_cast<double>(y), wx, wy);
+  center_cost_ = static_cast<float>(costmap_->getCost(
+    static_cast<unsigned int>(x + 0.5f), static_cast<unsigned int>(y + 0.5f)));
 
   if (!footprint_is_radius_) {
     // 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_ = static_cast<float>(costmap_->getCost(
-        static_cast<unsigned int>(x + 0.5f), static_cast<unsigned int>(y + 0.5f)));
-
-    if (footprint_cost_ < possible_collision_cost_ && possible_collision_cost_ > 0.0f) {
+    if (center_cost_ < possible_collision_cost_ && possible_collision_cost_ > 0.0f) {
       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) {
+    if (center_cost_ == UNKNOWN && !traverse_unknown) {
       return true;
     }
 
-    if (footprint_cost_ == INSCRIBED || footprint_cost_ == OCCUPIED) {
+    if (center_cost_ == INSCRIBED || center_cost_ == OCCUPIED) {
       return true;
     }
 
     // if possible inscribed, need to check actual footprint pose.
     // Use precomputed oriented footprints are done on initialization,
     // offset by translation value to collision check
+    double wx, wy;
+    costmap_->mapToWorld(static_cast<double>(x), static_cast<double>(y), wx, wy);
     geometry_msgs::msg::Point new_pt;
     const nav2_costmap_2d::Footprint & oriented_footprint = oriented_footprints_[angle_bin];
     nav2_costmap_2d::Footprint current_footprint;
@@ -151,45 +150,42 @@ bool GridCollisionChecker::inCollision(
       current_footprint.push_back(new_pt);
     }
 
-    footprint_cost_ = static_cast<float>(footprintCost(current_footprint));
+    float footprint_cost = static_cast<float>(footprintCost(current_footprint));
 
-    if (footprint_cost_ == UNKNOWN && traverse_unknown) {
+    if (footprint_cost == UNKNOWN && traverse_unknown) {
       return false;
     }
 
     // if occupied or unknown and not to traverse unknown space
-    return footprint_cost_ >= OCCUPIED;
+    return footprint_cost >= OCCUPIED;
   } else {
     // if radius, then we can check the center of the cost assuming inflation is used
-    footprint_cost_ = static_cast<float>(costmap_->getCost(
-        static_cast<unsigned int>(x + 0.5f), static_cast<unsigned int>(y + 0.5f)));
-
-    if (footprint_cost_ == UNKNOWN && traverse_unknown) {
+    if (center_cost_ == UNKNOWN && traverse_unknown) {
       return false;
     }
 
     // if occupied or unknown and not to traverse unknown space
-    return footprint_cost_ >= INSCRIBED;
+    return center_cost_ >= INSCRIBED;
   }
 }
 
 bool GridCollisionChecker::inCollision(
   const unsigned int & i,
   const bool & traverse_unknown)
 {
-  footprint_cost_ = costmap_->getCost(i);
-  if (footprint_cost_ == UNKNOWN && traverse_unknown) {
+  center_cost_ = costmap_->getCost(i);
+  if (center_cost_ == UNKNOWN && traverse_unknown) {
     return false;
   }
 
   // if occupied or unknown and not to traverse unknown space
-  return footprint_cost_ >= INSCRIBED;
+  return center_cost_ >= INSCRIBED;
 }
 
 float GridCollisionChecker::getCost()
 {
   // Assumes inCollision called prior
-  return static_cast<float>(footprint_cost_);
+  return static_cast<float>(center_cost_);
 }
 
 bool GridCollisionChecker::outsideRange(const unsigned int & max, const float & value)

nav2_smac_planner/src/node_hybrid.cpp

@@ -41,7 +41,6 @@ HybridMotionTable NodeHybrid::motion_table;
 float NodeHybrid::size_lookup = 25;
 LookupTable NodeHybrid::dist_heuristic_lookup_table;
 std::shared_ptr<nav2_costmap_2d::Costmap2DROS> NodeHybrid::costmap_ros = nullptr;
-std::shared_ptr<nav2_costmap_2d::InflationLayer> NodeHybrid::inflation_layer = nullptr;
 
 ObstacleHeuristicQueue NodeHybrid::obstacle_heuristic_queue;
 
@@ -492,7 +491,6 @@ void NodeHybrid::resetObstacleHeuristic(
   // erosion of path quality after even modest smoothing. The error would be no more
   // than 0.05 * normalized cost. Since this is just a search prior, there's no loss in generality
   costmap_ros = costmap_ros_i;
-  inflation_layer = nav2_costmap_2d::InflationLayer::getInflationLayer(costmap_ros);
   auto costmap = costmap_ros->getCostmap();
 
   // Clear lookup table
@@ -539,37 +537,13 @@ void NodeHybrid::resetObstacleHeuristic(
   obstacle_heuristic_lookup_table[goal_index] = -0.00001f;
 }
 
-float NodeHybrid::adjustedFootprintCost(const float & cost)
-{
-  if (!inflation_layer) {
-    return cost;
-  }
-
-  const auto layered_costmap = costmap_ros->getLayeredCostmap();
-  const float scale_factor = inflation_layer->getCostScalingFactor();
-  const float min_radius = layered_costmap->getInscribedRadius();
-  float dist_to_obj = (scale_factor * min_radius - log(cost) + log(253.0f)) / scale_factor;
-
-  // Subtract minimum radius for edge cost
-  dist_to_obj -= min_radius;
-  if (dist_to_obj < 0.0f) {
-    dist_to_obj = 0.0f;
-  }
-
-  // Compute cost at this value
-  return static_cast<float>(
-    inflation_layer->computeCost(dist_to_obj / layered_costmap->getCostmap()->getResolution()));
-}
-
-
 float NodeHybrid::getObstacleHeuristic(
   const Coordinates & node_coords,
   const Coordinates & goal_coords,
   const float & cost_penalty)
 {
   // If already expanded, return the cost
   auto costmap = costmap_ros->getCostmap();
-  const bool is_circular = costmap_ros->getUseRadius();
   unsigned int size_x = 0u;
   unsigned int size_y = 0u;
   if (motion_table.downsample_obstacle_heuristic) {
@@ -671,13 +645,7 @@ float NodeHybrid::getObstacleHeuristic(
           cost = static_cast<float>(costmap->getCost(new_idx));
         }
 
-        if (!is_circular) {
-          // Adjust cost value if using SE2 footprint checks
-          cost = adjustedFootprintCost(cost);
-          if (cost >= OCCUPIED) {
-            continue;
-          }
-        } else if (cost >= INSCRIBED) {
+        if (cost >= INSCRIBED) {
           continue;
         }
 

nav2_smac_planner/test/test_collision_checker.cpp

@@ -153,17 +153,22 @@ TEST(collision_footprint, test_footprint_at_pose_with_movement)
   nav2_smac_planner::GridCollisionChecker collision_checker(costmap_ros, 72, node);
   collision_checker.setFootprint(footprint, false /*use footprint*/, 0.0);
 
-  collision_checker.inCollision(50, 50, 0.0, false);
+  EXPECT_FALSE(collision_checker.inCollision(50, 50, 0.0, false));
   float cost = collision_checker.getCost();
   EXPECT_NEAR(cost, 128.0, 0.001);
 
-  collision_checker.inCollision(50, 49, 0.0, false);
+  EXPECT_TRUE(collision_checker.inCollision(50, 49, 0.0, false));
   float up_value = collision_checker.getCost();
-  EXPECT_NEAR(up_value, 254.0, 0.001);
+  EXPECT_NEAR(up_value, 128.0, 0.001);  // center cost
 
-  collision_checker.inCollision(50, 52, 0.0, false);
+  EXPECT_TRUE(collision_checker.inCollision(50, 52, 0.0, false));
   float down_value = collision_checker.getCost();
-  EXPECT_NEAR(down_value, 254.0, 0.001);
+  EXPECT_NEAR(down_value, 128.0, 0.001);  // center cost
+
+  EXPECT_TRUE(collision_checker.inCollision(11, 11, 0.0, false));
+  float other_value = collision_checker.getCost();
+  EXPECT_NEAR(other_value, 254.0, 0.001);  // center cost
+
   delete costmap_;
 }
 
@@ -200,16 +205,21 @@ TEST(collision_footprint, test_point_and_line_cost)
   nav2_smac_planner::GridCollisionChecker collision_checker(costmap_ros, 72, node);
   collision_checker.setFootprint(footprint, false /*use footprint*/, 0.0);
 
-  collision_checker.inCollision(50, 50, 0.0, false);
+  EXPECT_FALSE(collision_checker.inCollision(50, 50, 0.0, false));
   float value = collision_checker.getCost();
   EXPECT_NEAR(value, 128.0, 0.001);
 
-  collision_checker.inCollision(49, 50, 0.0, false);
+  EXPECT_TRUE(collision_checker.inCollision(49, 50, 0.0, false));
   float left_value = collision_checker.getCost();
-  EXPECT_NEAR(left_value, 254.0, 0.001);
+  EXPECT_NEAR(left_value, 128.0, 0.001);  // center cost
 
-  collision_checker.inCollision(52, 50, 0.0, false);
+  EXPECT_TRUE(collision_checker.inCollision(52, 50, 0.0, false));
   float right_value = collision_checker.getCost();
-  EXPECT_NEAR(right_value, 254.0, 0.001);
+  EXPECT_NEAR(right_value, 128.0, 0.001);  // center cost
+
+  EXPECT_TRUE(collision_checker.inCollision(39, 60, 0.0, false));
+  float other_value = collision_checker.getCost();
+  EXPECT_NEAR(other_value, 254.0, 0.001);  // center cost
+
   delete costmap_;
 }