ParkingRobot/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationHandoffAnalyzer.cs

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);
    }
}