40 lines
4.0 KiB
C#
40 lines
4.0 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
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>();
|
|
AddTrajectory(polylines, "previous", "上一轮轨迹", previousTrajectoryForVisualization, VisualizationLineStyle.Dashed);
|
|
AddTrajectory(polylines, "current", "当前轨迹", current, VisualizationLineStyle.Solid);
|
|
var status = new List<VisualizationValue>();
|
|
status.Add(Value("活动段", "ActiveSegmentIndex", activeSegment.SegmentIndex.ToString(), "", "normal"));
|
|
status.Add(Value("方向", "ActiveDirection", activeSegment.Direction.ToString(), "", "normal"));
|
|
IReadOnlyList<VisualizationChart> chartList = current == null ? Array.Empty<VisualizationChart>() : charts.Build(current, activeSegment, configuration);
|
|
TrajectoryObservationHandoffMetrics metrics = handoff.Analyze(current, previousTrajectoryForVisualization, activeSegment);
|
|
status.Add(Value("交接位置差", "DeltaPositionMeters", F(metrics.DeltaPositionMeters), "m", metrics.Available ? "normal" : "notice"));
|
|
status.Add(Value("交接参考站差", "DeltaReferenceSMeters", F(metrics.DeltaReferenceSMeters), "m", metrics.DeltaReferenceSMeters.HasValue ? "normal" : "notice"));
|
|
EmTrajectoryPoint terminal = current == null ? null : current.Points[current.Points.Count-1];
|
|
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 AddTrajectory(ICollection<VisualizationPolyline> lines,string id,string legend,EmTrajectory trajectory,VisualizationLineStyle style) { if(trajectory==null)return; var points=new List<VisualizationPoint>(); foreach(var point in trajectory.Points)points.Add(new VisualizationPoint(point.X,point.Y)); lines.Add(new VisualizationPolyline(id,legend,id,style,points)); }
|
|
private static VisualizationValue Value(string c,string raw,string v,string unit,string severity)=>new VisualizationValue(c,raw,v,unit,severity);
|
|
private static string F(double? v)=>v.HasValue?v.Value.ToString(System.Globalization.CultureInfo.InvariantCulture):"不可用";
|
|
}
|