ParkingRobot/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDynamicSnapshotBuilder.cs

using System;
using System.Collections.Generic;
using System.Globalization;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.EMPlanner;
using TrajectoryPlanningVisualization;

namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;

public sealed class TrajectoryObservationDynamicSnapshotBuilder
{
    private readonly TrajectoryObservationKinematicChartBuilder charts = new TrajectoryObservationKinematicChartBuilder();
    private readonly TrajectoryObservationHandoffAnalyzer handoff = new TrajectoryObservationHandoffAnalyzer();

    public PlanningVisualizationDynamicSnapshot Build(long sequence, TrajectoryObservationLoopTick tick,
        DirectionSegmentView activeSegment, EmTrajectory previousTrajectoryForVisualization, EmPlannerConfiguration configuration)
    {
        if (tick == null) throw new ArgumentNullException(nameof(tick));
        if (activeSegment == null) throw new ArgumentNullException(nameof(activeSegment));
        if (configuration == null) throw new ArgumentNullException(nameof(configuration));

        EmTrajectory current = tick.Observation.PublishedTrajectory;
        var polylines = new List<VisualizationPolyline>();
        AddSegment(polylines, activeSegment);
        AddTrajectory(polylines, "previous", "上一轨迹", previousTrajectoryForVisualization, VisualizationLineStyle.Dashed);
        AddTrajectory(polylines, "current", "当前轨迹", current, VisualizationLineStyle.Solid);
        AddTrajectory(polylines, "current-horizon", "当前规划视界", current, VisualizationLineStyle.Solid);

        IReadOnlyList<VisualizationChart> chartList = current == null
            ? Array.Empty<VisualizationChart>()
            : charts.Build(current, activeSegment, configuration);
        int projectionFailures = current == null ? 0 : charts.CountProjectionFailures(current, activeSegment, configuration);
        TrajectoryObservationHandoffMetrics metrics = handoff.Analyze(current, previousTrajectoryForVisualization, activeSegment);
        EmTrajectoryPoint terminal = current == null ? null : current.Points[current.Points.Count - 1];
        var status = new List<VisualizationValue>
        {
            Value("活动段", "ActiveSegmentIndex", activeSegment.SegmentIndex.ToString(CultureInfo.InvariantCulture), "", "normal"),
            Value("方向", "ActiveDirection", activeSegment.Direction.ToString(), "", "normal"),
            Value("规划状态", "PlanningStatus", tick.LatestCycle == null ? "pending" : tick.LatestCycle.Result.Status.ToString(), "", "normal"),
            Value("纵向模式", "LongitudinalMode", current == null ? "不可用" : current.Metadata.LongitudinalMode.ToString(), "", "normal"),
            Value("终端类型", "TerminalType", current == null ? "不可用" : current.Metadata.TerminalType.ToString(), "", "normal"),
            Value("规划耗时", "PlanningElapsedMilliseconds", F(Math.Max(0d, tick.LatestPlanningElapsed.TotalMilliseconds)), "ms", "normal"),
            Value("轨迹年龄", "TrajectoryAgeSeconds", current == null ? "不可用" : F(Math.Max(0d,
                (tick.Observation.ObservedAtUtc - current.Metadata.EffectiveAtUtc).TotalSeconds)), "s", "normal"),
            Value("LS 投影失败", "LsProjectionFailures", projectionFailures.ToString(CultureInfo.InvariantCulture), "", projectionFailures == 0 ? "normal" : "notice"),
            Value("滚动终端语义", "RollingTerminalConstraint", RollingTerminalText(current), "", "normal"),
            Value("终端速度", "TerminalVelocity", terminal == null ? "不可用" : F(terminal.SignedLongitudinalVelocity), "m/s", "normal"),
            Value("终端加速度", "TerminalAcceleration", terminal == null ? "不可用" : F(terminal.LongitudinalAcceleration), "m/s²", "normal"),
            Value("交接位置差", "DeltaPositionMeters", F(metrics.DeltaPositionMeters), "m", metrics.Available ? "normal" : "notice"),
            Value("交接参考站差", "DeltaReferenceSMeters", F(metrics.DeltaReferenceSMeters), "m", metrics.DeltaReferenceSMeters.HasValue ? "normal" : "notice"),
            Value("交接速度差", "DeltaVelocityMetersPerSecond", F(metrics.DeltaVelocityMetersPerSecond), "m/s", metrics.Available ? "normal" : "notice"),
            Value("交接加速度差", "DeltaAccelerationMetersPerSecondSquared", F(metrics.DeltaAccelerationMetersPerSecondSquared), "m/s²", metrics.Available ? "normal" : "notice"),
            Value("换向确认", "GearConfirmation", GearConfirmationText(tick.SegmentState), "", "normal"),
        };

        var cycle = tick.LatestCycle;
        return new PlanningVisualizationDynamicSnapshot(sequence, tick.Observation.ObservedAtUtc, "OBSERVE_ONLY",
            activeSegment.SegmentIndex, activeSegment.Direction.ToString(), new VisualizationPose(
                tick.Observation.VehicleState.Pose.X, tick.Observation.VehicleState.Pose.Y,
                tick.Observation.VehicleState.Pose.Heading), polylines, Array.Empty<VisualizationMarker>(), chartList, status,
            new VisualizationCycleSummary(cycle == null ? 0L : cycle.Version, tick.Observation.ObservedAtUtc,
                cycle == null ? "pending" : cycle.Result.Status.ToString(), cycle != null && cycle.Published,
                Math.Max(0d, tick.LatestPlanningElapsed.TotalMilliseconds), activeSegment.SegmentIndex,
                activeSegment.Direction.ToString(), current == null ? "" : current.Metadata.LongitudinalMode.ToString(),
                current == null ? "" : current.Metadata.TerminalType.ToString(), terminal?.SignedLongitudinalVelocity,
                terminal?.LongitudinalAcceleration, cycle?.Diagnostic ?? ""));
    }

