chore: save current workspace progress
This commit is contained in:
梁薄云
@@ -0,0 +1,245 @@
|
||||
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;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,167 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Output;
|
||||
|
||||
/// <summary>
|
||||
/// 成功搜索父链重新生成后的原语序列。
|
||||
/// 起点位置使用 m/rad，起始曲率使用 1/m；原语集合按从起点到终点的顺序排列。
|
||||
/// </summary>
|
||||
public sealed class BacktrackedPath
|
||||
{
|
||||
internal BacktrackedPath(Pose2D start, TravelDirection startDirection, double startCurvaturePerMeter,
|
||||
IReadOnlyList<MotionPrimitive> primitives)
|
||||
{
|
||||
Start = start;
|
||||
StartDirection = startDirection;
|
||||
StartCurvaturePerMeter = startCurvaturePerMeter;
|
||||
Primitives = primitives;
|
||||
}
|
||||
|
||||
/// <summary>原始请求中的起始车辆中心位姿；位置单位 m，航向单位 rad。</summary>
|
||||
public Pose2D Start { get; }
|
||||
|
||||
/// <summary>成功父链根节点记录的起步方向。</summary>
|
||||
public TravelDirection StartDirection { get; }
|
||||
|
||||
/// <summary>成功父链根节点离散后的起始曲率，单位 1/m。</summary>
|
||||
public double StartCurvaturePerMeter { get; }
|
||||
|
||||
/// <summary>按起点到终点顺序重新积分的恒曲率原语；每项都不包含自身起点。</summary>
|
||||
public IReadOnlyList<MotionPrimitive> Primitives { get; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 仅依据成功节点的父索引和原语描述，确定性地重建稠密原语序列。
|
||||
/// 不复用搜索期保存的积分点，以保证输出与当前解析积分、扫掠检查规则一致。
|
||||
/// </summary>
|
||||
public sealed class PathBacktracker
|
||||
{
|
||||
private const double PoseComparisonTolerance = 1e-7d;
|
||||
private const double LengthComparisonTolerance = 1e-7d;
|
||||
private readonly MotionPrimitiveGenerator _primitiveGenerator;
|
||||
|
||||
/// <summary>创建使用默认解析积分器的回溯器。</summary>
|
||||
public PathBacktracker()
|
||||
: this(new MotionPrimitiveGenerator())
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>创建使用指定原语生成器的回溯器。</summary>
|
||||
public PathBacktracker(MotionPrimitiveGenerator primitiveGenerator)
|
||||
{
|
||||
_primitiveGenerator = primitiveGenerator ?? throw new ArgumentNullException(nameof(primitiveGenerator));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 依据搜索成功节点重建从起点到终点的原语序列。
|
||||
/// 参数：searchResult 必须是携带成功节点索引的搜索结果；request 必须仍指向执行搜索的同一不可变地图和配置。
|
||||
/// 返回：父链无环、索引连续且每条原语按同一解析规则可重建时为 true；失败时 path 为 null 并返回可读原因。
|
||||
/// </summary>
|
||||
public bool TryBacktrack(HybridAStarSearchResult searchResult, PlanningRequest request,
|
||||
out BacktrackedPath path, out string failureReason)
|
||||
{
|
||||
path = null;
|
||||
failureReason = string.Empty;
|
||||
if (searchResult == null || request == null || searchResult.Status != PlanningStatus.Success ||
|
||||
!searchResult.SuccessNodeIndex.HasValue || searchResult.Nodes == null)
|
||||
{
|
||||
failureReason = "搜索结果不含可回溯的成功节点。";
|
||||
return false;
|
||||
}
|
||||
|
||||
int currentIndex = searchResult.SuccessNodeIndex.Value;
|
||||
var reverseNodes = new List<HybridAStarNode>();
|
||||
var visited = new HashSet<int>();
|
||||
while (currentIndex >= 0)
|
||||
{
|
||||
if (currentIndex >= searchResult.Nodes.Count || !visited.Add(currentIndex))
|
||||
{
|
||||
failureReason = "搜索父链索引越界或存在环。";
|
||||
return false;
|
||||
}
|
||||
|
||||
HybridAStarNode current = searchResult.Nodes[currentIndex];
|
||||
if (current == null || current.NodeIndex != currentIndex || current.Pose == null)
|
||||
{
|
||||
failureReason = "搜索节点索引或连续位姿不一致。";
|
||||
return false;
|
||||
}
|
||||
|
||||
reverseNodes.Add(current);
|
||||
currentIndex = current.ParentNodeIndex;
|
||||
}
|
||||
|
||||
if (reverseNodes.Count == 0)
|
||||
{
|
||||
failureReason = "搜索父链为空。";
|
||||
return false;
|
||||
}
|
||||
|
||||
reverseNodes.Reverse();
|
||||
HybridAStarNode root = reverseNodes[0];
|
||||
if (root.ParentNodeIndex != -1 || root.IncomingPrimitive != null || !IsFinitePose(root.Pose))
|
||||
{
|
||||
failureReason = "搜索父链根节点无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
var primitives = new List<MotionPrimitive>(Math.Max(0, reverseNodes.Count - 1));
|
||||
Pose2D previousPose = root.Pose;
|
||||
for (int index = 1; index < reverseNodes.Count; index++)
|
||||
{
|
||||
HybridAStarNode node = reverseNodes[index];
|
||||
MotionPrimitive descriptor = node.IncomingPrimitive;
|
||||
if (node.ParentNodeIndex != reverseNodes[index - 1].NodeIndex || descriptor == null ||
|
||||
!IsFinitePose(node.Pose) || !NumericGuard.IsFinite(descriptor.CurvaturePerMeter) ||
|
||||
!IsTravelDirection(descriptor.Direction) || descriptor.ActualLengthMeters <= 0d)
|
||||
{
|
||||
failureReason = "搜索父链中的原语描述无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
// 只把恒曲率、方向和长度描述作为真源，再走一遍相同的解析积分与连续碰撞检查。
|
||||
MotionPrimitive rebuilt = _primitiveGenerator.Generate(previousPose, descriptor.CurvaturePerMeter, descriptor.Direction, request);
|
||||
if (rebuilt == null || !IsEquivalent(descriptor, rebuilt) || !IsSamePose(rebuilt.End, node.Pose))
|
||||
{
|
||||
failureReason = "搜索原语无法按当前解析规则确定性重建。";
|
||||
return false;
|
||||
}
|
||||
|
||||
primitives.Add(rebuilt);
|
||||
previousPose = rebuilt.End;
|
||||
}
|
||||
|
||||
path = new BacktrackedPath(root.Pose, root.Direction, root.CurvaturePerMeter,
|
||||
new ReadOnlyCollection<MotionPrimitive>(primitives));
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool IsEquivalent(MotionPrimitive expected, MotionPrimitive actual)
|
||||
{
|
||||
return expected.Direction == actual.Direction && expected.IsGoalTruncation == actual.IsGoalTruncation &&
|
||||
Math.Abs(expected.CurvaturePerMeter - actual.CurvaturePerMeter) <= PoseComparisonTolerance &&
|
||||
Math.Abs(expected.ActualLengthMeters - actual.ActualLengthMeters) <= LengthComparisonTolerance;
|
||||
}
|
||||
|
||||
private static bool IsSamePose(Pose2D first, Pose2D second)
|
||||
{
|
||||
return IsFinitePose(first) && IsFinitePose(second) &&
|
||||
Math.Abs(first.X - second.X) <= PoseComparisonTolerance &&
|
||||
Math.Abs(first.Y - second.Y) <= PoseComparisonTolerance &&
|
||||
Math.Abs(AngleMath.ShortestSignedDifference(first.Heading, second.Heading)) <= PoseComparisonTolerance;
|
||||
}
|
||||
|
||||
private static bool IsFinitePose(Pose2D pose)
|
||||
{
|
||||
return pose != null && NumericGuard.IsFinite(pose.X) && NumericGuard.IsFinite(pose.Y) && NumericGuard.IsFinite(pose.Heading);
|
||||
}
|
||||
|
||||
private static bool IsTravelDirection(TravelDirection direction)
|
||||
{
|
||||
return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user