844 lines
48 KiB
C#
844 lines
48 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.IO;
|
|
using System.Text;
|
|
using System.Threading;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
|
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
using MultiWheelC.TrajectoryPlanning.Mapping;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;
|
|
using MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
|
|
|
namespace EMPlannerVerificationHost;
|
|
|
|
internal static class TrajectoryObservationChecks
|
|
{
|
|
public static void Run()
|
|
{
|
|
VerifiesObservationSourceHasNoActuatorCalls();
|
|
VerifiesObservationSourceUsesRequiredOperatorText();
|
|
VerifiesOperatorDocumentationUsesExactUiEntry();
|
|
VerifiesMovementTestVehicleInputsAndSettingsSnapshot();
|
|
VerifiesPlanningConfigurationDiagnosticUsesEffectiveConfiguration();
|
|
RejectsInvalidObservationSettings();
|
|
VerifiesStartGoalBoundsUseOnlyConfiguredPadding();
|
|
VerifiesValidNonEmptyObstacleSource();
|
|
RejectsObstacleOutsideConfiguredBounds();
|
|
VerifiesObserverTicksWhilePlanningIsDelayed();
|
|
VerifiesSessionLayerCleanupDecisions();
|
|
VerifiesGearSwitchWaitStateForWorldPresentation();
|
|
VerifiesLsAndStUsePublishedTrajectoryData();
|
|
VerifiesPresentationTextDescribesObservationWithoutSendingCommand();
|
|
VerifiesPlanningDiagnosticsKeepRawFailureReason();
|
|
VerifiesPublishedPlanningDiagnosticsIncludeTrajectorySummary();
|
|
VerifiesEmptyChartsReceivePersistentPlanningDiagnostic();
|
|
VerifiesLsPresentationUsesPathSOnHorizontalAxis();
|
|
VerifiesRollingRequestUsesOnePublishedTrajectorySnapshot();
|
|
FreezesBootstrapVehicleForRollingRequests();
|
|
VerifiesEmptyMapBootstrapProducesPublishableReference();
|
|
}
|
|
|
|
private static void VerifiesObservationSourceHasNoActuatorCalls()
|
|
{
|
|
string sourceDirectory = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot",
|
|
"ParkrobTrajplanner", "tarjplanner_movementtest");
|
|
string movementTestSource = Path.Combine(sourceDirectory, "MovementTest.TrajectoryObservationTest.cs");
|
|
Verification.True(File.Exists(movementTestSource), "observation MovementTest source exists");
|
|
|
|
string[] forbiddenTokens =
|
|
{
|
|
".SendXYThSpeed(", ".SendMotion(", ".SendTh(", ".AccumulateSpeed(",
|
|
".ComputeWheelsGeometrically(", ".DriveStop(", ".PredefinedDriveStop("
|
|
};
|
|
string[] runtimeSources = Directory.GetFiles(sourceDirectory, "*.cs", SearchOption.TopDirectoryOnly);
|
|
for (int sourceIndex = 0; sourceIndex < runtimeSources.Length; sourceIndex++)
|
|
{
|
|
string source = File.ReadAllText(runtimeSources[sourceIndex]);
|
|
for (int tokenIndex = 0; tokenIndex < forbiddenTokens.Length; tokenIndex++)
|
|
{
|
|
Verification.True(source.IndexOf(forbiddenTokens[tokenIndex], StringComparison.Ordinal) < 0,
|
|
"observation runtime source excludes actuator token " + forbiddenTokens[tokenIndex]);
|
|
}
|
|
}
|
|
}
|
|
|
|
private static void VerifiesObservationSourceUsesRequiredOperatorText()
|
|
{
|
|
string movementTestSource = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot",
|
|
"ParkrobTrajplanner", "tarjplanner_movementtest", "MovementTest.TrajectoryObservationTest.cs");
|
|
string source = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(movementTestSource));
|
|
|
|
Verification.True(source.Contains("[MovementTest(name = \"EM轨迹规划观察闭环测试\")]"),
|
|
"observation MovementTest uses required Chinese display name");
|
|
Verification.Equal("等待真实档位/方向确认；观察模式不会推进下一方向段",
|
|
TrajectoryObservationRuntimeState.GearSwitchWaitingNotice,
|
|
"observation runtime uses required gear-switch notice");
|
|
Verification.True(source.Contains("Console.WriteLine(\"[TrajectoryObserver] \" + text);"),
|
|
"observer status is mirrored to the host terminal");
|
|
}
|
|
|
|
private static void VerifiesOperatorDocumentationUsesExactUiEntry()
|
|
{
|
|
string sourceDirectory = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot",
|
|
"ParkrobTrajplanner", "tarjplanner_movementtest");
|
|
string movementTestSource = Path.Combine(sourceDirectory, "MovementTest.TrajectoryObservationTest.cs");
|
|
string readmePath = Path.Combine(sourceDirectory, "README.md");
|
|
string movementTestText = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(movementTestSource));
|
|
string readmeText = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(readmePath));
|
|
const string uiEntry = "EM轨迹规划观察闭环测试";
|
|
|
|
Verification.True(movementTestText.Contains("[MovementTest(name = \"" + uiEntry + "\")]"),
|
|
"observation MovementTest has the exact UI entry");
|
|
Verification.True(readmeText.Contains(uiEntry),
|
|
"observation README has the exact UI entry");
|
|
}
|
|
|
|
private static void VerifiesMovementTestVehicleInputsAndSettingsSnapshot()
|
|
{
|
|
string movementTestSource = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot",
|
|
"ParkrobTrajplanner", "tarjplanner_movementtest", "MovementTest.TrajectoryObservationTest.cs");
|
|
string source = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(movementTestSource));
|
|
Verification.True(source.Contains("public double VehicleLengthMeters = 0.80d;"),
|
|
"observer MovementTest exposes vehicle length with Task-1 default");
|
|
Verification.True(source.Contains("public double VehicleWidthMeters = 0.60d;"),
|
|
"observer MovementTest exposes vehicle width with Task-1 default");
|
|
Verification.True(source.Contains("public double SafetyMarginMeters = 0.05d;"),
|
|
"observer MovementTest exposes safety margin with Task-1 default");
|
|
Verification.True(source.Contains("public double MaximumCurvaturePerMeter = 1d / 1.20d;"),
|
|
"observer MovementTest exposes maximum curvature with Task-1 default");
|
|
Verification.True(source.Contains("public double SolverTimeoutSeconds ="),
|
|
"observer MovementTest exposes a configurable solver timeout");
|
|
Verification.True(source.Contains("public int MaximumOsqpIterations ="),
|
|
"observer MovementTest exposes a configurable OSQP iteration limit");
|
|
Verification.True(source.Contains("public double TimeHorizonSeconds ="),
|
|
"observer MovementTest exposes a configurable ST time horizon");
|
|
Verification.True(source.Contains("public double OutputTimeStepSeconds ="),
|
|
"observer MovementTest exposes a configurable ST timestamp spacing");
|
|
Verification.True(source.Contains("不等于观察循环周期"),
|
|
"observer MovementTest documents the distinction between output timestamps and observer ticks");
|
|
Verification.True(source.Contains("VehicleLengthMeters = VehicleLengthMeters,"),
|
|
"observer MovementTest copies vehicle length into settings");
|
|
Verification.True(source.Contains("VehicleWidthMeters = VehicleWidthMeters,"),
|
|
"observer MovementTest copies vehicle width into settings");
|
|
Verification.True(source.Contains("SafetyMarginMeters = SafetyMarginMeters,"),
|
|
"observer MovementTest copies safety margin into settings");
|
|
Verification.True(source.Contains("MaximumCurvaturePerMeter = MaximumCurvaturePerMeter,"),
|
|
"observer MovementTest copies maximum curvature into settings");
|
|
Verification.True(source.Contains("SolverTimeoutSeconds = SolverTimeoutSeconds,"),
|
|
"observer MovementTest copies solver timeout into settings");
|
|
Verification.True(source.Contains("MaximumOsqpIterations = MaximumOsqpIterations,"),
|
|
"observer MovementTest copies OSQP iteration limit into settings");
|
|
Verification.True(source.Contains("TimeHorizonSeconds = TimeHorizonSeconds,"),
|
|
"observer MovementTest copies ST time horizon into settings");
|
|
Verification.True(source.Contains("OutputTimeStepSeconds = OutputTimeStepSeconds,"),
|
|
"observer MovementTest copies ST timestamp spacing into settings");
|
|
string normalizedSource = source.Replace("\r\n", "\n");
|
|
Verification.True(normalizedSource.Contains(
|
|
"VehicleMotionState state = ReadVehicleState();\n DateTimeOffset now = state.CapturedAtUtc;"),
|
|
"observer host uses the fresh state snapshot time for each observation tick");
|
|
Verification.NearlyEqual(0.50d, new TrajectoryObservationSettings().SolverTimeoutSeconds,
|
|
"observer settings use the test solver timeout default");
|
|
Verification.Equal(12000, new TrajectoryObservationSettings().MaximumOsqpIterations,
|
|
"observer settings use the test OSQP iteration default");
|
|
Verification.NearlyEqual(6d, new TrajectoryObservationSettings().TimeHorizonSeconds,
|
|
"observer settings use the ST time-horizon default");
|
|
Verification.NearlyEqual(0.10d, new TrajectoryObservationSettings().OutputTimeStepSeconds,
|
|
"observer settings use the ST timestamp-spacing default");
|
|
|
|
var configured = new TrajectoryObservationSettings
|
|
{
|
|
VehicleLengthMeters = 1.10d,
|
|
VehicleWidthMeters = 0.70d,
|
|
SafetyMarginMeters = 0.08d,
|
|
MaximumCurvaturePerMeter = 0.55d,
|
|
SolverTimeoutSeconds = 0.42d,
|
|
MaximumOsqpIterations = 9000,
|
|
TimeHorizonSeconds = 4d,
|
|
OutputTimeStepSeconds = 0.20d,
|
|
};
|
|
TrajectoryObservationSettings snapshot = configured.CreateValidatedSnapshot();
|
|
configured.VehicleLengthMeters = 9.10d;
|
|
configured.VehicleWidthMeters = 9.20d;
|
|
configured.SafetyMarginMeters = 9.30d;
|
|
configured.MaximumCurvaturePerMeter = 9.40d;
|
|
configured.SolverTimeoutSeconds = 9.50d;
|
|
configured.MaximumOsqpIterations = 9500;
|
|
configured.TimeHorizonSeconds = 5d;
|
|
configured.OutputTimeStepSeconds = 0.25d;
|
|
|
|
Verification.NearlyEqual(1.10d, snapshot.VehicleLengthMeters,
|
|
"observer settings snapshot freezes vehicle length");
|
|
Verification.NearlyEqual(0.70d, snapshot.VehicleWidthMeters,
|
|
"observer settings snapshot freezes vehicle width");
|
|
Verification.NearlyEqual(0.08d, snapshot.SafetyMarginMeters,
|
|
"observer settings snapshot freezes safety margin");
|
|
Verification.NearlyEqual(0.55d, snapshot.MaximumCurvaturePerMeter,
|
|
"observer settings snapshot freezes maximum curvature");
|
|
Verification.NearlyEqual(0.42d, snapshot.SolverTimeoutSeconds,
|
|
"observer settings snapshot freezes solver timeout");
|
|
Verification.Equal(9000, snapshot.MaximumOsqpIterations,
|
|
"observer settings snapshot freezes OSQP iteration limit");
|
|
Verification.NearlyEqual(4d, snapshot.TimeHorizonSeconds,
|
|
"observer settings snapshot freezes ST time horizon");
|
|
Verification.NearlyEqual(0.20d, snapshot.OutputTimeStepSeconds,
|
|
"observer settings snapshot freezes ST timestamp spacing");
|
|
|
|
CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
|
|
new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), snapshot,
|
|
Array.Empty<TrajectoryObservationObstacle>(), 0L);
|
|
AssertVehicleSnapshot(job.Vehicle, 1.10d, 0.70d, 0.08d, 0.55d, null,
|
|
"observer configured bootstrap vehicle");
|
|
}
|
|
|
|
private static void VerifiesPlanningConfigurationDiagnosticUsesEffectiveConfiguration()
|
|
{
|
|
DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 5, 0, 0, 0, TimeSpan.Zero);
|
|
var settings = new TrajectoryObservationSettings
|
|
{
|
|
ReplanPeriodSeconds = 0.25d,
|
|
SolverTimeoutSeconds = 1.25d,
|
|
MaximumOsqpIterations = 54321,
|
|
TimeHorizonSeconds = 3.5d,
|
|
OutputTimeStepSeconds = 0.10d,
|
|
};
|
|
CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
|
|
new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings,
|
|
Array.Empty<TrajectoryObservationObstacle>(), 0L);
|
|
TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper()
|
|
.Bootstrap(job, CancellationToken.None);
|
|
Verification.True(bootstrap.Succeeded, "observer configuration-diagnostic bootstrap succeeds");
|
|
var controller = new TrajectoryObservationController(bootstrap, settings,
|
|
new FixedTrajectoryPlanningService(CreatePublishedTrajectory(effectiveAt)), "config-diagnostic");
|
|
|
|
string text = controller.CreateConfigurationDiagnostic().Text;
|
|
foreach (string expected in new[]
|
|
{
|
|
"planning configuration:", "timeHorizon=3.50s", "distanceHorizon=5.00m", "outputTimeStep=0.10s",
|
|
"outputFrequency=10.00Hz", "trajectoryKnots=36", "maximumOsqpIterations=54321",
|
|
"solverTimeout=1.25s", "replanPeriod=0.25s",
|
|
})
|
|
{
|
|
Verification.True(text.Contains(expected), "configuration diagnostic includes " + expected);
|
|
}
|
|
|
|
string movementTestSource = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot",
|
|
"ParkrobTrajplanner", "tarjplanner_movementtest", "MovementTest.TrajectoryObservationTest.cs");
|
|
string runnerSource = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(movementTestSource));
|
|
int firstCall = runnerSource.IndexOf("CreateConfigurationDiagnostic()", StringComparison.Ordinal);
|
|
Verification.True(firstCall >= 0, "runner prints configuration diagnostic");
|
|
Verification.Equal(-1, runnerSource.IndexOf("CreateConfigurationDiagnostic()", firstCall + 1,
|
|
StringComparison.Ordinal), "runner prints configuration diagnostic once");
|
|
}
|
|
|
|
private static void RejectsInvalidObservationSettings()
|
|
{
|
|
AssertInvalidSetting(settings => settings.MapPaddingMeters = 0d, "map padding");
|
|
AssertInvalidSetting(settings => settings.MapResolutionMillimeters = float.NaN, "map resolution");
|
|
AssertInvalidSetting(settings => settings.ReplanPeriodSeconds = 0d, "replan period");
|
|
AssertInvalidSetting(settings => settings.ObserverPeriodSeconds = double.PositiveInfinity,
|
|
"observer period");
|
|
AssertInvalidSetting(settings => settings.VehicleLengthMeters = 0d, "vehicle length");
|
|
AssertInvalidSetting(settings => settings.VehicleWidthMeters = double.NaN, "vehicle width");
|
|
AssertInvalidSetting(settings => settings.SafetyMarginMeters = 0d, "safety margin");
|
|
AssertInvalidSetting(settings => settings.MaximumCurvaturePerMeter = double.PositiveInfinity,
|
|
"maximum curvature");
|
|
AssertInvalidSetting(settings => settings.SolverTimeoutSeconds = 0d, "solver timeout");
|
|
AssertInvalidSetting(settings => settings.MaximumOsqpIterations = 0, "OSQP iteration limit");
|
|
AssertInvalidSetting(settings => settings.MaximumOsqpIterations = -1, "negative OSQP iteration limit");
|
|
AssertInvalidSetting(settings => settings.TimeHorizonSeconds = 0d, "ST time horizon");
|
|
AssertInvalidSetting(settings => settings.OutputTimeStepSeconds = 0d, "ST timestamp spacing");
|
|
AssertInvalidSetting(settings =>
|
|
{
|
|
settings.TimeHorizonSeconds = 1d;
|
|
settings.OutputTimeStepSeconds = 1.01d;
|
|
}, "ST timestamp spacing larger than the horizon");
|
|
}
|
|
|
|
private static void AssertInvalidSetting(Action<TrajectoryObservationSettings> mutate, string name)
|
|
{
|
|
var settings = new TrajectoryObservationSettings();
|
|
mutate(settings);
|
|
Verification.True(Throws(settings.Validate), "observer rejects invalid " + name);
|
|
}
|
|
|
|
private static void VerifiesStartGoalBoundsUseOnlyConfiguredPadding()
|
|
{
|
|
var settings = new TrajectoryObservationSettings
|
|
{
|
|
MapPaddingMeters = 2d,
|
|
MapResolutionMillimeters = 50f,
|
|
};
|
|
CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
|
|
new Pose2D(10d, -5d, 0d), new Pose2D(13d, -1d, 0d), settings,
|
|
Array.Empty<TrajectoryObservationObstacle>(), 17L);
|
|
|
|
Verification.NearlyEqual(8000d, job.MapRequest.Bounds.XMin, "observer map x min");
|
|
Verification.NearlyEqual(15000d, job.MapRequest.Bounds.XMax, "observer map x max");
|
|
Verification.NearlyEqual(-7000d, job.MapRequest.Bounds.YMin, "observer map y min");
|
|
Verification.NearlyEqual(1000d, job.MapRequest.Bounds.YMax, "observer map y max");
|
|
Verification.NearlyEqual(50d, job.MapRequest.ResolutionMm, "observer map resolution");
|
|
}
|
|
|
|
private static void VerifiesValidNonEmptyObstacleSource()
|
|
{
|
|
CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
|
|
new Pose2D(0d, 0d, 0d), new Pose2D(1d, 1d, 0d), new TrajectoryObservationSettings(),
|
|
new[] { TrajectoryObservationObstacle.Circle(500d, 500d, 100d) }, 23L);
|
|
|
|
Verification.True(!job.MapRequest.AllowExplicitEmptyMap,
|
|
"observer non-empty obstacle map is not explicitly empty");
|
|
Verification.Equal(1, job.MapRequest.ObstacleSources.Count,
|
|
"observer non-empty map has exactly one obstacle source");
|
|
IMapObstacleSource source = job.MapRequest.ObstacleSources[0];
|
|
Verification.Equal("trajectory-observer-manual", source.SourceId,
|
|
"observer manual obstacle source ID");
|
|
Verification.Equal(23L, source.SourceVersion,
|
|
"observer manual obstacle source version");
|
|
Verification.True(source.IsRequired, "observer manual obstacle source is required");
|
|
ObstacleProjectionResult projection = source.ProjectToWorld();
|
|
Verification.Equal(ObstacleSourceStatus.Applied, projection.Status,
|
|
"observer valid manual obstacle source applies");
|
|
Verification.Equal(1, projection.Obstacles.Count,
|
|
"observer valid manual obstacle source preserves geometry");
|
|
}
|
|
|
|
private static void RejectsObstacleOutsideConfiguredBounds()
|
|
{
|
|
Verification.True(Throws(() => TrajectoryObservationSetupFactory.CreateBootstrapJob(
|
|
new Pose2D(0d, 0d, 0d), new Pose2D(1d, 1d, 0d), new TrajectoryObservationSettings(),
|
|
new[] { TrajectoryObservationObstacle.Rectangle(-2100d, -2000d, 0d, 100d) }, 1L)),
|
|
"observer obstacle outside configured bounds");
|
|
}
|
|
|
|
private static void VerifiesObserverTicksWhilePlanningIsDelayed()
|
|
{
|
|
DateTimeOffset startedAt = new DateTimeOffset(2026, 8, 4, 3, 0, 0, TimeSpan.Zero);
|
|
var settings = new TrajectoryObservationSettings();
|
|
CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
|
|
new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings,
|
|
Array.Empty<TrajectoryObservationObstacle>(), 0L);
|
|
TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper()
|
|
.Bootstrap(job, CancellationToken.None);
|
|
Verification.True(bootstrap.Succeeded, "observer delayed-planner bootstrap succeeds");
|
|
|
|
EmTrajectory published = CreatePublishedTrajectory(startedAt);
|
|
var planningService = new DelayedTrajectoryPlanningService(published, 2);
|
|
try
|
|
{
|
|
var controller = new TrajectoryObservationController(
|
|
bootstrap, settings, planningService, "cadence-check");
|
|
var loop = new TrajectoryObservationLoop(controller);
|
|
var firstState = new VehicleMotionState(
|
|
new Pose2D(0.10d, 0d, 0d), 0.10d, null, startedAt, 10L);
|
|
|
|
TrajectoryObservationLoopTick firstTick = loop.Tick(startedAt, firstState, CancellationToken.None);
|
|
|
|
Verification.True(firstTick.PlanningStarted, "observer first tick reports a planning-cycle start");
|
|
Verification.True(!firstTick.PlanningCompleted, "observer first tick has no completed cycle");
|
|
Verification.True(firstTick.ShouldLog,
|
|
"observer first cadence tick is eligible for session-guarded logging");
|
|
Verification.Equal(10L, firstTick.Observation.VehicleState.SequenceId,
|
|
"observer first tick uses first fresh state");
|
|
|
|
TrajectoryObservationLoopTick? initialPublicationTick = null;
|
|
bool initiallyPublished = SpinWait.SpinUntil(() =>
|
|
{
|
|
var currentState = new VehicleMotionState(
|
|
new Pose2D(0.15d, 0d, 0d), 0.15d, null,
|
|
startedAt.AddSeconds(0.10d), 11L);
|
|
initialPublicationTick = loop.Tick(startedAt.AddSeconds(0.10d), currentState,
|
|
CancellationToken.None);
|
|
return !initialPublicationTick.PlanningInFlight;
|
|
}, TimeSpan.FromSeconds(5d));
|
|
Verification.True(initiallyPublished && initialPublicationTick != null &&
|
|
ReferenceEquals(published, initialPublicationTick.Observation.PublishedTrajectory),
|
|
"observer establishes a published trajectory before delayed rolling planning");
|
|
|
|
DateTimeOffset replanAt = startedAt.AddSeconds(settings.ReplanPeriodSeconds);
|
|
var replanState = new VehicleMotionState(
|
|
new Pose2D(0.20d, 0d, 0d), 0.20d, null, replanAt, 12L);
|
|
TrajectoryObservationLoopTick replanTick = loop.Tick(replanAt, replanState,
|
|
CancellationToken.None);
|
|
Verification.True(replanTick.PlanningInFlight,
|
|
"observer delayed rolling planning remains in flight after replan tick");
|
|
Verification.True(planningService.WaitUntilEntered(TimeSpan.FromSeconds(5d)),
|
|
"observer delayed planner enters planning service");
|
|
|
|
DateTimeOffset secondAt = replanAt.AddSeconds(settings.ObserverPeriodSeconds);
|
|
var secondState = new VehicleMotionState(
|
|
new Pose2D(0.25d, 0d, 0d), 0.25d, null, secondAt, 13L);
|
|
TrajectoryObservationLoopTick secondTick = loop.Tick(secondAt, secondState, CancellationToken.None);
|
|
|
|
Verification.True(secondTick.PlanningInFlight,
|
|
"observer second tick does not wait for delayed planning");
|
|
Verification.True(secondTick.ShouldLog,
|
|
"observer second cadence tick is eligible for session-guarded logging");
|
|
Verification.True(ReferenceEquals(published, secondTick.Observation.PublishedTrajectory),
|
|
"observer keeps observing the existing publication during delayed rolling planning");
|
|
Verification.Equal(13L, secondTick.Observation.VehicleState.SequenceId,
|
|
"observer second tick uses second fresh state");
|
|
Verification.Equal(secondAt, secondTick.Observation.ObservedAtUtc,
|
|
"observer second tick observes at its own time");
|
|
|
|
planningService.Release();
|
|
TrajectoryObservationLoopTick? completedTick = null;
|
|
bool completed = SpinWait.SpinUntil(() =>
|
|
{
|
|
var currentState = new VehicleMotionState(
|
|
new Pose2D(0.30d, 0d, 0d), 0.30d, null,
|
|
replanAt.AddSeconds(0.10d), 14L);
|
|
completedTick = loop.Tick(replanAt.AddSeconds(0.10d), currentState,
|
|
CancellationToken.None);
|
|
return !completedTick.PlanningInFlight;
|
|
}, TimeSpan.FromSeconds(5d));
|
|
|
|
Verification.True(completed, "observer delayed planning completes deterministically");
|
|
Verification.True(completedTick != null, "observer delayed planning produces a completion tick");
|
|
TrajectoryObservationLoopTick finalTick = completedTick!;
|
|
Verification.True(finalTick.PlanningCompleted, "observer completion tick reports cycle completion");
|
|
Verification.True(finalTick.LatestCycle != null && finalTick.LatestCycle.Published,
|
|
"observer delayed planning result is consumed without a continuation");
|
|
Verification.Equal(14L, finalTick.Observation.VehicleState.SequenceId,
|
|
"observer post-plan observation does not reuse pre-plan state");
|
|
Verification.Equal(replanAt.AddSeconds(0.10d), finalTick.Observation.ObservedAtUtc,
|
|
"observer post-plan observation does not reuse pre-plan time");
|
|
}
|
|
finally
|
|
{
|
|
planningService.Release();
|
|
}
|
|
}
|
|
|
|
private static void VerifiesSessionLayerCleanupDecisions()
|
|
{
|
|
Verification.True(!TrajectoryObservationSessionLifecycle.ShouldClearLayers(
|
|
TrajectoryObservationSessionEndReason.BootstrapFailure),
|
|
"observer bootstrap failure preserves the diagnostic world view");
|
|
Verification.True(TrajectoryObservationSessionLifecycle.ShouldClearLayers(
|
|
TrajectoryObservationSessionEndReason.RuntimeFault),
|
|
"observer runtime fault clears all painter layers");
|
|
Verification.True(TrajectoryObservationSessionLifecycle.ShouldClearLayers(
|
|
TrajectoryObservationSessionEndReason.Cancellation),
|
|
"observer cancellation clears all painter layers");
|
|
}
|
|
|
|
private static void VerifiesGearSwitchWaitStateForWorldPresentation()
|
|
{
|
|
DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 4, 0, 0, TimeSpan.Zero);
|
|
EmTrajectory trajectory = CreatePublishedTrajectory(effectiveAt, EmTerminalType.GearSwitch);
|
|
|
|
TrajectoryObservationRuntimeState beforeFinal = TrajectoryObservationRuntimeState.Create(
|
|
effectiveAt.AddSeconds(0.99d), trajectory);
|
|
Verification.True(!beforeFinal.WaitingAtGearSwitch,
|
|
"observer does not paint gear-switch wait state before final time");
|
|
Verification.Equal(string.Empty, beforeFinal.WorldNotice,
|
|
"observer has no gear-switch world notice before final time");
|
|
|
|
TrajectoryObservationRuntimeState atFinal = TrajectoryObservationRuntimeState.Create(
|
|
effectiveAt.AddSeconds(1d), trajectory);
|
|
Verification.True(atFinal.WaitingAtGearSwitch,
|
|
"observer enters gear-switch wait state at final time");
|
|
Verification.Equal("等待真实档位/方向确认；观察模式不会推进下一方向段", atFinal.WorldNotice,
|
|
"observer exposes the exact gear-switch state to the world painter");
|
|
Verification.Equal(0, trajectory.Metadata.SegmentIndex,
|
|
"observer gear-switch wait state remains on segment zero");
|
|
|
|
string presentationPath = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot",
|
|
"ParkrobTrajplanner", "tarjplanner_movementtest", "TrajectoryObservationPresentation.cs");
|
|
string presentationSource = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(presentationPath));
|
|
Verification.True(presentationSource.Contains(
|
|
"worldPainter.DrawText(Color.OrangeRed, runtimeState.WorldNotice"),
|
|
"observer world painter draws the exact runtime wait state");
|
|
}
|
|
|
|
private static void VerifiesLsAndStUsePublishedTrajectoryData()
|
|
{
|
|
DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 0, 0, 0, TimeSpan.Zero);
|
|
DirectionSegmentView segment = CreateStraightSegment();
|
|
EmTrajectory trajectory = CreatePublishedTrajectory(effectiveAt);
|
|
|
|
TrajectoryObservationCharts charts = TrajectoryObservationCharts.Build(trajectory, segment, 0.5d);
|
|
Verification.Equal(2, charts.StSamples.Count, "observer ST sample count");
|
|
Verification.NearlyEqual(0d, charts.StSamples[0].TimeFromStart, "observer first ST time");
|
|
Verification.NearlyEqual(4d, charts.StSamples[0].PathS, "observer first ST path S");
|
|
Verification.NearlyEqual(1d, charts.StSamples[1].TimeFromStart, "observer second ST time");
|
|
Verification.NearlyEqual(5d, charts.StSamples[1].PathS, "observer second ST path S");
|
|
Verification.Equal(2, charts.SpeedSamples.Count, "observer speed sample count");
|
|
Verification.NearlyEqual(0.20d, charts.SpeedSamples[0].SignedLongitudinalVelocity,
|
|
"observer first signed speed");
|
|
Verification.NearlyEqual(0.40d, charts.SpeedSamples[1].SignedLongitudinalVelocity,
|
|
"observer second signed speed");
|
|
Verification.Equal(2, charts.LsSamples.Count, "observer LS projection count");
|
|
Verification.Equal(0, charts.FailedProjectionCount, "observer LS projection failure count");
|
|
Verification.NearlyEqual(10.25d, charts.LsSamples[0].PathS, "observer first LS path S");
|
|
Verification.NearlyEqual(0.10d, charts.LsSamples[0].LateralOffset, "observer first LS offset");
|
|
|
|
var settings = new TrajectoryObservationSettings
|
|
{
|
|
MaximumOsqpIterations = 9000,
|
|
TimeHorizonSeconds = 4d,
|
|
OutputTimeStepSeconds = 0.20d,
|
|
};
|
|
CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
|
|
new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings,
|
|
Array.Empty<TrajectoryObservationObstacle>(), 0L);
|
|
TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper(
|
|
new CoarsePathPlanningService(), new PathSmoothingService())
|
|
.Bootstrap(job, CancellationToken.None);
|
|
Verification.True(bootstrap.Succeeded, "observer bootstrap succeeds");
|
|
|
|
var planningService = new FixedTrajectoryPlanningService(trajectory);
|
|
var controller = new TrajectoryObservationController(bootstrap, settings, planningService, "observer-check");
|
|
var state = new VehicleMotionState(new Pose2D(0.25d, 0.10d, 0d), 0.20d, null, effectiveAt, 1L);
|
|
PlanningCycleResult firstCycle = controller.StartCycle(effectiveAt, state, CancellationToken.None)
|
|
.GetAwaiter().GetResult();
|
|
Verification.True(firstCycle.Published, "observer first cycle publishes");
|
|
Verification.Equal(0, planningService.Requests[0].SegmentIndex, "observer starts at segment zero");
|
|
Verification.Equal("observer-check-trajectory-1", planningService.Requests[0].OutputTrajectoryId,
|
|
"observer first trajectory identity");
|
|
Verification.NearlyEqual(settings.ReplanPeriodSeconds,
|
|
planningService.Requests[0].Configuration.Scheduling.ReplanPeriodSeconds,
|
|
"observer configured replan period");
|
|
Verification.NearlyEqual(settings.SolverTimeoutSeconds,
|
|
planningService.Requests[0].Configuration.Scheduling.SolverTimeoutSeconds,
|
|
"observer configured solver timeout");
|
|
Verification.Equal(settings.MaximumOsqpIterations,
|
|
planningService.Requests[0].Configuration.Solver.MaximumOsqpIterations,
|
|
"observer configured OSQP iteration limit");
|
|
Verification.NearlyEqual(settings.TimeHorizonSeconds,
|
|
planningService.Requests[0].Configuration.Scheduling.TimeHorizonSeconds,
|
|
"observer configured ST time horizon");
|
|
Verification.NearlyEqual(settings.OutputTimeStepSeconds,
|
|
planningService.Requests[0].Configuration.Scheduling.OutputTimeStepSeconds,
|
|
"observer configured ST timestamp spacing");
|
|
|
|
TrajectoryObservationObservation observation = controller.Observe(effectiveAt.AddSeconds(0.5d), state);
|
|
Verification.NearlyEqual(0.5d, observation.SelectedPoint.TimeFromStart,
|
|
"observer executor interpolates from published effective time");
|
|
}
|
|
|
|
private static void VerifiesRollingRequestUsesOnePublishedTrajectorySnapshot()
|
|
{
|
|
DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 1, 0, 0, TimeSpan.Zero);
|
|
var settings = new TrajectoryObservationSettings();
|
|
CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
|
|
new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings,
|
|
Array.Empty<TrajectoryObservationObstacle>(), 0L);
|
|
TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper()
|
|
.Bootstrap(job, CancellationToken.None);
|
|
Verification.True(bootstrap.Succeeded, "observer rolling snapshot bootstrap succeeds");
|
|
|
|
EmTrajectory published = CreatePublishedTrajectory(effectiveAt);
|
|
var planningService = new FixedTrajectoryPlanningService(published);
|
|
var controller = new TrajectoryObservationController(bootstrap, settings, planningService, "snapshot-check");
|
|
var state = new VehicleMotionState(new Pose2D(0.25d, 0.10d, 0d), 0.20d, null, effectiveAt, 2L);
|
|
controller.StartCycle(effectiveAt, state, CancellationToken.None).GetAwaiter().GetResult();
|
|
controller.StartCycle(effectiveAt.AddSeconds(settings.ReplanPeriodSeconds), state, CancellationToken.None)
|
|
.GetAwaiter().GetResult();
|
|
|
|
EmPlanningRequest rollingRequest = planningService.Requests[1];
|
|
Verification.True(ReferenceEquals(published, rollingRequest.PreviousTrajectory),
|
|
"observer rolling request uses the controlled published trajectory object");
|
|
Verification.Equal(rollingRequest.PreviousTrajectory.Metadata.TrajectoryId,
|
|
rollingRequest.PreviousTrajectoryId,
|
|
"observer rolling request trajectory object and ID use one publication snapshot");
|
|
}
|
|
|
|
private static void VerifiesPresentationTextDescribesObservationWithoutSendingCommand()
|
|
{
|
|
DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 0, 0, 0, TimeSpan.Zero);
|
|
var settings = new TrajectoryObservationSettings();
|
|
CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
|
|
new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings,
|
|
Array.Empty<TrajectoryObservationObstacle>(), 0L);
|
|
TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper()
|
|
.Bootstrap(job, CancellationToken.None);
|
|
var controller = new TrajectoryObservationController(bootstrap, settings,
|
|
new FixedTrajectoryPlanningService(CreatePublishedTrajectory(effectiveAt)), "presentation-check");
|
|
var state = new VehicleMotionState(new Pose2D(0.25d, 0.10d, 0d), 0.20d, null, effectiveAt, 4L);
|
|
controller.StartCycle(effectiveAt, state, CancellationToken.None).GetAwaiter().GetResult();
|
|
TrajectoryObservationObservation observation = controller.Observe(effectiveAt.AddSeconds(0.5d), state);
|
|
TrajectoryObservationCharts charts = TrajectoryObservationCharts.Build(
|
|
observation.PublishedTrajectory, CreateStraightSegment(), 0.5d);
|
|
|
|
string text = TrajectoryObservationPresentationText.Create(observation, charts);
|
|
|
|
Verification.True(text.Contains("OBSERVE_ONLY: no chassis command is sent."),
|
|
"observer presentation observe-only notice");
|
|
Verification.True(text.Contains("selected t=0.50 s, path-S=4.50 m"),
|
|
"observer presentation selected point time and path-S");
|
|
Verification.True(text.Contains("signed speed=0.30 m/s"),
|
|
"observer presentation signed speed");
|
|
Verification.True(text.Contains("yaw rate=0.00 rad/s"),
|
|
"observer presentation yaw rate");
|
|
Verification.True(text.Contains("LS projection failures=0"),
|
|
"observer presentation LS projection failures");
|
|
}
|
|
|
|
private static void VerifiesPlanningDiagnosticsKeepRawFailureReason()
|
|
{
|
|
var failed = new PlanningCycleResult(
|
|
4L,
|
|
new PlanningCycleIdentity(3L, "diagnostic-reference", 7L, string.Empty, 0),
|
|
new EmPlanningResult(EmPlanningStatus.CorridorInfeasible, null,
|
|
"map=3;reference=diagnostic-reference;state=7;previous=;segment=0;reason=no connected corridor"),
|
|
false,
|
|
"map=3;reference=diagnostic-reference;state=7;previous=;segment=0;reason=no connected corridor");
|
|
|
|
TrajectoryObservationDiagnostic diagnostic = TrajectoryObservationDiagnostics.Create(
|
|
failed, TimeSpan.FromMilliseconds(18d), false, null);
|
|
|
|
Verification.True(diagnostic.Text.Contains("cycle=4"), "diagnostic has cycle version");
|
|
Verification.True(diagnostic.Text.Contains("status=CorridorInfeasible"), "diagnostic preserves raw status");
|
|
Verification.True(diagnostic.Text.Contains("published=False"), "diagnostic preserves publish state");
|
|
Verification.True(diagnostic.Text.Contains("elapsed=18ms"), "diagnostic preserves elapsed time");
|
|
Verification.True(diagnostic.Text.Contains("reason=map=3;reference=diagnostic-reference"),
|
|
"diagnostic preserves planner failure reason");
|
|
Verification.True(!diagnostic.Text.Contains("trajectory summary:"),
|
|
"failed diagnostic has no stale trajectory summary");
|
|
|
|
TrajectoryObservationDiagnostic pending = TrajectoryObservationDiagnostics.Create(
|
|
null, TimeSpan.Zero, true, null);
|
|
Verification.Equal("planning status=pending", pending.Text, "diagnostic reports pending before completion");
|
|
}
|
|
|
|
private static void VerifiesPublishedPlanningDiagnosticsIncludeTrajectorySummary()
|
|
{
|
|
DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 5, 0, 0, 0, TimeSpan.Zero);
|
|
EmTrajectory trajectory = CreatePublishedTrajectory(effectiveAt);
|
|
var succeeded = new PlanningCycleResult(9L,
|
|
new PlanningCycleIdentity(1L, "summary-reference", 2L, string.Empty, 0),
|
|
new EmPlanningResult(EmPlanningStatus.Success, trajectory, string.Empty), true, string.Empty);
|
|
|
|
string text = TrajectoryObservationDiagnostics.Create(
|
|
succeeded, TimeSpan.FromMilliseconds(12d), false, trajectory).Text;
|
|
foreach (string expected in new[]
|
|
{
|
|
"trajectory summary:", "trajectoryId=observer-published", "points=2", "duration=1.000s",
|
|
"pathLength=1.000m", "maxSpeed=0.400m/s", "maxAcceleration=0.200m/s2", "maxJerk=0.000m/s3",
|
|
})
|
|
{
|
|
Verification.True(text.Contains(expected), "published diagnostic includes " + expected);
|
|
}
|
|
}
|
|
|
|
private static void VerifiesEmptyChartsReceivePersistentPlanningDiagnostic()
|
|
{
|
|
string presentationPath = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot",
|
|
"ParkrobTrajplanner", "tarjplanner_movementtest", "TrajectoryObservationPresentation.cs");
|
|
string source = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(presentationPath));
|
|
|
|
Verification.True(source.Contains("DrawLs(TrajectoryObservationCharts charts, string diagnosticText)"),
|
|
"LS painter accepts planning diagnostic input");
|
|
Verification.True(source.Contains("DrawSt(TrajectoryObservationCharts charts, string diagnosticText)"),
|
|
"ST painter accepts planning diagnostic input");
|
|
Verification.True(source.Contains("No published trajectory available for L-S chart.\\n"),
|
|
"LS empty state includes diagnostic after chart label");
|
|
Verification.True(source.Contains("No published trajectory available for T-S/T-V charts.\\n"),
|
|
"ST empty state includes diagnostic after chart label");
|
|
}
|
|
|
|
private static void VerifiesLsPresentationUsesPathSOnHorizontalAxis()
|
|
{
|
|
TrajectoryObservationCharts charts = TrajectoryObservationCharts.Build(
|
|
CreatePublishedTrajectory(new DateTimeOffset(2026, 8, 4, 0, 0, 0, TimeSpan.Zero)),
|
|
CreateStraightSegment(), 0.5d);
|
|
|
|
TrajectoryObservationLsPresentationModel model = TrajectoryObservationLsPresentationModel.Create(charts);
|
|
|
|
Verification.Equal("path-S (m)", model.HorizontalAxisLabel, "observer LS horizontal axis label");
|
|
Verification.Equal("lateral offset (m)", model.VerticalAxisLabel, "observer LS vertical axis label");
|
|
Verification.NearlyEqual(10.25d, model.Samples[0].HorizontalPathS,
|
|
"observer LS path-S is horizontal");
|
|
Verification.NearlyEqual(0.10d, model.Samples[0].VerticalLateralOffset,
|
|
"observer LS lateral offset is vertical");
|
|
}
|
|
|
|
private static void FreezesBootstrapVehicleForRollingRequests()
|
|
{
|
|
DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 2, 0, 0, TimeSpan.Zero);
|
|
var settings = new TrajectoryObservationSettings();
|
|
CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
|
|
new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings,
|
|
Array.Empty<TrajectoryObservationObstacle>(), 0L);
|
|
VehicleParameters originalVehicle = job.Vehicle;
|
|
double expectedLength = originalVehicle.LengthMeters;
|
|
double expectedWidth = originalVehicle.WidthMeters;
|
|
double expectedMargin = originalVehicle.SafetyMarginMeters;
|
|
double? expectedMaximumCurvature = originalVehicle.MaximumCurvaturePerMeter;
|
|
double? expectedMinimumRadius = originalVehicle.MinimumTurningRadiusMeters;
|
|
TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper()
|
|
.Bootstrap(job, CancellationToken.None);
|
|
Verification.True(bootstrap.Succeeded, "observer vehicle snapshot bootstrap succeeds");
|
|
|
|
originalVehicle.LengthMeters = 91d;
|
|
originalVehicle.WidthMeters = 92d;
|
|
originalVehicle.SafetyMarginMeters = 93d;
|
|
originalVehicle.MaximumCurvaturePerMeter = 94d;
|
|
originalVehicle.MinimumTurningRadiusMeters = 95d;
|
|
|
|
EmTrajectory published = CreatePublishedTrajectory(effectiveAt);
|
|
var planningService = new FixedTrajectoryPlanningService(published);
|
|
var controller = new TrajectoryObservationController(bootstrap, settings, planningService, "vehicle-check");
|
|
var state = new VehicleMotionState(new Pose2D(0.25d, 0.10d, 0d), 0.20d, null, effectiveAt, 3L);
|
|
controller.StartCycle(effectiveAt, state, CancellationToken.None).GetAwaiter().GetResult();
|
|
AssertVehicleSnapshot(planningService.Requests[0].Vehicle, expectedLength, expectedWidth, expectedMargin,
|
|
expectedMaximumCurvature, expectedMinimumRadius, "mutated bootstrap job vehicle");
|
|
Verification.True(!ReferenceEquals(originalVehicle, planningService.Requests[0].Vehicle),
|
|
"observer request does not retain mutable bootstrap vehicle object");
|
|
|
|
job.Vehicle = new VehicleParameters
|
|
{
|
|
LengthMeters = 101d,
|
|
WidthMeters = 102d,
|
|
SafetyMarginMeters = 103d,
|
|
MaximumCurvaturePerMeter = 104d,
|
|
MinimumTurningRadiusMeters = 105d,
|
|
};
|
|
controller.StartCycle(effectiveAt.AddSeconds(settings.ReplanPeriodSeconds), state, CancellationToken.None)
|
|
.GetAwaiter().GetResult();
|
|
AssertVehicleSnapshot(planningService.Requests[1].Vehicle, expectedLength, expectedWidth, expectedMargin,
|
|
expectedMaximumCurvature, expectedMinimumRadius, "replaced bootstrap job vehicle");
|
|
}
|
|
|
|
private static void VerifiesEmptyMapBootstrapProducesPublishableReference()
|
|
{
|
|
var settings = new TrajectoryObservationSettings();
|
|
CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
|
|
new Pose2D(0.5d, 0.5d, 0d), new Pose2D(3.5d, 0.5d, 0d), settings,
|
|
Array.Empty<TrajectoryObservationObstacle>(), 0L);
|
|
|
|
TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper()
|
|
.Bootstrap(job, CancellationToken.None);
|
|
|
|
Verification.True(bootstrap.Succeeded, "observer empty-map bootstrap succeeds");
|
|
Verification.True(bootstrap.CoarseResult.MapResult.Succeeded,
|
|
"observer empty-map bootstrap builds a successful map");
|
|
Verification.True(bootstrap.Map != null, "observer empty-map bootstrap returns a map");
|
|
Verification.Equal(PlanningStatus.Success, bootstrap.CoarseResult.PlanningResult.Status,
|
|
"observer empty-map coarse planning succeeds");
|
|
PathSmoothingResult smoothedPath = bootstrap.SmoothedPath ?? throw new InvalidOperationException(
|
|
"observer empty-map bootstrap has no smoothing result");
|
|
Verification.True(IsPublishableSmoothingStatus(smoothedPath.Status),
|
|
"observer empty-map bootstrap returns a publishable smoothing result");
|
|
for (int index = 0; index < smoothedPath.Path.Count; index++)
|
|
{
|
|
Verification.True(!double.IsNaN(smoothedPath.Path[index].BodyClearance) &&
|
|
!double.IsInfinity(smoothedPath.Path[index].BodyClearance),
|
|
"observer empty-map smoothing path has finite body clearance at " + index);
|
|
}
|
|
Verification.True(bootstrap.Segments.Count > 0,
|
|
"observer empty-map bootstrap returns at least one direction segment");
|
|
}
|
|
|
|
private static bool IsPublishableSmoothingStatus(PathSmoothingStatus status)
|
|
{
|
|
return status == PathSmoothingStatus.Complete ||
|
|
status == PathSmoothingStatus.PartialImprovement ||
|
|
status == PathSmoothingStatus.NotNeeded ||
|
|
status == PathSmoothingStatus.Unchanged;
|
|
}
|
|
|
|
private static void AssertVehicleSnapshot(VehicleParameters actual, double expectedLength, double expectedWidth,
|
|
double expectedMargin, double? expectedMaximumCurvature, double? expectedMinimumRadius, string name)
|
|
{
|
|
Verification.NearlyEqual(expectedLength, actual.LengthMeters, name + " length");
|
|
Verification.NearlyEqual(expectedWidth, actual.WidthMeters, name + " width");
|
|
Verification.NearlyEqual(expectedMargin, actual.SafetyMarginMeters, name + " safety margin");
|
|
Verification.Equal(expectedMaximumCurvature, actual.MaximumCurvaturePerMeter,
|
|
name + " maximum curvature");
|
|
Verification.Equal(expectedMinimumRadius, actual.MinimumTurningRadiusMeters,
|
|
name + " minimum turning radius");
|
|
}
|
|
|
|
private static DirectionSegmentView CreateStraightSegment()
|
|
{
|
|
var points = new List<SmoothedPathPoint>
|
|
{
|
|
new SmoothedPathPoint(0d, 0d, 0d, 0d, 0d, TravelDirection.Forward,
|
|
0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
|
|
new SmoothedPathPoint(2d, 0d, 0d, 0d, 2d, TravelDirection.Forward,
|
|
0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
|
|
};
|
|
return new DirectionSegmentView(0, TravelDirection.Forward, points,
|
|
new ReferenceBoundary(0, 0d, EmBoundaryType.None, 10d),
|
|
new ReferenceBoundary(0, 2d, EmBoundaryType.Goal, 12d), 10d);
|
|
}
|
|
|
|
private static EmTrajectory CreatePublishedTrajectory(DateTimeOffset effectiveAt,
|
|
EmTerminalType terminalType = EmTerminalType.Goal)
|
|
{
|
|
var metadata = new EmTrajectoryMetadata("observer-published", effectiveAt, effectiveAt, 1L,
|
|
"observer-reference", 1L, string.Empty, 0, TravelDirection.Forward, terminalType,
|
|
EmLongitudinalMode.RollingContinuation);
|
|
return new EmTrajectory(metadata, new[]
|
|
{
|
|
new EmTrajectoryPoint(0.25d, 0.10d, 0d, 0.20d, 0d, 0d, 0, 0.25d, 4d,
|
|
TravelDirection.Forward, EmBoundaryType.None, 0d, 0d),
|
|
new EmTrajectoryPoint(1.25d, -0.20d, 0d, 0.40d, 1d, 0d, 0, 1.25d, 5d,
|
|
TravelDirection.Forward, EmBoundaryType.None, 0d, 0d),
|
|
});
|
|
}
|
|
|
|
private sealed class FixedTrajectoryPlanningService : IEmPlanningService
|
|
{
|
|
private readonly EmTrajectory trajectory;
|
|
|
|
public FixedTrajectoryPlanningService(EmTrajectory trajectory)
|
|
{
|
|
this.trajectory = trajectory;
|
|
}
|
|
|
|
public List<EmPlanningRequest> Requests { get; } = new List<EmPlanningRequest>();
|
|
|
|
public EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken)
|
|
{
|
|
Requests.Add(request);
|
|
return new EmPlanningResult(EmPlanningStatus.Success, trajectory, string.Empty);
|
|
}
|
|
}
|
|
|
|
private sealed class DelayedTrajectoryPlanningService : IEmPlanningService
|
|
{
|
|
private readonly EmTrajectory trajectory;
|
|
private readonly int delayedCall;
|
|
private readonly ManualResetEventSlim entered = new ManualResetEventSlim(false);
|
|
private readonly ManualResetEventSlim release = new ManualResetEventSlim(false);
|
|
private int callCount;
|
|
|
|
public DelayedTrajectoryPlanningService(EmTrajectory trajectory, int delayedCall)
|
|
{
|
|
this.trajectory = trajectory;
|
|
this.delayedCall = delayedCall;
|
|
}
|
|
|
|
public EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken)
|
|
{
|
|
if (Interlocked.Increment(ref callCount) == delayedCall)
|
|
{
|
|
entered.Set();
|
|
release.Wait(cancellationToken);
|
|
}
|
|
return new EmPlanningResult(EmPlanningStatus.Success, trajectory, string.Empty);
|
|
}
|
|
|
|
public bool WaitUntilEntered(TimeSpan timeout)
|
|
{
|
|
return entered.Wait(timeout);
|
|
}
|
|
|
|
public void Release()
|
|
{
|
|
release.Set();
|
|
}
|
|
}
|
|
|
|
private static bool Throws(Action action)
|
|
{
|
|
try { action(); return false; }
|
|
catch (ArgumentOutOfRangeException) { return true; }
|
|
}
|
|
}
|