fix: complete EM observation visualization contracts
This commit is contained in:
梁薄云
+364
-6
@@ -1,4 +1,14 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Globalization;
|
||||
using System.Linq;
|
||||
using System.Reflection;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||||
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
using MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
||||
using TrajectoryPlanningVisualization;
|
||||
|
||||
namespace EMPlannerVerificationHost;
|
||||
|
||||
@@ -6,11 +16,359 @@ internal static class TrajectoryObservationVisualizationChecks
|
||||
{
|
||||
public static void Run()
|
||||
{
|
||||
var staticBuilder = new TrajectoryObservationStaticSnapshotBuilder();
|
||||
var chartBuilder = new TrajectoryObservationKinematicChartBuilder();
|
||||
var dynamicBuilder = new TrajectoryObservationDynamicSnapshotBuilder();
|
||||
var handoffAnalyzer = new TrajectoryObservationHandoffAnalyzer();
|
||||
Verification.True(staticBuilder != null && chartBuilder != null && dynamicBuilder != null && handoffAnalyzer != null,
|
||||
"observation visualization adapters are available");
|
||||
VerifiesStaticSnapshotExportsFrozenConfigurationAndGeometry();
|
||||
VerifiesKinematicChartsKeepKnotAndIntervalSemantics();
|
||||
VerifiesHandoffUsesSharedReferenceSAndSurvivesProjectionFailure();
|
||||
VerifiesDynamicSnapshotContainsObservationEvidenceWithoutCyclePoints();
|
||||
}
|
||||
|
||||
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");
|
||||
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");
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
IReadOnlyList<VisualizationChart> charts = new TrajectoryObservationKinematicChartBuilder()
|
||||
.Build(trajectory, segment, 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");
|
||||
EmTrajectory unprojectable = CreateTrajectory("unprojectable", DateTimeOffset.UnixEpoch, 21,
|
||||
EmTerminalType.Goal, EmLongitudinalMode.RollingContinuation, TravelDirection.Forward, 100d);
|
||||
Verification.True(FindChart(new TrajectoryObservationKinematicChartBuilder().Build(unprojectable, segment, 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, configuration);
|
||||
|
||||
Verification.True(snapshot.DynamicPolylines.Any(x => x.Id == "current"), "dynamic snapshot contains current trajectory");
|
||||
Verification.True(snapshot.DynamicPolylines.Any(x => x.Id == "previous"), "dynamic snapshot contains previous trajectory");
|
||||
Verification.True(snapshot.DynamicPolylines.Any(x => x.Id == "active-segment"), "dynamic snapshot contains active segment highlight");
|
||||
Verification.True(snapshot.DynamicPolylines.Any(x => x.Id == "current-horizon"), "dynamic snapshot contains projected horizon");
|
||||
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, 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, 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 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 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 EmTrajectory CreateTrajectory(string id, DateTimeOffset effectiveAt, int count, EmTerminalType terminal,
|
||||
EmLongitudinalMode mode, TravelDirection direction, double startX = 0d, double startPathS = 1000d)
|
||||
{
|
||||
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), 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user