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

fix: publish distinct EM observation semantics

Browse Source
This commit is contained in:
梁薄云
2026-08-07 09:28:55 +08:00
parent 5d1c875584
commit d33257cc46
8 changed files with 313 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationVisualizationChecks.cs
+142 -8
View File
@@ -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<VisualizationChart> 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<object>();
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<object>();
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<MultiWheelC.TrajectoryPlanning.PathSmoothing.SmoothedPathPoint>
@@ -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<MultiWheelC.TrajectoryPlanning.PathSmoothing.SmoothedPathPoint>
{
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<EmTrajectoryPoint>();
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