From a7dde0f5e4c37438f775b706e3886ace00fe0e04 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Thu, 6 Aug 2026 11:19:40 +0800 Subject: [PATCH] feat: visualize EM rolling kinematics --- ...ectoryObservationDynamicSnapshotBuilder.cs | 39 +++++++++++++++++++ .../TrajectoryObservationHandoffAnalyzer.cs | 34 ++++++++++++++++ ...jectoryObservationKinematicChartBuilder.cs | 33 ++++++++++++++++ ...rajectoryObservationVisualizationChecks.cs | 4 +- 4 files changed, 109 insertions(+), 1 deletion(-) create mode 100644 ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDynamicSnapshotBuilder.cs create mode 100644 ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationHandoffAnalyzer.cs create mode 100644 ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationKinematicChartBuilder.cs diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDynamicSnapshotBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDynamicSnapshotBuilder.cs new file mode 100644 index 0000000..da77ec2 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDynamicSnapshotBuilder.cs @@ -0,0 +1,39 @@ +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(); + AddTrajectory(polylines, "previous", "上一轮轨迹", previousTrajectoryForVisualization, VisualizationLineStyle.Dashed); + AddTrajectory(polylines, "current", "当前轨迹", current, VisualizationLineStyle.Solid); + var status = new List(); + status.Add(Value("活动段", "ActiveSegmentIndex", activeSegment.SegmentIndex.ToString(), "", "normal")); + status.Add(Value("方向", "ActiveDirection", activeSegment.Direction.ToString(), "", "normal")); + IReadOnlyList chartList = current == null ? Array.Empty() : 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(), 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 lines,string id,string legend,EmTrajectory trajectory,VisualizationLineStyle style) { if(trajectory==null)return; var points=new List(); 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):"不可用"; +} diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationHandoffAnalyzer.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationHandoffAnalyzer.cs new file mode 100644 index 0000000..9bbcbaa --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationHandoffAnalyzer.cs @@ -0,0 +1,34 @@ +using System; +using MultiWheelC.TrajectoryPlanning.CoarsePath; +using MultiWheelC.TrajectoryPlanning.EMPlanner; + +namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation; + +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; } +} + +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); + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationKinematicChartBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationKinematicChartBuilder.cs new file mode 100644 index 0000000..bf39a3f --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationKinematicChartBuilder.cs @@ -0,0 +1,33 @@ +using System; +using System.Collections.Generic; +using MultiWheelC.TrajectoryPlanning.EMPlanner; +using TrajectoryPlanningVisualization; + +namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation; + +public sealed class TrajectoryObservationKinematicChartBuilder +{ + public IReadOnlyList Build(EmTrajectory trajectory, DirectionSegmentView segment, + EmPlannerConfiguration configuration) + { + if (trajectory == null) throw new ArgumentNullException(nameof(trajectory)); + if (segment == null) throw new ArgumentNullException(nameof(segment)); + if (configuration == null) throw new ArgumentNullException(nameof(configuration)); + return new[] + { + Chart("ls", "横向偏移", "ReferenceS (m)", "l (m)", Ls(trajectory, segment), ""), + Chart("st", "时空轨迹", "t (s)", "PathS (m)", Points(trajectory, p => p.PathS), ""), + Chart("curvature-s", "曲率-距离", "s (m)", "κ (m⁻¹)", Points(trajectory, p => p.VehicleCurvature), ""), + Chart("curvature-t", "曲率-时间", "t (s)", "κ (m⁻¹)", Points(trajectory, p => p.VehicleCurvature), ""), + WithLimit("velocity-t", "速度", "t (s)", "v (m/s)", Points(trajectory, p => p.SignedLongitudinalVelocity), configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond), + WithLimit("acceleration-t", "加速度", "t (s)", "a (m/s²)", Points(trajectory, p => p.LongitudinalAcceleration), configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared), + Jerk(trajectory, configuration), + Chart("yaw-rate-t", "横摆角速度", "t (s)", "yaw rate (rad/s)", Points(trajectory, p => p.YawRate), "无独立横摆角速度上限") + }; + } + private static VisualizationChart WithLimit(string id, string title, string x, string y, IReadOnlyList points, double limit) => new VisualizationChart(id, title, x, y, new[] { new VisualizationSeries(id + "-current", "当前轨迹", VisualizationLineStyle.Solid, points), new VisualizationSeries(id + "-limit", "生效限制", VisualizationLineStyle.Limit, new[] { new VisualizationPoint(0d, limit), new VisualizationPoint(points[points.Count - 1].X, limit) }) }); + private static VisualizationChart Jerk(EmTrajectory trajectory, EmPlannerConfiguration configuration) { var p = new List(); for (int i=0;i+1 points,string note) => new VisualizationChart(id,title,x,y,new[] { new VisualizationSeries(id+"-current","当前轨迹",VisualizationLineStyle.Solid,points) },note); + private static IReadOnlyList Points(EmTrajectory t, Func y) { var r=new List(); foreach(var p in t.Points) r.Add(new VisualizationPoint(p.TimeFromStart,y(p))); return r; } + private static IReadOnlyList Ls(EmTrajectory t, DirectionSegmentView s) { var r=new List(); var projector=new FrenetProjector(); double seed=0d; foreach(var p in t.Points) if(projector.TryProject(new CoarsePath.Pose2D(p.X,p.Y,p.Yaw),s,0d,s.LengthMeters,0.5d,seed,out FrenetProjection projection)) { r.Add(new VisualizationPoint(s.SourceStartArcLength+projection.ReferenceS,projection.LateralOffset)); seed=projection.ReferenceS; } return r; } +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationVisualizationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationVisualizationChecks.cs index c3e5d3d..6a0c618 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationVisualizationChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationVisualizationChecks.cs @@ -8,7 +8,9 @@ internal static class TrajectoryObservationVisualizationChecks { var staticBuilder = new TrajectoryObservationStaticSnapshotBuilder(); var chartBuilder = new TrajectoryObservationKinematicChartBuilder(); - Verification.True(staticBuilder != null && chartBuilder != null, + var dynamicBuilder = new TrajectoryObservationDynamicSnapshotBuilder(); + var handoffAnalyzer = new TrajectoryObservationHandoffAnalyzer(); + Verification.True(staticBuilder != null && chartBuilder != null && dynamicBuilder != null && handoffAnalyzer != null, "observation visualization adapters are available"); } }