246 lines
11 KiB
C#
246 lines
11 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
|
using MultiWheelC.TrajectoryPlanning.Utils;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Output;
|
|
|
|
/// <summary>
|
|
/// 对准备发布的粗路径执行独立的连续安全与输出契约复核。
|
|
/// 复核失败的路径不得被包装为 <see cref="PlanningStatus.Success"/>。
|
|
/// </summary>
|
|
public sealed class CoarsePathValidator
|
|
{
|
|
private const double NumericTolerance = 1e-6d;
|
|
private readonly FootprintCollisionChecker _collisionChecker;
|
|
|
|
/// <summary>创建使用默认连续车体检查器的最终路径复核器。</summary>
|
|
public CoarsePathValidator()
|
|
: this(new FootprintCollisionChecker())
|
|
{
|
|
}
|
|
|
|
/// <summary>创建使用指定连续车体检查器的最终路径复核器。</summary>
|
|
public CoarsePathValidator(FootprintCollisionChecker collisionChecker)
|
|
{
|
|
_collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
|
|
}
|
|
|
|
/// <summary>
|
|
/// 复核路径数值、曲率、连续扫掠碰撞、终点约束、累计弧长和方向分段。
|
|
/// 参数：path 与 segments 为待发布输出；request 必须是生成该路径的请求；minimumBodyClearanceMeters 返回沿途的保守净空下界。
|
|
/// 返回：所有规则通过时为 true；否则返回 false、写入失败原因，调用方必须丢弃 path 和 segments。
|
|
/// </summary>
|
|
public bool TryValidate(IReadOnlyList<CoarsePathPoint> path, IReadOnlyList<PathSegment> segments,
|
|
PlanningRequest request, out double minimumBodyClearanceMeters, out string failureReason)
|
|
{
|
|
minimumBodyClearanceMeters = 0d;
|
|
failureReason = string.Empty;
|
|
if (path == null || segments == null || request == null || request.Map == null || request.Vehicle == null ||
|
|
request.Configuration == null || path.Count == 0 || segments.Count == 0)
|
|
{
|
|
failureReason = "最终路径或复核请求为空。";
|
|
return false;
|
|
}
|
|
|
|
if (!VehicleKinematics.TryGetMaximumCurvaturePerMeter(request.Vehicle, out double maximumCurvaturePerMeter))
|
|
{
|
|
failureReason = "车辆曲率约束无效。";
|
|
return false;
|
|
}
|
|
|
|
CoarsePathPoint first = path[0];
|
|
if (!IsValidPoint(first) || first.Source != CoarsePathPointSource.Start || first.IsGearSwitchPoint ||
|
|
Math.Abs(first.ArcLength) > NumericTolerance || !IsSamePose(first, request.Start))
|
|
{
|
|
failureReason = "最终路径首点不符合起点契约。";
|
|
return false;
|
|
}
|
|
|
|
double minimumClearance = double.PositiveInfinity;
|
|
for (int index = 0; index < path.Count; index++)
|
|
{
|
|
CoarsePathPoint current = path[index];
|
|
if (!IsValidPoint(current) || Math.Abs(current.VehicleCurvature) > maximumCurvaturePerMeter + NumericTolerance)
|
|
{
|
|
failureReason = "最终路径包含非法数值或超限曲率。";
|
|
return false;
|
|
}
|
|
|
|
var currentPose = new Pose2D(current.X, current.Y, current.Heading);
|
|
if (!_collisionChecker.IsPoseCollisionFree(currentPose, request.Map, request.Vehicle, 0d, out double poseClearanceMeters) ||
|
|
IsClearanceOverclaimed(current.BodyClearance, poseClearanceMeters))
|
|
{
|
|
failureReason = "最终路径点未通过连续车体碰撞复核。";
|
|
return false;
|
|
}
|
|
|
|
minimumClearance = Math.Min(minimumClearance, current.BodyClearance);
|
|
if (index == 0) continue;
|
|
|
|
CoarsePathPoint previous = path[index - 1];
|
|
if (current.ArcLength + NumericTolerance < previous.ArcLength ||
|
|
!IsUnwrappedHeadingContinuous(previous, current))
|
|
{
|
|
failureReason = "最终路径的弧长或展开航向不连续。";
|
|
return false;
|
|
}
|
|
|
|
bool isDuplicate = IsDuplicatePoseAndArcLength(previous, current);
|
|
if (isDuplicate)
|
|
{
|
|
if (previous.Direction == current.Direction || !current.IsGearSwitchPoint)
|
|
{
|
|
failureReason = "相邻重复点不是合法换向对。";
|
|
return false;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (current.IsGearSwitchPoint || current.ArcLength <= previous.ArcLength + NumericTolerance ||
|
|
!IsArcIncrementConsistent(previous, current))
|
|
{
|
|
failureReason = "非换向路径点的弧长增量不一致。";
|
|
return false;
|
|
}
|
|
}
|
|
|
|
var previousPose = new Pose2D(previous.X, previous.Y, previous.Heading);
|
|
if (!_collisionChecker.IsSweptMotionCollisionFree(previousPose, currentPose, request.Map, request.Vehicle,
|
|
request.Configuration.MaximumCollisionCheckStepMeters, out double sweptClearanceMeters))
|
|
{
|
|
failureReason = "最终路径相邻点之间的车体扫掠碰撞复核失败。";
|
|
return false;
|
|
}
|
|
|
|
minimumClearance = Math.Min(minimumClearance, sweptClearanceMeters);
|
|
}
|
|
|
|
CoarsePathPoint last = path[path.Count - 1];
|
|
if (!GoalToleranceChecker.IsSatisfied(new Pose2D(last.X, last.Y, last.Heading), request.Goal, request.Configuration,
|
|
last.Direction, request.GoalDirection) || (path.Count > 1 && last.Source != CoarsePathPointSource.GoalTruncation))
|
|
{
|
|
failureReason = "最终路径末点未满足终点容差、方向或来源契约。";
|
|
return false;
|
|
}
|
|
|
|
if (!AreSegmentsValid(path, segments, out failureReason)) return false;
|
|
minimumBodyClearanceMeters = minimumClearance;
|
|
return true;
|
|
}
|
|
|
|
private static bool AreSegmentsValid(IReadOnlyList<CoarsePathPoint> path, IReadOnlyList<PathSegment> segments, out string failureReason)
|
|
{
|
|
failureReason = string.Empty;
|
|
int expectedStartIndex = 0;
|
|
for (int segmentIndex = 0; segmentIndex < segments.Count; segmentIndex++)
|
|
{
|
|
PathSegment segment = segments[segmentIndex];
|
|
if (segment == null || segment.SegmentIndex != segmentIndex || segment.StartIndex != expectedStartIndex ||
|
|
segment.StartIndex < 0 || segment.EndIndex < segment.StartIndex || segment.EndIndex >= path.Count ||
|
|
segment.StartsAtGearSwitch != path[segment.StartIndex].IsGearSwitchPoint)
|
|
{
|
|
failureReason = "方向分段索引或起始换向标记无效。";
|
|
return false;
|
|
}
|
|
|
|
for (int pointIndex = segment.StartIndex; pointIndex <= segment.EndIndex; pointIndex++)
|
|
{
|
|
if (path[pointIndex].Direction != segment.Direction)
|
|
{
|
|
failureReason = "方向分段包含不同方向的路径点。";
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool hasNextSegment = segmentIndex + 1 < segments.Count;
|
|
bool expectedEndsAtGearSwitch = hasNextSegment && segment.EndIndex + 1 < path.Count &&
|
|
path[segment.EndIndex + 1].IsGearSwitchPoint;
|
|
if (segment.EndsAtGearSwitch != expectedEndsAtGearSwitch)
|
|
{
|
|
failureReason = "方向分段末尾换向标记无效。";
|
|
return false;
|
|
}
|
|
|
|
expectedStartIndex = segment.EndIndex + 1;
|
|
}
|
|
|
|
if (expectedStartIndex != path.Count)
|
|
{
|
|
failureReason = "方向分段未完整覆盖路径点。";
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
private static bool IsValidPoint(CoarsePathPoint point)
|
|
{
|
|
if (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.VehicleCurvature) || !IsTravelDirection(point.Direction) ||
|
|
!IsValidClearance(point.BodyClearance))
|
|
return false;
|
|
|
|
return Math.Abs(AngleMath.ShortestSignedDifference(point.Heading, AngleMath.NormalizeRadians(point.Heading))) <= NumericTolerance;
|
|
}
|
|
|
|
private static bool IsSamePose(CoarsePathPoint point, Pose2D pose)
|
|
{
|
|
return point != null && pose != null && Math.Abs(point.X - pose.X) <= NumericTolerance &&
|
|
Math.Abs(point.Y - pose.Y) <= NumericTolerance &&
|
|
Math.Abs(AngleMath.ShortestSignedDifference(point.Heading, pose.Heading)) <= NumericTolerance;
|
|
}
|
|
|
|
private static bool IsUnwrappedHeadingContinuous(CoarsePathPoint previous, CoarsePathPoint current)
|
|
{
|
|
double expectedDelta = AngleMath.ShortestSignedDifference(previous.Heading, current.Heading);
|
|
return NumericGuard.IsFinite(expectedDelta) &&
|
|
Math.Abs((current.UnwrappedHeading - previous.UnwrappedHeading) - expectedDelta) <= NumericTolerance;
|
|
}
|
|
|
|
private static bool IsDuplicatePoseAndArcLength(CoarsePathPoint previous, CoarsePathPoint current)
|
|
{
|
|
return Math.Abs(previous.X - current.X) <= NumericTolerance && Math.Abs(previous.Y - current.Y) <= NumericTolerance &&
|
|
Math.Abs(AngleMath.ShortestSignedDifference(previous.Heading, current.Heading)) <= NumericTolerance &&
|
|
Math.Abs(previous.ArcLength - current.ArcLength) <= NumericTolerance;
|
|
}
|
|
|
|
private static bool IsArcIncrementConsistent(CoarsePathPoint previous, CoarsePathPoint current)
|
|
{
|
|
double expectedIncrement;
|
|
if (Math.Abs(current.VehicleCurvature) < 1e-12d)
|
|
{
|
|
double deltaX = current.X - previous.X;
|
|
double deltaY = current.Y - previous.Y;
|
|
expectedIncrement = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
|
}
|
|
else
|
|
{
|
|
double headingDelta = AngleMath.ShortestSignedDifference(previous.Heading, current.Heading);
|
|
expectedIncrement = Math.Abs(headingDelta / current.VehicleCurvature);
|
|
}
|
|
|
|
return NumericGuard.IsFinite(expectedIncrement) && expectedIncrement > 0d &&
|
|
Math.Abs((current.ArcLength - previous.ArcLength) - expectedIncrement) <= NumericTolerance;
|
|
}
|
|
|
|
private static bool IsClearanceOverclaimed(double reportedClearanceMeters, double checkedClearanceMeters)
|
|
{
|
|
if (double.IsPositiveInfinity(reportedClearanceMeters)) return !double.IsPositiveInfinity(checkedClearanceMeters);
|
|
return reportedClearanceMeters > checkedClearanceMeters + NumericTolerance;
|
|
}
|
|
|
|
private static bool IsValidClearance(double clearanceMeters)
|
|
{
|
|
return !double.IsNaN(clearanceMeters) && clearanceMeters >= 0d;
|
|
}
|
|
|
|
private static bool IsTravelDirection(TravelDirection direction)
|
|
{
|
|
return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
|
|
}
|
|
}
|