285 lines
16 KiB
C#
285 lines
16 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Threading;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
|
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;
|
|
using MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
|
|
|
namespace EMPlannerVerificationHost;
|
|
|
|
internal static class TrajectoryObservationChecks
|
|
{
|
|
public static void Run()
|
|
{
|
|
VerifiesStartGoalBoundsUseOnlyConfiguredPadding();
|
|
RejectsObstacleOutsideConfiguredBounds();
|
|
VerifiesLsAndStUsePublishedTrajectoryData();
|
|
VerifiesPresentationTextDescribesObservationWithoutSendingCommand();
|
|
VerifiesLsPresentationUsesPathSOnHorizontalAxis();
|
|
VerifiesRollingRequestUsesOnePublishedTrajectorySnapshot();
|
|
FreezesBootstrapVehicleForRollingRequests();
|
|
}
|
|
|
|
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 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 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();
|
|
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");
|
|
|
|
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 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 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)
|
|
{
|
|
var metadata = new EmTrajectoryMetadata("observer-published", effectiveAt, effectiveAt, 1L,
|
|
"observer-reference", 1L, string.Empty, 0, TravelDirection.Forward, EmTerminalType.Goal);
|
|
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 static bool Throws(Action action)
|
|
{
|
|
try { action(); return false; }
|
|
catch (ArgumentOutOfRangeException) { return true; }
|
|
}
|
|
}
|