Full-area footprint check

nav2_costmap_2d/CMakeLists.txt

@@ -25,6 +25,7 @@ find_package(tf2_ros REQUIRED)
 find_package(nav2_ros_common REQUIRED)
 find_package(tf2_sensor_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
+find_package(OpenCV REQUIRED)
 
 nav2_package()
 
@@ -60,6 +61,7 @@ target_link_libraries(nav2_costmap_2d_core PUBLIC
   tf2_ros::tf2_ros
   tf2_sensor_msgs::tf2_sensor_msgs
   nav2_ros_common::nav2_ros_common
+  ${OpenCV_LIBRARIES}
 )
 
 add_library(layers SHARED

nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp

@@ -50,12 +50,17 @@ class FootprintCollisionChecker
   explicit FootprintCollisionChecker(CostmapT costmap);
   /**
    * @brief Find the footprint cost in oriented footprint
+   * @param footprint The footprint to check
+   * @param check_full_area If true, checks the full area after perimeter check (default: false)
+   * @return The maximum cost found (perimeter only or full area depending on parameter)
    */
-  double footprintCost(const Footprint & footprint);
+  double footprintCost(const Footprint & footprint, bool check_full_area = false);
   /**
    * @brief Find the footprint cost a a post with an unoriented footprint
    */
-  double footprintCostAtPose(double x, double y, double theta, const Footprint & footprint);
+  double footprintCostAtPose(
+    double x, double y, double theta, const Footprint & footprint,
+    bool check_full_area = false);
   /**
    * @brief Get the cost for a line segment
    */

nav2_costmap_2d/package.xml

@@ -41,6 +41,7 @@
   <depend>tf2_sensor_msgs</depend>
   <depend>visualization_msgs</depend>
   <depend>nav2_ros_common</depend>
+  <depend>libopencv-dev</depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>

nav2_costmap_2d/src/footprint_collision_checker.cpp

@@ -18,6 +18,8 @@
 #include <string>
 #include <vector>
 #include <algorithm>
+#include <cmath>
+#include <opencv2/imgproc.hpp>
 
 #include "nav2_costmap_2d/footprint_collision_checker.hpp"
 
@@ -45,9 +47,11 @@ FootprintCollisionChecker<CostmapT>::FootprintCollisionChecker(
 }
 
 template<typename CostmapT>
-double FootprintCollisionChecker<CostmapT>::footprintCost(const Footprint & footprint)
+double FootprintCollisionChecker<CostmapT>::footprintCost(
+  const Footprint & footprint,
+  bool check_full_area)
 {
-  // now we really have to lay down the footprint in the costmap_ grid
+  // First check the perimeter
   unsigned int x0, x1, y0, y1;
   double footprint_cost = 0.0;
 
@@ -81,7 +85,66 @@ double FootprintCollisionChecker<CostmapT>::footprintCost(const Footprint & foot
 
   // we also need to connect the first point in the footprint to the last point
   // the last iteration's x1, y1 are the last footprint point's coordinates
-  return std::max(lineCost(xstart, x1, ystart, y1), footprint_cost);
+  double perimeter_cost = std::max(lineCost(xstart, x1, ystart, y1), footprint_cost);
+
+  // If perimeter check found collision or full area check not requested, return perimeter result
+  if (perimeter_cost == static_cast<double>(LETHAL_OBSTACLE) || !check_full_area) {
+    return perimeter_cost;
+  }
+
+  // If no collision on perimeter and full area check requested, rasterize the full area
+
+  // Convert footprint to map coordinates for rasterization
+  std::vector<cv::Point> polygon_points;
+  polygon_points.reserve(footprint.size());
+
+  for (const auto & point : footprint) {
+    unsigned int mx, my;
+    if (!worldToMap(point.x, point.y, mx, my)) {
+      return static_cast<double>(NO_INFORMATION);
+    }
+    polygon_points.emplace_back(static_cast<int>(mx), static_cast<int>(my));
+  }
+
+  // Find bounding box for the polygon
+  cv::Rect bbox = cv::boundingRect(polygon_points);
+
+  // Create a mask for the polygon area
+  cv::Mat mask = cv::Mat::zeros(bbox.height, bbox.width, CV_8UC1);
+
+  // Translate polygon points to mask coordinates (relative to bounding box)
+  std::vector<cv::Point> mask_points;
+  mask_points.reserve(polygon_points.size());
+  for (const auto & pt : polygon_points) {
+    mask_points.emplace_back(pt.x - bbox.x, pt.y - bbox.y);
+  }
+
+  // Fill the polygon in the mask using OpenCV rasterization
+  cv::fillPoly(mask, std::vector<std::vector<cv::Point>>{mask_points}, cv::Scalar(255));
+
+  double max_cost = perimeter_cost;
+
+  // Iterate through the mask and check costs only for cells inside the polygon
+  for (int y = 0; y < mask.rows; ++y) {
+    for (int x = 0; x < mask.cols; ++x) {
+      if (mask.at<uint8_t>(y, x) > 0) {  // Cell is inside polygon
+        // Convert back to map coordinates
+        unsigned int map_x = static_cast<unsigned int>(bbox.x + x);
+        unsigned int map_y = static_cast<unsigned int>(bbox.y + y);
+
+        double cell_cost = pointCost(static_cast<int>(map_x), static_cast<int>(map_y));
+
+        // Early termination if lethal obstacle found
+        if (cell_cost == static_cast<double>(LETHAL_OBSTACLE)) {
+          return cell_cost;
+        }
+
+        max_cost = std::max(max_cost, cell_cost);
+      }
+    }
+  }
+
+  return max_cost;
 }
 
 template<typename CostmapT>
@@ -127,7 +190,7 @@ void FootprintCollisionChecker<CostmapT>::setCostmap(CostmapT costmap)
 
 template<typename CostmapT>
 double FootprintCollisionChecker<CostmapT>::footprintCostAtPose(
-  double x, double y, double theta, const Footprint & footprint)
+  double x, double y, double theta, const Footprint & footprint, bool check_full_area)
 {
   double cos_th = cos(theta);
   double sin_th = sin(theta);
@@ -140,7 +203,7 @@ double FootprintCollisionChecker<CostmapT>::footprintCostAtPose(
     oriented_footprint.push_back(new_pt);
   }
 
-  return footprintCost(oriented_footprint);
+  return footprintCost(oriented_footprint, check_full_area);
 }
 
 // declare our valid template parameters