37 lines
2.6 KiB
C#
37 lines
2.6 KiB
C#
using System;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
|
|
|
/// <summary>描述相邻已发布轨迹在交接处的位置、航向和速度连续性，仅作诊断。</summary>
|
|
public sealed class TrajectoryObservationHandoffMetrics
|
|
{
|
|
internal TrajectoryObservationHandoffMetrics(bool available, double? position, double? referenceS, double? velocity, double? acceleration)
|
|
{ Available = available; DeltaPositionMeters = position; DeltaReferenceSMeters = referenceS; DeltaVelocityMetersPerSecond = velocity; DeltaAccelerationMetersPerSecondSquared = acceleration; }
|
|
public bool Available { get; }
|
|
public double? DeltaPositionMeters { get; }
|
|
public double? DeltaReferenceSMeters { get; }
|
|
public double? DeltaVelocityMetersPerSecond { get; }
|
|
public double? DeltaAccelerationMetersPerSecondSquared { get; }
|
|
}
|
|
|
|
/// <summary>计算观察会话中的轨迹交接指标，不参与轨迹发布或控制决策。</summary>
|
|
public sealed class TrajectoryObservationHandoffAnalyzer
|
|
{
|
|
public TrajectoryObservationHandoffMetrics Analyze(EmTrajectory current, EmTrajectory previous, DirectionSegmentView segment)
|
|
{
|
|
if (current == null || previous == null || segment == null || current.Points.Count == 0) return new TrajectoryObservationHandoffMetrics(false, null, null, null, null);
|
|
double time = (current.Metadata.EffectiveAtUtc - previous.Metadata.EffectiveAtUtc).TotalSeconds;
|
|
if (!new TrajectorySampler().TrySample(previous, time, out EmTrajectoryPoint oldPoint)) return new TrajectoryObservationHandoffMetrics(false, null, null, null, null);
|
|
EmTrajectoryPoint newPoint = current.Points[0];
|
|
double position = Math.Sqrt((newPoint.X-oldPoint.X)*(newPoint.X-oldPoint.X)+(newPoint.Y-oldPoint.Y)*(newPoint.Y-oldPoint.Y));
|
|
double velocity = newPoint.SignedLongitudinalVelocity-oldPoint.SignedLongitudinalVelocity;
|
|
double acceleration = newPoint.LongitudinalAcceleration-oldPoint.LongitudinalAcceleration;
|
|
var projector = new FrenetProjector();
|
|
bool oldOk = projector.TryProject(new Pose2D(oldPoint.X, oldPoint.Y, oldPoint.Yaw), segment, 0d, segment.LengthMeters, 0.5d, 0d, out FrenetProjection oldProjection);
|
|
bool newOk = projector.TryProject(new Pose2D(newPoint.X, newPoint.Y, newPoint.Yaw), segment, 0d, segment.LengthMeters, 0.5d, 0d, out FrenetProjection newProjection);
|
|
return new TrajectoryObservationHandoffMetrics(true, position, oldOk && newOk ? newProjection.ReferenceS-oldProjection.ReferenceS : null, velocity, acceleration);
|
|
}
|
|
}
|