chore: save current workspace progress

2026-08-09 22:13:18 +08:00
parent 650c2ab0e3
commit 2f4fd15e52
449 changed files with 76593 additions and 971 deletions
@@ -81,7 +81,7 @@ public sealed class EmTrajectoryAssembler
            EmBoundaryType boundaryType = isTerminalAnchor ? ToBoundaryType(metadata.TerminalType) : EmBoundaryType.None;
            double signedSpeed = directionSign * sample.ProgressSpeed;
            points.Add(new EmTrajectoryPoint(geometry.X, geometry.Y, geometry.Yaw, signedSpeed, sample.TimeFromStart,
-                geometry.VehicleCurvature, metadata.SegmentIndex, sample.PathS, sample.PathS, metadata.Direction,
+                geometry.VehicleCurvature, metadata.SegmentIndex, geometry.ReferenceS, sample.PathS, metadata.Direction,
                boundaryType, sample.Acceleration, sample.Jerk));
        }

@@ -46,6 +46,7 @@ internal sealed class LateralPathInterpolator
                LateralPathPoint left = path.Points[index - 1];
                double ratio = (pathS - left.PathS) / (right.PathS - left.PathS);
                return new InterpolatedLateralPathPoint(
+                    Linear(left.ReferenceS, right.ReferenceS, ratio),
                    Linear(left.X, right.X, ratio),
                    Linear(left.Y, right.Y, ratio),
                    NormalizeYaw(left.VehicleYaw + ratio * NormalizeYaw(right.VehicleYaw - left.VehicleYaw)),
@@ -58,7 +59,8 @@ internal sealed class LateralPathInterpolator

    private static InterpolatedLateralPathPoint From(LateralPathPoint point)
    {
-        return new InterpolatedLateralPathPoint(point.X, point.Y, NormalizeYaw(point.VehicleYaw), point.VehicleCurvature);
+        return new InterpolatedLateralPathPoint(point.ReferenceS, point.X, point.Y, NormalizeYaw(point.VehicleYaw),
+            point.VehicleCurvature);
    }

    private static double Linear(double left, double right, double ratio)
@@ -84,14 +86,16 @@ internal sealed class LateralPathInterpolator

 internal sealed class InterpolatedLateralPathPoint
 {
-    public InterpolatedLateralPathPoint(double x, double y, double yaw, double vehicleCurvature)
+    public InterpolatedLateralPathPoint(double referenceS, double x, double y, double yaw, double vehicleCurvature)
    {
+        ReferenceS = referenceS;
        X = x;
        Y = y;
        Yaw = yaw;
        VehicleCurvature = vehicleCurvature;
    }

+    public double ReferenceS { get; }
    public double X { get; }
    public double Y { get; }
    public double Yaw { get; }