184 lines
9.6 KiB
C#
184 lines
9.6 KiB
C#
using System;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
/// <summary>下一轮规划起点的来源；只允许安全未来样本或调用方测量状态二选一。</summary>
|
|
public enum TrajectoryHandoffSource
|
|
{
|
|
MeasuredState,
|
|
PreviousTrajectory,
|
|
}
|
|
|
|
/// <summary>拒绝使用已发布轨迹进行交接的稳定原因；拒绝时必须回退到测量状态。</summary>
|
|
public enum TrajectoryHandoffRejectionReason
|
|
{
|
|
None,
|
|
MissingTrajectory,
|
|
InvalidConfiguration,
|
|
TrajectoryNotYetEffective,
|
|
TrajectoryTooOld,
|
|
SegmentMismatch,
|
|
DirectionMismatch,
|
|
TrackingErrorExceeded,
|
|
GearBoundary,
|
|
TerminalBoundary,
|
|
HandoffBeyondTrajectory,
|
|
SampleUnavailable,
|
|
}
|
|
|
|
/// <summary>下一次单次规划的不可变交接决策；不会跨方向段、方向或终端边界复用旧轨迹。</summary>
|
|
public sealed class TrajectoryHandoffSelection
|
|
{
|
|
internal TrajectoryHandoffSelection(TrajectoryHandoffSource source, VehicleMotionState startState,
|
|
EmTrajectory previousTrajectory, EmTrajectoryPoint sampledPoint, TrajectoryHandoffRejectionReason rejectionReason)
|
|
{
|
|
Source = source;
|
|
StartState = startState ?? throw new ArgumentNullException(nameof(startState));
|
|
PreviousTrajectory = previousTrajectory;
|
|
SampledPoint = sampledPoint;
|
|
RejectionReason = rejectionReason;
|
|
}
|
|
|
|
/// <summary>交接使用未来旧轨迹样本还是调用方测量状态。</summary>
|
|
public TrajectoryHandoffSource Source { get; }
|
|
|
|
public VehicleMotionState StartState { get; }
|
|
|
|
public EmTrajectory PreviousTrajectory { get; }
|
|
|
|
public EmTrajectoryPoint SampledPoint { get; }
|
|
|
|
public TrajectoryHandoffRejectionReason RejectionReason { get; }
|
|
}
|
|
|
|
/// <summary>选择安全的同段同方向未来轨迹种子；任一检查失败时保留调用方测量状态。</summary>
|
|
public sealed class TrajectoryHandoffSelector
|
|
{
|
|
private const double TimeEpsilonSeconds = 1e-9d;
|
|
private readonly TrajectorySampler sampler;
|
|
|
|
/// <summary>使用默认同质区间采样器创建交接选择器。</summary>
|
|
public TrajectoryHandoffSelector()
|
|
: this(new TrajectorySampler())
|
|
{
|
|
}
|
|
|
|
internal TrajectoryHandoffSelector(TrajectorySampler sampler)
|
|
{
|
|
this.sampler = sampler ?? throw new ArgumentNullException(nameof(sampler));
|
|
}
|
|
|
|
/// <summary>从已发布轨迹选择下一轮规划的未来交接状态。</summary>
|
|
/// <param name="previousTrajectory">最后一条完整已发布轨迹；为 <see langword="null"/> 时直接使用测量状态。</param>
|
|
/// <param name="measuredState">调用方当前车辆状态快照；位置 m、航向 rad、带符号速度 m/s。</param>
|
|
/// <param name="expectedSegmentIndex">下一轮请求期望的非负方向段索引。</param>
|
|
/// <param name="expectedDirection">下一轮请求期望的实际行驶方向。</param>
|
|
/// <param name="now">调用方当前时刻，用于检查旧轨迹年龄和 lookahead。</param>
|
|
/// <param name="configuration">包含最大状态年龄、lookahead 和追踪容差的配置快照。</param>
|
|
/// <returns>只有年龄、追踪、同段同方向、末点和边界检查均通过时返回未来轨迹样本；否则返回测量状态及拒绝原因。</returns>
|
|
public TrajectoryHandoffSelection Select(EmTrajectory previousTrajectory, VehicleMotionState measuredState,
|
|
int expectedSegmentIndex, TravelDirection expectedDirection, DateTimeOffset now,
|
|
EmPlannerConfiguration configuration)
|
|
{
|
|
if (measuredState == null)
|
|
throw new ArgumentNullException(nameof(measuredState));
|
|
if (expectedSegmentIndex < 0 || !Enum.IsDefined(typeof(TravelDirection), expectedDirection))
|
|
throw new ArgumentOutOfRangeException(nameof(expectedSegmentIndex));
|
|
if (previousTrajectory == null)
|
|
return Measured(measuredState, TrajectoryHandoffRejectionReason.MissingTrajectory);
|
|
if (!TryReadConfiguration(configuration, out double maximumAgeSeconds, out double lookaheadSeconds,
|
|
out double spatialToleranceMeters, out double kinematicTolerance))
|
|
{
|
|
return Measured(measuredState, TrajectoryHandoffRejectionReason.InvalidConfiguration);
|
|
}
|
|
if (previousTrajectory.Metadata.SegmentIndex != expectedSegmentIndex)
|
|
return Measured(measuredState, TrajectoryHandoffRejectionReason.SegmentMismatch);
|
|
if (previousTrajectory.Metadata.Direction != expectedDirection)
|
|
return Measured(measuredState, TrajectoryHandoffRejectionReason.DirectionMismatch);
|
|
|
|
double trajectoryAgeSeconds = (now - previousTrajectory.Metadata.EffectiveAtUtc).TotalSeconds;
|
|
if (trajectoryAgeSeconds < -TimeEpsilonSeconds)
|
|
return Measured(measuredState, TrajectoryHandoffRejectionReason.TrajectoryNotYetEffective);
|
|
if (trajectoryAgeSeconds > maximumAgeSeconds + TimeEpsilonSeconds)
|
|
return Measured(measuredState, TrajectoryHandoffRejectionReason.TrajectoryTooOld);
|
|
if (!sampler.TrySample(previousTrajectory, trajectoryAgeSeconds, out EmTrajectoryPoint currentPoint))
|
|
return Measured(measuredState, TrajectoryHandoffRejectionReason.SampleUnavailable);
|
|
if (!Tracks(currentPoint, measuredState, spatialToleranceMeters, kinematicTolerance))
|
|
return Measured(measuredState, TrajectoryHandoffRejectionReason.TrackingErrorExceeded);
|
|
|
|
double handoffTime = trajectoryAgeSeconds + lookaheadSeconds;
|
|
double terminalTime = previousTrajectory.Points[previousTrajectory.Points.Count - 1].TimeFromStart;
|
|
if (handoffTime > terminalTime + TimeEpsilonSeconds)
|
|
return Measured(measuredState, TrajectoryHandoffRejectionReason.HandoffBeyondTrajectory);
|
|
TrajectoryHandoffRejectionReason boundaryReason = BoundaryInInterval(previousTrajectory, trajectoryAgeSeconds,
|
|
handoffTime);
|
|
if (boundaryReason != TrajectoryHandoffRejectionReason.None)
|
|
return Measured(measuredState, boundaryReason);
|
|
if (!sampler.TrySample(previousTrajectory, handoffTime, out EmTrajectoryPoint handoffPoint))
|
|
return Measured(measuredState, TrajectoryHandoffRejectionReason.SampleUnavailable);
|
|
|
|
var startState = new VehicleMotionState(new Pose2D(handoffPoint.X, handoffPoint.Y, handoffPoint.Yaw),
|
|
handoffPoint.SignedLongitudinalVelocity, handoffPoint.LongitudinalAcceleration,
|
|
previousTrajectory.Metadata.EffectiveAtUtc.AddSeconds(handoffTime), measuredState.SequenceId);
|
|
return new TrajectoryHandoffSelection(TrajectoryHandoffSource.PreviousTrajectory, startState,
|
|
previousTrajectory, handoffPoint, TrajectoryHandoffRejectionReason.None);
|
|
}
|
|
|
|
private static TrajectoryHandoffSelection Measured(VehicleMotionState measuredState,
|
|
TrajectoryHandoffRejectionReason rejectionReason)
|
|
{
|
|
return new TrajectoryHandoffSelection(TrajectoryHandoffSource.MeasuredState, measuredState, null, null,
|
|
rejectionReason);
|
|
}
|
|
|
|
private static bool TryReadConfiguration(EmPlannerConfiguration configuration, out double maximumAgeSeconds,
|
|
out double lookaheadSeconds, out double spatialToleranceMeters, out double kinematicTolerance)
|
|
{
|
|
maximumAgeSeconds = configuration?.Scheduling?.MaximumVehicleStateAgeSeconds ?? double.NaN;
|
|
lookaheadSeconds = configuration?.Scheduling?.HandoffLookaheadSeconds ?? double.NaN;
|
|
spatialToleranceMeters = configuration?.Validation?.SpatialToleranceMeters ?? double.NaN;
|
|
kinematicTolerance = configuration?.Validation?.KinematicTolerance ?? double.NaN;
|
|
return IsNonNegativeFinite(maximumAgeSeconds) && IsNonNegativeFinite(lookaheadSeconds) &&
|
|
IsNonNegativeFinite(spatialToleranceMeters) && IsNonNegativeFinite(kinematicTolerance);
|
|
}
|
|
|
|
private static bool Tracks(EmTrajectoryPoint trajectoryPoint, VehicleMotionState measuredState,
|
|
double spatialToleranceMeters, double kinematicTolerance)
|
|
{
|
|
double dx = trajectoryPoint.X - measuredState.Pose.X;
|
|
double dy = trajectoryPoint.Y - measuredState.Pose.Y;
|
|
if (Math.Sqrt(dx * dx + dy * dy) > spatialToleranceMeters)
|
|
return false;
|
|
double yawError = Math.Atan2(Math.Sin(trajectoryPoint.Yaw - measuredState.Pose.Heading),
|
|
Math.Cos(trajectoryPoint.Yaw - measuredState.Pose.Heading));
|
|
return Math.Abs(yawError) <= kinematicTolerance &&
|
|
Math.Abs(trajectoryPoint.SignedLongitudinalVelocity -
|
|
measuredState.SignedLongitudinalSpeedMetersPerSecond) <= kinematicTolerance;
|
|
}
|
|
|
|
private static TrajectoryHandoffRejectionReason BoundaryInInterval(EmTrajectory trajectory, double startTime,
|
|
double endTime)
|
|
{
|
|
for (int index = 0; index < trajectory.Points.Count; index++)
|
|
{
|
|
EmTrajectoryPoint point = trajectory.Points[index];
|
|
if (point.TimeFromStart <= startTime + TimeEpsilonSeconds || point.TimeFromStart > endTime + TimeEpsilonSeconds)
|
|
continue;
|
|
if (point.BoundaryType == EmBoundaryType.GearSwitchApproach ||
|
|
point.BoundaryType == EmBoundaryType.GearSwitchDeparture)
|
|
{
|
|
return TrajectoryHandoffRejectionReason.GearBoundary;
|
|
}
|
|
if (point.BoundaryType == EmBoundaryType.RollingSafetyStop || point.BoundaryType == EmBoundaryType.Goal)
|
|
return TrajectoryHandoffRejectionReason.TerminalBoundary;
|
|
}
|
|
return TrajectoryHandoffRejectionReason.None;
|
|
}
|
|
|
|
private static bool IsNonNegativeFinite(double value)
|
|
{
|
|
return !double.IsNaN(value) && !double.IsInfinity(value) && value >= 0d;
|
|
}
|
|
}
|