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 c7cf2b54c6..36b1675584 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 9fc667d52e..de847dc4b8 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. @@ -18,6 +19,7 @@ #include #include #include +#include #include "nav2_costmap_2d/footprint_collision_checker.hpp" @@ -31,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) @@ -45,43 +109,89 @@ 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 - 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); + // 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); - // the second point is next iteration's first point - x0 = x1; - y0 = y1; + perimeter_cost = std::max(perimeter_cost, line_cost); - // if in collision, no need to continue - if (footprint_cost == static_cast(LETHAL_OBSTACLE)) { - return footprint_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 - return 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 + 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; + bbox_polygon.reserve(polygon_points.size()); + for (const auto & pt : polygon_points) { + bbox_polygon.emplace_back(pt.x - bbox.x, pt.y - bbox.y); + } + + double max_cost = perimeter_cost; + + // 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)) { + return cell_cost; + } + + max_cost = std::max(max_cost, cell_cost); + } + } + } + + return max_cost; } template @@ -116,6 +226,14 @@ 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)); } @@ -127,7 +245,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 +258,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 7824d97e7c..c11fbb14d5 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,36 @@ #include #include #include +#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 +52,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 +89,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 +140,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 +184,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 +210,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 +253,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 +267,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 +290,504 @@ 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 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 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 + 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); +} + +// Performance test for footprint collision checking +// Measures execution time for both perimeter and full-area checking methods +// 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); + + // 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; + + 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; + + // Define test scenarios + struct TestScenario + { + std::string name; + std::vector> obstacle_positions; + }; + + 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", {}}); + + 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: 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 + } + + // 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; +}