First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat: visualize EM rolling kinematics

Browse Source
This commit is contained in:
梁薄云
2026-08-06 11:19:40 +08:00
parent 0a1cbe9fbd
commit a7dde0f5e4
4 changed files with 109 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDynamicSnapshotBuilder.cs
+39
View File
@@ -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<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):"不可用";
}
ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationHandoffAnalyzer.cs
+34
View File
@@ -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);
}
}
ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationKinematicChartBuilder.cs
+33
View File
@@ -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<VisualizationChart> 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<VisualizationPoint> 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<VisualizationPoint>(); for (int i=0;i+1<trajectory.Points.Count;i++) p.Add(new VisualizationPoint(trajectory.Points[i].TimeFromStart, trajectory.Points[i].LongitudinalJerk)); return new VisualizationChart("jerk-t", "加加速度", "t (s)", "j (m/s³)", new[] { new VisualizationSeries("jerk-t-current", "当前轨迹", VisualizationLineStyle.Solid, p), new VisualizationSeries("jerk-t-limit", "生效限制", VisualizationLineStyle.Limit, new[] { new VisualizationPoint(0d, configuration.Longitudinal.MaximumJerkMetersPerSecondCubed), new VisualizationPoint(p.Count == 0 ? 0d : p[p.Count-1].X, configuration.Longitudinal.MaximumJerkMetersPerSecondCubed) }) }, "末点后无时间区间"); }
private static VisualizationChart Chart(string id,string title,string x,string y,IReadOnlyList<VisualizationPoint> points,string note) => new VisualizationChart(id,title,x,y,new[] { new VisualizationSeries(id+"-current","当前轨迹",VisualizationLineStyle.Solid,points) },note);
private static IReadOnlyList<VisualizationPoint> Points(EmTrajectory t, Func<EmTrajectoryPoint,double> y) { var r=new List<VisualizationPoint>(); foreach(var p in t.Points) r.Add(new VisualizationPoint(p.TimeFromStart,y(p))); return r; }
private static IReadOnlyList<VisualizationPoint> Ls(EmTrajectory t, DirectionSegmentView s) { var r=new List<VisualizationPoint>(); 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; }
}
ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationVisualizationChecks.cs
+3 -1
View File
@@ -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");
}
}