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 c4d4418c748..d7ad73d57dc 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 @@ -52,6 +52,10 @@ class FootprintCollisionChecker * @brief Find the footprint cost in oriented footprint */ double footprintCost(const Footprint & footprint); + /** + * @brief Find the footprint cost for entire footprint area using full coverage + */ + double footprintAreaCost(const Footprint & footprint); /** * @brief Find the footprint cost a a post with an unoriented footprint */ @@ -81,6 +85,17 @@ class FootprintCollisionChecker } protected: + /** + * @brief Check if a point is inside the footprint polygon + * @param x X coordinate in world frame + * @param y Y coordinate in world frame + * @param footprint Footprint polygon + * @return True if point is inside footprint + */ + bool isPointInFootprint( + double x, double y, + const Footprint & footprint); + CostmapT costmap_; }; diff --git a/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp b/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp index bd70e70442f..7f59b249d0b 100644 --- a/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp +++ b/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp @@ -94,7 +94,7 @@ double CostmapTopicCollisionChecker::scorePose( throw IllegalPoseException(name_, "Pose Goes Off Grid."); } - return collision_checker_.footprintCost(getFootprint(pose, fetch_costmap_and_footprint)); + return collision_checker_.footprintAreaCost(getFootprint(pose, fetch_costmap_and_footprint)); } Footprint CostmapTopicCollisionChecker::getFootprint( diff --git a/nav2_costmap_2d/src/footprint_collision_checker.cpp b/nav2_costmap_2d/src/footprint_collision_checker.cpp index 9fc667d52e4..d0b088d0e8e 100644 --- a/nav2_costmap_2d/src/footprint_collision_checker.cpp +++ b/nav2_costmap_2d/src/footprint_collision_checker.cpp @@ -84,6 +84,55 @@ double FootprintCollisionChecker::footprintCost(const Footprint & foot return std::max(lineCost(xstart, x1, ystart, y1), footprint_cost); } +template +double FootprintCollisionChecker::footprintAreaCost(const Footprint & footprint) +{ + // Find bounding box of footprint + double min_x = std::numeric_limits::max(); + double max_x = std::numeric_limits::lowest(); + double min_y = std::numeric_limits::max(); + double max_y = std::numeric_limits::lowest(); + + for (const auto & point : footprint) { + min_x = std::min(min_x, point.x); + max_x = std::max(max_x, point.x); + min_y = std::min(min_y, point.y); + max_y = std::max(max_y, point.y); + } + + // Convert to grid coordinates + unsigned int min_mx, min_my, max_mx, max_my; + if (!worldToMap(min_x, min_y, min_mx, min_my) || + !worldToMap(max_x, max_y, max_mx, max_my)) + { + return static_cast(LETHAL_OBSTACLE); + } + + double max_cost = 0.0; + + // Check all cells within bounding box + for (unsigned int my = min_my; my <= max_my; ++my) { + for (unsigned int mx = min_mx; mx <= max_mx; ++mx) { + double wx, wy; + costmap_->mapToWorld(mx, my, wx, wy); + + if (isPointInFootprint(wx, wy, footprint)) { + double cost = pointCost(mx, my); + + if (cost == static_cast(LETHAL_OBSTACLE)) { + return cost; + } else if (cost == static_cast(NO_INFORMATION)) { + // Return NO_INFORMATION for unknown cells, let caller decide how to handle + return cost; + } + max_cost = std::max(max_cost, cost); + } + } + } + + return max_cost; +} + template double FootprintCollisionChecker::lineCost(int x0, int x1, int y0, int y1) const { @@ -143,6 +192,41 @@ double FootprintCollisionChecker::footprintCostAtPose( return footprintCost(oriented_footprint); } +template +bool FootprintCollisionChecker::isPointInFootprint( + double x, double y, + const Footprint & footprint) +{ + // Adaptation of Shimrat, Moshe. "Algorithm 112: position of point relative to polygon." + // Communications of the ACM 5.8 (1962): 434. + // Implementation of ray crossings algorithm for point in polygon task solving. + // Y coordinate is fixed. Moving the ray on X+ axis starting from given point. + // Odd number of intersections with polygon boundaries means the point is inside polygon. + const int poly_size = footprint.size(); + int i, j; // Polygon vertex iterators + bool res = false; // Final result, initialized with already inverted value + + // Starting from the edge where the last point of polygon is connected to the first + i = poly_size - 1; + for (j = 0; j < poly_size; j++) { + // Checking the edge only if given point is between edge boundaries by Y coordinates. + // One of the condition should contain equality in order to exclude the edges + // parallel to X+ ray. + if ((y <= footprint[i].y) == (y > footprint[j].y)) { + // Calculating the intersection coordinate of X+ ray + const double x_inter = footprint[i].x + + (y - footprint[i].y) * (footprint[j].x - footprint[i].x) / + (footprint[j].y - footprint[i].y); + // If intersection with checked edge is greater than point.x coordinate, inverting the result + if (x_inter > x) { + res = !res; + } + } + i = j; + } + return res; +} + // declare our valid template parameters template class FootprintCollisionChecker>; template class FootprintCollisionChecker; diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp index 98910084d64..8c3cd372c98 100644 --- a/nav2_smac_planner/src/collision_checker.cpp +++ b/nav2_smac_planner/src/collision_checker.cpp @@ -135,9 +135,7 @@ bool GridCollisionChecker::inCollision( return true; } - // if possible inscribed, need to check actual footprint pose. - // Use precomputed oriented footprints are done on initialization, - // offset by translation value to collision check + // Use full area checking instead of edge-only checking double wx, wy; costmap_->mapToWorld(static_cast(x), static_cast(y), wx, wy); geometry_msgs::msg::Point new_pt; @@ -150,7 +148,8 @@ bool GridCollisionChecker::inCollision( current_footprint.push_back(new_pt); } - float footprint_cost = static_cast(footprintCost(current_footprint)); + // Check full area covered by footprint + float footprint_cost = static_cast(footprintAreaCost(current_footprint)); if (footprint_cost == UNKNOWN_COST && traverse_unknown) { return false;