diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationStaticSnapshotBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationStaticSnapshotBuilder.cs index fbac54b..d4435e7 100644 --- a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationStaticSnapshotBuilder.cs +++ b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationStaticSnapshotBuilder.cs @@ -56,6 +56,12 @@ public sealed class TrajectoryObservationStaticSnapshotBuilder private static IReadOnlyList CreateMarkers(TrajectoryObservationBootstrapResult bootstrap) { var markers = new List(); + if (bootstrap.SmoothedPath.Path.Count > 0) + { + var start = bootstrap.SmoothedPath.Path[0]; + markers.Add(new VisualizationMarker("smooth-start", "smooth-start", + "平滑路径起点", new VisualizationPoint(start.X, start.Y))); + } foreach (DirectionSegmentView segment in bootstrap.Segments) { if (segment.EndBoundary.BoundaryType == EmBoundaryType.GearSwitchApproach) diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationVisualizationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationVisualizationChecks.cs index b167358..8d3be8f 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationVisualizationChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationVisualizationChecks.cs @@ -82,6 +82,13 @@ internal static class TrajectoryObservationVisualizationChecks Verification.Equal(bootstrap.Segments.Count, snapshot.DirectionSegments.Count, "all direction segments exported"); Verification.True(snapshot.StaticPolylines.Any(x => x.Id == "coarse-path"), "coarse path exported"); Verification.True(snapshot.StaticPolylines.Any(x => x.Id == "local-g2-path"), "full Local G2 path exported"); + VisualizationMarker smoothStart = snapshot.StaticMarkers.Single(x => x.Kind == "smooth-start"); + Verification.Equal("平滑路径起点", smoothStart.LabelChinese, + "static snapshot labels the smoothed-path start"); + Verification.NearlyEqual(bootstrap.SmoothedPath.Path[0].X, smoothStart.Position.X, + "smoothed-path start marker X matches the first smoothed point"); + Verification.NearlyEqual(bootstrap.SmoothedPath.Path[0].Y, smoothStart.Position.Y, + "smoothed-path start marker Y matches the first smoothed point"); byte[] occupancy = Convert.FromBase64String(snapshot.OccupancyGrid.OccupancyBitsBase64); Verification.Equal((bootstrap.Map.Rows * bootstrap.Map.Cols + 7) / 8, occupancy.Length, "occupancy bitset compact length");