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,
        VehicleParameters vehicle, EmPlannerConfiguration configuration)
    {
        if (tick == null) throw new ArgumentNullException(nameof(tick));
        if (activeSegment == null) throw new ArgumentNullException(nameof(activeSegment));
        if (vehicle == null) throw new ArgumentNullException(nameof(vehicle));
        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);

        IReadOnlyList<VisualizationChart> chartList = current == null
            ? Array.Empty<VisualizationChart>()
            : charts.Build(current, activeSegment, vehicle, 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 markers = new List<VisualizationMarker>();
        AddVehicleMarker(markers, tick);
        AddPlanStartMarker(markers, current);
        AddBoundaryMarker(markers, activeSegment);
        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, markers, 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 void AddVehicleMarker(ICollection<VisualizationMarker> markers,
        TrajectoryObservationLoopTick tick)
    {
        markers.Add(new VisualizationMarker("vehicle", "vehicle", "车辆",
            new VisualizationPoint(tick.Observation.VehicleState.Pose.X,
                tick.Observation.VehicleState.Pose.Y)));
    }

    private static void AddPlanStartMarker(ICollection<VisualizationMarker> markers, EmTrajectory trajectory)
    {
        if (trajectory == null || trajectory.Points.Count == 0) return;
        EmTrajectoryPoint start = trajectory.Points[0];
        markers.Add(new VisualizationMarker("plan-start", "plan-start", "计划起点",
            new VisualizationPoint(start.X, start.Y)));
    }

    private static void AddBoundaryMarker(ICollection<VisualizationMarker> markers,
        DirectionSegmentView segment)
    {
        var point = segment.Points[segment.Points.Count - 1];
        if (segment.EndBoundary.BoundaryType == EmBoundaryType.GearSwitchApproach)
        {
            markers.Add(new VisualizationMarker("gear-switch-end", "gear-switch-end",
                "换向点 " + (segment.SegmentIndex + 1) + " / s_end",
                new VisualizationPoint(point.X, point.Y)));
        }
        else if (segment.EndBoundary.BoundaryType == EmBoundaryType.Goal)
        {
            markers.Add(new VisualizationMarker("final-goal", "final-goal", "终点 / s_end",
                new VisualizationPoint(point.X, point.Y)));
        }
    }

    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) : "不可用";
    }
}