159 lines
9.6 KiB
C#
159 lines
9.6 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
using System.Globalization;
|
||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||
using TrajectoryPlanningVisualization;
|
||
|
||
namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
||
|
||
/// <summary>将每个 tick 的新鲜车辆状态与已发布轨迹转换为不可变动态可视化快照。</summary>
|
||
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) : "不可用";
|
||
}
|
||
}
|