feat: visualize EM rolling kinematics

2026-08-06 11:19:40 +08:00
parent 0a1cbe9fbd
commit a7dde0f5e4
4 changed files with 109 additions and 1 deletions
@@ -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);
+    }
+}