diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs index e77fb89..6b877cb 100644 --- a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs +++ b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs @@ -321,7 +321,8 @@ internal static class TrajectoryObservationMovementTestRunner DrawIfCurrent(sessionId, bootstrap, observation, charts, runtimeState, diagnostic); webPublisher?.TryPublish(now, () => dynamicSnapshotBuilder.Build( Interlocked.Increment(ref visualizationSequence), tick, controller.ActiveSegment, - controller.PreviousTrajectoryForVisualization, controller.CreateEffectiveConfigurationSnapshot())); + controller.PreviousTrajectoryForVisualization, bootstrap.Vehicle, + controller.CreateEffectiveConfigurationSnapshot())); if (runtimeState.WaitingAtGearSwitch && !gearSwitchWaitingPrinted) { LogIfCurrent(sessionId, runtimeState.WorldNotice); diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDynamicSnapshotBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDynamicSnapshotBuilder.cs index b93190e..e642722 100644 --- a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDynamicSnapshotBuilder.cs +++ b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDynamicSnapshotBuilder.cs @@ -13,10 +13,12 @@ public sealed class TrajectoryObservationDynamicSnapshotBuilder private readonly TrajectoryObservationHandoffAnalyzer handoff = new TrajectoryObservationHandoffAnalyzer(); public PlanningVisualizationDynamicSnapshot Build(long sequence, TrajectoryObservationLoopTick tick, - DirectionSegmentView activeSegment, EmTrajectory previousTrajectoryForVisualization, EmPlannerConfiguration configuration) + DirectionSegmentView activeSegment, EmTrajectory previousTrajectoryForVisualization, + VehicleParameters vehicle, EmPlannerConfiguration configuration) { if (tick == null) throw new ArgumentNullException(nameof(tick)); if (activeSegment == null) throw new ArgumentNullException(nameof(activeSegment)); + if (vehicle == null) throw new ArgumentNullException(nameof(vehicle)); if (configuration == null) throw new ArgumentNullException(nameof(configuration)); EmTrajectory current = tick.Observation.PublishedTrajectory; @@ -24,14 +26,17 @@ public sealed class TrajectoryObservationDynamicSnapshotBuilder AddSegment(polylines, activeSegment); AddTrajectory(polylines, "previous", "上一轨迹", previousTrajectoryForVisualization, VisualizationLineStyle.Dashed); AddTrajectory(polylines, "current", "当前轨迹", current, VisualizationLineStyle.Solid); - AddTrajectory(polylines, "current-horizon", "当前规划视界", current, VisualizationLineStyle.Solid); IReadOnlyList chartList = current == null ? Array.Empty() - : charts.Build(current, activeSegment, configuration); + : charts.Build(current, activeSegment, vehicle, configuration); int projectionFailures = current == null ? 0 : charts.CountProjectionFailures(current, activeSegment, configuration); TrajectoryObservationHandoffMetrics metrics = handoff.Analyze(current, previousTrajectoryForVisualization, activeSegment); EmTrajectoryPoint terminal = current == null ? null : current.Points[current.Points.Count - 1]; + var markers = new List(); + AddVehicleMarker(markers, tick); + AddPlanStartMarker(markers, current); + AddBoundaryMarker(markers, activeSegment); var status = new List { Value("活动段", "ActiveSegmentIndex", activeSegment.SegmentIndex.ToString(CultureInfo.InvariantCulture), "", "normal"), @@ -57,7 +62,7 @@ public sealed class TrajectoryObservationDynamicSnapshotBuilder 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, + tick.Observation.VehicleState.Pose.Heading), polylines, markers, 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, @@ -84,6 +89,39 @@ public sealed class TrajectoryObservationDynamicSnapshotBuilder lines.Add(new VisualizationPolyline(id, legend, id, style, points)); } + private static void AddVehicleMarker(ICollection markers, + TrajectoryObservationLoopTick tick) + { + markers.Add(new VisualizationMarker("vehicle", "vehicle", "车辆", + new VisualizationPoint(tick.Observation.VehicleState.Pose.X, + tick.Observation.VehicleState.Pose.Y))); + } + + private static void AddPlanStartMarker(ICollection markers, EmTrajectory trajectory) + { + if (trajectory == null || trajectory.Points.Count == 0) return; + EmTrajectoryPoint start = trajectory.Points[0]; + markers.Add(new VisualizationMarker("plan-start", "plan-start", "计划起点", + new VisualizationPoint(start.X, start.Y))); + } + + private static void AddBoundaryMarker(ICollection markers, + DirectionSegmentView segment) + { + var point = segment.Points[segment.Points.Count - 1]; + if (segment.EndBoundary.BoundaryType == EmBoundaryType.GearSwitchApproach) + { + markers.Add(new VisualizationMarker("gear-switch-end", "gear-switch-end", + "换向点 " + (segment.SegmentIndex + 1) + " / s_end", + new VisualizationPoint(point.X, point.Y))); + } + else if (segment.EndBoundary.BoundaryType == EmBoundaryType.Goal) + { + markers.Add(new VisualizationMarker("final-goal", "final-goal", "终点 / s_end", + new VisualizationPoint(point.X, point.Y))); + } + } + private static string RollingTerminalText(EmTrajectory trajectory) { if (trajectory == null) return "不可用"; diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationKinematicChartBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationKinematicChartBuilder.cs index ca19520..564d651 100644 --- a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationKinematicChartBuilder.cs +++ b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationKinematicChartBuilder.cs @@ -10,26 +10,32 @@ namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation; public sealed class TrajectoryObservationKinematicChartBuilder { public IReadOnlyList Build(EmTrajectory trajectory, DirectionSegmentView segment, - EmPlannerConfiguration configuration) + VehicleParameters vehicle, EmPlannerConfiguration configuration) { if (trajectory == null) throw new ArgumentNullException(nameof(trajectory)); if (segment == null) throw new ArgumentNullException(nameof(segment)); + if (vehicle == null) throw new ArgumentNullException(nameof(vehicle)); if (configuration == null) throw new ArgumentNullException(nameof(configuration)); LsBuildResult ls = BuildLs(trajectory, segment, configuration.Frenet.MaximumProjectionDistanceMeters); + double curvatureLimit = CurvatureLimit(vehicle); + double terminalTime = trajectory.Points.Count == 0 ? 0d : trajectory.Points[trajectory.Points.Count - 1].TimeFromStart; return new[] { Chart("ls", "横向偏移", "ReferenceS (m)", "l (m)", ls.Points, - "完整活动方向段投影；投影失败=" + ls.ProjectionFailureCount.ToString(CultureInfo.InvariantCulture)), - Chart("st", "时空轨迹", "t (s)", "PathS (m)", Points(trajectory, p => p.PathS), ""), + "完整活动方向段投影；投影失败=" + ls.ProjectionFailureCount.ToString(CultureInfo.InvariantCulture), + new[] { new VisualizationChartAnnotation("ls-s-end", "s-end", "s_end", + segment.SourceStartArcLength + segment.LengthMeters) }), + Chart("st", "时空轨迹", "t (s)", "PathS (m)", Points(trajectory, p => p.PathS), "", + new[] { new VisualizationChartAnnotation("st-s-end", "s-end", "s_end", terminalTime) }), WithSymmetricLimit("curvature-s", "曲率-距离", "s (m)", "κ (m⁻¹)", - PointsBySegmentS(trajectory), configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond), + PointsBySegmentS(trajectory), curvatureLimit), WithSymmetricLimit("curvature-t", "曲率-时间", "t (s)", "κ (m⁻¹)", - Points(trajectory, p => p.VehicleCurvature), configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond), + Points(trajectory, p => p.VehicleCurvature), curvatureLimit), Velocity(trajectory, configuration), Acceleration(trajectory, configuration), Jerk(trajectory, configuration), - Chart("yaw-rate-t", "横摆角速度", "t (s)", "yaw rate (rad/s)", + Chart("yaw-rate-t", "横摆角速度", "t (s)", "ω (rad/s)", Points(trajectory, p => p.YawRate), "生效配置没有独立横摆角速度上限"), }; } @@ -98,10 +104,12 @@ public sealed class TrajectoryObservationKinematicChartBuilder } private static VisualizationChart Chart(string id, string title, string xAxis, string yAxis, - IReadOnlyList points, string note) + IReadOnlyList points, string note, + IReadOnlyList annotations = null) { return new VisualizationChart(id, title, xAxis, yAxis, - new[] { new VisualizationSeries(id + "-current", "当前轨迹", VisualizationLineStyle.Solid, points) }, note); + new[] { new VisualizationSeries(id + "-current", "当前轨迹", VisualizationLineStyle.Solid, points) }, + note, annotations ?? Array.Empty()); } private static IReadOnlyList Points(EmTrajectory trajectory, Func y) @@ -120,6 +128,21 @@ public sealed class TrajectoryObservationKinematicChartBuilder return points; } + private static double CurvatureLimit(VehicleParameters vehicle) + { + if (vehicle.MaximumCurvaturePerMeter.HasValue) + { + return Math.Abs(vehicle.MaximumCurvaturePerMeter.Value); + } + + if (vehicle.MinimumTurningRadiusMeters.HasValue && vehicle.MinimumTurningRadiusMeters.Value > 0d) + { + return 1d / vehicle.MinimumTurningRadiusMeters.Value; + } + + throw new ArgumentException("Vehicle curvature limit is required.", nameof(vehicle)); + } + private static LsBuildResult BuildLs(EmTrajectory trajectory, DirectionSegmentView segment, double maximumProjectionDistanceMeters) { diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationStaticSnapshotBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationStaticSnapshotBuilder.cs index 4915a0e..fbac54b 100644 --- a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationStaticSnapshotBuilder.cs +++ b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationStaticSnapshotBuilder.cs @@ -57,12 +57,21 @@ public sealed class TrajectoryObservationStaticSnapshotBuilder { var markers = new List(); foreach (DirectionSegmentView segment in bootstrap.Segments) + { if (segment.EndBoundary.BoundaryType == EmBoundaryType.GearSwitchApproach) { var point = segment.Points[segment.Points.Count - 1]; - markers.Add(new VisualizationMarker("gear-switch-" + segment.SegmentIndex, "gear-switch", - "换向 " + (segment.SegmentIndex + 1), new VisualizationPoint(point.X, point.Y))); + markers.Add(new VisualizationMarker("gear-switch-end-" + segment.SegmentIndex, "gear-switch-end", + "换向点 " + (segment.SegmentIndex + 1) + " / s_end", + new VisualizationPoint(point.X, point.Y))); } + else if (segment.EndBoundary.BoundaryType == EmBoundaryType.Goal) + { + var point = segment.Points[segment.Points.Count - 1]; + markers.Add(new VisualizationMarker("final-goal-" + segment.SegmentIndex, "final-goal", + "终点 / s_end", new VisualizationPoint(point.X, point.Y))); + } + } return markers; } diff --git a/ClumsyPilot/TrajectoryPlanningVisualization/Contracts/VisualizationCharts.cs b/ClumsyPilot/TrajectoryPlanningVisualization/Contracts/VisualizationCharts.cs index c829f98..8977048 100644 --- a/ClumsyPilot/TrajectoryPlanningVisualization/Contracts/VisualizationCharts.cs +++ b/ClumsyPilot/TrajectoryPlanningVisualization/Contracts/VisualizationCharts.cs @@ -1,3 +1,4 @@ +using System; using System.Collections.Generic; namespace TrajectoryPlanningVisualization; @@ -26,10 +27,39 @@ public sealed class VisualizationSeries public IReadOnlyList Points { get; } } +public sealed class VisualizationChartAnnotation +{ + public VisualizationChartAnnotation(string id, string kind, string labelChinese, + double x, double? y = null) + { + Id = VisualizationContract.RequireText(id, nameof(id)); + Kind = VisualizationContract.RequireText(kind, nameof(kind)); + LabelChinese = VisualizationContract.RequireText(labelChinese, nameof(labelChinese)); + VisualizationContract.RequireFinite(x, nameof(x)); + VisualizationContract.RequireFinite(y, nameof(y)); + X = x; + Y = y; + } + + public string Id { get; } + public string Kind { get; } + public string LabelChinese { get; } + public double X { get; } + public double? Y { get; } +} + public sealed class VisualizationChart { public VisualizationChart(string id, string chineseTitle, string xAxisLabel, string yAxisLabel, IReadOnlyList series, string noteChinese = "") + : this(id, chineseTitle, xAxisLabel, yAxisLabel, series, noteChinese, + Array.Empty()) + { + } + + public VisualizationChart(string id, string chineseTitle, string xAxisLabel, string yAxisLabel, + IReadOnlyList series, string noteChinese, + IReadOnlyList annotations) { Id = VisualizationContract.RequireText(id, nameof(id)); ChineseTitle = VisualizationContract.RequireText(chineseTitle, nameof(chineseTitle)); @@ -37,6 +67,7 @@ public sealed class VisualizationChart YAxisLabel = VisualizationContract.RequireText(yAxisLabel, nameof(yAxisLabel)); Series = VisualizationContract.Copy(series, nameof(series)); NoteChinese = VisualizationContract.RequireText(noteChinese, nameof(noteChinese)); + Annotations = VisualizationContract.Copy(annotations, nameof(annotations)); } public string Id { get; } @@ -45,4 +76,5 @@ public sealed class VisualizationChart public string YAxisLabel { get; } public IReadOnlyList Series { get; } public string NoteChinese { get; } + public IReadOnlyList Annotations { get; } } diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationVisualizationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationVisualizationChecks.cs index c61001d..2ca1609 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationVisualizationChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationVisualizationChecks.cs @@ -21,6 +21,7 @@ internal static class TrajectoryObservationVisualizationChecks VerifiesKinematicChartsKeepKnotAndIntervalSemantics(); VerifiesHandoffUsesSharedReferenceSAndSurvivesProjectionFailure(); VerifiesDynamicSnapshotContainsObservationEvidenceWithoutCyclePoints(); + VerifiesFullModeSnapshotPublishesSingleCurrentTrajectoryAndBoundaryMarkers(); VerifiesWebPublisherFusesFaultWithoutStoppingObserverTicks(); VerifiesWebPublisherGatesAtConfiguredCadenceAndSkipsDisabledOutput(); } @@ -94,6 +95,17 @@ internal static class TrajectoryObservationVisualizationChecks break; } Verification.True(verifiedOccupiedCell, "static fixture has an occupied cell"); + + EmPlannerConfiguration defaults = EmPlannerConfiguration.CreateDefault(); + TrajectoryObservationBootstrapResult gearBootstrap = TrajectoryObservationBootstrapResult.Success( + bootstrap.Job, bootstrap.CoarseResult, bootstrap.SmoothedPath, + new[] { CreateGearSwitchSegment(0, 0d) }); + PlanningVisualizationStaticSnapshot gearStatic = new TrajectoryObservationStaticSnapshotBuilder() + .Build(gearBootstrap, defaults, settings.CreateValidatedSnapshot(), 0L); + Verification.True(gearStatic.StaticMarkers.Any(x => x.Kind == "gear-switch-end"), + "static snapshot marks gear-switch s_end"); + Verification.True(snapshot.StaticMarkers.Any(x => x.Kind == "final-goal"), + "static snapshot marks final goal s_end"); } private static void VerifiesKinematicChartsKeepKnotAndIntervalSemantics() @@ -103,8 +115,16 @@ internal static class TrajectoryObservationVisualizationChecks EmTrajectory trajectory = CreateTrajectory("rolling", DateTimeOffset.UnixEpoch, 21, EmTerminalType.Goal, EmLongitudinalMode.RollingContinuation, TravelDirection.Forward); + var vehicle = new VehicleParameters + { + LengthMeters = 0.8d, + WidthMeters = 0.6d, + SafetyMarginMeters = 0.05d, + MaximumCurvaturePerMeter = 0.4d, + MinimumTurningRadiusMeters = 2.5d, + }; IReadOnlyList charts = new TrajectoryObservationKinematicChartBuilder() - .Build(trajectory, segment, configuration); + .Build(trajectory, segment, vehicle, configuration); Verification.Equal(8, charts.Count, "all eight kinematic charts exported"); Verification.Equal(20, FindChart(charts, "jerk-t").Series[0].Points.Count, "jerk has N-1 intervals"); @@ -118,9 +138,49 @@ internal static class TrajectoryObservationVisualizationChecks id + " has an effective limit line"); Verification.True(!FindChart(charts, "yaw-rate-t").Series.Any(x => x.LineStyle == VisualizationLineStyle.Limit), "yaw rate has no invented limit"); + configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond = 1.25d; + charts = new TrajectoryObservationKinematicChartBuilder().Build(trajectory, segment, vehicle, configuration); + Verification.Equal("ω (rad/s)", FindChart(charts, "yaw-rate-t").YAxisLabel, + "yaw-rate axis uses omega unit"); + Verification.NearlyEqual(0.4d, FindChart(charts, "curvature-s") + .Series.Single(x => x.Id == "curvature-s-positive-limit").Points[0].Y, + "curvature hard limit uses vehicle curvature"); + Verification.True(FindChart(charts, "curvature-s").Series[0].Points.Any(p => Math.Abs(p.Y) > 1e-6), + "curvature-distance uses vehicle curvature data"); + PropertyInfo? annotationsProperty = typeof(VisualizationChart).GetProperty("Annotations"); + Verification.True(annotationsProperty != null, "chart exposes annotation collection"); + if (annotationsProperty != null) + { + object annotationList = annotationsProperty!.GetValue(FindChart(charts, "st"))!; + Verification.True(annotationList is System.Collections.IEnumerable, + "ST annotations are enumerable"); + var annotations = new List(); + foreach (object annotation in (System.Collections.IEnumerable)annotationList!) + { + annotations.Add(annotation); + } + Verification.True(annotations.Count > 0, "ST chart marks s_end"); + Verification.True(annotations.Any(annotation => + (string)annotation.GetType().GetProperty("Kind")!.GetValue(annotation)! == "s-end"), + "ST s_end annotation kind is explicit"); + object lsAnnotationList = annotationsProperty!.GetValue(FindChart(charts, "ls"))!; + var lsAnnotations = new List(); + foreach (object annotation in (System.Collections.IEnumerable)lsAnnotationList!) + { + lsAnnotations.Add(annotation); + } + Verification.True(lsAnnotations.Count > 0, "LS chart marks s_end"); + Verification.True(lsAnnotations.Any(annotation => + (string)annotation.GetType().GetProperty("Kind")!.GetValue(annotation)! == "s-end"), + "LS s_end annotation kind is explicit"); + } + Verification.True(typeof(TrajectoryObservationDynamicSnapshotBuilder).GetMethods().Any(method => + method.Name == "Build" && + method.GetParameters().Any(parameter => parameter.ParameterType == typeof(VehicleParameters))), + "dynamic builder accepts vehicle parameters"); EmTrajectory unprojectable = CreateTrajectory("unprojectable", DateTimeOffset.UnixEpoch, 21, EmTerminalType.Goal, EmLongitudinalMode.RollingContinuation, TravelDirection.Forward, 100d); - Verification.True(FindChart(new TrajectoryObservationKinematicChartBuilder().Build(unprojectable, segment, configuration), "ls") + Verification.True(FindChart(new TrajectoryObservationKinematicChartBuilder().Build(unprojectable, segment, vehicle, configuration), "ls") .NoteChinese.Contains("投影失败=21"), "LS chart reports every failed full-segment projection"); } @@ -166,12 +226,15 @@ internal static class TrajectoryObservationVisualizationChecks EmLongitudinalMode.RollingContinuation, TravelDirection.Forward); PlanningVisualizationDynamicSnapshot snapshot = new TrajectoryObservationDynamicSnapshotBuilder() - .Build(5L, tick, controller.ActiveSegment, previous, configuration); + .Build(5L, tick, controller.ActiveSegment, previous, bootstrap.Vehicle, configuration); Verification.True(snapshot.DynamicPolylines.Any(x => x.Id == "current"), "dynamic snapshot contains current trajectory"); Verification.True(snapshot.DynamicPolylines.Any(x => x.Id == "previous"), "dynamic snapshot contains previous trajectory"); Verification.True(snapshot.DynamicPolylines.Any(x => x.Id == "active-segment"), "dynamic snapshot contains active segment highlight"); - Verification.True(snapshot.DynamicPolylines.Any(x => x.Id == "current-horizon"), "dynamic snapshot contains projected horizon"); + Verification.Equal(1, snapshot.DynamicPolylines.Count(x => x.Id == "current"), + "same current trajectory is published once"); + Verification.True(!snapshot.DynamicPolylines.Any(x => x.Id == "current-horizon"), + "identical current trajectory is not duplicated as current horizon"); foreach (string rawName in new[] { "ActiveSegmentIndex", "ActiveDirection", "PlanningStatus", "LongitudinalMode", "TerminalType", "PlanningElapsedMilliseconds", "TrajectoryAgeSeconds", "LsProjectionFailures", "RollingTerminalConstraint", "TerminalVelocity", "TerminalAcceleration", "DeltaPositionMeters", @@ -197,7 +260,8 @@ internal static class TrajectoryObservationVisualizationChecks TrajectoryObservationLoopTick exactTick = new TrajectoryObservationLoop(exactController) .Tick(now.AddSeconds(settings.ObserverPeriodSeconds), state, CancellationToken.None); PlanningVisualizationDynamicSnapshot exactSnapshot = new TrajectoryObservationDynamicSnapshotBuilder().Build(6L, - exactTick, exactController.ActiveSegment, null, ReadEffectiveConfiguration(exactController)); + exactTick, exactController.ActiveSegment, null, bootstrap.Vehicle, + ReadEffectiveConfiguration(exactController)); Verification.True(exactSnapshot.StatusValues.Single(x => x.RawName == "RollingTerminalConstraint").Value.Contains("精确停车锚点"), "exact-stop status describes the true stop anchor"); Verification.NearlyEqual(exactStop.Points[exactStop.Points.Count - 1].SignedLongitudinalVelocity, @@ -213,11 +277,50 @@ internal static class TrajectoryObservationVisualizationChecks TrajectoryObservationLoopTick offSegmentTick = new TrajectoryObservationLoop(offSegmentController) .Tick(now.AddSeconds(settings.ObserverPeriodSeconds), state, CancellationToken.None); PlanningVisualizationDynamicSnapshot recovered = new TrajectoryObservationDynamicSnapshotBuilder().Build(7L, - offSegmentTick, offSegmentController.ActiveSegment, null, ReadEffectiveConfiguration(offSegmentController)); + offSegmentTick, offSegmentController.ActiveSegment, null, bootstrap.Vehicle, + ReadEffectiveConfiguration(offSegmentController)); Verification.Equal("21", recovered.StatusValues.Single(x => x.RawName == "LsProjectionFailures").Value, "projection failure is reported while dynamic snapshot construction remains available"); } + private static void VerifiesFullModeSnapshotPublishesSingleCurrentTrajectoryAndBoundaryMarkers() + { + var settings = new TrajectoryObservationSettings(); + TrajectoryObservationBootstrapResult goalBootstrap = Bootstrap(settings); + TrajectoryObservationBootstrapResult gearBootstrap = TrajectoryObservationBootstrapResult.Success( + goalBootstrap.Job, goalBootstrap.CoarseResult, goalBootstrap.SmoothedPath, + new[] { CreateGearSwitchSegment(0, 0d) }); + EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault(); + + PlanningVisualizationDynamicSnapshot gearSnapshot = BuildDynamicSnapshot(gearBootstrap, settings, configuration, + CreateTrajectory("full-gear", DateTimeOffset.UnixEpoch.AddSeconds(1d), 21, EmTerminalType.GearSwitch, + EmLongitudinalMode.ExactStopAtBoundary, TravelDirection.Forward, 0d, 0d, + EmPlanningScope.FullDirectionSegment)); + Verification.Equal(1, gearSnapshot.DynamicPolylines.Count(x => x.Kind == "current"), + "full mode publishes one current trajectory"); + Verification.True(!gearSnapshot.DynamicPolylines.Any(x => x.Id == "current-horizon"), + "full mode does not duplicate current horizon"); + Verification.True(gearSnapshot.DynamicMarkers.Any(x => x.Kind == "vehicle"), + "dynamic snapshot marks the vehicle"); + Verification.True(gearSnapshot.DynamicMarkers.Any(x => x.Kind == "plan-start"), + "dynamic snapshot marks the plan start"); + VisualizationMarker gearEnd = gearSnapshot.DynamicMarkers.Single(x => x.Kind == "gear-switch-end"); + Verification.True(gearEnd.LabelChinese.Contains("换向点") && gearEnd.LabelChinese.Contains("s_end"), + "gear switch boundary uses the explicit s_end label"); + + PlanningVisualizationDynamicSnapshot goalSnapshot = BuildDynamicSnapshot(goalBootstrap, settings, configuration, + CreateTrajectory("full-goal", DateTimeOffset.UnixEpoch.AddSeconds(2d), 21, EmTerminalType.Goal, + EmLongitudinalMode.ExactStopAtBoundary, TravelDirection.Forward, 0d, 0d, + EmPlanningScope.FullDirectionSegment)); + Verification.Equal(1, goalSnapshot.DynamicPolylines.Count(x => x.Kind == "current"), + "goal full mode publishes one current trajectory"); + Verification.True(!goalSnapshot.DynamicPolylines.Any(x => x.Id == "current-horizon"), + "goal full mode does not duplicate current horizon"); + VisualizationMarker finalGoal = goalSnapshot.DynamicMarkers.Single(x => x.Kind == "final-goal"); + Verification.True(finalGoal.LabelChinese.Contains("终点") && finalGoal.LabelChinese.Contains("s_end"), + "final goal boundary uses the explicit s_end label"); + } + private static void VerifiesWebPublisherFusesFaultWithoutStoppingObserverTicks() { var settings = new TrajectoryObservationSettings { EnableWebVisualization = true }; @@ -444,6 +547,22 @@ internal static class TrajectoryObservationVisualizationChecks return charts.Single(x => x.Id == id); } + private static PlanningVisualizationDynamicSnapshot BuildDynamicSnapshot( + TrajectoryObservationBootstrapResult bootstrap, TrajectoryObservationSettings settings, + EmPlannerConfiguration configuration, EmTrajectory trajectory) + { + DateTimeOffset now = DateTimeOffset.UnixEpoch.AddMinutes(1); + var controller = new TrajectoryObservationController(bootstrap, settings, + new FixedPlanningService(trajectory), "snapshot-" + trajectory.Metadata.TrajectoryId); + var loop = new TrajectoryObservationLoop(controller); + var state = new VehicleMotionState(new Pose2D(0d, 0d, 0d), 0.1d, null, now, 1L); + controller.StartCycle(now, state, CancellationToken.None).GetAwaiter().GetResult(); + TrajectoryObservationLoopTick tick = loop.Tick(now.AddSeconds(settings.ObserverPeriodSeconds), + state, CancellationToken.None); + return new TrajectoryObservationDynamicSnapshotBuilder().Build( + 1L, tick, controller.ActiveSegment, null, bootstrap.Vehicle, configuration); + } + private static DirectionSegmentView CreateStraightSegment(int index, double sourceStart) { var points = new List @@ -458,8 +577,23 @@ internal static class TrajectoryObservationVisualizationChecks new ReferenceBoundary(index, 10d, EmBoundaryType.Goal, sourceStart + 10d), sourceStart); } + private static DirectionSegmentView CreateGearSwitchSegment(int index, double sourceStart) + { + var points = new List + { + new MultiWheelC.TrajectoryPlanning.PathSmoothing.SmoothedPathPoint(0d, 0d, 0d, 0d, 0d, + TravelDirection.Forward, 0d, 0d, 1d, false, MultiWheelC.TrajectoryPlanning.PathSmoothing.SmoothedPathPointSource.Anchor), + new MultiWheelC.TrajectoryPlanning.PathSmoothing.SmoothedPathPoint(10d, 0d, 0d, 0d, 10d, + TravelDirection.Forward, 0d, 0d, 1d, false, MultiWheelC.TrajectoryPlanning.PathSmoothing.SmoothedPathPointSource.Anchor), + }; + return new DirectionSegmentView(index, TravelDirection.Forward, points, + new ReferenceBoundary(index, 0d, EmBoundaryType.None, sourceStart), + new ReferenceBoundary(index, 10d, EmBoundaryType.GearSwitchApproach, sourceStart + 10d), sourceStart); + } + private static EmTrajectory CreateTrajectory(string id, DateTimeOffset effectiveAt, int count, EmTerminalType terminal, - EmLongitudinalMode mode, TravelDirection direction, double startX = 0d, double startPathS = 1000d) + EmLongitudinalMode mode, TravelDirection direction, double startX = 0d, double startPathS = 1000d, + EmPlanningScope planningScope = EmPlanningScope.RollingHorizon) { var points = new List(); for (int index = 0; index < count; index++) @@ -470,7 +604,7 @@ internal static class TrajectoryObservationVisualizationChecks 0.2d + t, 0.3d + t)); } return new EmTrajectory(new EmTrajectoryMetadata(id, effectiveAt, effectiveAt, 1L, "visualization-reference", 1L, - string.Empty, 0, direction, terminal, mode, EmPlanningScope.RollingHorizon), points); + string.Empty, 0, direction, terminal, mode, planningScope), points); } private sealed class FixedPlanningService : IEmPlanningService diff --git a/ClumsyPilot/tests/TrajectoryPlanningVisualizationVerificationHost/ContractChecks.cs b/ClumsyPilot/tests/TrajectoryPlanningVisualizationVerificationHost/ContractChecks.cs index c3a63b7..6155040 100644 --- a/ClumsyPilot/tests/TrajectoryPlanningVisualizationVerificationHost/ContractChecks.cs +++ b/ClumsyPilot/tests/TrajectoryPlanningVisualizationVerificationHost/ContractChecks.cs @@ -13,6 +13,7 @@ internal static class ContractChecks CopiesCollectionsAndRejectsNonFiniteNumbers(); StoresOccupancyAsDefensiveCompactBits(); ExposesImmutableSnapshotContracts(); + ExposesChartAnnotationContract(); DocumentsPublicSafetyAndLifecycleBoundary(); } @@ -87,6 +88,36 @@ internal static class ContractChecks } } + private static void ExposesChartAnnotationContract() + { + Type chartType = typeof(VisualizationChart); + PropertyInfo annotationsProperty = chartType.GetProperty("Annotations"); + Verification.True(annotationsProperty != null, "chart exposes immutable annotations"); + Type annotationType = chartType.Assembly.GetType("TrajectoryPlanningVisualization.VisualizationChartAnnotation"); + Verification.True(annotationType != null, "chart annotation contract type exists"); + + var chart = new VisualizationChart("annotation-legacy", "标题", "x (m)", "y (m)", + new[] + { + new VisualizationSeries("s", "轨迹", VisualizationLineStyle.Solid, + new[] { new VisualizationPoint(0d, 0d) }) + }); + if (annotationsProperty == null) + { + return; + } + + object value = annotationsProperty.GetValue(chart); + Verification.True(value is System.Collections.IEnumerable, "chart annotations are enumerable"); + int count = 0; + foreach (object annotation in (System.Collections.IEnumerable)value) + { + count++; + } + Verification.Equal(0, count, "legacy chart constructor keeps annotations empty"); + AssertNoSetters(chartType); + } + private static void DocumentsPublicSafetyAndLifecycleBoundary() { string readmePath = Path.GetFullPath(Path.Combine(AppContext.BaseDirectory, diff --git a/ClumsyPilot/tests/TrajectoryPlanningVisualizationVerificationHost/SampleSnapshotFactory.cs b/ClumsyPilot/tests/TrajectoryPlanningVisualizationVerificationHost/SampleSnapshotFactory.cs index a59155b..4f9c3f7 100644 --- a/ClumsyPilot/tests/TrajectoryPlanningVisualizationVerificationHost/SampleSnapshotFactory.cs +++ b/ClumsyPilot/tests/TrajectoryPlanningVisualizationVerificationHost/SampleSnapshotFactory.cs @@ -20,7 +20,12 @@ internal static class SampleSnapshotFactory new VisualizationPolyline("global", "全局参考路径", "global", VisualizationLineStyle.Solid, Points((0d, 1d), (3d, 1d), (6d, 2d), (8d, 4d), (11d, 6d))) }, - new[] { new VisualizationMarker("gear-1", "gear-switch", "换向 1", new VisualizationPoint(6d, 2d)) }, + new[] + { + new VisualizationMarker("gear-1", "gear-switch", "换向 1", new VisualizationPoint(6d, 2d)), + new VisualizationMarker("gear-1-end", "gear-switch-end", "换向点 1 / s_end", new VisualizationPoint(6d, 2d)), + new VisualizationMarker("final-goal", "final-goal", "终点 / s_end", new VisualizationPoint(11d, 6d)) + }, new[] { new VisualizationDirectionSegment(0, "forward", false, true, Points((0d, 1d), (3d, 1d), (6d, 2d))), @@ -55,7 +60,12 @@ internal static class SampleSnapshotFactory new VisualizationPolyline("previous", "上一轮轨迹", "previous", VisualizationLineStyle.Dashed, Points((1d, 1d), (2.5d, 1d), (4d, 1.3d))), new VisualizationPolyline("current", "当前轨迹", "current", VisualizationLineStyle.Solid, Points((2d + offset, 1d), (3.8d + offset, 1.2d), (5.8d + offset, 1.9d))) }, - new[] { new VisualizationMarker("handoff", "handoff", "交接", new VisualizationPoint(2d + offset, 1d)) }, + new[] + { + new VisualizationMarker("vehicle", "vehicle", "车辆", new VisualizationPoint(2d + offset, 1d)), + new VisualizationMarker("plan-start", "plan-start", "计划起点", new VisualizationPoint(2d + offset, 1d)), + new VisualizationMarker("gear-switch-end", "gear-switch-end", "换向点 1 / s_end", new VisualizationPoint(6d, 2d)) + }, CreateCharts(offset), new[] { @@ -70,21 +80,26 @@ internal static class SampleSnapshotFactory { return new[] { - Chart("ls", "横向偏移", "ReferenceS (m)", "l (m)", Points((0d, .05d), (2d, .08d), (4d, .02d))), - Chart("st", "时空轨迹", "t (s)", "PathS (m)", Points((0d, 0d), (1d, 1.2d), (2d, 3.0d))), + Chart("ls", "横向偏移", "ReferenceS (m)", "l (m)", Points((0d, .05d), (2d, .08d), (4d, .02d)), + new[] { new VisualizationChartAnnotation("ls-s-end", "s-end", "s_end", 4d) }), + Chart("st", "时空轨迹", "t (s)", "PathS (m)", Points((0d, 0d), (1d, 1.2d), (2d, 3.0d)), + new[] { new VisualizationChartAnnotation("st-s-end", "s-end", "s_end", 2d) }), Chart("curvature-s", "曲率—距离", "s (m)", "κ (m⁻¹)", Points((0d, .01d), (2d, .04d), (4d, .02d))), Chart("curvature-t", "曲率—时间", "t (s)", "κ (m⁻¹)", Points((0d, .01d), (1d, .04d), (2d, .02d))), Chart("velocity-t", "速度", "t (s)", "v (m/s)", Points((0d, 0d), (1d, 1d), (2d, 1.2d + offset))), Chart("acceleration-t", "加速度", "t (s)", "a (m/s²)", Points((0d, .8d), (1d, .2d), (2d, 0d))), Chart("jerk-t", "加加速度", "t (s)", "j (m/s³)", Points((0d, -.6d), (1d, -.2d))), - Chart("yaw-rate-t", "横摆角速度", "t (s)", "yaw rate (rad/s)", Points((0d, 0d), (1d, .08d), (2d, .04d))) + Chart("yaw-rate-t", "横摆角速度", "t (s)", "ω (rad/s)", Points((0d, 0d), (1d, .08d), (2d, .04d))) }; } - private static VisualizationChart Chart(string id, string title, string xAxis, string yAxis, IReadOnlyList points) + private static VisualizationChart Chart(string id, string title, string xAxis, string yAxis, + IReadOnlyList points, + IReadOnlyList annotations = null) { return new VisualizationChart(id, title, xAxis, yAxis, - new[] { new VisualizationSeries(id + "-current", "当前轨迹", VisualizationLineStyle.Solid, points) }); + new[] { new VisualizationSeries(id + "-current", "当前轨迹", VisualizationLineStyle.Solid, points) }, + "", annotations ?? Array.Empty()); } private static IReadOnlyList Points(params (double x, double y)[] values)