chore: save current workspace progress
This commit is contained in:
梁薄云
@@ -0,0 +1,216 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Output;
|
||||
|
||||
/// <summary>
|
||||
/// 将回溯原语装配为调用方可消费的稠密路径和包含式方向分段。
|
||||
/// 装配器保留原语边界的换向双点，其余相邻重复位姿会被删除。
|
||||
/// </summary>
|
||||
public sealed class CoarsePathAssembler
|
||||
{
|
||||
private const double DuplicateTolerance = 1e-8d;
|
||||
private readonly FootprintCollisionChecker _collisionChecker;
|
||||
|
||||
/// <summary>创建使用默认连续车体检查器的路径装配器。</summary>
|
||||
public CoarsePathAssembler()
|
||||
: this(new FootprintCollisionChecker())
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>创建使用指定连续车体检查器的路径装配器。</summary>
|
||||
public CoarsePathAssembler(FootprintCollisionChecker collisionChecker)
|
||||
{
|
||||
_collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从已回溯的恒曲率原语构造稠密路径。
|
||||
/// 参数：backtrackedPath 提供原语顺序；request 提供地图和车辆；path、segments 为成功时的只读输出。
|
||||
/// 返回：首点可通过连续车体检查、每个原语积分点数据一致且方向分段完整覆盖时为 true；否则返回 false 且输出为空。
|
||||
/// </summary>
|
||||
public bool TryAssemble(BacktrackedPath backtrackedPath, PlanningRequest request,
|
||||
out IReadOnlyList<CoarsePathPoint> path, out IReadOnlyList<PathSegment> segments, out string failureReason)
|
||||
{
|
||||
path = EmptyPath();
|
||||
segments = EmptySegments();
|
||||
failureReason = string.Empty;
|
||||
if (backtrackedPath == null || request == null || request.Map == null || request.Vehicle == null ||
|
||||
!IsFinitePose(backtrackedPath.Start) || !IsTravelDirection(backtrackedPath.StartDirection) ||
|
||||
!NumericGuard.IsFinite(backtrackedPath.StartCurvaturePerMeter))
|
||||
{
|
||||
failureReason = "路径装配输入无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!_collisionChecker.IsPoseCollisionFree(backtrackedPath.Start, request.Map, request.Vehicle, 0d, out double startClearanceMeters))
|
||||
{
|
||||
failureReason = "回溯路径起点未通过连续车体检查。";
|
||||
return false;
|
||||
}
|
||||
|
||||
var points = new List<CoarsePathPoint>();
|
||||
TravelDirection currentDirection = backtrackedPath.Primitives.Count > 0
|
||||
? backtrackedPath.Primitives[0].Direction
|
||||
: backtrackedPath.StartDirection;
|
||||
double currentCurvature = request.StartVehicleCurvature;
|
||||
double normalizedStartHeading = AngleMath.NormalizeRadians(backtrackedPath.Start.Heading);
|
||||
if (!NumericGuard.IsFinite(normalizedStartHeading))
|
||||
{
|
||||
failureReason = "回溯路径起点航向无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
points.Add(new CoarsePathPoint(backtrackedPath.Start.X, backtrackedPath.Start.Y, normalizedStartHeading,
|
||||
backtrackedPath.Start.Heading, 0d, currentDirection, currentCurvature, startClearanceMeters, false,
|
||||
CoarsePathPointSource.Start));
|
||||
|
||||
for (int primitiveIndex = 0; primitiveIndex < backtrackedPath.Primitives.Count; primitiveIndex++)
|
||||
{
|
||||
MotionPrimitive primitive = backtrackedPath.Primitives[primitiveIndex];
|
||||
if (!IsValidPrimitive(primitive))
|
||||
{
|
||||
failureReason = "回溯路径包含无效原语。";
|
||||
return false;
|
||||
}
|
||||
|
||||
CoarsePathPoint lastPoint = points[points.Count - 1];
|
||||
if (primitive.Direction != lastPoint.Direction)
|
||||
{
|
||||
// 换向处的旧方向终点和新方向起点必须共存，二者位置、航向、弧长完全相同。
|
||||
points.Add(new CoarsePathPoint(lastPoint.X, lastPoint.Y, lastPoint.Heading, lastPoint.UnwrappedHeading,
|
||||
lastPoint.ArcLength, primitive.Direction, primitive.CurvaturePerMeter, lastPoint.BodyClearance,
|
||||
true, CoarsePathPointSource.MotionPrimitive));
|
||||
}
|
||||
|
||||
Pose2D previousPose = primitive.Start;
|
||||
for (int pointIndex = 0; pointIndex < primitive.Points.Count; pointIndex++)
|
||||
{
|
||||
Pose2D pose = primitive.Points[pointIndex];
|
||||
double bodyClearanceMeters = primitive.BodyClearancesMeters[pointIndex];
|
||||
if (!IsFinitePose(pose) || !IsValidClearance(bodyClearanceMeters))
|
||||
{
|
||||
failureReason = "原语积分点或净空无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
CoarsePathPoint previousPoint = points[points.Count - 1];
|
||||
double arcIncrementMeters = CalculateArcIncrement(previousPose, pose, primitive.CurvaturePerMeter);
|
||||
if (!NumericGuard.IsFinite(arcIncrementMeters) || arcIncrementMeters <= 0d)
|
||||
{
|
||||
failureReason = "原语积分点未产生正弧长。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (IsSamePose(previousPoint, pose))
|
||||
{
|
||||
// 非换向情况下不允许重复采样点泄露到对外路径。
|
||||
previousPose = pose;
|
||||
continue;
|
||||
}
|
||||
|
||||
double normalizedHeading = AngleMath.NormalizeRadians(pose.Heading);
|
||||
double headingDelta = AngleMath.ShortestSignedDifference(previousPoint.Heading, normalizedHeading);
|
||||
if (!NumericGuard.IsFinite(normalizedHeading) || !NumericGuard.IsFinite(headingDelta))
|
||||
{
|
||||
failureReason = "原语积分点航向无法展开。";
|
||||
return false;
|
||||
}
|
||||
|
||||
CoarsePathPointSource source = primitive.IsGoalTruncation && pointIndex == primitive.Points.Count - 1
|
||||
? CoarsePathPointSource.GoalTruncation
|
||||
: CoarsePathPointSource.MotionPrimitive;
|
||||
points.Add(new CoarsePathPoint(pose.X, pose.Y, normalizedHeading,
|
||||
previousPoint.UnwrappedHeading + headingDelta, previousPoint.ArcLength + arcIncrementMeters,
|
||||
primitive.Direction, primitive.CurvaturePerMeter, bodyClearanceMeters, false, source));
|
||||
previousPose = pose;
|
||||
}
|
||||
}
|
||||
|
||||
if (points.Count == 0)
|
||||
{
|
||||
failureReason = "路径装配未产生起点。";
|
||||
return false;
|
||||
}
|
||||
|
||||
path = new ReadOnlyCollection<CoarsePathPoint>(points);
|
||||
segments = BuildSegments(points);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static IReadOnlyList<PathSegment> BuildSegments(IReadOnlyList<CoarsePathPoint> points)
|
||||
{
|
||||
var segments = new List<PathSegment>();
|
||||
int startIndex = 0;
|
||||
TravelDirection direction = points[0].Direction;
|
||||
for (int index = 1; index < points.Count; index++)
|
||||
{
|
||||
if (points[index].Direction == direction) continue;
|
||||
segments.Add(new PathSegment(segments.Count, direction, startIndex, index - 1,
|
||||
points[startIndex].IsGearSwitchPoint, true));
|
||||
startIndex = index;
|
||||
direction = points[index].Direction;
|
||||
}
|
||||
|
||||
segments.Add(new PathSegment(segments.Count, direction, startIndex, points.Count - 1,
|
||||
points[startIndex].IsGearSwitchPoint, false));
|
||||
return new ReadOnlyCollection<PathSegment>(segments);
|
||||
}
|
||||
|
||||
private static double CalculateArcIncrement(Pose2D from, Pose2D to, double curvaturePerMeter)
|
||||
{
|
||||
if (!IsFinitePose(from) || !IsFinitePose(to) || !NumericGuard.IsFinite(curvaturePerMeter)) return double.NaN;
|
||||
if (Math.Abs(curvaturePerMeter) < 1e-12d)
|
||||
{
|
||||
double deltaX = to.X - from.X;
|
||||
double deltaY = to.Y - from.Y;
|
||||
return Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
||||
}
|
||||
|
||||
double headingDelta = AngleMath.ShortestSignedDifference(from.Heading, to.Heading);
|
||||
return Math.Abs(headingDelta / curvaturePerMeter);
|
||||
}
|
||||
|
||||
private static bool IsValidPrimitive(MotionPrimitive primitive)
|
||||
{
|
||||
return primitive != null && primitive.Start != null && primitive.Points != null && primitive.BodyClearancesMeters != null &&
|
||||
primitive.Points.Count == primitive.BodyClearancesMeters.Count && primitive.Points.Count > 0 &&
|
||||
IsTravelDirection(primitive.Direction) && NumericGuard.IsFinite(primitive.CurvaturePerMeter) &&
|
||||
NumericGuard.IsPositiveFinite(primitive.ActualLengthMeters);
|
||||
}
|
||||
|
||||
private static bool IsSamePose(CoarsePathPoint point, Pose2D pose)
|
||||
{
|
||||
return Math.Abs(point.X - pose.X) <= DuplicateTolerance && Math.Abs(point.Y - pose.Y) <= DuplicateTolerance &&
|
||||
Math.Abs(AngleMath.ShortestSignedDifference(point.Heading, pose.Heading)) <= DuplicateTolerance;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
private static bool IsValidClearance(double clearanceMeters)
|
||||
{
|
||||
return !double.IsNaN(clearanceMeters) && clearanceMeters >= 0d;
|
||||
}
|
||||
|
||||
private static IReadOnlyList<CoarsePathPoint> EmptyPath()
|
||||
{
|
||||
return new ReadOnlyCollection<CoarsePathPoint>(new List<CoarsePathPoint>());
|
||||
}
|
||||
|
||||
private static IReadOnlyList<PathSegment> EmptySegments()
|
||||
{
|
||||
return new ReadOnlyCollection<PathSegment>(new List<PathSegment>());
|
||||
}
|
||||
}
|
||||
@@ -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