649 lines
44 KiB
C#
649 lines
44 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Globalization;
|
|
using System.IO;
|
|
using System.Linq;
|
|
using System.Reflection;
|
|
using System.Text;
|
|
using System.Threading;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
|
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
using MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
|
using TrajectoryPlanningVisualization;
|
|
|
|
namespace EMPlannerVerificationHost;
|
|
|
|
internal static class TrajectoryObservationVisualizationChecks
|
|
{
|
|
public static void Run()
|
|
{
|
|
VerifiesStaticSnapshotExportsFrozenConfigurationAndGeometry();
|
|
VerifiesKinematicChartsKeepKnotAndIntervalSemantics();
|
|
VerifiesHandoffUsesSharedReferenceSAndSurvivesProjectionFailure();
|
|
VerifiesDynamicSnapshotContainsObservationEvidenceWithoutCyclePoints();
|
|
VerifiesFullModeSnapshotPublishesSingleCurrentTrajectoryAndBoundaryMarkers();
|
|
VerifiesPainterBoundarySemanticsMatchWeb();
|
|
VerifiesWebPublisherFusesFaultWithoutStoppingObserverTicks();
|
|
VerifiesWebPublisherGatesAtConfiguredCadenceAndSkipsDisabledOutput();
|
|
}
|
|
|
|
private static void VerifiesStaticSnapshotExportsFrozenConfigurationAndGeometry()
|
|
{
|
|
var settings = new TrajectoryObservationSettings();
|
|
TrajectoryObservationBootstrapResult bootstrap = Bootstrap(settings);
|
|
var controller = new TrajectoryObservationController(bootstrap, settings,
|
|
new FixedPlanningService(CreateTrajectory("static", DateTimeOffset.UnixEpoch, 2, EmTerminalType.Goal,
|
|
EmLongitudinalMode.RollingContinuation, TravelDirection.Forward)), "static-configuration");
|
|
EmPlannerConfiguration effective = ReadEffectiveConfiguration(controller);
|
|
settings.TimeHorizonSeconds = 99d;
|
|
|
|
PlanningVisualizationStaticSnapshot snapshot = new TrajectoryObservationStaticSnapshotBuilder()
|
|
.Build(bootstrap, effective, settings.CreateValidatedSnapshot(), 17L);
|
|
|
|
Verification.Equal(10, snapshot.ConfigurationGroups.Count, "static configuration group count");
|
|
Verification.Equal("调度", snapshot.ConfigurationGroups[0].ChineseTitle, "static configuration group 0");
|
|
Verification.Equal("OSQP", snapshot.ConfigurationGroups[1].ChineseTitle, "static configuration group 1");
|
|
Verification.Equal("车辆与安全", snapshot.ConfigurationGroups[2].ChineseTitle, "static configuration group 2");
|
|
Verification.Equal("纵向限制", snapshot.ConfigurationGroups[3].ChineseTitle, "static configuration group 3");
|
|
Verification.Equal("ST 权重", snapshot.ConfigurationGroups[4].ChineseTitle, "static configuration group 4");
|
|
Verification.Equal("横向限制", snapshot.ConfigurationGroups[5].ChineseTitle, "static configuration group 5");
|
|
Verification.Equal("LS 权重", snapshot.ConfigurationGroups[6].ChineseTitle, "static configuration group 6");
|
|
Verification.Equal("走廊与投影", snapshot.ConfigurationGroups[7].ChineseTitle, "static configuration group 7");
|
|
Verification.Equal("地图", snapshot.ConfigurationGroups[8].ChineseTitle, "static configuration group 8");
|
|
Verification.Equal("可视化与换向确认", snapshot.ConfigurationGroups[9].ChineseTitle, "static configuration group 9");
|
|
|
|
IReadOnlyDictionary<string, VisualizationValue> values = Flatten(snapshot);
|
|
AssertValue(values, "TimeHorizonSeconds", "2", "effective configuration is frozen before editable settings change");
|
|
AssertValue(values, "MaximumOsqpIterations", "100000", "effective OSQP iteration limit");
|
|
AssertEffectiveValues(values, effective, settings, bootstrap);
|
|
foreach (string rawName in new[]
|
|
{
|
|
"AbsoluteTolerance", "RelativeTolerance", "StrictResidualTolerance", "WarmStart", "Polish", "NativeVerbose",
|
|
"LengthMeters", "WidthMeters", "SafetyMarginMeters", "MaximumCurvaturePerMeter",
|
|
"MaximumForwardSpeedMetersPerSecond", "MaximumReverseSpeedMetersPerSecond",
|
|
"MaximumAccelerationMetersPerSecondSquared", "MaximumDecelerationMetersPerSecondSquared",
|
|
"MaximumJerkMetersPerSecondCubed", "MaximumLateralAccelerationMetersPerSecondSquared",
|
|
"MaximumCurvatureRatePerMeterPerSecond", "StopSpeedToleranceMetersPerSecond", "ZeroSpeedHoldSeconds",
|
|
"ReferenceSpeed", "Acceleration", "Jerk", "PreviousTrajectory", "TerminalAcceleration",
|
|
"ReferenceOffset", "HeadingDeviation", "SecondDerivative", "ThirdDerivative", "Curvature",
|
|
"CurvatureVariation", "RollingTerminal", "LongitudinalSampleSpacingMeters",
|
|
"LateralSampleSpacingMeters", "MaximumLateralOffsetMeters", "AdditionalClearanceReserveMeters",
|
|
"MaximumCollisionCheckStepMeters", "MaximumProjectionDistanceMeters", "MinimumFrenetDenominator",
|
|
"BoundaryAnchorToleranceMeters", "SpatialToleranceMeters", "KinematicTolerance", "ObstacleSnapshotVersion",
|
|
"ResolutionMm", "Bounds.XMin", "Bounds.XMax", "Bounds.YMin", "Bounds.YMax", "Rows", "Cols",
|
|
"EnableWebVisualization", "AutoOpenWebVisualization", "WebVisualizationPort", "WebRefreshRateHz",
|
|
"VisualizationHistoryCycleLimit", "EnableNativePainterVisualization",
|
|
"DirectionConfirmationSpeedMetersPerSecond", "DirectionConfirmationSamples",
|
|
"GearSwitchProjectionToleranceMeters", "GearSwitchStopHoldSeconds",
|
|
})
|
|
Verification.True(values.ContainsKey(rawName), "static configuration exports " + rawName);
|
|
|
|
Verification.Equal(bootstrap.Segments.Count, snapshot.DirectionSegments.Count, "all direction segments exported");
|
|
Verification.True(snapshot.StaticPolylines.Any(x => x.Id == "coarse-path"), "coarse path exported");
|
|
Verification.True(snapshot.StaticPolylines.Any(x => x.Id == "local-g2-path"), "full Local G2 path exported");
|
|
VisualizationMarker smoothStart = snapshot.StaticMarkers.Single(x => x.Kind == "smooth-start");
|
|
Verification.Equal("平滑路径起点", smoothStart.LabelChinese,
|
|
"static snapshot labels the smoothed-path start");
|
|
Verification.NearlyEqual(bootstrap.SmoothedPath.Path[0].X, smoothStart.Position.X,
|
|
"smoothed-path start marker X matches the first smoothed point");
|
|
Verification.NearlyEqual(bootstrap.SmoothedPath.Path[0].Y, smoothStart.Position.Y,
|
|
"smoothed-path start marker Y matches the first smoothed point");
|
|
byte[] occupancy = Convert.FromBase64String(snapshot.OccupancyGrid.OccupancyBitsBase64);
|
|
Verification.Equal((bootstrap.Map.Rows * bootstrap.Map.Cols + 7) / 8, occupancy.Length,
|
|
"occupancy bitset compact length");
|
|
bool verifiedOccupiedCell = false;
|
|
for (int row = 0; row < bootstrap.Map.Rows; row++)
|
|
for (int col = 0; col < bootstrap.Map.Cols; col++)
|
|
if (bootstrap.Map.IsOccupied(row, col))
|
|
{
|
|
int bit = row * bootstrap.Map.Cols + col;
|
|
Verification.True((occupancy[bit >> 3] & (1 << (bit & 7))) != 0,
|
|
"occupancy uses row-major LSB-first bitset");
|
|
verifiedOccupiedCell = true;
|
|
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()
|
|
{
|
|
DirectionSegmentView segment = CreateStraightSegment(0, 50d);
|
|
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
|
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, 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");
|
|
Verification.True(FindChart(charts, "jerk-t").NoteChinese.Contains("末点后无时间区间"),
|
|
"jerk explains no terminal successor interval");
|
|
Verification.Equal(21, FindChart(charts, "velocity-t").Series[0].Points.Count, "velocity remains knot based");
|
|
Verification.Equal("ReferenceS (m)", FindChart(charts, "ls").XAxisLabel, "LS uses shared reference station");
|
|
Verification.Equal("PathS (m)", FindChart(charts, "st").YAxisLabel, "ST uses plan-local PathS");
|
|
foreach (string id in new[] { "curvature-s", "curvature-t", "velocity-t", "acceleration-t", "jerk-t" })
|
|
Verification.True(FindChart(charts, id).Series.Any(x => x.LineStyle == VisualizationLineStyle.Limit),
|
|
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, vehicle, configuration), "ls")
|
|
.NoteChinese.Contains("投影失败=21"), "LS chart reports every failed full-segment projection");
|
|
}
|
|
|
|
private static void VerifiesHandoffUsesSharedReferenceSAndSurvivesProjectionFailure()
|
|
{
|
|
DirectionSegmentView segment = CreateStraightSegment(0, 100d);
|
|
DateTimeOffset t0 = DateTimeOffset.UnixEpoch;
|
|
EmTrajectory previous = CreateTrajectory("previous", t0, 11, EmTerminalType.Goal,
|
|
EmLongitudinalMode.RollingContinuation, TravelDirection.Forward, 2d, 0d);
|
|
EmTrajectory current = CreateTrajectory("current", t0.AddSeconds(1d), 2, EmTerminalType.Goal,
|
|
EmLongitudinalMode.RollingContinuation, TravelDirection.Forward, 4d, 0d);
|
|
TrajectoryObservationHandoffMetrics metrics = new TrajectoryObservationHandoffAnalyzer().Analyze(current, previous, segment);
|
|
|
|
Verification.True(metrics.Available, "handoff samples absolute-time aligned trajectories");
|
|
Verification.NearlyEqual(1d, metrics.DeltaPositionMeters!.Value, "handoff world delta");
|
|
Verification.NearlyEqual(1d, metrics.DeltaReferenceSMeters!.Value, "handoff compares shared ReferenceS");
|
|
Verification.True(metrics.DeltaReferenceSMeters.Value != current.Points[0].PathS - previous.Points[0].PathS,
|
|
"handoff does not compare local PathS origins");
|
|
|
|
EmTrajectory offSegment = CreateTrajectory("off-segment", t0.AddSeconds(1d), 2, EmTerminalType.Goal,
|
|
EmLongitudinalMode.RollingContinuation, TravelDirection.Forward, 300d, 0d);
|
|
TrajectoryObservationHandoffMetrics unavailable = new TrajectoryObservationHandoffAnalyzer().Analyze(offSegment, previous, segment);
|
|
Verification.True(unavailable.Available, "handoff keeps non-projection deltas available");
|
|
Verification.True(!unavailable.DeltaReferenceSMeters.HasValue, "projection failure makes only ReferenceS unavailable");
|
|
Verification.True(unavailable.DeltaPositionMeters.HasValue && unavailable.DeltaVelocityMetersPerSecond.HasValue &&
|
|
unavailable.DeltaAccelerationMetersPerSecondSquared.HasValue, "projection failure preserves world and kinematic deltas");
|
|
}
|
|
|
|
private static void VerifiesDynamicSnapshotContainsObservationEvidenceWithoutCyclePoints()
|
|
{
|
|
var settings = new TrajectoryObservationSettings();
|
|
TrajectoryObservationBootstrapResult bootstrap = Bootstrap(settings);
|
|
DateTimeOffset now = DateTimeOffset.UnixEpoch.AddMinutes(1);
|
|
EmTrajectory current = CreateTrajectory("dynamic", now, 21, EmTerminalType.Goal,
|
|
EmLongitudinalMode.RollingContinuation, TravelDirection.Forward);
|
|
var controller = new TrajectoryObservationController(bootstrap, settings, new FixedPlanningService(current), "dynamic");
|
|
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);
|
|
EmPlannerConfiguration configuration = ReadEffectiveConfiguration(controller);
|
|
EmTrajectory previous = CreateTrajectory("dynamic-previous", now.AddSeconds(-1d), 21, EmTerminalType.Goal,
|
|
EmLongitudinalMode.RollingContinuation, TravelDirection.Forward);
|
|
|
|
PlanningVisualizationDynamicSnapshot snapshot = new TrajectoryObservationDynamicSnapshotBuilder()
|
|
.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.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",
|
|
"DeltaReferenceSMeters", "DeltaVelocityMetersPerSecond", "DeltaAccelerationMetersPerSecondSquared",
|
|
"GearConfirmation" })
|
|
Verification.True(snapshot.StatusValues.Any(x => x.RawName == rawName), "dynamic status includes " + rawName);
|
|
Verification.True(snapshot.StatusValues.Single(x => x.RawName == "RollingTerminalConstraint").Value.Contains("滚动末端停车硬约束：未启用"),
|
|
"rolling status describes disabled exact-stop constraint");
|
|
Verification.NearlyEqual(current.Points[current.Points.Count - 1].SignedLongitudinalVelocity,
|
|
snapshot.CycleSummary.TerminalVelocity!.Value, "cycle summary uses actual terminal velocity");
|
|
Verification.NearlyEqual(ReadAcceleration(current.Points[current.Points.Count - 1]),
|
|
snapshot.CycleSummary.TerminalAcceleration!.Value, "cycle summary uses actual terminal acceleration");
|
|
Verification.True(!typeof(VisualizationCycleSummary).GetProperties().Any(x =>
|
|
x.PropertyType == typeof(EmTrajectory) || x.PropertyType == typeof(EmTrajectoryPoint) ||
|
|
(x.PropertyType.IsGenericType && x.PropertyType.GetGenericArguments().Any(t => t == typeof(EmTrajectoryPoint)))),
|
|
"cycle summary does not carry complete point arrays");
|
|
|
|
EmTrajectory exactStop = CreateTrajectory("exact-stop", now, 21, EmTerminalType.GearSwitch,
|
|
EmLongitudinalMode.ExactStopAtBoundary, TravelDirection.Forward);
|
|
var exactController = new TrajectoryObservationController(bootstrap, settings,
|
|
new FixedPlanningService(exactStop), "exact-stop");
|
|
exactController.StartCycle(now, state, CancellationToken.None).GetAwaiter().GetResult();
|
|
TrajectoryObservationLoopTick exactTick = new TrajectoryObservationLoop(exactController)
|
|
.Tick(now.AddSeconds(settings.ObserverPeriodSeconds), state, CancellationToken.None);
|
|
PlanningVisualizationDynamicSnapshot exactSnapshot = new TrajectoryObservationDynamicSnapshotBuilder().Build(6L,
|
|
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,
|
|
exactSnapshot.CycleSummary.TerminalVelocity!.Value, "exact-stop terminal velocity is real final knot");
|
|
Verification.NearlyEqual(ReadAcceleration(exactStop.Points[exactStop.Points.Count - 1]),
|
|
exactSnapshot.CycleSummary.TerminalAcceleration!.Value, "exact-stop terminal acceleration is real final knot");
|
|
|
|
EmTrajectory offSegment = CreateTrajectory("dynamic-off-segment", now, 21, EmTerminalType.Goal,
|
|
EmLongitudinalMode.RollingContinuation, TravelDirection.Forward, 100d);
|
|
var offSegmentController = new TrajectoryObservationController(bootstrap, settings,
|
|
new FixedPlanningService(offSegment), "dynamic-off-segment");
|
|
offSegmentController.StartCycle(now, state, CancellationToken.None).GetAwaiter().GetResult();
|
|
TrajectoryObservationLoopTick offSegmentTick = new TrajectoryObservationLoop(offSegmentController)
|
|
.Tick(now.AddSeconds(settings.ObserverPeriodSeconds), state, CancellationToken.None);
|
|
PlanningVisualizationDynamicSnapshot recovered = new TrajectoryObservationDynamicSnapshotBuilder().Build(7L,
|
|
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 };
|
|
var sink = new RecordingVisualizationSink { ThrowOnPublish = true };
|
|
var logs = new List<string>();
|
|
var publisher = new TrajectoryObservationVisualizationPublisher(settings, sink, logs.Add);
|
|
DateTimeOffset now = DateTimeOffset.UnixEpoch;
|
|
|
|
publisher.Start(CreateStaticSnapshot(settings));
|
|
Verification.True(!publisher.TryPublish(now, () => CreateDynamicSnapshot(now)),
|
|
"publisher reports a failed web publish");
|
|
Verification.True(!publisher.TryPublish(now.AddSeconds(1d), () => throw new InvalidOperationException("must not build after fuse")),
|
|
"publisher permanently disables web output after the first fault");
|
|
Verification.Equal(1, sink.PublishCalls, "publisher invokes the throwing sink once");
|
|
Verification.Equal(1, sink.StopCalls, "publisher stops the sink once after its fault");
|
|
Verification.Equal(1, logs.Count, "publisher logs the web fault once");
|
|
Verification.True(publisher.FaultReason.Contains("网页"), "publisher records an immutable Chinese web fault reason");
|
|
|
|
TrajectoryObservationBootstrapResult bootstrap = Bootstrap(settings);
|
|
DateTimeOffset tickAt = now.AddSeconds(1d);
|
|
var controller = new TrajectoryObservationController(bootstrap, settings,
|
|
new FixedPlanningService(CreateTrajectory("publisher-loop", tickAt, 2, EmTerminalType.Goal,
|
|
EmLongitudinalMode.RollingContinuation, TravelDirection.Forward)), "publisher-loop");
|
|
var loop = new TrajectoryObservationLoop(controller);
|
|
var state = new VehicleMotionState(new Pose2D(0d, 0d, 0d), 0d, null, tickAt, 1L);
|
|
TrajectoryObservationLoopTick firstTick = loop.Tick(tickAt, state, CancellationToken.None);
|
|
TrajectoryObservationLoopTick secondTick = loop.Tick(tickAt.AddSeconds(settings.ObserverPeriodSeconds), state,
|
|
CancellationToken.None);
|
|
Verification.True(firstTick.PlanningStarted || secondTick.PlanningStarted || secondTick.PlanningCompleted,
|
|
"observer loop continues producing ticks after a web publisher fault");
|
|
}
|
|
|
|
private static void VerifiesWebPublisherGatesAtConfiguredCadenceAndSkipsDisabledOutput()
|
|
{
|
|
var settings = new TrajectoryObservationSettings
|
|
{
|
|
EnableWebVisualization = true,
|
|
WebRefreshRateHz = 10d,
|
|
};
|
|
var sink = new RecordingVisualizationSink();
|
|
var publisher = new TrajectoryObservationVisualizationPublisher(settings, sink, _ => { });
|
|
DateTimeOffset start = DateTimeOffset.UnixEpoch;
|
|
publisher.Start(CreateStaticSnapshot(settings));
|
|
for (int index = 0; index <= 20; index++)
|
|
publisher.TryPublish(start.AddMilliseconds(index * 50), () => CreateDynamicSnapshot(start));
|
|
Verification.True(sink.PublishCalls <= 11, "20 Hz observer ticks publish no more than 11 snapshots at 10 Hz over one second");
|
|
Verification.Equal(11, sink.PublishCalls, "publisher includes the initial 10 Hz snapshot");
|
|
publisher.Stop();
|
|
publisher.Stop();
|
|
Verification.Equal(1, sink.StopCalls, "publisher stop is idempotent");
|
|
|
|
var disabledSettings = new TrajectoryObservationSettings { EnableWebVisualization = false };
|
|
var disabledSink = new RecordingVisualizationSink();
|
|
var disabledPublisher = new TrajectoryObservationVisualizationPublisher(disabledSettings, disabledSink, _ => { });
|
|
int factoryCalls = 0;
|
|
disabledPublisher.Start(CreateStaticSnapshot(disabledSettings));
|
|
disabledPublisher.TryPublish(start, () =>
|
|
{
|
|
factoryCalls++;
|
|
return CreateDynamicSnapshot(start);
|
|
});
|
|
Verification.Equal(0, disabledSink.StartCalls, "disabled web visualization does not start a service");
|
|
Verification.Equal(0, factoryCalls, "disabled web visualization does not build dynamic snapshots");
|
|
}
|
|
|
|
private static void VerifiesPainterBoundarySemanticsMatchWeb()
|
|
{
|
|
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("DrawBoundaryMarkers"),
|
|
"observer painter draws shared boundary markers");
|
|
foreach (string expected in new[]
|
|
{
|
|
"计划起点", "车辆", "换向点 ", " / s_end", "终点 / s_end",
|
|
})
|
|
{
|
|
Verification.True(source.Contains(expected),
|
|
"observer painter boundary semantics include " + expected);
|
|
}
|
|
}
|
|
|
|
private static PlanningVisualizationStaticSnapshot CreateStaticSnapshot(TrajectoryObservationSettings settings)
|
|
{
|
|
TrajectoryObservationBootstrapResult bootstrap = Bootstrap(settings);
|
|
var controller = new TrajectoryObservationController(bootstrap, settings,
|
|
new FixedPlanningService(CreateTrajectory("publisher-static", DateTimeOffset.UnixEpoch, 2,
|
|
EmTerminalType.Goal, EmLongitudinalMode.RollingContinuation, TravelDirection.Forward)), "publisher-static");
|
|
return new TrajectoryObservationStaticSnapshotBuilder().Build(bootstrap,
|
|
ReadEffectiveConfiguration(controller), settings.CreateValidatedSnapshot(), 0L);
|
|
}
|
|
|
|
private static PlanningVisualizationDynamicSnapshot CreateDynamicSnapshot(DateTimeOffset now)
|
|
{
|
|
return new PlanningVisualizationDynamicSnapshot(1L, now, "OBSERVE_ONLY", 0, "Forward",
|
|
new VisualizationPose(0d, 0d, 0d), Array.Empty<VisualizationPolyline>(),
|
|
Array.Empty<VisualizationMarker>(), Array.Empty<VisualizationChart>(), Array.Empty<VisualizationValue>(),
|
|
new VisualizationCycleSummary(1L, now, "Success", true, 0d, 0, "Forward", "RollingContinuation",
|
|
"Goal", 0d, 0d, string.Empty));
|
|
}
|
|
|
|
private sealed class RecordingVisualizationSink : ITrajectoryObservationVisualizationSink
|
|
{
|
|
public bool ThrowOnPublish { get; set; }
|
|
public int StartCalls { get; private set; }
|
|
public int PublishCalls { get; private set; }
|
|
public int StopCalls { get; private set; }
|
|
|
|
public PlanningVisualizationSessionInfo Start(PlanningVisualizationOptions options,
|
|
PlanningVisualizationStaticSnapshot snapshot)
|
|
{
|
|
StartCalls++;
|
|
return new PlanningVisualizationSessionInfo(new Uri("http://127.0.0.1:12345/?token=test"), "test");
|
|
}
|
|
|
|
public void Publish(PlanningVisualizationDynamicSnapshot snapshot)
|
|
{
|
|
PublishCalls++;
|
|
if (ThrowOnPublish) throw new InvalidOperationException("synthetic publish failure");
|
|
}
|
|
|
|
public void Stop()
|
|
{
|
|
StopCalls++;
|
|
}
|
|
}
|
|
|
|
private static TrajectoryObservationBootstrapResult Bootstrap(TrajectoryObservationSettings settings)
|
|
{
|
|
CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
|
|
new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings,
|
|
new[] { TrajectoryObservationObstacle.Circle(1500d, 1500d, 100d) }, 17L);
|
|
TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper().Bootstrap(job, CancellationToken.None);
|
|
Verification.True(bootstrap.Succeeded, "visualization bootstrap succeeds");
|
|
return bootstrap;
|
|
}
|
|
|
|
private static EmPlannerConfiguration ReadEffectiveConfiguration(TrajectoryObservationController controller)
|
|
{
|
|
MethodInfo method = typeof(TrajectoryObservationController).GetMethod("CreateEffectiveConfigurationSnapshot",
|
|
BindingFlags.Instance | BindingFlags.NonPublic)!;
|
|
return (EmPlannerConfiguration)method.Invoke(controller, Array.Empty<object>())!;
|
|
}
|
|
|
|
private static IReadOnlyDictionary<string, VisualizationValue> Flatten(PlanningVisualizationStaticSnapshot snapshot)
|
|
{
|
|
return snapshot.ConfigurationGroups.SelectMany(x => x.Entries)
|
|
.GroupBy(x => x.RawName).ToDictionary(x => x.Key, x => x.Last());
|
|
}
|
|
|
|
private static double ReadAcceleration(EmTrajectoryPoint point)
|
|
{
|
|
PropertyInfo property = typeof(EmTrajectoryPoint).GetProperty("LongitudinalAcceleration",
|
|
BindingFlags.Instance | BindingFlags.NonPublic)!;
|
|
return (double)property.GetValue(point)!;
|
|
}
|
|
|
|
private static void AssertValue(IReadOnlyDictionary<string, VisualizationValue> values, string rawName, string expected, string name)
|
|
{
|
|
Verification.Equal(expected, values[rawName].Value, name);
|
|
}
|
|
|
|
private static void AssertEffectiveValues(IReadOnlyDictionary<string, VisualizationValue> values,
|
|
EmPlannerConfiguration configuration, TrajectoryObservationSettings settings,
|
|
TrajectoryObservationBootstrapResult bootstrap)
|
|
{
|
|
string F(double value) => value.ToString("R", CultureInfo.InvariantCulture);
|
|
string I(int value) => value.ToString(CultureInfo.InvariantCulture);
|
|
string B(bool value) => value ? "true" : "false";
|
|
foreach ((string RawName, string Value) expected in new[]
|
|
{
|
|
("ReplanPeriodSeconds", F(configuration.Scheduling.ReplanPeriodSeconds)),
|
|
("TimeHorizonSeconds", F(configuration.Scheduling.TimeHorizonSeconds)),
|
|
("DistanceHorizonMeters", F(configuration.Scheduling.DistanceHorizonMeters)),
|
|
("OutputTimeStepSeconds", F(configuration.Scheduling.OutputTimeStepSeconds)),
|
|
("SolverTimeoutSeconds", F(configuration.Scheduling.SolverTimeoutSeconds)),
|
|
("HandoffLookaheadSeconds", F(configuration.Scheduling.HandoffLookaheadSeconds)),
|
|
("MaximumVehicleStateAgeSeconds", F(configuration.Scheduling.MaximumVehicleStateAgeSeconds)),
|
|
("MaximumOuterIterations", I(configuration.Solver.MaximumOuterIterations)),
|
|
("MaximumOsqpIterations", I(configuration.Solver.MaximumOsqpIterations)),
|
|
("AbsoluteTolerance", F(configuration.Solver.AbsoluteTolerance)),
|
|
("RelativeTolerance", F(configuration.Solver.RelativeTolerance)),
|
|
("StrictResidualTolerance", F(configuration.Solver.StrictResidualTolerance)),
|
|
("WarmStart", B(configuration.Solver.WarmStart)), ("Polish", B(configuration.Solver.Polish)),
|
|
("NativeVerbose", B(configuration.Solver.NativeVerbose)),
|
|
("MaximumForwardSpeedMetersPerSecond", F(configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond)),
|
|
("MaximumReverseSpeedMetersPerSecond", F(configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond)),
|
|
("MaximumAccelerationMetersPerSecondSquared", F(configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared)),
|
|
("MaximumDecelerationMetersPerSecondSquared", F(configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared)),
|
|
("MaximumJerkMetersPerSecondCubed", F(configuration.Longitudinal.MaximumJerkMetersPerSecondCubed)),
|
|
("MaximumLateralAccelerationMetersPerSecondSquared", F(configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared)),
|
|
("MaximumCurvatureRatePerMeterPerSecond", F(configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond)),
|
|
("StopSpeedToleranceMetersPerSecond", F(configuration.Longitudinal.StopSpeedToleranceMetersPerSecond)),
|
|
("ZeroSpeedHoldSeconds", F(configuration.Longitudinal.ZeroSpeedHoldSeconds)),
|
|
("ReferenceSpeed", F(configuration.Longitudinal.Weights.ReferenceSpeed)),
|
|
("Acceleration", F(configuration.Longitudinal.Weights.Acceleration)),
|
|
("Jerk", F(configuration.Longitudinal.Weights.Jerk)),
|
|
("TerminalAcceleration", F(configuration.Longitudinal.Weights.TerminalAcceleration)),
|
|
("ReferenceOffset", F(configuration.Lateral.Weights.ReferenceOffset)),
|
|
("HeadingDeviation", F(configuration.Lateral.Weights.HeadingDeviation)),
|
|
("SecondDerivative", F(configuration.Lateral.Weights.SecondDerivative)),
|
|
("ThirdDerivative", F(configuration.Lateral.Weights.ThirdDerivative)),
|
|
("Curvature", F(configuration.Lateral.Weights.Curvature)),
|
|
("CurvatureVariation", F(configuration.Lateral.Weights.CurvatureVariation)),
|
|
("RollingTerminal", F(configuration.Lateral.Weights.RollingTerminal)),
|
|
("MaximumLateralStepPerIterationMeters", F(configuration.Lateral.MaximumLateralStepPerIterationMeters)),
|
|
("MaximumLateralSlope", F(configuration.Lateral.MaximumLateralSlope)),
|
|
("MaximumLateralSecondDerivativePerMeter", F(configuration.Lateral.MaximumLateralSecondDerivativePerMeter)),
|
|
("MaximumLateralThirdDerivativePerSquareMeter", F(configuration.Lateral.MaximumLateralThirdDerivativePerSquareMeter)),
|
|
("LongitudinalSampleSpacingMeters", F(configuration.Corridor.LongitudinalSampleSpacingMeters)),
|
|
("LateralSampleSpacingMeters", F(configuration.Corridor.LateralSampleSpacingMeters)),
|
|
("MaximumLateralOffsetMeters", F(configuration.Corridor.MaximumLateralOffsetMeters)),
|
|
("AdditionalClearanceReserveMeters", F(configuration.Corridor.AdditionalClearanceReserveMeters)),
|
|
("MaximumCollisionCheckStepMeters", F(configuration.Corridor.MaximumCollisionCheckStepMeters)),
|
|
("MaximumProjectionDistanceMeters", F(configuration.Frenet.MaximumProjectionDistanceMeters)),
|
|
("MinimumFrenetDenominator", F(configuration.Frenet.MinimumFrenetDenominator)),
|
|
("BoundaryAnchorToleranceMeters", F(configuration.Frenet.BoundaryAnchorToleranceMeters)),
|
|
("SpatialToleranceMeters", F(configuration.Validation.SpatialToleranceMeters)),
|
|
("KinematicTolerance", F(configuration.Validation.KinematicTolerance)),
|
|
("LengthMeters", F(bootstrap.Vehicle.LengthMeters)), ("WidthMeters", F(bootstrap.Vehicle.WidthMeters)),
|
|
("SafetyMarginMeters", F(bootstrap.Vehicle.SafetyMarginMeters)),
|
|
("MaximumCurvaturePerMeter", F(bootstrap.Vehicle.MaximumCurvaturePerMeter!.Value)),
|
|
("ObstacleSnapshotVersion", "17"), ("ResolutionMm", F(bootstrap.Map.ResolutionMm)),
|
|
("Bounds.XMin", F(bootstrap.Map.Bounds.XMin)), ("Bounds.XMax", F(bootstrap.Map.Bounds.XMax)),
|
|
("Bounds.YMin", F(bootstrap.Map.Bounds.YMin)), ("Bounds.YMax", F(bootstrap.Map.Bounds.YMax)),
|
|
("Rows", I(bootstrap.Map.Rows)), ("Cols", I(bootstrap.Map.Cols)),
|
|
("EnableWebVisualization", B(settings.EnableWebVisualization)), ("AutoOpenWebVisualization", B(settings.AutoOpenWebVisualization)),
|
|
("WebVisualizationPort", I(settings.WebVisualizationPort)), ("WebRefreshRateHz", F(settings.WebRefreshRateHz)),
|
|
("VisualizationHistoryCycleLimit", I(settings.VisualizationHistoryCycleLimit)),
|
|
("EnableNativePainterVisualization", B(settings.EnableNativePainterVisualization)),
|
|
("DirectionConfirmationSpeedMetersPerSecond", F(settings.DirectionConfirmationSpeedMetersPerSecond)),
|
|
("DirectionConfirmationSamples", I(settings.DirectionConfirmationSamples)),
|
|
("GearSwitchProjectionToleranceMeters", F(settings.GearSwitchProjectionToleranceMeters)),
|
|
("GearSwitchStopHoldSeconds", F(settings.GearSwitchStopHoldSeconds)),
|
|
})
|
|
AssertValue(values, expected.RawName, expected.Value, "effective configuration value " + expected.RawName);
|
|
}
|
|
|
|
private static VisualizationChart FindChart(IReadOnlyList<VisualizationChart> charts, string id)
|
|
{
|
|
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>
|
|
{
|
|
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.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,
|
|
EmPlanningScope planningScope = EmPlanningScope.RollingHorizon)
|
|
{
|
|
var points = new List<EmTrajectoryPoint>();
|
|
for (int index = 0; index < count; index++)
|
|
{
|
|
double t = index * 0.1d;
|
|
points.Add(new EmTrajectoryPoint(startX + t, 0d, 0d, 0.1d + t, t, 0.01d, 0,
|
|
startX + t, startPathS + t, direction, index == count - 1 ? EmBoundaryType.Goal : EmBoundaryType.None,
|
|
0.2d + t, 0.3d + t));
|
|
}
|
|
return new EmTrajectory(new EmTrajectoryMetadata(id, effectiveAt, effectiveAt, 1L, "visualization-reference", 1L,
|
|
string.Empty, 0, direction, terminal, mode, planningScope), points);
|
|
}
|
|
|
|
private sealed class FixedPlanningService : IEmPlanningService
|
|
{
|
|
private readonly EmTrajectory trajectory;
|
|
|
|
public FixedPlanningService(EmTrajectory trajectory) { this.trajectory = trajectory; }
|
|
|
|
public EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken)
|
|
{
|
|
return new EmPlanningResult(EmPlanningStatus.Success, trajectory, string.Empty);
|
|
}
|
|
}
|
|
}
|