Add footprintAreaCost

nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp

@@ -52,6 +52,10 @@ class FootprintCollisionChecker
    * @brief Find the footprint cost in oriented footprint
    */
   double footprintCost(const Footprint & footprint);
+  /**
+   * @brief Find the footprint cost for entire footprint area using full coverage
+   */
+  double footprintAreaCost(const Footprint & footprint);
   /**
    * @brief Find the footprint cost a a post with an unoriented footprint
    */
@@ -81,6 +85,31 @@ class FootprintCollisionChecker
   }
 
 protected:
+  /**
+   * @brief Check if a point is inside the footprint polygon
+   * @param x X coordinate in world frame
+   * @param y Y coordinate in world frame  
+   * @param footprint Footprint polygon
+   * @return True if point is inside footprint
+   */
+  bool isPointInFootprint(
+    double x, double y,
+    const Footprint & footprint);
+
+  /**
+   * @brief Check if footprint is a rectangular (axis-aligned) shape
+   * @param footprint Footprint to check
+   * @param min_x Output minimum x coordinate
+   * @param max_x Output maximum x coordinate
+   * @param min_y Output minimum y coordinate
+   * @param max_y Output maximum y coordinate
+   * @return True if footprint is rectangular
+   */
+  bool isRectangularFootprint(
+    const Footprint & footprint, 
+    double & min_x, double & max_x, 
+    double & min_y, double & max_y) const;
+
   CostmapT costmap_;
 };
 

nav2_costmap_2d/src/footprint_collision_checker.cpp

@@ -84,6 +84,106 @@ double FootprintCollisionChecker<CostmapT>::footprintCost(const Footprint & foot
   return std::max(lineCost(xstart, x1, ystart, y1), footprint_cost);
 }
 
