217 lines
9.7 KiB
C#
217 lines
9.7 KiB
C#
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>());
|
|
}
|
|
}
|