From 0a1cbe9fbda318947f351dc1e503098154b4134a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Thu, 6 Aug 2026 11:17:57 +0800 Subject: [PATCH] feat: export EM observation configuration --- ...jectoryObservationStaticSnapshotBuilder.cs | 93 +++++++++++++++++++ .../TrajectoryObservationChecks.cs | 1 + ...rajectoryObservationVisualizationChecks.cs | 14 +++ 3 files changed, 108 insertions(+) create mode 100644 ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationStaticSnapshotBuilder.cs create mode 100644 ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationVisualizationChecks.cs diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationStaticSnapshotBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationStaticSnapshotBuilder.cs new file mode 100644 index 0000000..dc538d7 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationStaticSnapshotBuilder.cs @@ -0,0 +1,93 @@ +using System; +using System.Collections.Generic; +using System.Globalization; +using MultiWheelC.TrajectoryPlanning.EMPlanner; +using TrajectoryPlanningVisualization; + +namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation; + +public sealed class TrajectoryObservationStaticSnapshotBuilder +{ + public PlanningVisualizationStaticSnapshot Build(TrajectoryObservationBootstrapResult bootstrap, + EmPlannerConfiguration effectiveConfiguration, TrajectoryObservationSettings settings, long obstacleSnapshotVersion) + { + if (bootstrap == null) throw new ArgumentNullException(nameof(bootstrap)); + if (effectiveConfiguration == null) throw new ArgumentNullException(nameof(effectiveConfiguration)); + if (settings == null) throw new ArgumentNullException(nameof(settings)); + var map = bootstrap.Map ?? throw new ArgumentException("Bootstrap map is required.", nameof(bootstrap)); + var bounds = new VisualizationBounds(map.Bounds.XMin / 1000d, map.Bounds.XMax / 1000d, + map.Bounds.YMin / 1000d, map.Bounds.YMax / 1000d); + return new PlanningVisualizationStaticSnapshot("EM轨迹规划观察", bounds, CreateGrid(map, bounds), + CreatePolylines(bootstrap), CreateMarkers(bootstrap), CreateSegments(bootstrap), + CreateConfiguration(effectiveConfiguration, settings, map, obstacleSnapshotVersion)); + } + + private static VisualizationOccupancyGrid CreateGrid(Mapping.PlanningGridMap map, VisualizationBounds bounds) + { + var bits = new byte[(map.Rows * map.Cols + 7) / 8]; + for (int row = 0; row < map.Rows; row++) + for (int column = 0; column < map.Cols; column++) + if (map.IsOccupied(row, column)) bits[(row * map.Cols + column) >> 3] |= (byte)(1 << ((row * map.Cols + column) & 7)); + return new VisualizationOccupancyGrid(bounds, map.ResolutionMeters, map.Rows, map.Cols, bits); + } + + private static IReadOnlyList CreatePolylines(TrajectoryObservationBootstrapResult bootstrap) + { + var lines = new List(); + var coarse = new List(); + foreach (var point in bootstrap.CoarseResult.PlanningResult.Path) coarse.Add(new VisualizationPoint(point.X, point.Y)); + if (coarse.Count > 0) lines.Add(new VisualizationPolyline("coarse-path", "粗路径", "coarse", VisualizationLineStyle.Dashed, coarse)); + var smooth = new List(); + foreach (var point in bootstrap.SmoothedPath.Path) smooth.Add(new VisualizationPoint(point.X, point.Y)); + if (smooth.Count > 0) lines.Add(new VisualizationPolyline("local-g2-path", "Local G2路径", "local-g2", VisualizationLineStyle.Solid, smooth)); + return lines; + } + + private static IReadOnlyList CreateMarkers(TrajectoryObservationBootstrapResult bootstrap) + { + var markers = new List(); + foreach (DirectionSegmentView segment in bootstrap.Segments) + if (segment.EndBoundary.BoundaryType == EmBoundaryType.GearSwitchApproach) + { + var point = segment.Points[segment.Points.Count - 1]; + markers.Add(new VisualizationMarker("gear-switch-" + segment.SegmentIndex, "gear-switch", + "换向 " + (segment.SegmentIndex + 1), new VisualizationPoint(point.X, point.Y))); + } + return markers; + } + + private static IReadOnlyList CreateSegments(TrajectoryObservationBootstrapResult bootstrap) + { + var result = new List(); + foreach (DirectionSegmentView segment in bootstrap.Segments) + { + var points = new List(); + foreach (var point in segment.Points) points.Add(new VisualizationPoint(point.X, point.Y)); + result.Add(new VisualizationDirectionSegment(segment.SegmentIndex, segment.Direction.ToString(), + segment.StartBoundary.BoundaryType == EmBoundaryType.GearSwitchDeparture, + segment.EndBoundary.BoundaryType == EmBoundaryType.GearSwitchApproach, points)); + } + return result; + } + + private static IReadOnlyList CreateConfiguration(EmPlannerConfiguration configuration, + TrajectoryObservationSettings settings, Mapping.PlanningGridMap map, long obstacleSnapshotVersion) + { + string F(double value) => value.ToString("R", CultureInfo.InvariantCulture); + var groups = new List(); + groups.Add(Group("调度", V("重规划周期", "ReplanPeriodSeconds", F(configuration.Scheduling.ReplanPeriodSeconds), "s"), + V("时域", "TimeHorizonSeconds", F(configuration.Scheduling.TimeHorizonSeconds), "s"), + V("距离视界", "DistanceHorizonMeters", F(configuration.Scheduling.DistanceHorizonMeters), "m"), + V("输出步长", "OutputTimeStepSeconds", F(configuration.Scheduling.OutputTimeStepSeconds), "s"), + V("求解超时", "SolverTimeoutSeconds", F(configuration.Scheduling.SolverTimeoutSeconds), "s"))); + groups.Add(Group("OSQP", V("最大外层迭代", "MaximumOuterIterations", configuration.Solver.MaximumOuterIterations.ToString(CultureInfo.InvariantCulture), ""), + V("最大OSQP迭代", "MaximumOsqpIterations", configuration.Solver.MaximumOsqpIterations.ToString(CultureInfo.InvariantCulture), ""), + V("绝对容差", "AbsoluteTolerance", F(configuration.Solver.AbsoluteTolerance), ""), V("相对容差", "RelativeTolerance", F(configuration.Solver.RelativeTolerance), ""), V("严格残差容差", "StrictResidualTolerance", F(configuration.Solver.StrictResidualTolerance), ""))); + groups.Add(Group("地图", V("快照版本", "ObstacleSnapshotVersion", obstacleSnapshotVersion.ToString(CultureInfo.InvariantCulture), ""), V("分辨率", "ResolutionMm", F(map.ResolutionMm), "mm"), V("X最小", "Bounds.XMin", F(map.Bounds.XMin), "mm"), V("X最大", "Bounds.XMax", F(map.Bounds.XMax), "mm"), V("Y最小", "Bounds.YMin", F(map.Bounds.YMin), "mm"), V("Y最大", "Bounds.YMax", F(map.Bounds.YMax), "mm"))); + groups.Add(Group("可视化与换向确认", V("网页刷新率", "WebRefreshRateHz", settings.WebRefreshRateHz.ToString(CultureInfo.InvariantCulture), "Hz"), V("历史容量", "VisualizationHistoryCycleLimit", settings.VisualizationHistoryCycleLimit.ToString(CultureInfo.InvariantCulture), "cycles"), V("方向确认速度", "DirectionConfirmationSpeedMetersPerSecond", F(settings.DirectionConfirmationSpeedMetersPerSecond), "m/s"), V("方向确认样本", "DirectionConfirmationSamples", settings.DirectionConfirmationSamples.ToString(CultureInfo.InvariantCulture), ""))); + return groups; + } + + private static VisualizationConfigurationGroup Group(string title, params VisualizationValue[] values) => new VisualizationConfigurationGroup(title, values); + private static VisualizationValue V(string chinese, string raw, string value, string unit) => new VisualizationValue(chinese, raw, value, unit, "normal"); +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs index a681d38..6a24b48 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs @@ -19,6 +19,7 @@ internal static class TrajectoryObservationChecks { TrajectoryObservationSettingsChecks.Run(); TrajectoryObservationSegmentChecks.Run(); + TrajectoryObservationVisualizationChecks.Run(); VerifiesObservationSourceHasNoActuatorCalls(); VerifiesObservationSourceUsesRequiredOperatorText(); VerifiesOperatorDocumentationUsesExactUiEntry(); diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationVisualizationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationVisualizationChecks.cs new file mode 100644 index 0000000..c3e5d3d --- /dev/null +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationVisualizationChecks.cs @@ -0,0 +1,14 @@ +using MultiWheelC.TrajectoryPlanning.TrajectoryObservation; + +namespace EMPlannerVerificationHost; + +internal static class TrajectoryObservationVisualizationChecks +{ + public static void Run() + { + var staticBuilder = new TrajectoryObservationStaticSnapshotBuilder(); + var chartBuilder = new TrajectoryObservationKinematicChartBuilder(); + Verification.True(staticBuilder != null && chartBuilder != null, + "observation visualization adapters are available"); + } +}