+template<typename CostmapT>
+bool FootprintCollisionChecker<CostmapT>::isRectangularFootprint(
+  const Footprint & footprint, double & min_x, double & max_x, 
+  double & min_y, double & max_y) const
+{
+  // Only 4-point footprints can be rectangles
+  if (footprint.size() != 4) {
+    return false;
+  }
+
+  // Find bounding box
+  min_x = std::numeric_limits<double>::max();
+  max_x = std::numeric_limits<double>::lowest();
+  min_y = std::numeric_limits<double>::max();
+  max_y = std::numeric_limits<double>::lowest();
+
+  for (const auto & point : footprint) {
+    min_x = std::min(min_x, point.x);
+    max_x = std::max(max_x, point.x);
+    min_y = std::min(min_y, point.y);
+    max_y = std::max(max_y, point.y);
+  }
+
+  // Check if all points are at corners of bounding box (axis-aligned rectangle)
+  const double tolerance = 1e-6;
+  int corner_count = 0;
+
+  for (const auto & point : footprint) {
+    bool at_corner = 
+      (std::abs(point.x - min_x) < tolerance || std::abs(point.x - max_x) < tolerance) &&
+      (std::abs(point.y - min_y) < tolerance || std::abs(point.y - max_y) < tolerance);
+
+    if (at_corner) {
+      corner_count++;
+    }
+  }
+
+  return corner_count == 4;
+}
+
+template<typename CostmapT>
+double FootprintCollisionChecker<CostmapT>::footprintAreaCost(const Footprint & footprint)
+{
+  // Find bounding box of footprint
+  double min_x = std::numeric_limits<double>::max();
+  double max_x = std::numeric_limits<double>::lowest();
+  double min_y = std::numeric_limits<double>::max();
+  double max_y = std::numeric_limits<double>::lowest();
+
+  for (const auto & point : footprint) {
+    min_x = std::min(min_x, point.x);
+    max_x = std::max(max_x, point.x);
+    min_y = std::min(min_y, point.y);
+    max_y = std::max(max_y, point.y);
+  }
+
+  // Convert to grid coordinates
+  unsigned int min_mx, min_my, max_mx, max_my;
+  if (!worldToMap(min_x, min_y, min_mx, min_my) ||
+      !worldToMap(max_x, max_y, max_mx, max_my)) {
+    return static_cast<double>(LETHAL_OBSTACLE);
+  }
+
+  double max_cost = 0.0;
+
+  // Check if footprint is rectangular for optimization
+  bool is_rectangular = isRectangularFootprint(footprint, min_x, max_x, min_y, max_y);
+
+  // Check all cells within bounding box
+  for (unsigned int mx = min_mx; mx <= max_mx; ++mx) {
+    for (unsigned int my = min_my; my <= max_my; ++my) {
+      bool cell_in_footprint;
+
+      if (is_rectangular) {
+        // For rectangular footprints, all cells in bounding box are inside
+        cell_in_footprint = true;
+      } else {
+        // For non-rectangular footprints, check point-in-polygon
+        double wx, wy;
+        costmap_->mapToWorld(mx, my, wx, wy);
+        cell_in_footprint = isPointInFootprint(wx, wy, footprint);
+      }
+
+      if (cell_in_footprint) {
+        double cost = pointCost(mx, my);
+
+        if (cost == static_cast<double>(LETHAL_OBSTACLE)) {
+          return cost;
+        } else if (cost == static_cast<double>(NO_INFORMATION)) {
+          // Return NO_INFORMATION for unknown cells, let caller decide how to handle
+          return cost;
+        }
+        max_cost = std::max(max_cost, cost);
+      }
+    }
+  }
+
+  return max_cost;
+}
+
 template<typename CostmapT>
 double FootprintCollisionChecker<CostmapT>::lineCost(int x0, int x1, int y0, int y1) const
 {
@@ -143,6 +243,41 @@ double FootprintCollisionChecker<CostmapT>::footprintCostAtPose(
   return footprintCost(oriented_footprint);
 }
 
+template<typename CostmapT>
+bool FootprintCollisionChecker<CostmapT>::isPointInFootprint(
+  double x, double y, 
+  const Footprint & footprint)
+{
+  // Adaptation of Shimrat, Moshe. "Algorithm 112: position of point relative to polygon."
+  // Communications of the ACM 5.8 (1962): 434.
+  // Implementation of ray crossings algorithm for point in polygon task solving.
+  // Y coordinate is fixed. Moving the ray on X+ axis starting from given point.
+  // Odd number of intersections with polygon boundaries means the point is inside polygon.
+  const int poly_size = footprint.size();
+  int i, j;  // Polygon vertex iterators
+  bool res = false;  // Final result, initialized with already inverted value
+
+  // Starting from the edge where the last point of polygon is connected to the first
+  i = poly_size - 1;
+  for (j = 0; j < poly_size; j++) {
+    // Checking the edge only if given point is between edge boundaries by Y coordinates.
+    // One of the condition should contain equality in order to exclude the edges
+    // parallel to X+ ray.
+    if ((y <= footprint[i].y) == (y > footprint[j].y)) {
+      // Calculating the intersection coordinate of X+ ray
+      const double x_inter = footprint[i].x +
+        (y - footprint[i].y) * (footprint[j].x - footprint[i].x) /
+        (footprint[j].y - footprint[i].y);
+      // If intersection with checked edge is greater than point.x coordinate, inverting the result
+      if (x_inter > x) {
+        res = !res;
+      }
+    }
+    i = j;
+  }
+  return res;
+}
+
 // declare our valid template parameters
 template class FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>;
 template class FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>;

nav2_smac_planner/src/collision_checker.cpp

@@ -135,9 +135,7 @@ bool GridCollisionChecker::inCollision(
       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
+    // Use full area checking instead of edge-only checking
     double wx, wy;
     costmap_->mapToWorld(static_cast<double>(x), static_cast<double>(y), wx, wy);
     geometry_msgs::msg::Point new_pt;
@@ -150,7 +148,8 @@ bool GridCollisionChecker::inCollision(
       current_footprint.push_back(new_pt);
     }
 
-    float footprint_cost = static_cast<float>(footprintCost(current_footprint));
+    // Check full area covered by footprint
+    float footprint_cost = static_cast<float>(footprintAreaCost(current_footprint));
 
     if (footprint_cost == UNKNOWN_COST && traverse_unknown) {
       return false;