using System; using System.Collections.Generic; using System.Globalization; using System.IO; 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; internal static class TrajectoryObservationVisualizationChecks { public static void Run() { VerifiesStaticSnapshotExportsFrozenConfigurationAndGeometry(); VerifiesKinematicChartsKeepKnotAndIntervalSemantics(); VerifiesHandoffUsesSharedReferenceSAndSurvivesProjectionFailure(); VerifiesDynamicSnapshotContainsObservationEvidenceWithoutCyclePoints(); 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 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 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 void VerifiesWebPublisherFusesFaultWithoutStoppingObserverTicks() { var settings = new TrajectoryObservationSettings { EnableWebVisualization = true }; var sink = new RecordingVisualizationSink { ThrowOnPublish = true }; var logs = new List(); 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 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(), Array.Empty(), Array.Empty(), Array.Empty(), 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())!; } private static IReadOnlyDictionary 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 values, string rawName, string expected, string name) { Verification.Equal(expected, values[rawName].Value, name); } private static void AssertEffectiveValues(IReadOnlyDictionary 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 charts, string id) { return charts.Single(x => x.Id == id); } private static DirectionSegmentView CreateStraightSegment(int index, double sourceStart) { var points = new List { 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(); 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); } } }