From 14b2cd88223eda36690fc55a583e89d724479194 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 5 Aug 2025 11:41:28 +0200 Subject: [PATCH 1/9] full footprint check Signed-off-by: Tony Najjar --- nav2_costmap_2d/CMakeLists.txt | 2 + .../footprint_collision_checker.hpp | 9 +- .../src/footprint_collision_checker.cpp | 74 +++- .../unit/footprint_collision_checker_test.cpp | 371 +++++++++++++++++- 4 files changed, 437 insertions(+), 19 deletions(-) diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index 221c5595515..3980ec19e23 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/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 diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp index c7cf2b54c6b..36b16755846 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp +++ b/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 */ diff --git a/nav2_costmap_2d/src/footprint_collision_checker.cpp b/nav2_costmap_2d/src/footprint_collision_checker.cpp index 9fc667d52e4..aa97d47c5f0 100644 --- a/nav2_costmap_2d/src/footprint_collision_checker.cpp +++ b/nav2_costmap_2d/src/footprint_collision_checker.cpp @@ -18,6 +18,9 @@ #include #include #include +#include + +#include #include "nav2_costmap_2d/footprint_collision_checker.hpp" @@ -45,9 +48,11 @@ FootprintCollisionChecker::FootprintCollisionChecker( } template -double FootprintCollisionChecker::footprintCost(const Footprint & footprint) +double FootprintCollisionChecker::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 +86,66 @@ double FootprintCollisionChecker::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(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 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(NO_INFORMATION); + } + polygon_points.emplace_back(static_cast(mx), static_cast(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 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>{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(y, x) > 0) { // Cell is inside polygon + // Convert back to map coordinates + unsigned int map_x = static_cast(bbox.x + x); + unsigned int map_y = static_cast(bbox.y + y); + + double cell_cost = pointCost(static_cast(map_x), static_cast(map_y)); + + // Early termination if lethal obstacle found + if (cell_cost == static_cast(LETHAL_OBSTACLE)) { + return cell_cost; + } + + max_cost = std::max(max_cost, cell_cost); + } + } + } + + return max_cost; } template @@ -127,7 +191,7 @@ void FootprintCollisionChecker::setCostmap(CostmapT costmap) template double FootprintCollisionChecker::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 +204,7 @@ double FootprintCollisionChecker::footprintCostAtPose( oriented_footprint.push_back(new_pt); } - return footprintCost(oriented_footprint); + return footprintCost(oriented_footprint, check_full_area); } // declare our valid template parameters diff --git a/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp b/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp index 7824d97e7ce..59ccb695d38 100644 --- a/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp +++ b/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp @@ -15,27 +15,33 @@ #include #include #include +#include #include "gtest/gtest.h" #include "nav2_costmap_2d/footprint_collision_checker.hpp" #include "nav2_costmap_2d/footprint.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +// Test basic footprint collision checking with no obstacles +// Verifies that a diamond-shaped footprint placed on an empty costmap returns zero cost TEST(collision_footprint, test_basic) { + // Create a 100x100 costmap with 0.1m resolution, all cells initialized to cost 0 std::shared_ptr costmap_ = std::make_shared(100, 100, 0.1, 0, 0, 0); + // Create a diamond-shaped footprint geometry_msgs::msg::Point p1; - p1.x = -0.5; + p1.x = -0.5; // Left vertex p1.y = 0.0; geometry_msgs::msg::Point p2; - p2.x = 0.0; + p2.x = 0.0; // Top vertex p2.y = 0.5; geometry_msgs::msg::Point p3; - p3.x = 0.5; + p3.x = 0.5; // Right vertex p3.y = 0.0; geometry_msgs::msg::Point p4; - p4.x = 0.0; + p4.x = 0.0; // Bottom vertex p4.y = -0.5; nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; @@ -43,24 +49,33 @@ TEST(collision_footprint, test_basic) nav2_costmap_2d::FootprintCollisionChecker> collision_checker(costmap_); + // Check footprint cost at position (5.0, 5.0) with no rotation + // Should return 0 since no obstacles are present auto value = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint); EXPECT_NEAR(value, 0.0, 0.001); } +// Test point cost functionality +// Verifies that the pointCost method correctly returns the cost value at a specific grid cell TEST(collision_footprint, test_point_cost) { + // Create empty costmap with all cells at cost 0 std::shared_ptr costmap_ = std::make_shared(100, 100, 0.1, 0, 0, 0); nav2_costmap_2d::FootprintCollisionChecker> collision_checker(costmap_); + // Test point cost at grid cell (50, 50) - should be 0 for empty costmap auto value = collision_checker.pointCost(50, 50); EXPECT_NEAR(value, 0.0, 0.001); } +// Test world-to-map coordinate conversion and point cost retrieval +// Verifies that world coordinates are correctly converted to map coordinates +// and that costs can be retrieved and set at those locations TEST(collision_footprint, test_world_to_map) { std::shared_ptr costmap_ = @@ -71,40 +86,50 @@ TEST(collision_footprint, test_world_to_map) unsigned int x, y; + // Convert world coordinates (1.0, 1.0) to map coordinates + // With 0.1m resolution and origin at (0,0), this should map to grid cell (10, 10) collision_checker.worldToMap(1.0, 1.0, x, y); + // Check that the cost at the converted coordinates is 0 (empty space) auto value = collision_checker.pointCost(x, y); - EXPECT_NEAR(value, 0.0, 0.001); + // Set a cost of 200 at grid cell (50, 50) and verify it can be retrieved costmap_->setCost(50, 50, 200); - collision_checker.worldToMap(5.0, 5.0, x, y); + collision_checker.worldToMap(5.0, 5.0, x, y); // World (5.0, 5.0) -> Grid (50, 50) EXPECT_NEAR(collision_checker.pointCost(x, y), 200.0, 0.001); } +// Test footprint collision detection when robot moves into obstacle areas +// Creates a costmap with obstacles everywhere except a safe zone, then tests +// footprint placement at different positions to verify collision detection TEST(collision_footprint, test_footprint_at_pose_with_movement) { + // Create costmap with all cells set to cost 254 (high obstacle cost) std::shared_ptr costmap_ = std::make_shared(100, 100, 0.1, 0, 0, 254); + // Create a safe zone (cost 0) in the center of the map from grid (40,40) to (60,60) + // This corresponds to world coordinates roughly (4.0,4.0) to (6.0,6.0) for (unsigned int i = 40; i <= 60; ++i) { for (unsigned int j = 40; j <= 60; ++j) { costmap_->setCost(i, j, 0); } } + // Create a 2x2 meter square footprint geometry_msgs::msg::Point p1; - p1.x = -1.0; + p1.x = -1.0; // Bottom-left p1.y = 1.0; geometry_msgs::msg::Point p2; - p2.x = 1.0; + p2.x = 1.0; // Bottom-right p2.y = 1.0; geometry_msgs::msg::Point p3; - p3.x = 1.0; + p3.x = 1.0; // Top-right p3.y = -1.0; geometry_msgs::msg::Point p4; - p4.x = -1.0; + p4.x = -1.0; // Top-left p4.y = -1.0; nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; @@ -112,24 +137,32 @@ TEST(collision_footprint, test_footprint_at_pose_with_movement) nav2_costmap_2d::FootprintCollisionChecker> collision_checker(costmap_); + // Test footprint at center of safe zone - should have zero cost auto value = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint); EXPECT_NEAR(value, 0.0, 0.001); + // Test footprint moved slightly up - should hit obstacle area (cost 254) auto up_value = collision_checker.footprintCostAtPose(5.0, 4.9, 0.0, footprint); EXPECT_NEAR(up_value, 254.0, 0.001); + // Test footprint moved slightly down - should hit obstacle area (cost 254) auto down_value = collision_checker.footprintCostAtPose(5.0, 5.2, 0.0, footprint); EXPECT_NEAR(down_value, 254.0, 0.001); } +// Test point and line cost detection with strategically placed obstacles +// Places obstacles at specific locations and verifies they are detected when the +// footprint perimeter intersects them during different robot movements TEST(collision_footprint, test_point_and_line_cost) { std::shared_ptr costmap_ = std::make_shared(100, 100, 0.10000, 0, 0.0, 0.0); - costmap_->setCost(62, 50, 254); - costmap_->setCost(39, 60, 254); + // Place obstacles at strategic locations: + costmap_->setCost(62, 50, 254); // Obstacle to the right of center + costmap_->setCost(39, 60, 254); // Obstacle to the left and up from center + // Create a 2x2 meter square footprint geometry_msgs::msg::Point p1; p1.x = -1.0; p1.y = 1.0; @@ -148,16 +181,22 @@ TEST(collision_footprint, test_point_and_line_cost) nav2_costmap_2d::FootprintCollisionChecker> collision_checker(costmap_); + // Test footprint at center position - should not hit obstacles auto value = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint); EXPECT_NEAR(value, 0.0, 0.001); + // Move footprint left - should detect obstacle at (39, 60) auto left_value = collision_checker.footprintCostAtPose(4.9, 5.0, 0.0, footprint); EXPECT_NEAR(left_value, 254.0, 0.001); + // Move footprint right - should detect obstacle at (62, 50) auto right_value = collision_checker.footprintCostAtPose(5.2, 5.0, 0.0, footprint); EXPECT_NEAR(right_value, 254.0, 0.001); } +// Test utility function for footprints with insufficient points +// Verifies that distance calculation functions handle edge cases correctly +// when footprint has fewer than 3 points (minimum for a valid polygon) TEST(collision_footprint, not_enough_points) { geometry_msgs::msg::Point p1; @@ -168,28 +207,37 @@ TEST(collision_footprint, not_enough_points) p2.x = -2.0; p2.y = -2.0; + // Create footprint with only 2 points (insufficient for polygon) std::vector footprint = {p1, p2}; double min_dist = 0.0; double max_dist = 0.0; + // calculateMinAndMaxDistances should handle this gracefully std::tie(min_dist, max_dist) = nav2_costmap_2d::calculateMinAndMaxDistances(footprint); EXPECT_EQ(min_dist, std::numeric_limits::max()); EXPECT_EQ(max_dist, 0.0f); } +// Test conversion from Point to Point32 message types +// Verifies that geometry message type conversions preserve coordinate values TEST(collision_footprint, to_point_32) { geometry_msgs::msg::Point p; p.x = 123.0; p.y = 456.0; p.z = 789.0; + // Convert to Point32 format geometry_msgs::msg::Point32 p32; p32 = nav2_costmap_2d::toPoint32(p); + + // Verify all coordinates are preserved EXPECT_NEAR(p.x, p32.x, 1e-5); EXPECT_NEAR(p.y, p32.y, 1e-5); EXPECT_NEAR(p.z, p32.z, 1e-5); } +// Test conversion from vector of Points to Polygon message type +// Verifies that footprint data can be converted to ROS message format TEST(collision_footprint, to_polygon) { geometry_msgs::msg::Point p1; p1.x = 1.2; @@ -202,9 +250,11 @@ TEST(collision_footprint, to_polygon) { p2.z = -9.1; std::vector pts = {p1, p2}; + // Convert to Polygon message format geometry_msgs::msg::Polygon poly; poly = nav2_costmap_2d::toPolygon(pts); + // Verify structure and coordinate preservation EXPECT_EQ(2u, sizeof(poly.points) / sizeof(poly.points[0])); EXPECT_NEAR(poly.points[0].x, p1.x, 1e-5); EXPECT_NEAR(poly.points[0].y, p1.y, 1e-5); @@ -214,12 +264,19 @@ TEST(collision_footprint, to_polygon) { EXPECT_NEAR(poly.points[1].z, p2.z, 1e-5); } +// Test successful parsing of footprint from string representation +// Verifies that string-based footprint configuration works correctly TEST(collision_footprint, make_footprint_from_string) { std::vector footprint; + + // Parse a valid footprint string with scientific notation bool result = nav2_costmap_2d::makeFootprintFromString( "[[1, 2.2], [.3, -4e4], [-.3, -4e4], [-1, 2.2]]", footprint); + EXPECT_EQ(result, true); EXPECT_EQ(4u, footprint.size()); + + // Verify all coordinate values are parsed correctly EXPECT_NEAR(footprint[0].x, 1.0, 1e-5); EXPECT_NEAR(footprint[0].y, 2.2, 1e-5); EXPECT_NEAR(footprint[1].x, 0.3, 1e-5); @@ -230,23 +287,313 @@ TEST(collision_footprint, make_footprint_from_string) { EXPECT_NEAR(footprint[3].y, 2.2, 1e-5); } +// Test error handling for malformed footprint strings +// Verifies that parsing fails gracefully with invalid input TEST(collision_footprint, make_footprint_from_string_parse_error) { std::vector footprint; + + // Try to parse a malformed string (missing closing bracket) bool result = nav2_costmap_2d::makeFootprintFromString( "[[bad_string", footprint); EXPECT_EQ(result, false); } +// Test error handling for footprints with too few points +// Verifies that polygons with insufficient vertices are rejected TEST(collision_footprint, make_footprint_from_string_two_points_error) { std::vector footprint; + + // Try to parse footprint with only 2 points (need at least 3 for polygon) bool result = nav2_costmap_2d::makeFootprintFromString( "[[1, 2.2], [.3, -4e4]", footprint); EXPECT_EQ(result, false); } +// Test error handling for points with wrong number of coordinates +// Verifies that points with extra coordinates are rejected TEST(collision_footprint, make_footprint_from_string_not_pairs) { std::vector footprint; + + // Try to parse footprint with a point having 3 coordinates (should be 2) bool result = nav2_costmap_2d::makeFootprintFromString( "[[1, 2.2], [.3, -4e4], [-.3, -4e4], [-1, 2.2, 5.6]]", footprint); EXPECT_EQ(result, false); } + +// Test full area checking functionality when no obstacles are present +// Verifies that both perimeter-only and full-area checking return the same result +// when the footprint area is completely free of obstacles +TEST(collision_footprint, test_full_area_check_no_collision) { + std::shared_ptr costmap_ = + std::make_shared(100, 100, 0.1, 0, 0, 0); + + // Create a 2x2 meter rectangular footprint + geometry_msgs::msg::Point p1; + p1.x = -1.0; // Bottom-left + p1.y = -1.0; + geometry_msgs::msg::Point p2; + p2.x = 1.0; // Bottom-right + p2.y = -1.0; + geometry_msgs::msg::Point p3; + p3.x = 1.0; // Top-right + p3.y = 1.0; + geometry_msgs::msg::Point p4; + p4.x = -1.0; // Top-left + p4.y = 1.0; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; + + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap_); + + // Test perimeter only check (default behavior) + auto perimeter_cost = collision_checker.footprintCost(footprint, false); + EXPECT_NEAR(perimeter_cost, 0.0, 0.001); + + // Test full area check (new functionality) + auto full_area_cost = collision_checker.footprintCost(footprint, true); + EXPECT_NEAR(full_area_cost, 0.0, 0.001); + + // Both should be the same when no obstacles present + EXPECT_NEAR(perimeter_cost, full_area_cost, 0.001); +} + +// Test that full area checking detects obstacles inside the footprint +// Obstacles that are inside the footprint but not on the perimeter +// should only be detected by full area checking +TEST(collision_footprint, test_full_area_check_interior_obstacle) { + std::shared_ptr costmap_ = + std::make_shared(100, 100, 0.1, 0, 0, 0); + + // Create a 2x2 meter rectangular footprint that will be centered at (5.0, 5.0) + // This maps to grid coordinates around (50, 50) with the 0.1m resolution + geometry_msgs::msg::Point p1; + p1.x = -1.0; + p1.y = -1.0; + geometry_msgs::msg::Point p2; + p2.x = 1.0; + p2.y = -1.0; + geometry_msgs::msg::Point p3; + p3.x = 1.0; + p3.y = 1.0; + geometry_msgs::msg::Point p4; + p4.x = -1.0; + p4.y = 1.0; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; + + // Place an obstacle in the interior of the footprint (not on perimeter) + // Grid cell (50, 50) corresponds to world coordinates (5.0, 5.0) - center of footprint + costmap_->setCost(50, 50, 200); + + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap_); + + // Test perimeter only check - should NOT detect interior obstacle + auto perimeter_cost = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint, false); + EXPECT_NEAR(perimeter_cost, 0.0, 0.001); + + // Test full area check - SHOULD detect interior obstacle + auto full_area_cost = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint, true); + EXPECT_NEAR(full_area_cost, 200.0, 0.001); +} + +// Test full area checking with lethal obstacles for early termination +// Verifies that when a lethal obstacle is found during full area checking, +// the algorithm returns immediately with lethal cost (performance optimization) +TEST(collision_footprint, test_full_area_check_lethal_obstacle) { + std::shared_ptr costmap_ = + std::make_shared(100, 100, 0.1, 0, 0, 0); + + // Create a triangular footprint (simpler shape for testing) + geometry_msgs::msg::Point p1; + p1.x = 0.0; // Top vertex + p1.y = 1.0; + geometry_msgs::msg::Point p2; + p2.x = -1.0; // Bottom-left vertex + p2.y = -1.0; + geometry_msgs::msg::Point p3; + p3.x = 1.0; // Bottom-right vertex + p3.y = -1.0; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3}; + + // Place a lethal obstacle in the interior of the triangle + // Grid cell (50, 49) should be inside the triangle when centered at (5.0, 5.0) + costmap_->setCost(50, 49, nav2_costmap_2d::LETHAL_OBSTACLE); + + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap_); + + // Test full area check with lethal obstacle - should return lethal cost immediately + auto full_area_cost = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint, true); + EXPECT_NEAR(full_area_cost, static_cast(nav2_costmap_2d::LETHAL_OBSTACLE), 0.001); +} + +// Test full area checking with rotated footprints +// Verifies that the full area checking works correctly when the footprint +// is rotated, ensuring the OpenCV polygon rasterization handles rotation properly +TEST(collision_footprint, test_full_area_check_rotated_footprint) { + std::shared_ptr costmap_ = + std::make_shared(100, 100, 0.1, 0, 0, 0); + + // Create a rectangular footprint that's taller than it is wide + // This will test rotation effects more clearly + geometry_msgs::msg::Point p1; + p1.x = -0.5; // Left edge + p1.y = -1.5; // Bottom edge (tall rectangle) + geometry_msgs::msg::Point p2; + p2.x = 0.5; // Right edge + p2.y = -1.5; + geometry_msgs::msg::Point p3; + p3.x = 0.5; + p3.y = 1.5; // Top edge + geometry_msgs::msg::Point p4; + p4.x = -0.5; + p4.y = 1.5; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; + + // Place obstacle where it would be inside the footprint when rotated 90 degrees + // Grid cell (52, 50) should be inside the rotated footprint + costmap_->setCost(52, 50, 150); + + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap_); + + // Test with 90-degree rotation (M_PI/2 radians) + // The tall rectangle becomes a wide rectangle, covering the obstacle + auto rotated_cost = collision_checker.footprintCostAtPose(5.0, 5.0, M_PI/2, footprint, true); + EXPECT_GT(rotated_cost, 0.0); // Should detect the obstacle +} + +// Test the lineCost function directly +// Verifies that line cost calculation works correctly for line segments, +// including early termination when lethal obstacles are encountered +TEST(collision_footprint, test_line_cost_function) { + std::shared_ptr costmap_ = + std::make_shared(100, 100, 0.1, 0, 0, 0); + + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap_); + + // Test line cost with no obstacles - should return 0 + auto line_cost = collision_checker.lineCost(10, 20, 10, 20); + EXPECT_NEAR(line_cost, 0.0, 0.001); + + // Add obstacle in the line path from (10,10) to (20,20) + costmap_->setCost(15, 15, 100); + line_cost = collision_checker.lineCost(10, 20, 10, 20); + EXPECT_NEAR(line_cost, 100.0, 0.001); + + // Test with lethal obstacle - should return immediately without checking rest of line + costmap_->setCost(12, 12, nav2_costmap_2d::LETHAL_OBSTACLE); + line_cost = collision_checker.lineCost(10, 20, 10, 20); + EXPECT_NEAR(line_cost, static_cast(nav2_costmap_2d::LETHAL_OBSTACLE), 0.001); +} + +// Test handling of footprints that extend outside the costmap boundaries +// Verifies that out-of-bounds footprint coordinates are handled correctly +// and return lethal cost (indicating collision/invalid placement) +TEST(collision_footprint, test_out_of_bounds_footprint) { + std::shared_ptr costmap_ = + std::make_shared(100, 100, 0.1, 0, 0, 0); + + // Create a large footprint that will extend beyond map boundaries + geometry_msgs::msg::Point p1; + p1.x = -2.0; // Will extend to negative world coordinates + p1.y = -2.0; + geometry_msgs::msg::Point p2; + p2.x = 2.0; + p2.y = -2.0; + geometry_msgs::msg::Point p3; + p3.x = 2.0; + p3.y = 2.0; + geometry_msgs::msg::Point p4; + p4.x = -2.0; + p4.y = 2.0; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; + + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap_); + + // Place footprint so part of it extends outside the map (negative world coordinates) + // With costmap starting at origin (0,0), placing at (-1.0, -1.0) will put parts out of bounds + auto out_of_bounds_cost = collision_checker.footprintCostAtPose(-1.0, -1.0, 0.0, footprint, true); + EXPECT_EQ(out_of_bounds_cost, static_cast(nav2_costmap_2d::LETHAL_OBSTACLE)); +} + +// Test the setCostmap and getCostmap functionality +// Verifies that the collision checker can switch between different costmaps +// and that cost queries are performed on the correct costmap +TEST(collision_footprint, test_set_costmap_function) { + // Create two different costmaps with different sizes + std::shared_ptr costmap1 = + std::make_shared(50, 50, 0.1, 0, 0, 0); + std::shared_ptr costmap2 = + std::make_shared(100, 100, 0.1, 0, 0, 0); + + // Set different costs at the same grid location in both costmaps + costmap1->setCost(25, 25, 100); + costmap2->setCost(25, 25, 200); + + // Initialize collision checker with first costmap + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap1); + + // Test with first costmap - should return cost 100 + auto cost1 = collision_checker.pointCost(25, 25); + EXPECT_NEAR(cost1, 100.0, 0.001); + + // Switch to second costmap using setCostmap + collision_checker.setCostmap(costmap2); + auto cost2 = collision_checker.pointCost(25, 25); + EXPECT_NEAR(cost2, 200.0, 0.001); + + // Verify getCostmap function returns the correct costmap reference + auto retrieved_costmap = collision_checker.getCostmap(); + EXPECT_EQ(retrieved_costmap, costmap2); +} + +// Test full area checking with complex polygons (non-rectangular shapes) +// Verifies that the OpenCV polygon rasterization works correctly for +// more complex shapes like pentagons, ensuring the implementation is robust +TEST(collision_footprint, test_complex_polygon_full_area) { + std::shared_ptr costmap_ = + std::make_shared(100, 100, 0.1, 0, 0, 0); + + // Create a complex polygon (pentagon) to test non-trivial polygon rasterization + geometry_msgs::msg::Point p1; + p1.x = 0.0; // Top vertex + p1.y = 1.5; + geometry_msgs::msg::Point p2; + p2.x = 1.4; // Top-right vertex + p2.y = 0.5; + geometry_msgs::msg::Point p3; + p3.x = 0.9; // Bottom-right vertex + p3.y = -1.2; + geometry_msgs::msg::Point p4; + p4.x = -0.9; // Bottom-left vertex + p4.y = -1.2; + geometry_msgs::msg::Point p5; + p5.x = -1.4; // Top-left vertex + p5.y = 0.5; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4, p5}; + + // Place obstacle in what should be the interior of the pentagon + costmap_->setCost(50, 50, 180); + + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap_); + + // Test that full area check detects interior obstacle in complex polygon + auto perimeter_cost = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint, false); + auto full_area_cost = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint, true); + + // Perimeter should not detect interior obstacle + EXPECT_LT(perimeter_cost, 180.0); + // Full area should detect it (demonstrating the value of full area checking) + EXPECT_NEAR(full_area_cost, 180.0, 0.001); +} From 56b79ba42486e2290aeb7093bdc9695fa43d47c9 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 5 Aug 2025 11:50:34 +0200 Subject: [PATCH 2/9] fix Signed-off-by: Tony Najjar --- .../test/unit/footprint_collision_checker_test.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp b/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp index 59ccb695d38..44b933624c9 100644 --- a/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp +++ b/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp @@ -346,12 +346,12 @@ TEST(collision_footprint, test_full_area_check_no_collision) { nav2_costmap_2d::FootprintCollisionChecker> collision_checker(costmap_); - // Test perimeter only check (default behavior) - auto perimeter_cost = collision_checker.footprintCost(footprint, false); + // Test perimeter only check at a valid position (5.0, 5.0) within the 10x10m costmap + auto perimeter_cost = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint, false); EXPECT_NEAR(perimeter_cost, 0.0, 0.001); - // Test full area check (new functionality) - auto full_area_cost = collision_checker.footprintCost(footprint, true); + // Test full area check at the same position + auto full_area_cost = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint, true); EXPECT_NEAR(full_area_cost, 0.0, 0.001); // Both should be the same when no obstacles present From 457641f7ac6ce544d33ba6e7e28655538869cb4e Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 5 Aug 2025 12:10:05 +0200 Subject: [PATCH 3/9] linting Signed-off-by: Tony Najjar --- .../src/footprint_collision_checker.cpp | 1 - .../unit/footprint_collision_checker_test.cpp | 16 ++++++++-------- 2 files changed, 8 insertions(+), 9 deletions(-) diff --git a/nav2_costmap_2d/src/footprint_collision_checker.cpp b/nav2_costmap_2d/src/footprint_collision_checker.cpp index aa97d47c5f0..7bf7dfc4a4b 100644 --- a/nav2_costmap_2d/src/footprint_collision_checker.cpp +++ b/nav2_costmap_2d/src/footprint_collision_checker.cpp @@ -19,7 +19,6 @@ #include #include #include - #include #include "nav2_costmap_2d/footprint_collision_checker.hpp" diff --git a/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp b/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp index 44b933624c9..a41d69c520e 100644 --- a/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp +++ b/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp @@ -229,7 +229,7 @@ TEST(collision_footprint, to_point_32) { // Convert to Point32 format geometry_msgs::msg::Point32 p32; p32 = nav2_costmap_2d::toPoint32(p); - + // Verify all coordinates are preserved EXPECT_NEAR(p.x, p32.x, 1e-5); EXPECT_NEAR(p.y, p32.y, 1e-5); @@ -268,14 +268,14 @@ TEST(collision_footprint, to_polygon) { // Verifies that string-based footprint configuration works correctly TEST(collision_footprint, make_footprint_from_string) { std::vector footprint; - + // Parse a valid footprint string with scientific notation bool result = nav2_costmap_2d::makeFootprintFromString( "[[1, 2.2], [.3, -4e4], [-.3, -4e4], [-1, 2.2]]", footprint); - + EXPECT_EQ(result, true); EXPECT_EQ(4u, footprint.size()); - + // Verify all coordinate values are parsed correctly EXPECT_NEAR(footprint[0].x, 1.0, 1e-5); EXPECT_NEAR(footprint[0].y, 2.2, 1e-5); @@ -291,7 +291,7 @@ TEST(collision_footprint, make_footprint_from_string) { // Verifies that parsing fails gracefully with invalid input TEST(collision_footprint, make_footprint_from_string_parse_error) { std::vector footprint; - + // Try to parse a malformed string (missing closing bracket) bool result = nav2_costmap_2d::makeFootprintFromString( "[[bad_string", footprint); @@ -302,7 +302,7 @@ TEST(collision_footprint, make_footprint_from_string_parse_error) { // Verifies that polygons with insufficient vertices are rejected TEST(collision_footprint, make_footprint_from_string_two_points_error) { std::vector footprint; - + // Try to parse footprint with only 2 points (need at least 3 for polygon) bool result = nav2_costmap_2d::makeFootprintFromString( "[[1, 2.2], [.3, -4e4]", footprint); @@ -313,7 +313,7 @@ TEST(collision_footprint, make_footprint_from_string_two_points_error) { // Verifies that points with extra coordinates are rejected TEST(collision_footprint, make_footprint_from_string_not_pairs) { std::vector footprint; - + // Try to parse footprint with a point having 3 coordinates (should be 2) bool result = nav2_costmap_2d::makeFootprintFromString( "[[1, 2.2], [.3, -4e4], [-.3, -4e4], [-1, 2.2, 5.6]]", footprint); @@ -463,7 +463,7 @@ TEST(collision_footprint, test_full_area_check_rotated_footprint) { // Test with 90-degree rotation (M_PI/2 radians) // The tall rectangle becomes a wide rectangle, covering the obstacle - auto rotated_cost = collision_checker.footprintCostAtPose(5.0, 5.0, M_PI/2, footprint, true); + auto rotated_cost = collision_checker.footprintCostAtPose(5.0, 5.0, M_PI / 2, footprint, true); EXPECT_GT(rotated_cost, 0.0); // Should detect the obstacle } From 102a964eec1f9def1ef7caa3d03b2fd7f9a45da0 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 5 Aug 2025 12:23:50 +0200 Subject: [PATCH 4/9] Add libopencv-dev as a dependency in package.xml Signed-off-by: Tony Najjar --- nav2_costmap_2d/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index c01e7bb2dd6..63de100d92d 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -41,6 +41,7 @@ tf2_sensor_msgs visualization_msgs nav2_ros_common + libopencv-dev ament_lint_common ament_lint_auto From 3501a0fbe6498fd30c857c52c7aecb6ac4f1554e Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 6 Aug 2025 17:54:57 +0200 Subject: [PATCH 5/9] replace opencv implementation Signed-off-by: Tony Najjar --- nav2_costmap_2d/CMakeLists.txt | 2 - nav2_costmap_2d/package.xml | 1 - .../src/footprint_collision_checker.cpp | 90 +++++++++++--- .../unit/footprint_collision_checker_test.cpp | 111 ++++++++++++++++++ 4 files changed, 184 insertions(+), 20 deletions(-) diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index 3980ec19e23..221c5595515 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -25,7 +25,6 @@ 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() @@ -61,7 +60,6 @@ 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 diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index 63de100d92d..c01e7bb2dd6 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -41,7 +41,6 @@ tf2_sensor_msgs visualization_msgs nav2_ros_common - libopencv-dev ament_lint_common ament_lint_auto diff --git a/nav2_costmap_2d/src/footprint_collision_checker.cpp b/nav2_costmap_2d/src/footprint_collision_checker.cpp index 7bf7dfc4a4b..18e100c204b 100644 --- a/nav2_costmap_2d/src/footprint_collision_checker.cpp +++ b/nav2_costmap_2d/src/footprint_collision_checker.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2019 Intel Corporation +// Copyright (c) 2025 Angsa Robotics // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -19,7 +20,6 @@ #include #include #include -#include #include "nav2_costmap_2d/footprint_collision_checker.hpp" @@ -33,6 +33,68 @@ using namespace std::chrono_literals; namespace nav2_costmap_2d { +// Simple 2D point structure +struct Point2D +{ + int x, y; + Point2D(int x_val, int y_val) + : x(x_val), y(y_val) {} +}; + +// Simple rectangle structure +struct Rectangle +{ + int x, y, width, height; + Rectangle(int x_val, int y_val, int w, int h) + : x(x_val), y(y_val), width(w), height(h) {} +}; + +// Calculate bounding rectangle for a set of points +Rectangle calculateBoundingRect(const std::vector & points) +{ + if (points.empty()) { + return Rectangle(0, 0, 0, 0); + } + + int min_x = points[0].x; + int max_x = points[0].x; + int min_y = points[0].y; + int max_y = points[0].y; + + for (const auto & pt : points) { + min_x = std::min(min_x, pt.x); + max_x = std::max(max_x, pt.x); + min_y = std::min(min_y, pt.y); + max_y = std::max(max_y, pt.y); + } + + return Rectangle(min_x, min_y, max_x - min_x + 1, max_y - min_y + 1); +} + +// Point-in-polygon test using ray casting algorithm +bool isPointInPolygon(int x, int y, const std::vector & polygon) +{ + if (polygon.size() < 3) { + return false; + } + + bool inside = false; + size_t j = polygon.size() - 1; + + for (size_t i = 0; i < polygon.size(); i++) { + if (((polygon[i].y > y) != (polygon[j].y > y)) && + (x < + (polygon[j].x - polygon[i].x) * (y - polygon[i].y) / (polygon[j].y - polygon[i].y) + + polygon[i].x)) + { + inside = !inside; + } + j = i; + } + + return inside; +} + template FootprintCollisionChecker::FootprintCollisionChecker() : costmap_(nullptr) @@ -95,7 +157,7 @@ double FootprintCollisionChecker::footprintCost( // If no collision on perimeter and full area check requested, rasterize the full area // Convert footprint to map coordinates for rasterization - std::vector polygon_points; + std::vector polygon_points; polygon_points.reserve(footprint.size()); for (const auto & point : footprint) { @@ -107,27 +169,21 @@ double FootprintCollisionChecker::footprintCost( } // Find bounding box for the polygon - cv::Rect bbox = cv::boundingRect(polygon_points); + Rectangle bbox = calculateBoundingRect(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 mask_points; - mask_points.reserve(polygon_points.size()); + // Translate polygon points to bounding box coordinates + std::vector bbox_polygon; + bbox_polygon.reserve(polygon_points.size()); for (const auto & pt : polygon_points) { - mask_points.emplace_back(pt.x - bbox.x, pt.y - bbox.y); + bbox_polygon.emplace_back(pt.x - bbox.x, pt.y - bbox.y); } - // Fill the polygon in the mask using OpenCV rasterization - cv::fillPoly(mask, std::vector>{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(y, x) > 0) { // Cell is inside polygon + // Iterate through the bounding box and check costs only for cells inside the polygon + for (int y = 0; y < bbox.height; ++y) { + for (int x = 0; x < bbox.width; ++x) { + if (isPointInPolygon(x, y, bbox_polygon)) { // Cell is inside polygon // Convert back to map coordinates unsigned int map_x = static_cast(bbox.x + x); unsigned int map_y = static_cast(bbox.y + y); diff --git a/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp b/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp index a41d69c520e..b59809922a0 100644 --- a/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp +++ b/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp @@ -16,6 +16,9 @@ #include #include #include +#include +#include +#include #include "gtest/gtest.h" #include "nav2_costmap_2d/footprint_collision_checker.hpp" @@ -597,3 +600,111 @@ TEST(collision_footprint, test_complex_polygon_full_area) { // Full area should detect it (demonstrating the value of full area checking) EXPECT_NEAR(full_area_cost, 180.0, 0.001); } + +// Performance test for footprint collision checking +// Measures execution time for both perimeter and full-area checking methods +// This test can be used to compare performance between different implementations +TEST(collision_footprint, test_performance_benchmark) { + // Create a reasonably large costmap for performance testing + std::shared_ptr costmap_ = + std::make_shared(500, 500, 0.05, 0, 0, 0); + + // Add some scattered obstacles to make the test more realistic + for (int i = 0; i < 1000; ++i) { + int x = rand() % 500; + int y = rand() % 500; + costmap_->setCost(x, y, rand() % 200 + 50); // Random costs 50-249 + } + + // Create a complex polygon footprint (octagon) + std::vector footprint; + const double radius = 1.0; + const int num_vertices = 8; + for (int i = 0; i < num_vertices; ++i) { + geometry_msgs::msg::Point p; + double angle = 2.0 * M_PI * i / num_vertices; + p.x = radius * cos(angle); + p.y = radius * sin(angle); + footprint.push_back(p); + } + + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap_); + + const int num_iterations = 1000; + const double test_positions[][3] = { + {5.0, 5.0, 0.0}, + {10.0, 10.0, M_PI/4}, + {15.0, 15.0, M_PI/2}, + {20.0, 20.0, M_PI}, + {8.5, 12.3, M_PI/6} + }; + const int num_positions = sizeof(test_positions) / sizeof(test_positions[0]); + + // Benchmark perimeter-only checking + auto start_perimeter = std::chrono::high_resolution_clock::now(); + for (int iter = 0; iter < num_iterations; ++iter) { + for (int pos = 0; pos < num_positions; ++pos) { + collision_checker.footprintCostAtPose( + test_positions[pos][0], + test_positions[pos][1], + test_positions[pos][2], + footprint, + false // perimeter only + ); + } + } + auto end_perimeter = std::chrono::high_resolution_clock::now(); + auto perimeter_duration = std::chrono::duration_cast( + end_perimeter - start_perimeter); + + // Benchmark full-area checking + auto start_full_area = std::chrono::high_resolution_clock::now(); + for (int iter = 0; iter < num_iterations; ++iter) { + for (int pos = 0; pos < num_positions; ++pos) { + collision_checker.footprintCostAtPose( + test_positions[pos][0], + test_positions[pos][1], + test_positions[pos][2], + footprint, + true // full area + ); + } + } + auto end_full_area = std::chrono::high_resolution_clock::now(); + auto full_area_duration = std::chrono::duration_cast( + end_full_area - start_full_area); + + // Calculate statistics + int total_calls = num_iterations * num_positions; + double avg_perimeter_us = static_cast(perimeter_duration.count()) / total_calls; + double avg_full_area_us = static_cast(full_area_duration.count()) / total_calls; + + // Print performance results + std::cout << "\n=== FOOTPRINT COLLISION CHECKER PERFORMANCE BENCHMARK ===" << std::endl; + std::cout << "Test configuration:" << std::endl; + std::cout << " - Costmap size: 500x500 cells (25m x 25m @ 0.05m resolution)" << std::endl; + std::cout << " - Footprint: Octagon with 1.0m radius" << std::endl; + std::cout << " - Test positions: " << num_positions << std::endl; + std::cout << " - Iterations per test: " << num_iterations << std::endl; + std::cout << " - Total function calls per test: " << total_calls << std::endl; + std::cout << " - Obstacles: 1000 random obstacles" << std::endl; + std::cout << "\nPerformance Results:" << std::endl; + std::cout << " Perimeter-only checking:" << std::endl; + std::cout << " - Total time: " << perimeter_duration.count() << " microseconds" << std::endl; + std::cout << " - Average per call: " << std::fixed << std::setprecision(2) + << avg_perimeter_us << " microseconds" << std::endl; + std::cout << " Full-area checking:" << std::endl; + std::cout << " - Total time: " << full_area_duration.count() << " microseconds" << std::endl; + std::cout << " - Average per call: " << std::fixed << std::setprecision(2) + << avg_full_area_us << " microseconds" << std::endl; + std::cout << " Performance ratio (full-area / perimeter): " + << std::fixed << std::setprecision(2) + << (avg_full_area_us / avg_perimeter_us) << "x" << std::endl; + std::cout << "============================================================\n" << std::endl; + + // Basic sanity checks - tests should complete in reasonable time + EXPECT_LT(avg_perimeter_us, 1000.0); // Should be less than 1ms per call + EXPECT_LT(avg_full_area_us, 5000.0); // Should be less than 5ms per call + EXPECT_GT(avg_full_area_us, avg_perimeter_us); // Full area should take longer +} From d85af3a0b268738b7ec2387b5fd383434f22415d Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 6 Aug 2025 17:55:42 +0200 Subject: [PATCH 6/9] fix Signed-off-by: Tony Najjar --- nav2_costmap_2d/src/footprint_collision_checker.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_costmap_2d/src/footprint_collision_checker.cpp b/nav2_costmap_2d/src/footprint_collision_checker.cpp index 18e100c204b..8fcfc96e76f 100644 --- a/nav2_costmap_2d/src/footprint_collision_checker.cpp +++ b/nav2_costmap_2d/src/footprint_collision_checker.cpp @@ -163,7 +163,7 @@ double FootprintCollisionChecker::footprintCost( for (const auto & point : footprint) { unsigned int mx, my; if (!worldToMap(point.x, point.y, mx, my)) { - return static_cast(NO_INFORMATION); + return static_cast(LETHAL_OBSTACLE); } polygon_points.emplace_back(static_cast(mx), static_cast(my)); } From 7877b7e3fe2cb8819d2169084c585a94f2f0cd62 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 7 Aug 2025 09:49:08 +0200 Subject: [PATCH 7/9] optimizations Signed-off-by: Tony Najjar --- .../src/footprint_collision_checker.cpp | 98 +++++++++---------- 1 file changed, 48 insertions(+), 50 deletions(-) diff --git a/nav2_costmap_2d/src/footprint_collision_checker.cpp b/nav2_costmap_2d/src/footprint_collision_checker.cpp index 8fcfc96e76f..e056e7f9a81 100644 --- a/nav2_costmap_2d/src/footprint_collision_checker.cpp +++ b/nav2_costmap_2d/src/footprint_collision_checker.cpp @@ -113,63 +113,54 @@ double FootprintCollisionChecker::footprintCost( const Footprint & footprint, bool check_full_area) { - // First check the perimeter - unsigned int x0, x1, y0, y1; - double footprint_cost = 0.0; - - // get the cell coord of the first point - if (!worldToMap(footprint[0].x, footprint[0].y, x0, y0)) { - return static_cast(LETHAL_OBSTACLE); + if (footprint.empty()) { + return static_cast(NO_INFORMATION); } - // cache the start to eliminate a worldToMap call - unsigned int xstart = x0; - unsigned int ystart = y0; + // Pre-convert all footprint points to map coordinates once + std::vector polygon_points; + polygon_points.reserve(footprint.size()); - // we need to rasterize each line in the footprint - for (unsigned int i = 0; i < footprint.size() - 1; ++i) { - // get the cell coord of the second point - if (!worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1)) { + for (const auto & point : footprint) { + unsigned int mx, my; + if (!worldToMap(point.x, point.y, mx, my)) { return static_cast(LETHAL_OBSTACLE); } + polygon_points.emplace_back(static_cast(mx), static_cast(my)); + } - footprint_cost = std::max(lineCost(x0, x1, y0, y1), footprint_cost); - - // the second point is next iteration's first point - x0 = x1; - y0 = y1; - - // if in collision, no need to continue - if (footprint_cost == static_cast(LETHAL_OBSTACLE)) { - return footprint_cost; + // Check perimeter using pre-converted coordinates + double perimeter_cost = 0.0; + for (size_t i = 0; i < polygon_points.size(); ++i) { + size_t next_i = (i + 1) % polygon_points.size(); + double line_cost = lineCost( + polygon_points[i].x, polygon_points[next_i].x, + polygon_points[i].y, polygon_points[next_i].y); + + perimeter_cost = std::max(perimeter_cost, line_cost); + + // Early termination if lethal obstacle found + if (perimeter_cost == static_cast(LETHAL_OBSTACLE)) { + return perimeter_cost; } } - // 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 - 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(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 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(LETHAL_OBSTACLE); - } - polygon_points.emplace_back(static_cast(mx), static_cast(my)); - } - - // Find bounding box for the polygon Rectangle bbox = calculateBoundingRect(polygon_points); + + // Clamp bounding box to costmap dimensions for safety + unsigned int costmap_width = costmap_->getSizeInCellsX(); + unsigned int costmap_height = costmap_->getSizeInCellsY(); + + int min_x = std::max(0, bbox.x); + int min_y = std::max(0, bbox.y); + int max_x = std::min(static_cast(costmap_width - 1), bbox.x + bbox.width - 1); + int max_y = std::min(static_cast(costmap_height - 1), bbox.y + bbox.height - 1); // Translate polygon points to bounding box coordinates std::vector bbox_polygon; @@ -180,15 +171,15 @@ double FootprintCollisionChecker::footprintCost( double max_cost = perimeter_cost; - // Iterate through the bounding box and check costs only for cells inside the polygon - for (int y = 0; y < bbox.height; ++y) { - for (int x = 0; x < bbox.width; ++x) { - if (isPointInPolygon(x, y, bbox_polygon)) { // Cell is inside polygon - // Convert back to map coordinates - unsigned int map_x = static_cast(bbox.x + x); - unsigned int map_y = static_cast(bbox.y + y); - - double cell_cost = pointCost(static_cast(map_x), static_cast(map_y)); + // Iterate through the clamped bounding box and check costs only for cells inside the polygon + for (int y = min_y; y <= max_y; ++y) { + for (int x = min_x; x <= max_x; ++x) { + // Convert to bounding box coordinates for polygon test + int bbox_x = x - bbox.x; + int bbox_y = y - bbox.y; + + if (isPointInPolygon(bbox_x, bbox_y, bbox_polygon)) { + double cell_cost = pointCost(x, y); // Early termination if lethal obstacle found if (cell_cost == static_cast(LETHAL_OBSTACLE)) { @@ -235,6 +226,13 @@ bool FootprintCollisionChecker::worldToMap( template double FootprintCollisionChecker::pointCost(int x, int y) const { + // Bounds checking to prevent segmentation faults + if (x < 0 || y < 0 || + x >= static_cast(costmap_->getSizeInCellsX()) || + y >= static_cast(costmap_->getSizeInCellsY())) { + return static_cast(LETHAL_OBSTACLE); + } + return static_cast(costmap_->getCost(x, y)); } From e6757b29b3958437d16a7b562135923bd501f115 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Fri, 8 Aug 2025 10:13:17 +0200 Subject: [PATCH 8/9] linting Signed-off-by: Tony Najjar --- .../src/footprint_collision_checker.cpp | 19 ++++++------ .../unit/footprint_collision_checker_test.cpp | 30 +++++++++---------- 2 files changed, 25 insertions(+), 24 deletions(-) diff --git a/nav2_costmap_2d/src/footprint_collision_checker.cpp b/nav2_costmap_2d/src/footprint_collision_checker.cpp index e056e7f9a81..de847dc4b8e 100644 --- a/nav2_costmap_2d/src/footprint_collision_checker.cpp +++ b/nav2_costmap_2d/src/footprint_collision_checker.cpp @@ -136,9 +136,9 @@ double FootprintCollisionChecker::footprintCost( double line_cost = lineCost( polygon_points[i].x, polygon_points[next_i].x, polygon_points[i].y, polygon_points[next_i].y); - + perimeter_cost = std::max(perimeter_cost, line_cost); - + // Early termination if lethal obstacle found if (perimeter_cost == static_cast(LETHAL_OBSTACLE)) { return perimeter_cost; @@ -152,11 +152,11 @@ double FootprintCollisionChecker::footprintCost( // If no collision on perimeter and full area check requested, rasterize the full area Rectangle bbox = calculateBoundingRect(polygon_points); - + // Clamp bounding box to costmap dimensions for safety unsigned int costmap_width = costmap_->getSizeInCellsX(); unsigned int costmap_height = costmap_->getSizeInCellsY(); - + int min_x = std::max(0, bbox.x); int min_y = std::max(0, bbox.y); int max_x = std::min(static_cast(costmap_width - 1), bbox.x + bbox.width - 1); @@ -177,7 +177,7 @@ double FootprintCollisionChecker::footprintCost( // Convert to bounding box coordinates for polygon test int bbox_x = x - bbox.x; int bbox_y = y - bbox.y; - + if (isPointInPolygon(bbox_x, bbox_y, bbox_polygon)) { double cell_cost = pointCost(x, y); @@ -227,12 +227,13 @@ template double FootprintCollisionChecker::pointCost(int x, int y) const { // Bounds checking to prevent segmentation faults - if (x < 0 || y < 0 || - x >= static_cast(costmap_->getSizeInCellsX()) || - y >= static_cast(costmap_->getSizeInCellsY())) { + if (x < 0 || y < 0 || + x >= static_cast(costmap_->getSizeInCellsX()) || + y >= static_cast(costmap_->getSizeInCellsY())) + { return static_cast(LETHAL_OBSTACLE); } - + return static_cast(costmap_->getCost(x, y)); } diff --git a/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp b/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp index b59809922a0..2f58aceb2c8 100644 --- a/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp +++ b/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp @@ -634,10 +634,10 @@ TEST(collision_footprint, test_performance_benchmark) { const int num_iterations = 1000; const double test_positions[][3] = { {5.0, 5.0, 0.0}, - {10.0, 10.0, M_PI/4}, - {15.0, 15.0, M_PI/2}, + {10.0, 10.0, M_PI / 4}, + {15.0, 15.0, M_PI / 2}, {20.0, 20.0, M_PI}, - {8.5, 12.3, M_PI/6} + {8.5, 12.3, M_PI / 6} }; const int num_positions = sizeof(test_positions) / sizeof(test_positions[0]); @@ -646,10 +646,10 @@ TEST(collision_footprint, test_performance_benchmark) { for (int iter = 0; iter < num_iterations; ++iter) { for (int pos = 0; pos < num_positions; ++pos) { collision_checker.footprintCostAtPose( - test_positions[pos][0], - test_positions[pos][1], - test_positions[pos][2], - footprint, + test_positions[pos][0], + test_positions[pos][1], + test_positions[pos][2], + footprint, false // perimeter only ); } @@ -663,10 +663,10 @@ TEST(collision_footprint, test_performance_benchmark) { for (int iter = 0; iter < num_iterations; ++iter) { for (int pos = 0; pos < num_positions; ++pos) { collision_checker.footprintCostAtPose( - test_positions[pos][0], - test_positions[pos][1], - test_positions[pos][2], - footprint, + test_positions[pos][0], + test_positions[pos][1], + test_positions[pos][2], + footprint, true // full area ); } @@ -692,14 +692,14 @@ TEST(collision_footprint, test_performance_benchmark) { std::cout << "\nPerformance Results:" << std::endl; std::cout << " Perimeter-only checking:" << std::endl; std::cout << " - Total time: " << perimeter_duration.count() << " microseconds" << std::endl; - std::cout << " - Average per call: " << std::fixed << std::setprecision(2) + std::cout << " - Average per call: " << std::fixed << std::setprecision(2) << avg_perimeter_us << " microseconds" << std::endl; std::cout << " Full-area checking:" << std::endl; std::cout << " - Total time: " << full_area_duration.count() << " microseconds" << std::endl; - std::cout << " - Average per call: " << std::fixed << std::setprecision(2) + std::cout << " - Average per call: " << std::fixed << std::setprecision(2) << avg_full_area_us << " microseconds" << std::endl; - std::cout << " Performance ratio (full-area / perimeter): " - << std::fixed << std::setprecision(2) + std::cout << " Performance ratio (full-area / perimeter): " + << std::fixed << std::setprecision(2) << (avg_full_area_us / avg_perimeter_us) << "x" << std::endl; std::cout << "============================================================\n" << std::endl; From 09d2c5d0527e848483ac389cf0376257b4acaba5 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Fri, 8 Aug 2025 11:12:15 +0200 Subject: [PATCH 9/9] better test Signed-off-by: Tony Najjar --- .../unit/footprint_collision_checker_test.cpp | 255 ++++++++++++------ 1 file changed, 169 insertions(+), 86 deletions(-) diff --git a/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp b/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp index 2f58aceb2c8..c11fbb14d52 100644 --- a/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp +++ b/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp @@ -603,108 +603,191 @@ TEST(collision_footprint, test_complex_polygon_full_area) { // Performance test for footprint collision checking // Measures execution time for both perimeter and full-area checking methods -// This test can be used to compare performance between different implementations +// Tests deterministic scenarios: no obstacles, vertex obstacle, edge obstacle, interior obstacle TEST(collision_footprint, test_performance_benchmark) { // Create a reasonably large costmap for performance testing std::shared_ptr costmap_ = std::make_shared(500, 500, 0.05, 0, 0, 0); - // Add some scattered obstacles to make the test more realistic - for (int i = 0; i < 1000; ++i) { - int x = rand() % 500; - int y = rand() % 500; - costmap_->setCost(x, y, rand() % 200 + 50); // Random costs 50-249 - } + // Create a rectangular footprint (2m x 1m) + geometry_msgs::msg::Point p1; + p1.x = -1.0; // Left edge + p1.y = -0.5; // Bottom edge + geometry_msgs::msg::Point p2; + p2.x = 1.0; // Right edge + p2.y = -0.5; + geometry_msgs::msg::Point p3; + p3.x = 1.0; + p3.y = 0.5; // Top edge + geometry_msgs::msg::Point p4; + p4.x = -1.0; + p4.y = 0.5; - // Create a complex polygon footprint (octagon) - std::vector footprint; - const double radius = 1.0; - const int num_vertices = 8; - for (int i = 0; i < num_vertices; ++i) { - geometry_msgs::msg::Point p; - double angle = 2.0 * M_PI * i / num_vertices; - p.x = radius * cos(angle); - p.y = radius * sin(angle); - footprint.push_back(p); - } + nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; nav2_costmap_2d::FootprintCollisionChecker> collision_checker(costmap_); + // Fixed test position (center of costmap) + const double test_x = 12.5; // Center of 25m x 25m map + const double test_y = 12.5; + const double test_theta = 0.0; + + // Convert footprint vertices to grid coordinates for obstacle placement + unsigned int grid_x, grid_y; + const int num_iterations = 1000; - const double test_positions[][3] = { - {5.0, 5.0, 0.0}, - {10.0, 10.0, M_PI / 4}, - {15.0, 15.0, M_PI / 2}, - {20.0, 20.0, M_PI}, - {8.5, 12.3, M_PI / 6} + + // Define test scenarios + struct TestScenario + { + std::string name; + std::vector> obstacle_positions; }; - const int num_positions = sizeof(test_positions) / sizeof(test_positions[0]); - - // Benchmark perimeter-only checking - auto start_perimeter = std::chrono::high_resolution_clock::now(); - for (int iter = 0; iter < num_iterations; ++iter) { - for (int pos = 0; pos < num_positions; ++pos) { - collision_checker.footprintCostAtPose( - test_positions[pos][0], - test_positions[pos][1], - test_positions[pos][2], - footprint, - false // perimeter only - ); - } - } - auto end_perimeter = std::chrono::high_resolution_clock::now(); - auto perimeter_duration = std::chrono::duration_cast( - end_perimeter - start_perimeter); - - // Benchmark full-area checking - auto start_full_area = std::chrono::high_resolution_clock::now(); - for (int iter = 0; iter < num_iterations; ++iter) { - for (int pos = 0; pos < num_positions; ++pos) { - collision_checker.footprintCostAtPose( - test_positions[pos][0], - test_positions[pos][1], - test_positions[pos][2], - footprint, - true // full area - ); - } - } - auto end_full_area = std::chrono::high_resolution_clock::now(); - auto full_area_duration = std::chrono::duration_cast( - end_full_area - start_full_area); - // Calculate statistics - int total_calls = num_iterations * num_positions; - double avg_perimeter_us = static_cast(perimeter_duration.count()) / total_calls; - double avg_full_area_us = static_cast(full_area_duration.count()) / total_calls; + std::vector scenarios; + + // Scenario 0: Obstacle at a vertex of the polygon (fastest) + // Place obstacle at the first vertex (p1: left edge, bottom edge) + collision_checker.worldToMap(test_x - 1.0, test_y - 0.5, grid_x, grid_y); + scenarios.push_back({"Obstacle at vertex", {{grid_x, grid_y}}}); + + // Scenario 1: Obstacle on a line segment (edge) + // Place obstacle on the last line segment (p4 to p1: top edge, from right to left) + collision_checker.worldToMap(test_x - 0.5, test_y + 0.5, grid_x, grid_y); + scenarios.push_back({"Obstacle on edge", {{grid_x, grid_y}}}); + + // Scenario 2: Obstacle inside the polygon + // Place obstacle at the center of the rectangle + collision_checker.worldToMap(test_x, test_y, grid_x, grid_y); + scenarios.push_back({"Obstacle inside", {{grid_x, grid_y}}}); + + // Scenario 3: No obstacles (slowest) + scenarios.push_back({"No obstacles", {}}); - // Print performance results std::cout << "\n=== FOOTPRINT COLLISION CHECKER PERFORMANCE BENCHMARK ===" << std::endl; std::cout << "Test configuration:" << std::endl; std::cout << " - Costmap size: 500x500 cells (25m x 25m @ 0.05m resolution)" << std::endl; - std::cout << " - Footprint: Octagon with 1.0m radius" << std::endl; - std::cout << " - Test positions: " << num_positions << std::endl; - std::cout << " - Iterations per test: " << num_iterations << std::endl; - std::cout << " - Total function calls per test: " << total_calls << std::endl; - std::cout << " - Obstacles: 1000 random obstacles" << std::endl; - std::cout << "\nPerformance Results:" << std::endl; - std::cout << " Perimeter-only checking:" << std::endl; - std::cout << " - Total time: " << perimeter_duration.count() << " microseconds" << std::endl; - std::cout << " - Average per call: " << std::fixed << std::setprecision(2) - << avg_perimeter_us << " microseconds" << std::endl; - std::cout << " Full-area checking:" << std::endl; - std::cout << " - Total time: " << full_area_duration.count() << " microseconds" << std::endl; - std::cout << " - Average per call: " << std::fixed << std::setprecision(2) - << avg_full_area_us << " microseconds" << std::endl; - std::cout << " Performance ratio (full-area / perimeter): " - << std::fixed << std::setprecision(2) - << (avg_full_area_us / avg_perimeter_us) << "x" << std::endl; - std::cout << "============================================================\n" << std::endl; + std::cout << " - Footprint: Rectangle 2m x 1m" << std::endl; + std::cout << " - Test position: (" << test_x << ", " << test_y << ", " << test_theta << ")" << + std::endl; + std::cout << " - Iterations per scenario: " << num_iterations << std::endl; + std::cout << " - Obstacle cost: 254" << std::endl; + + // Run tests for each scenario + std::vector full_area_times; // Store timing for early exit verification + + for (const auto & scenario : scenarios) { + // Clear costmap + for (unsigned int i = 0; i < costmap_->getSizeInCellsX(); ++i) { + for (unsigned int j = 0; j < costmap_->getSizeInCellsY(); ++j) { + costmap_->setCost(i, j, 0); + } + } + + // Place obstacles for this scenario + for (const auto & pos : scenario.obstacle_positions) { + costmap_->setCost(pos.first, pos.second, 254); + } + + // Test perimeter-only checking + auto start_perimeter = std::chrono::high_resolution_clock::now(); + double perimeter_result = 0.0; + for (int iter = 0; iter < num_iterations; ++iter) { + perimeter_result = collision_checker.footprintCostAtPose( + test_x, test_y, test_theta, footprint, false); + } + auto end_perimeter = std::chrono::high_resolution_clock::now(); + auto perimeter_duration = std::chrono::duration_cast( + end_perimeter - start_perimeter); + + // Test full-area checking + auto start_full_area = std::chrono::high_resolution_clock::now(); + double full_area_result = 0.0; + for (int iter = 0; iter < num_iterations; ++iter) { + full_area_result = collision_checker.footprintCostAtPose( + test_x, test_y, test_theta, footprint, true); + } + auto end_full_area = std::chrono::high_resolution_clock::now(); + auto full_area_duration = std::chrono::duration_cast( + end_full_area - start_full_area); + + // Calculate averages + double avg_perimeter_us = static_cast(perimeter_duration.count()) / num_iterations; + double avg_full_area_us = static_cast(full_area_duration.count()) / num_iterations; + + // Store full-area timing for early exit verification + full_area_times.push_back(avg_full_area_us); + + // Print results for this scenario + std::cout << "\nScenario: " << scenario.name << std::endl; + std::cout << " Perimeter checking:" << std::endl; + std::cout << " - Result: " << perimeter_result << std::endl; + std::cout << " - Average time: " << std::fixed << std::setprecision(2) + << avg_perimeter_us << " microseconds" << std::endl; + std::cout << " Full-area checking:" << std::endl; + std::cout << " - Result: " << full_area_result << std::endl; + std::cout << " - Average time: " << std::fixed << std::setprecision(2) + << avg_full_area_us << " microseconds" << std::endl; + std::cout << " Performance ratio (full-area / perimeter): " + << std::fixed << std::setprecision(2) + << (avg_perimeter_us > + 0 ? avg_full_area_us / avg_perimeter_us : 0.0) << "x" << std::endl; + + // Verify expected behavior for each scenario + if (scenario.name == "No obstacles") { + EXPECT_NEAR(perimeter_result, 0.0, 0.001); + EXPECT_NEAR(full_area_result, 0.0, 0.001); + } else if (scenario.name == "Obstacle at vertex" || scenario.name == "Obstacle on edge") { + // Both methods should detect obstacles on perimeter + EXPECT_NEAR(perimeter_result, 254.0, 0.001); + EXPECT_NEAR(full_area_result, 254.0, 0.001); + } else if (scenario.name == "Obstacle inside") { + // Only full-area should detect interior obstacles + EXPECT_LT(perimeter_result, 254.0); // Should not detect interior obstacle + EXPECT_NEAR(full_area_result, 254.0, 0.001); // Should detect interior obstacle + } + + // Performance sanity checks + EXPECT_LT(avg_perimeter_us, 1000.0); // Should be less than 1ms per call + EXPECT_LT(avg_full_area_us, 5000.0); // Should be less than 5ms per call + } - // Basic sanity checks - tests should complete in reasonable time - EXPECT_LT(avg_perimeter_us, 1000.0); // Should be less than 1ms per call - EXPECT_LT(avg_full_area_us, 5000.0); // Should be less than 5ms per call - EXPECT_GT(avg_full_area_us, avg_perimeter_us); // Full area should take longer + // Verify early exit performance ordering for full-area checking + // Expected order (fastest to slowest due to early exit): + // 0: "Obstacle at vertex" - exits earliest when scanning perimeter (fastest) + // 1: "Obstacle on edge" - exits when scanning perimeter + // 2: "Obstacle inside" - exits when scanning interior + // 3: "No obstacles" - must scan entire area (slowest) + std::cout << "\nEarly exit performance verification (full-area checking):" << std::endl; + std::cout << " Obstacle at vertex: " << std::fixed << std::setprecision(2) + << full_area_times[0] << " μs (fastest)" << std::endl; + std::cout << " Obstacle on edge: " << std::fixed << std::setprecision(2) + << full_area_times[1] << " μs" << std::endl; + std::cout << " Obstacle inside: " << std::fixed << std::setprecision(2) + << full_area_times[2] << " μs" << std::endl; + std::cout << " No obstacles: " << std::fixed << std::setprecision(2) + << full_area_times[3] << " μs (slowest)" << std::endl; + + // Verify early exit behavior: obstacles found earlier should result in faster execution + // Expected order: vertex < edge < inside < no obstacles + // Allow some tolerance for measurement noise, but expect clear ordering + double tolerance_factor = 1.2; // Allow 20% tolerance for timing variations + + // Obstacle at vertex should be fastest (first vertex checked) + EXPECT_LT(full_area_times[0] * tolerance_factor, full_area_times[1]) // vertex < edge + << "Obstacle at vertex (" << full_area_times[0] << + " μs) should be faster than obstacle on edge (" + << full_area_times[1] << " μs) due to early exit"; + + EXPECT_LT(full_area_times[1] * tolerance_factor, full_area_times[2]) // edge < inside + << "Obstacle on edge (" << full_area_times[1] << " μs) should be faster than obstacle inside (" + << full_area_times[2] << " μs) due to early exit"; + + EXPECT_LT(full_area_times[2] * tolerance_factor, full_area_times[3]) // inside < no obstacles + << "Obstacle inside (" << full_area_times[2] << " μs) should be faster than no obstacles (" + << full_area_times[3] << " μs) due to early exit"; + + std::cout << " ✓ Early exit performance ordering verified" << std::endl; + std::cout << "============================================================\n" << std::endl; }