    private static void AddSegment(ICollection<VisualizationPolyline> lines, DirectionSegmentView segment)
    {
        var points = new List<VisualizationPoint>();
        foreach (var point in segment.Points)
            points.Add(new VisualizationPoint(point.X, point.Y));
        lines.Add(new VisualizationPolyline("active-segment", "活动方向段", "active-segment", VisualizationLineStyle.Solid, points));
    }

    private static void AddTrajectory(ICollection<VisualizationPolyline> lines, string id, string legend,
        EmTrajectory trajectory, VisualizationLineStyle style)
    {
        if (trajectory == null) return;
        var points = new List<VisualizationPoint>();
        foreach (EmTrajectoryPoint point in trajectory.Points)
            points.Add(new VisualizationPoint(point.X, point.Y));
        lines.Add(new VisualizationPolyline(id, legend, id, style, points));
    }

    private static string RollingTerminalText(EmTrajectory trajectory)
    {
        if (trajectory == null) return "不可用";
        return trajectory.Metadata.LongitudinalMode == EmLongitudinalMode.ExactStopAtBoundary
            ? "精确停车锚点：已启用"
            : "滚动末端停车硬约束：未启用";
    }

    private static string GearConfirmationText(TrajectoryObservationSegmentState state)
    {
        if (state.Phase == TrajectoryObservationSegmentPhase.WaitingForStop)
            return "未满足：等待停车保持";
        if (state.Phase == TrajectoryObservationSegmentPhase.WaitingForDirection)
            return "未满足：等待 " + state.ExpectedDirection + " 连续方向样本（" +
                state.ConfirmedDirectionSamples.ToString(CultureInfo.InvariantCulture) + "）";
        return state.Phase == TrajectoryObservationSegmentPhase.Completed ? "已完成" : "当前段规划中";
    }

    private static VisualizationValue Value(string chineseName, string rawName, string value, string unit, string severity)
    {
        return new VisualizationValue(chineseName, rawName, value, unit, severity);
    }

    private static string F(double value)
    {
        return value.ToString("R", CultureInfo.InvariantCulture);
    }

    private static string F(double? value)
    {
        return value.HasValue ? F(value.Value) : "不可用";
    }
}