161 lines
8.8 KiB
C#
161 lines
8.8 KiB
C#
using System;
|
||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||
|
||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||
|
||
/// <summary>
|
||
/// 对以车辆几何中心表示的扩大矩形执行连续碰撞检查。
|
||
/// 地图查询使用 m；任何地图外车辆部分、占据格相交或擦边均按碰撞处理。
|
||
/// </summary>
|
||
public sealed class FootprintCollisionChecker
|
||
{
|
||
/// <summary>创建连续车辆碰撞检查器。</summary>
|
||
public FootprintCollisionChecker()
|
||
{
|
||
}
|
||
|
||
/// <summary>
|
||
/// 判断单个车辆位姿是否无碰撞。
|
||
/// 参数：pose 为车辆几何中心的世界 m/rad 位姿；map 为不可变规划地图；vehicle 为车辆尺寸；
|
||
/// additionalMarginMeters 为临时额外安全余量，单位 m；bodyClearanceMeters 输出不含该临时余量的保守车体净空下界，单位 m。
|
||
/// 返回：扩大车辆矩形完整位于地图内且不与任何占据格相交或擦边时为 true；无效输入保守地返回 false。
|
||
/// </summary>
|
||
public bool IsPoseCollisionFree(Pose2D pose, PlanningGridMap map, VehicleParameters vehicle,
|
||
double additionalMarginMeters, out double bodyClearanceMeters)
|
||
{
|
||
bodyClearanceMeters = 0d;
|
||
if (map == null || !NumericGuard.IsFinite(additionalMarginMeters) || additionalMarginMeters < 0d ||
|
||
!VehicleFootprint.TryCreate(pose, vehicle, 0d, out VehicleFootprint bodyFootprint) ||
|
||
!VehicleFootprint.TryCreate(pose, vehicle, additionalMarginMeters, out VehicleFootprint checkedFootprint))
|
||
return false;
|
||
|
||
if (!AreCornersInsideMap(checkedFootprint, map)) return false;
|
||
|
||
double centerDistanceMeters = map.GetConservativeObstacleDistanceMeters(pose.X, pose.Y);
|
||
bodyClearanceMeters = GetBodyClearance(centerDistanceMeters, bodyFootprint.CircumscribedRadiusMeters);
|
||
if (centerDistanceMeters > checkedFootprint.CircumscribedRadiusMeters) return true;
|
||
|
||
return !IntersectsOccupiedCell(checkedFootprint, map);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 判断两个位姿之间的平移和转向扫掠是否无碰撞。
|
||
/// 参数：from、to 为世界 m/rad 位姿；maximumCenterStepMeters 为允许的最大中心采样间距，单位 m；
|
||
/// minimumBodyClearanceMeters 输出沿途不含临时扫掠余量的保守车体净空下界，单位 m。
|
||
/// 返回：端点和每个分段扫掠均无碰撞时为 true；无效输入、地图外或任一中间碰撞时返回 false。
|
||
/// </summary>
|
||
public bool IsSweptMotionCollisionFree(Pose2D from, Pose2D to, PlanningGridMap map, VehicleParameters vehicle,
|
||
double maximumCenterStepMeters, out double minimumBodyClearanceMeters)
|
||
{
|
||
minimumBodyClearanceMeters = 0d;
|
||
if (map == null || from == null || to == null || !NumericGuard.IsFinite(maximumCenterStepMeters) || maximumCenterStepMeters <= 0d ||
|
||
!NumericGuard.IsFinite(from.X) || !NumericGuard.IsFinite(from.Y) || !NumericGuard.IsFinite(from.Heading) ||
|
||
!NumericGuard.IsFinite(to.X) || !NumericGuard.IsFinite(to.Y) || !NumericGuard.IsFinite(to.Heading) ||
|
||
!VehicleFootprint.TryCreate(from, vehicle, 0d, out VehicleFootprint bodyFootprint))
|
||
return false;
|
||
|
||
double allowedStepMeters = Math.Min(maximumCenterStepMeters, map.ResolutionMeters / 2d);
|
||
if (!NumericGuard.IsPositiveFinite(allowedStepMeters)) return false;
|
||
|
||
if (!IsPoseCollisionFree(from, map, vehicle, 0d, out double fromClearanceMeters)) return false;
|
||
minimumBodyClearanceMeters = fromClearanceMeters;
|
||
|
||
double deltaX = to.X - from.X;
|
||
double deltaY = to.Y - from.Y;
|
||
double centerDistanceMeters = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
||
if (!NumericGuard.IsFinite(centerDistanceMeters)) return false;
|
||
double headingDeltaRadians = AngleMath.ShortestSignedDifference(from.Heading, to.Heading);
|
||
if (!NumericGuard.IsFinite(headingDeltaRadians)) return false;
|
||
double rawSegmentCount = Math.Ceiling(centerDistanceMeters / allowedStepMeters);
|
||
if (!NumericGuard.IsFinite(rawSegmentCount) || rawSegmentCount > int.MaxValue) return false;
|
||
int segmentCount = Math.Max(1, (int)rawSegmentCount);
|
||
|
||
Pose2D previousPose = from;
|
||
for (int segment = 1; segment <= segmentCount; segment++)
|
||
{
|
||
double endFraction = (double)segment / segmentCount;
|
||
double middleFraction = ((double)segment - 0.5d) / segmentCount;
|
||
var currentPose = new Pose2D(
|
||
from.X + deltaX * endFraction,
|
||
from.Y + deltaY * endFraction,
|
||
from.Heading + headingDeltaRadians * endFraction);
|
||
var middlePose = new Pose2D(
|
||
from.X + deltaX * middleFraction,
|
||
from.Y + deltaY * middleFraction,
|
||
from.Heading + headingDeltaRadians * middleFraction);
|
||
double segmentDeltaX = currentPose.X - previousPose.X;
|
||
double segmentDeltaY = currentPose.Y - previousPose.Y;
|
||
double segmentCenterDisplacementMeters = Math.Sqrt(segmentDeltaX * segmentDeltaX + segmentDeltaY * segmentDeltaY);
|
||
double segmentHeadingDeltaRadians = currentPose.Heading - previousPose.Heading;
|
||
double temporaryMarginMeters = 0.5d * (segmentCenterDisplacementMeters +
|
||
bodyFootprint.CircumscribedRadiusMeters * Math.Abs(segmentHeadingDeltaRadians));
|
||
if (!NumericGuard.IsFinite(temporaryMarginMeters) ||
|
||
!IsPoseCollisionFree(middlePose, map, vehicle, temporaryMarginMeters, out double middleClearanceMeters))
|
||
return false;
|
||
minimumBodyClearanceMeters = Math.Min(minimumBodyClearanceMeters, middleClearanceMeters);
|
||
previousPose = currentPose;
|
||
}
|
||
|
||
if (!IsPoseCollisionFree(to, map, vehicle, 0d, out double toClearanceMeters)) return false;
|
||
minimumBodyClearanceMeters = Math.Min(minimumBodyClearanceMeters, toClearanceMeters);
|
||
return true;
|
||
}
|
||
|
||
private static bool AreCornersInsideMap(VehicleFootprint footprint, PlanningGridMap map)
|
||
{
|
||
for (int index = 0; index < 4; index++)
|
||
{
|
||
footprint.GetCorner(index, out double cornerX, out double cornerY);
|
||
if (!map.TryWorldToGrid(cornerX, cornerY, out _, out _)) return false;
|
||
}
|
||
return true;
|
||
}
|
||
|
||
private static double GetBodyClearance(double centerDistanceMeters, double bodyRadiusMeters)
|
||
{
|
||
if (double.IsPositiveInfinity(centerDistanceMeters)) return double.PositiveInfinity;
|
||
if (!NumericGuard.IsFinite(centerDistanceMeters) || !NumericGuard.IsFinite(bodyRadiusMeters)) return 0d;
|
||
return Math.Max(0d, centerDistanceMeters - bodyRadiusMeters);
|
||
}
|
||
|
||
private static bool IntersectsOccupiedCell(VehicleFootprint footprint, PlanningGridMap map)
|
||
{
|
||
GetCellRange(map, footprint.MinX, footprint.MaxX, footprint.MinY, footprint.MaxY,
|
||
out int firstRow, out int lastRow, out int firstCol, out int lastCol);
|
||
for (int row = firstRow; row <= lastRow; row++)
|
||
for (int col = firstCol; col <= lastCol; col++)
|
||
{
|
||
if (!map.IsOccupied(row, col)) continue;
|
||
GetCellBoundsMeters(map, row, col, out double cellMinX, out double cellMaxX, out double cellMinY, out double cellMaxY);
|
||
if (OrientedRectangleCellIntersection.Intersects(footprint, cellMinX, cellMaxX, cellMinY, cellMaxY)) return true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
private static void GetCellRange(PlanningGridMap map, double minX, double maxX, double minY, double maxY,
|
||
out int firstRow, out int lastRow, out int firstCol, out int lastCol)
|
||
{
|
||
double minimumMapX = map.Bounds.XMin / 1000d;
|
||
double minimumMapY = map.Bounds.YMin / 1000d;
|
||
firstCol = Clamp((int)Math.Floor((minX - minimumMapX) / map.ResolutionMeters) - 1, 0, map.Cols - 1);
|
||
lastCol = Clamp((int)Math.Floor((maxX - minimumMapX) / map.ResolutionMeters), 0, map.Cols - 1);
|
||
firstRow = Clamp((int)Math.Floor((minY - minimumMapY) / map.ResolutionMeters) - 1, 0, map.Rows - 1);
|
||
lastRow = Clamp((int)Math.Floor((maxY - minimumMapY) / map.ResolutionMeters), 0, map.Rows - 1);
|
||
}
|
||
|
||
private static void GetCellBoundsMeters(PlanningGridMap map, int row, int col,
|
||
out double cellMinX, out double cellMaxX, out double cellMinY, out double cellMaxY)
|
||
{
|
||
cellMinX = map.Bounds.XMin / 1000d + col * map.ResolutionMeters;
|
||
cellMinY = map.Bounds.YMin / 1000d + row * map.ResolutionMeters;
|
||
cellMaxX = Math.Min(map.Bounds.XMax / 1000d, cellMinX + map.ResolutionMeters);
|
||
cellMaxY = Math.Min(map.Bounds.YMax / 1000d, cellMinY + map.ResolutionMeters);
|
||
}
|
||
|
||
private static int Clamp(int value, int minimum, int maximum)
|
||
{
|
||
return value < minimum ? minimum : value > maximum ? maximum : value;
|
||
}
|
||
}
|