288 lines
13 KiB
C#
288 lines
13 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Collections.ObjectModel;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
|
using MultiWheelC.TrajectoryPlanning.Mapping;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
|
using MultiWheelC.TrajectoryPlanning.Utils;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
|
|
|
|
/// <summary>独立复核平滑候选的端点拓扑、车辆曲率和连续车体安全性。</summary>
|
|
public sealed class SmoothedPathValidator
|
|
{
|
|
private const double Tolerance = 1e-6d;
|
|
private readonly FootprintCollisionChecker _collisionChecker;
|
|
|
|
/// <summary>创建使用默认连续车体碰撞检查器的平滑路径验证器。</summary>
|
|
public SmoothedPathValidator()
|
|
: this(new FootprintCollisionChecker())
|
|
{
|
|
}
|
|
|
|
/// <summary>创建使用指定连续车体碰撞检查器的平滑路径验证器。</summary>
|
|
public SmoothedPathValidator(FootprintCollisionChecker collisionChecker)
|
|
{
|
|
_collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
|
|
}
|
|
|
|
/// <summary>
|
|
/// 复核候选平滑路径。每个候选方向段必须保持原始段的端点和换向拓扑；
|
|
/// 输出中的净空均由本次实际车体检查重新计算，绝不沿用候选声明值。
|
|
/// </summary>
|
|
public bool TryValidate(
|
|
IReadOnlyList<SmoothedPathPoint> candidatePath,
|
|
IReadOnlyList<SmoothedPathSegment> candidateSegments,
|
|
PreparedPath originalPath,
|
|
PlanningGridMap map,
|
|
VehicleParameters vehicle,
|
|
double maximumCollisionCheckStepMeters,
|
|
out IReadOnlyList<SmoothedPathPoint> pathWithClearance,
|
|
out double minimumClearanceMeters,
|
|
out string reason)
|
|
{
|
|
pathWithClearance = EmptyPath();
|
|
minimumClearanceMeters = 0d;
|
|
reason = string.Empty;
|
|
if (candidatePath == null || candidateSegments == null || originalPath == null || map == null || vehicle == null ||
|
|
candidatePath.Count == 0 || candidateSegments.Count == 0 || !NumericGuard.IsPositiveFinite(maximumCollisionCheckStepMeters))
|
|
{
|
|
reason = "平滑候选、原始路径、地图、车辆或碰撞步长无效。";
|
|
return false;
|
|
}
|
|
|
|
if (!VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvatureMeters))
|
|
{
|
|
reason = "车辆曲率约束无效。";
|
|
return false;
|
|
}
|
|
|
|
if (!TryValidateSegmentTopology(candidatePath, candidateSegments, originalPath, out reason)) return false;
|
|
if (Math.Abs(candidatePath[0].ArcLength) > Tolerance)
|
|
{
|
|
reason = "平滑路径首点的全局弧长必须为零。";
|
|
return false;
|
|
}
|
|
|
|
if (!TryGetFiniteClearanceCap(map, out double clearanceCapMeters))
|
|
{
|
|
reason = "Planning map bounds cannot produce a finite clearance cap.";
|
|
return false;
|
|
}
|
|
|
|
var checkedClearances = new double[candidatePath.Count];
|
|
double minimumClearance = double.PositiveInfinity;
|
|
for (int index = 0; index < candidatePath.Count; index++)
|
|
{
|
|
SmoothedPathPoint current = candidatePath[index];
|
|
if (!IsValidPoint(current) || Math.Abs(current.VehicleCurvature) > maximumCurvatureMeters + Tolerance)
|
|
{
|
|
reason = "平滑路径包含非法数值或超限车辆曲率。";
|
|
return false;
|
|
}
|
|
|
|
var currentPose = new Pose2D(current.X, current.Y, current.Heading);
|
|
if (!_collisionChecker.IsPoseCollisionFree(currentPose, map, vehicle, 0d, out double poseClearance))
|
|
{
|
|
reason = "平滑路径点未通过完整车体碰撞或边界复核。";
|
|
return false;
|
|
}
|
|
|
|
if (!TryNormalizeClearance(poseClearance, clearanceCapMeters, out poseClearance))
|
|
{
|
|
reason = "Pose collision verification returned an invalid clearance.";
|
|
return false;
|
|
}
|
|
|
|
checkedClearances[index] = poseClearance;
|
|
minimumClearance = Math.Min(minimumClearance, poseClearance);
|
|
if (index == 0) continue;
|
|
|
|
SmoothedPathPoint previous = candidatePath[index - 1];
|
|
if (!IsUnwrappedHeadingContinuous(previous, current))
|
|
{
|
|
reason = "平滑路径展开航向不连续。";
|
|
return false;
|
|
}
|
|
|
|
if (IsDuplicatePoseAndArcLength(previous, current))
|
|
{
|
|
if (previous.Direction == current.Direction || !current.IsGearSwitchPoint)
|
|
{
|
|
reason = "相邻重复点不是合法换向对。";
|
|
return false;
|
|
}
|
|
continue;
|
|
}
|
|
|
|
if (current.IsGearSwitchPoint || current.ArcLength <= previous.ArcLength + Tolerance)
|
|
{
|
|
reason = "非换向点必须保持正弧长增量，换向点必须保留重复位姿。";
|
|
return false;
|
|
}
|
|
|
|
var previousPose = new Pose2D(previous.X, previous.Y, previous.Heading);
|
|
if (!_collisionChecker.IsSweptMotionCollisionFree(previousPose, currentPose, map, vehicle,
|
|
maximumCollisionCheckStepMeters, out double sweptClearance))
|
|
{
|
|
reason = "平滑路径相邻点之间的完整车体扫掠碰撞复核失败。";
|
|
return false;
|
|
}
|
|
|
|
if (!TryNormalizeClearance(sweptClearance, clearanceCapMeters, out sweptClearance))
|
|
{
|
|
reason = "Swept collision verification returned an invalid clearance.";
|
|
return false;
|
|
}
|
|
|
|
checkedClearances[index - 1] = Math.Min(checkedClearances[index - 1], sweptClearance);
|
|
checkedClearances[index] = Math.Min(checkedClearances[index], sweptClearance);
|
|
minimumClearance = Math.Min(minimumClearance, sweptClearance);
|
|
}
|
|
|
|
var output = new List<SmoothedPathPoint>(candidatePath.Count);
|
|
for (int index = 0; index < candidatePath.Count; index++)
|
|
{
|
|
SmoothedPathPoint point = candidatePath[index];
|
|
output.Add(new SmoothedPathPoint(
|
|
point.X, point.Y, point.Heading, point.UnwrappedHeading, point.ArcLength, point.Direction,
|
|
point.GeometricCurvature, point.VehicleCurvature, point.VehicleCurvatureDerivative,
|
|
checkedClearances[index], point.IsGearSwitchPoint, point.Source));
|
|
}
|
|
|
|
pathWithClearance = new ReadOnlyCollection<SmoothedPathPoint>(output);
|
|
minimumClearanceMeters = minimumClearance;
|
|
return true;
|
|
}
|
|
|
|
private static bool TryValidateSegmentTopology(
|
|
IReadOnlyList<SmoothedPathPoint> candidatePath,
|
|
IReadOnlyList<SmoothedPathSegment> candidateSegments,
|
|
PreparedPath originalPath,
|
|
out string reason)
|
|
{
|
|
reason = string.Empty;
|
|
if (originalPath.Segments == null || originalPath.Segments.Count != candidateSegments.Count)
|
|
{
|
|
reason = "平滑路径方向段数量必须保持原始换向拓扑。";
|
|
return false;
|
|
}
|
|
|
|
int expectedStartIndex = 0;
|
|
for (int segmentIndex = 0; segmentIndex < candidateSegments.Count; segmentIndex++)
|
|
{
|
|
SmoothedPathSegment candidateSegment = candidateSegments[segmentIndex];
|
|
PreparedDirectionSegment originalSegment = originalPath.Segments[segmentIndex];
|
|
if (candidateSegment == null || originalSegment == null || candidateSegment.SegmentIndex != segmentIndex ||
|
|
candidateSegment.StartIndex != expectedStartIndex || candidateSegment.StartIndex < 0 ||
|
|
candidateSegment.EndIndex < candidateSegment.StartIndex || candidateSegment.EndIndex >= candidatePath.Count ||
|
|
candidateSegment.Direction != originalSegment.Direction ||
|
|
candidateSegment.StartsAtGearSwitch != originalSegment.StartsAtGearSwitch ||
|
|
candidateSegment.EndsAtGearSwitch != originalSegment.EndsAtGearSwitch ||
|
|
originalSegment.Points == null || originalSegment.Points.Count == 0)
|
|
{
|
|
reason = "平滑路径方向段索引、方向或换向拓扑无效。";
|
|
return false;
|
|
}
|
|
|
|
SmoothedPathPoint candidateStart = candidatePath[candidateSegment.StartIndex];
|
|
SmoothedPathPoint candidateEnd = candidatePath[candidateSegment.EndIndex];
|
|
SmoothingPoint2D originalStart = originalSegment.Points[0];
|
|
SmoothingPoint2D originalEnd = originalSegment.Points[originalSegment.Points.Count - 1];
|
|
if (!SamePose(candidateStart, originalStart) || !SamePose(candidateEnd, originalEnd) ||
|
|
candidateStart.Direction != originalSegment.Direction || candidateEnd.Direction != originalSegment.Direction ||
|
|
candidateStart.IsGearSwitchPoint != originalStart.IsGearSwitchPoint ||
|
|
candidateEnd.IsGearSwitchPoint != originalEnd.IsGearSwitchPoint)
|
|
{
|
|
reason = "平滑路径改变了原始方向段端点或换向点。";
|
|
return false;
|
|
}
|
|
|
|
for (int pointIndex = candidateSegment.StartIndex; pointIndex <= candidateSegment.EndIndex; pointIndex++)
|
|
{
|
|
if (candidatePath[pointIndex] == null || candidatePath[pointIndex].Direction != originalSegment.Direction)
|
|
{
|
|
reason = "平滑路径方向段包含与段方向不一致的点。";
|
|
return false;
|
|
}
|
|
}
|
|
|
|
expectedStartIndex = candidateSegment.EndIndex + 1;
|
|
}
|
|
|
|
if (expectedStartIndex != candidatePath.Count)
|
|
{
|
|
reason = "平滑路径方向段未完整覆盖候选点。";
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
private static bool IsValidPoint(SmoothedPathPoint point)
|
|
{
|
|
return point != null && NumericGuard.IsFinite(point.X) && NumericGuard.IsFinite(point.Y) &&
|
|
NumericGuard.IsFinite(point.Heading) && NumericGuard.IsFinite(point.UnwrappedHeading) &&
|
|
NumericGuard.IsFinite(point.ArcLength) && point.ArcLength >= 0d &&
|
|
NumericGuard.IsFinite(point.GeometricCurvature) && NumericGuard.IsFinite(point.VehicleCurvature) &&
|
|
NumericGuard.IsFinite(point.VehicleCurvatureDerivative) &&
|
|
IsDirection(point.Direction) && Enum.IsDefined(typeof(SmoothedPathPointSource), point.Source) &&
|
|
Math.Abs(AngleMath.ShortestSignedDifference(point.Heading, AngleMath.NormalizeRadians(point.Heading))) <= Tolerance;
|
|
}
|
|
|
|
private static bool SamePose(SmoothedPathPoint candidate, SmoothingPoint2D original)
|
|
{
|
|
return candidate != null && original != null && Math.Abs(candidate.X - original.X) <= Tolerance &&
|
|
Math.Abs(candidate.Y - original.Y) <= Tolerance &&
|
|
Math.Abs(AngleMath.ShortestSignedDifference(candidate.Heading, original.Heading)) <= Tolerance;
|
|
}
|
|
|
|
private static bool IsUnwrappedHeadingContinuous(SmoothedPathPoint previous, SmoothedPathPoint current)
|
|
{
|
|
double expectedDelta = AngleMath.ShortestSignedDifference(previous.Heading, current.Heading);
|
|
return NumericGuard.IsFinite(expectedDelta) &&
|
|
Math.Abs((current.UnwrappedHeading - previous.UnwrappedHeading) - expectedDelta) <= Tolerance;
|
|
}
|
|
|
|
private static bool IsDuplicatePoseAndArcLength(SmoothedPathPoint previous, SmoothedPathPoint current)
|
|
{
|
|
return Math.Abs(previous.X - current.X) <= Tolerance && Math.Abs(previous.Y - current.Y) <= Tolerance &&
|
|
Math.Abs(AngleMath.ShortestSignedDifference(previous.Heading, current.Heading)) <= Tolerance &&
|
|
Math.Abs(previous.ArcLength - current.ArcLength) <= Tolerance;
|
|
}
|
|
|
|
private static bool IsDirection(TravelDirection direction)
|
|
{
|
|
return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
|
|
}
|
|
|
|
private static bool TryGetFiniteClearanceCap(PlanningGridMap map, out double capMeters)
|
|
{
|
|
capMeters = 0d;
|
|
if (map == null || map.Bounds == null) return false;
|
|
double widthMeters = (map.Bounds.XMax - map.Bounds.XMin) / 1000d;
|
|
double heightMeters = (map.Bounds.YMax - map.Bounds.YMin) / 1000d;
|
|
capMeters = Math.Sqrt(widthMeters * widthMeters + heightMeters * heightMeters);
|
|
return NumericGuard.IsPositiveFinite(capMeters);
|
|
}
|
|
|
|
private static bool TryNormalizeClearance(double clearanceMeters, double capMeters, out double normalizedMeters)
|
|
{
|
|
normalizedMeters = 0d;
|
|
if (double.IsPositiveInfinity(clearanceMeters))
|
|
{
|
|
normalizedMeters = capMeters;
|
|
return true;
|
|
}
|
|
if (!NumericGuard.IsFinite(clearanceMeters) || clearanceMeters < 0d)
|
|
return false;
|
|
normalizedMeters = clearanceMeters;
|
|
return true;
|
|
}
|
|
|
|
private static IReadOnlyList<SmoothedPathPoint> EmptyPath()
|
|
{
|
|
return new ReadOnlyCollection<SmoothedPathPoint>(new List<SmoothedPathPoint>());
|
|
}
|
|
}
|