First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix: complete EM observation visualization contracts

Browse Source
This commit is contained in:
梁薄云
2026-08-06 11:38:02 +08:00
parent a7dde0f5e4
commit cc6b0f1a51
4 changed files with 714 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDynamicSnapshotBuilder.cs
+93 -13
View File
@@ -1,5 +1,6 @@
using System;
using System.Collections.Generic;
using System.Globalization;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.EMPlanner;
using TrajectoryPlanningVisualization;
@@ -10,30 +11,109 @@ public sealed class TrajectoryObservationDynamicSnapshotBuilder
{
private readonly TrajectoryObservationKinematicChartBuilder charts = new TrajectoryObservationKinematicChartBuilder();
private readonly TrajectoryObservationHandoffAnalyzer handoff = new TrajectoryObservationHandoffAnalyzer();
public PlanningVisualizationDynamicSnapshot Build(long sequence, TrajectoryObservationLoopTick tick,
DirectionSegmentView activeSegment, EmTrajectory previousTrajectoryForVisualization, EmPlannerConfiguration configuration)
{
if (tick == null) throw new ArgumentNullException(nameof(tick));
if (activeSegment == null) throw new ArgumentNullException(nameof(activeSegment));
if (configuration == null) throw new ArgumentNullException(nameof(configuration));
EmTrajectory current = tick.Observation.PublishedTrajectory;
var polylines = new List<VisualizationPolyline>();
AddTrajectory(polylines, "previous", "上一轮轨迹", previousTrajectoryForVisualization, VisualizationLineStyle.Dashed);
AddSegment(polylines, activeSegment);
AddTrajectory(polylines, "previous", "上一轨迹", previousTrajectoryForVisualization, VisualizationLineStyle.Dashed);
AddTrajectory(polylines, "current", "当前轨迹", current, VisualizationLineStyle.Solid);
var status = new List<VisualizationValue>();
status.Add(Value("活动段", "ActiveSegmentIndex", activeSegment.SegmentIndex.ToString(), "", "normal"));
status.Add(Value("方向", "ActiveDirection", activeSegment.Direction.ToString(), "", "normal"));
IReadOnlyList<VisualizationChart> chartList = current == null ? Array.Empty<VisualizationChart>() : charts.Build(current, activeSegment, configuration);
AddTrajectory(polylines, "current-horizon", "当前规划视界", current, VisualizationLineStyle.Solid);
IReadOnlyList<VisualizationChart> chartList = current == null
? Array.Empty<VisualizationChart>()
: charts.Build(current, activeSegment, configuration);
int projectionFailures = current == null ? 0 : charts.CountProjectionFailures(current, activeSegment, configuration);
TrajectoryObservationHandoffMetrics metrics = handoff.Analyze(current, previousTrajectoryForVisualization, activeSegment);
status.Add(Value("交接位置差", "DeltaPositionMeters", F(metrics.DeltaPositionMeters), "m", metrics.Available ? "normal" : "notice"));
status.Add(Value("交接参考站差", "DeltaReferenceSMeters", F(metrics.DeltaReferenceSMeters), "m", metrics.DeltaReferenceSMeters.HasValue ? "normal" : "notice"));
EmTrajectoryPoint terminal = current == null ? null : current.Points[current.Points.Count - 1];
var status = new List<VisualizationValue>
{
Value("活动段", "ActiveSegmentIndex", activeSegment.SegmentIndex.ToString(CultureInfo.InvariantCulture), "", "normal"),
Value("方向", "ActiveDirection", activeSegment.Direction.ToString(), "", "normal"),
Value("规划状态", "PlanningStatus", tick.LatestCycle == null ? "pending" : tick.LatestCycle.Result.Status.ToString(), "", "normal"),
Value("纵向模式", "LongitudinalMode", current == null ? "不可用" : current.Metadata.LongitudinalMode.ToString(), "", "normal"),
Value("终端类型", "TerminalType", current == null ? "不可用" : current.Metadata.TerminalType.ToString(), "", "normal"),
Value("规划耗时", "PlanningElapsedMilliseconds", F(Math.Max(0d, tick.LatestPlanningElapsed.TotalMilliseconds)), "ms", "normal"),
Value("轨迹年龄", "TrajectoryAgeSeconds", current == null ? "不可用" : F(Math.Max(0d,
(tick.Observation.ObservedAtUtc - current.Metadata.EffectiveAtUtc).TotalSeconds)), "s", "normal"),
Value("LS 投影失败", "LsProjectionFailures", projectionFailures.ToString(CultureInfo.InvariantCulture), "", projectionFailures == 0 ? "normal" : "notice"),
Value("滚动终端语义", "RollingTerminalConstraint", RollingTerminalText(current), "", "normal"),
Value("终端速度", "TerminalVelocity", terminal == null ? "不可用" : F(terminal.SignedLongitudinalVelocity), "m/s", "normal"),
Value("终端加速度", "TerminalAcceleration", terminal == null ? "不可用" : F(terminal.LongitudinalAcceleration), "m/s²", "normal"),
Value("交接位置差", "DeltaPositionMeters", F(metrics.DeltaPositionMeters), "m", metrics.Available ? "normal" : "notice"),
Value("交接参考站差", "DeltaReferenceSMeters", F(metrics.DeltaReferenceSMeters), "m", metrics.DeltaReferenceSMeters.HasValue ? "normal" : "notice"),
Value("交接速度差", "DeltaVelocityMetersPerSecond", F(metrics.DeltaVelocityMetersPerSecond), "m/s", metrics.Available ? "normal" : "notice"),
Value("交接加速度差", "DeltaAccelerationMetersPerSecondSquared", F(metrics.DeltaAccelerationMetersPerSecondSquared), "m/s²", metrics.Available ? "normal" : "notice"),
Value("换向确认", "GearConfirmation", GearConfirmationText(tick.SegmentState), "", "normal"),
};
var cycle = tick.LatestCycle;
return new PlanningVisualizationDynamicSnapshot(sequence, tick.Observation.ObservedAtUtc, "OBSERVE_ONLY", activeSegment.SegmentIndex, activeSegment.Direction.ToString(),
new VisualizationPose(tick.Observation.VehicleState.Pose.X, tick.Observation.VehicleState.Pose.Y, tick.Observation.VehicleState.Pose.Heading), polylines, Array.Empty<VisualizationMarker>(), chartList, status,
new VisualizationCycleSummary(cycle == null ? 0L : cycle.Version, tick.Observation.ObservedAtUtc, cycle == null ? "pending" : cycle.Result.Status.ToString(), cycle != null && cycle.Published, Math.Max(0d,tick.LatestPlanningElapsed.TotalMilliseconds), activeSegment.SegmentIndex, activeSegment.Direction.ToString(), current == null ? "" : current.Metadata.LongitudinalMode.ToString(), current == null ? "" : current.Metadata.TerminalType.ToString(), terminal?.SignedLongitudinalVelocity, terminal?.LongitudinalAcceleration, cycle?.Diagnostic ?? ""));
return new PlanningVisualizationDynamicSnapshot(sequence, tick.Observation.ObservedAtUtc, "OBSERVE_ONLY",
activeSegment.SegmentIndex, activeSegment.Direction.ToString(), new VisualizationPose(
tick.Observation.VehicleState.Pose.X, tick.Observation.VehicleState.Pose.Y,
tick.Observation.VehicleState.Pose.Heading), polylines, Array.Empty<VisualizationMarker>(), chartList, status,
new VisualizationCycleSummary(cycle == null ? 0L : cycle.Version, tick.Observation.ObservedAtUtc,
cycle == null ? "pending" : cycle.Result.Status.ToString(), cycle != null && cycle.Published,
Math.Max(0d, tick.LatestPlanningElapsed.TotalMilliseconds), activeSegment.SegmentIndex,
activeSegment.Direction.ToString(), current == null ? "" : current.Metadata.LongitudinalMode.ToString(),
current == null ? "" : current.Metadata.TerminalType.ToString(), terminal?.SignedLongitudinalVelocity,
terminal?.LongitudinalAcceleration, cycle?.Diagnostic ?? ""));
}
private static void AddSegment(ICollection<VisualizationPolyline> lines, DirectionSegmentView segment)
{
var points = new List<VisualizationPoint>();
foreach (var point in segment.Points)
points.Add(new VisualizationPoint(point.X, point.Y));
lines.Add(new VisualizationPolyline("active-segment", "活动方向段", "active-segment", VisualizationLineStyle.Solid, points));
}
private static void AddTrajectory(ICollection<VisualizationPolyline> lines, string id, string legend,
EmTrajectory trajectory, VisualizationLineStyle style)
{
if (trajectory == null) return;
var points = new List<VisualizationPoint>();
foreach (EmTrajectoryPoint point in trajectory.Points)
points.Add(new VisualizationPoint(point.X, point.Y));
lines.Add(new VisualizationPolyline(id, legend, id, style, points));
}
private static string RollingTerminalText(EmTrajectory trajectory)
{
if (trajectory == null) return "不可用";
return trajectory.Metadata.LongitudinalMode == EmLongitudinalMode.ExactStopAtBoundary
? "精确停车锚点：已启用"
: "滚动末端停车硬约束：未启用";
}
private static string GearConfirmationText(TrajectoryObservationSegmentState state)
{
if (state.Phase == TrajectoryObservationSegmentPhase.WaitingForStop)
return "未满足：等待停车保持";
if (state.Phase == TrajectoryObservationSegmentPhase.WaitingForDirection)
return "未满足：等待 " + state.ExpectedDirection + " 连续方向样本（" +
state.ConfirmedDirectionSamples.ToString(CultureInfo.InvariantCulture) + "）";
return state.Phase == TrajectoryObservationSegmentPhase.Completed ? "已完成" : "当前段规划中";
}
private static VisualizationValue Value(string chineseName, string rawName, string value, string unit, string severity)
{
return new VisualizationValue(chineseName, rawName, value, unit, severity);
}
private static string F(double value)
{
return value.ToString("R", CultureInfo.InvariantCulture);
}
private static string F(double? value)
{
return value.HasValue ? F(value.Value) : "不可用";
}
private static void AddTrajectory(ICollection<VisualizationPolyline> lines,string id,string legend,EmTrajectory trajectory,VisualizationLineStyle style) { if(trajectory==null)return; var points=new List<VisualizationPoint>(); foreach(var point in trajectory.Points)points.Add(new VisualizationPoint(point.X,point.Y)); lines.Add(new VisualizationPolyline(id,legend,id,style,points)); }
private static VisualizationValue Value(string c,string raw,string v,string unit,string severity)=>new VisualizationValue(c,raw,v,unit,severity);
private static string F(double? v)=>v.HasValue?v.Value.ToString(System.Globalization.CultureInfo.InvariantCulture):"不可用";
}
ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationKinematicChartBuilder.cs
+134 -11
View File
@@ -1,5 +1,7 @@
using System;
using System.Collections.Generic;
using System.Globalization;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.EMPlanner;
using TrajectoryPlanningVisualization;
@@ -13,21 +15,142 @@ public sealed class TrajectoryObservationKinematicChartBuilder
if (trajectory == null) throw new ArgumentNullException(nameof(trajectory));
if (segment == null) throw new ArgumentNullException(nameof(segment));
if (configuration == null) throw new ArgumentNullException(nameof(configuration));
LsBuildResult ls = BuildLs(trajectory, segment, configuration.Frenet.MaximumProjectionDistanceMeters);
return new[]
{
Chart("ls", "横向偏移", "ReferenceS (m)", "l (m)", Ls(trajectory, segment), ""),
Chart("ls", "横向偏移", "ReferenceS (m)", "l (m)", ls.Points,
"完整活动方向段投影；投影失败=" + ls.ProjectionFailureCount.ToString(CultureInfo.InvariantCulture)),
Chart("st", "时空轨迹", "t (s)", "PathS (m)", Points(trajectory, p => p.PathS), ""),
Chart("curvature-s", "曲率-距离", "s (m)", "κ (m⁻¹)", Points(trajectory, p => p.VehicleCurvature), ""),
Chart("curvature-t", "曲率-时间", "t (s)", "κ (m⁻¹)", Points(trajectory, p => p.VehicleCurvature), ""),
WithLimit("velocity-t", "速度", "t (s)", "v (m/s)", Points(trajectory, p => p.SignedLongitudinalVelocity), configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond),
WithLimit("acceleration-t", "加速度", "t (s)", "a (m/s²)", Points(trajectory, p => p.LongitudinalAcceleration), configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared),
WithSymmetricLimit("curvature-s", "曲率-距离", "s (m)", "κ (m⁻¹)",
PointsBySegmentS(trajectory), configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond),
WithSymmetricLimit("curvature-t", "曲率-时间", "t (s)", "κ (m⁻¹)",
Points(trajectory, p => p.VehicleCurvature), configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond),
Velocity(trajectory, configuration),
Acceleration(trajectory, configuration),
Jerk(trajectory, configuration),
Chart("yaw-rate-t", "横摆角速度", "t (s)", "yaw rate (rad/s)", Points(trajectory, p => p.YawRate), "无独立横摆角速度上限")
Chart("yaw-rate-t", "横摆角速度", "t (s)", "yaw rate (rad/s)",
Points(trajectory, p => p.YawRate), "生效配置没有独立横摆角速度上限"),
};
}
private static VisualizationChart WithLimit(string id, string title, string x, string y, IReadOnlyList<VisualizationPoint> points, double limit) => new VisualizationChart(id, title, x, y, new[] { new VisualizationSeries(id + "-current", "当前轨迹", VisualizationLineStyle.Solid, points), new VisualizationSeries(id + "-limit", "生效限制", VisualizationLineStyle.Limit, new[] { new VisualizationPoint(0d, limit), new VisualizationPoint(points[points.Count - 1].X, limit) }) });
private static VisualizationChart Jerk(EmTrajectory trajectory, EmPlannerConfiguration configuration) { var p = new List<VisualizationPoint>(); for (int i=0;i+1<trajectory.Points.Count;i++) p.Add(new VisualizationPoint(trajectory.Points[i].TimeFromStart, trajectory.Points[i].LongitudinalJerk)); return new VisualizationChart("jerk-t", "加加速度", "t (s)", "j (m/s³)", new[] { new VisualizationSeries("jerk-t-current", "当前轨迹", VisualizationLineStyle.Solid, p), new VisualizationSeries("jerk-t-limit", "生效限制", VisualizationLineStyle.Limit, new[] { new VisualizationPoint(0d, configuration.Longitudinal.MaximumJerkMetersPerSecondCubed), new VisualizationPoint(p.Count == 0 ? 0d : p[p.Count-1].X, configuration.Longitudinal.MaximumJerkMetersPerSecondCubed) }) }, "末点后无时间区间"); }
private static VisualizationChart Chart(string id,string title,string x,string y,IReadOnlyList<VisualizationPoint> points,string note) => new VisualizationChart(id,title,x,y,new[] { new VisualizationSeries(id+"-current","当前轨迹",VisualizationLineStyle.Solid,points) },note);
private static IReadOnlyList<VisualizationPoint> Points(EmTrajectory t, Func<EmTrajectoryPoint,double> y) { var r=new List<VisualizationPoint>(); foreach(var p in t.Points) r.Add(new VisualizationPoint(p.TimeFromStart,y(p))); return r; }
private static IReadOnlyList<VisualizationPoint> Ls(EmTrajectory t, DirectionSegmentView s) { var r=new List<VisualizationPoint>(); var projector=new FrenetProjector(); double seed=0d; foreach(var p in t.Points) if(projector.TryProject(new CoarsePath.Pose2D(p.X,p.Y,p.Yaw),s,0d,s.LengthMeters,0.5d,seed,out FrenetProjection projection)) { r.Add(new VisualizationPoint(s.SourceStartArcLength+projection.ReferenceS,projection.LateralOffset)); seed=projection.ReferenceS; } return r; }
public int CountProjectionFailures(EmTrajectory trajectory, DirectionSegmentView segment,
EmPlannerConfiguration configuration)
{
if (trajectory == null) throw new ArgumentNullException(nameof(trajectory));
if (segment == null) throw new ArgumentNullException(nameof(segment));
if (configuration == null) throw new ArgumentNullException(nameof(configuration));
return BuildLs(trajectory, segment, configuration.Frenet.MaximumProjectionDistanceMeters).ProjectionFailureCount;
}
private static VisualizationChart Velocity(EmTrajectory trajectory, EmPlannerConfiguration configuration)
{
IReadOnlyList<VisualizationPoint> points = Points(trajectory, p => p.SignedLongitudinalVelocity);
return new VisualizationChart("velocity-t", "速度", "t (s)", "v (m/s)", new[]
{
new VisualizationSeries("velocity-t-current", "当前轨迹", VisualizationLineStyle.Solid, points),
Limit("velocity-t-forward-limit", "前进生效限制", points, configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond),
Limit("velocity-t-reverse-limit", "倒车生效限制", points, -configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond),
});
}
private static VisualizationChart Acceleration(EmTrajectory trajectory, EmPlannerConfiguration configuration)
{
IReadOnlyList<VisualizationPoint> points = Points(trajectory, p => p.LongitudinalAcceleration);
return new VisualizationChart("acceleration-t", "加速度", "t (s)", "a (m/s²)", new[]
{
new VisualizationSeries("acceleration-t-current", "当前轨迹", VisualizationLineStyle.Solid, points),
Limit("acceleration-t-positive-limit", "加速生效限制", points, configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared),
Limit("acceleration-t-negative-limit", "减速生效限制", points, -configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared),
});
}
private static VisualizationChart Jerk(EmTrajectory trajectory, EmPlannerConfiguration configuration)
{
var intervals = new List<VisualizationPoint>();
for (int index = 0; index + 1 < trajectory.Points.Count; index++)
intervals.Add(new VisualizationPoint(trajectory.Points[index].TimeFromStart,
trajectory.Points[index].LongitudinalJerk));
return new VisualizationChart("jerk-t", "加加速度", "t (s)", "j (m/s³)", new[]
{
new VisualizationSeries("jerk-t-current", "当前轨迹", VisualizationLineStyle.Solid, intervals),
Limit("jerk-t-positive-limit", "正向生效限制", intervals, configuration.Longitudinal.MaximumJerkMetersPerSecondCubed),
Limit("jerk-t-negative-limit", "负向生效限制", intervals, -configuration.Longitudinal.MaximumJerkMetersPerSecondCubed),
}, "j[i] 表示 [tᵢ, tᵢ₊₁)；末点后无时间区间");
}
private static VisualizationChart WithSymmetricLimit(string id, string title, string xAxis, string yAxis,
IReadOnlyList<VisualizationPoint> points, double limit)
{
return new VisualizationChart(id, title, xAxis, yAxis, new[]
{
new VisualizationSeries(id + "-current", "当前轨迹", VisualizationLineStyle.Solid, points),
Limit(id + "-positive-limit", "生效限制", points, limit),
Limit(id + "-negative-limit", "生效限制", points, -limit),
});
}
private static VisualizationSeries Limit(string id, string legend, IReadOnlyList<VisualizationPoint> points, double y)
{
double end = points.Count == 0 ? 0d : points[points.Count - 1].X;
return new VisualizationSeries(id, legend, VisualizationLineStyle.Limit,
new[] { new VisualizationPoint(0d, y), new VisualizationPoint(end, y) });
}
private static VisualizationChart Chart(string id, string title, string xAxis, string yAxis,
IReadOnlyList<VisualizationPoint> points, string note)
{
return new VisualizationChart(id, title, xAxis, yAxis,
new[] { new VisualizationSeries(id + "-current", "当前轨迹", VisualizationLineStyle.Solid, points) }, note);
}
private static IReadOnlyList<VisualizationPoint> Points(EmTrajectory trajectory, Func<EmTrajectoryPoint, double> y)
{
var points = new List<VisualizationPoint>();
foreach (EmTrajectoryPoint point in trajectory.Points)
points.Add(new VisualizationPoint(point.TimeFromStart, y(point)));
return points;
}
private static IReadOnlyList<VisualizationPoint> PointsBySegmentS(EmTrajectory trajectory)
{
var points = new List<VisualizationPoint>();
foreach (EmTrajectoryPoint point in trajectory.Points)
points.Add(new VisualizationPoint(point.SegmentLocalS, point.VehicleCurvature));
return points;
}
private static LsBuildResult BuildLs(EmTrajectory trajectory, DirectionSegmentView segment,
double maximumProjectionDistanceMeters)
{
var points = new List<VisualizationPoint>();
var projector = new FrenetProjector();
double seed = 0d;
int failures = 0;
foreach (EmTrajectoryPoint point in trajectory.Points)
{
if (!projector.TryProject(new Pose2D(point.X, point.Y, point.Yaw), segment, 0d, segment.LengthMeters,
maximumProjectionDistanceMeters, seed, out FrenetProjection projection))
{
failures++;
continue;
}
points.Add(new VisualizationPoint(segment.SourceStartArcLength + projection.ReferenceS,
projection.LateralOffset));
seed = projection.ReferenceS;
}
return new LsBuildResult(points, failures);
}
private sealed class LsBuildResult
{
public LsBuildResult(IReadOnlyList<VisualizationPoint> points, int projectionFailureCount)
{
Points = points;
ProjectionFailureCount = projectionFailureCount;
}
public IReadOnlyList<VisualizationPoint> Points { get; }
public int ProjectionFailureCount { get; }
}
}
ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationStaticSnapshotBuilder.cs
+120 -20
View File
@@ -1,6 +1,8 @@
using System;
using System.Collections.Generic;
using System.Globalization;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
using MultiWheelC.TrajectoryPlanning.EMPlanner;
using TrajectoryPlanningVisualization;
@@ -19,7 +21,7 @@ public sealed class TrajectoryObservationStaticSnapshotBuilder
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));
CreateConfiguration(effectiveConfiguration, settings, bootstrap.Vehicle, map, obstacleSnapshotVersion));
}
private static VisualizationOccupancyGrid CreateGrid(Mapping.PlanningGridMap map, VisualizationBounds bounds)
@@ -27,7 +29,11 @@ public sealed class TrajectoryObservationStaticSnapshotBuilder
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));
if (map.IsOccupied(row, column))
{
int bitIndex = row * map.Cols + column;
bits[bitIndex >> 3] |= (byte)(1 << (bitIndex & 7));
}
return new VisualizationOccupancyGrid(bounds, map.ResolutionMeters, map.Rows, map.Cols, bits);
}
@@ -35,11 +41,15 @@ public sealed class TrajectoryObservationStaticSnapshotBuilder
{
var lines = new List<VisualizationPolyline>();
var coarse = new List<VisualizationPoint>();
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));
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<VisualizationPoint>();
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));
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;
}
@@ -62,7 +72,8 @@ public sealed class TrajectoryObservationStaticSnapshotBuilder
foreach (DirectionSegmentView segment in bootstrap.Segments)
{
var points = new List<VisualizationPoint>();
foreach (var point in segment.Points) points.Add(new VisualizationPoint(point.X, point.Y));
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));
@@ -71,23 +82,112 @@ public sealed class TrajectoryObservationStaticSnapshotBuilder
}
private static IReadOnlyList<VisualizationConfigurationGroup> CreateConfiguration(EmPlannerConfiguration configuration,
TrajectoryObservationSettings settings, Mapping.PlanningGridMap map, long obstacleSnapshotVersion)
TrajectoryObservationSettings settings, VehicleParameters vehicle, Mapping.PlanningGridMap map,
long obstacleSnapshotVersion)
{
string F(double value) => value.ToString("R", CultureInfo.InvariantCulture);
var groups = new List<VisualizationConfigurationGroup>();
groups.Add(Group("调度", V("重规划周期", "ReplanPeriodSeconds", F(configuration.Scheduling.ReplanPeriodSeconds), "s"),
V("时域", "TimeHorizonSeconds", F(configuration.Scheduling.TimeHorizonSeconds), "s"),
string I(int value) => value.ToString(CultureInfo.InvariantCulture);
string L(long value) => value.ToString(CultureInfo.InvariantCulture);
string B(bool value) => value ? "true" : "false";
string N(double? value) => value.HasValue ? F(value.Value) : "不可用";
return new List<VisualizationConfigurationGroup>
{
Group("调度",
V("重规划周期", "ReplanPeriodSeconds", F(configuration.Scheduling.ReplanPeriodSeconds), "s"),
V("ST 时域", "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;
V("求解超时", "SolverTimeoutSeconds", F(configuration.Scheduling.SolverTimeoutSeconds), "s"),
V("交接前视", "HandoffLookaheadSeconds", F(configuration.Scheduling.HandoffLookaheadSeconds), "s"),
V("状态最大年龄", "MaximumVehicleStateAgeSeconds", F(configuration.Scheduling.MaximumVehicleStateAgeSeconds), "s")),
Group("OSQP",
V("最大外层迭代", "MaximumOuterIterations", I(configuration.Solver.MaximumOuterIterations), ""),
V("最大 OSQP 迭代", "MaximumOsqpIterations", I(configuration.Solver.MaximumOsqpIterations), ""),
V("绝对容差", "AbsoluteTolerance", F(configuration.Solver.AbsoluteTolerance), ""),
V("相对容差", "RelativeTolerance", F(configuration.Solver.RelativeTolerance), ""),
V("严格残差容差", "StrictResidualTolerance", F(configuration.Solver.StrictResidualTolerance), ""),
V("Warm start", "WarmStart", B(configuration.Solver.WarmStart), ""),
V("Polish", "Polish", B(configuration.Solver.Polish), ""),
V("原生详细输出", "NativeVerbose", B(configuration.Solver.NativeVerbose), "")),
Group("车辆与安全",
V("车辆长度", "LengthMeters", F(vehicle.LengthMeters), "m"),
V("车辆宽度", "WidthMeters", F(vehicle.WidthMeters), "m"),
V("安全余量", "SafetyMarginMeters", F(vehicle.SafetyMarginMeters), "m"),
V("最大曲率", "MaximumCurvaturePerMeter", N(vehicle.MaximumCurvaturePerMeter), "1/m"),
V("最小转弯半径", "MinimumTurningRadiusMeters", N(vehicle.MinimumTurningRadiusMeters), "m")),
Group("纵向限制",
V("最大前进速度", "MaximumForwardSpeedMetersPerSecond", F(configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond), "m/s"),
V("最大倒车速度", "MaximumReverseSpeedMetersPerSecond", F(configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond), "m/s"),
V("最大加速度", "MaximumAccelerationMetersPerSecondSquared", F(configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared), "m/s²"),
V("最大减速度", "MaximumDecelerationMetersPerSecondSquared", F(configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared), "m/s²"),
V("最大加加速度", "MaximumJerkMetersPerSecondCubed", F(configuration.Longitudinal.MaximumJerkMetersPerSecondCubed), "m/s³"),
V("最大横向加速度", "MaximumLateralAccelerationMetersPerSecondSquared", F(configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared), "m/s²"),
V("最大曲率变化率", "MaximumCurvatureRatePerMeterPerSecond", F(configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond), "1/(m·s)"),
V("停车速度容差", "StopSpeedToleranceMetersPerSecond", F(configuration.Longitudinal.StopSpeedToleranceMetersPerSecond), "m/s"),
V("零速保持", "ZeroSpeedHoldSeconds", F(configuration.Longitudinal.ZeroSpeedHoldSeconds), "s")),
Group("ST 权重",
V("参考速度", "ReferenceSpeed", F(configuration.Longitudinal.Weights.ReferenceSpeed), ""),
V("加速度", "Acceleration", F(configuration.Longitudinal.Weights.Acceleration), ""),
V("加加速度", "Jerk", F(configuration.Longitudinal.Weights.Jerk), ""),
V("上一轨迹", "PreviousTrajectory", F(configuration.Longitudinal.Weights.PreviousTrajectory), ""),
V("终端加速度", "TerminalAcceleration", F(configuration.Longitudinal.Weights.TerminalAcceleration), "")),
Group("横向限制",
V("单次横向步长", "MaximumLateralStepPerIterationMeters", F(configuration.Lateral.MaximumLateralStepPerIterationMeters), "m"),
V("最大横向斜率", "MaximumLateralSlope", F(configuration.Lateral.MaximumLateralSlope), ""),
V("最大二阶导", "MaximumLateralSecondDerivativePerMeter", F(configuration.Lateral.MaximumLateralSecondDerivativePerMeter), "1/m"),
V("最大三阶导", "MaximumLateralThirdDerivativePerSquareMeter", F(configuration.Lateral.MaximumLateralThirdDerivativePerSquareMeter), "1/m²")),
Group("LS 权重",
V("参考偏移", "ReferenceOffset", F(configuration.Lateral.Weights.ReferenceOffset), ""),
V("航向偏差", "HeadingDeviation", F(configuration.Lateral.Weights.HeadingDeviation), ""),
V("二阶导", "SecondDerivative", F(configuration.Lateral.Weights.SecondDerivative), ""),
V("三阶导", "ThirdDerivative", F(configuration.Lateral.Weights.ThirdDerivative), ""),
V("曲率", "Curvature", F(configuration.Lateral.Weights.Curvature), ""),
V("曲率变化", "CurvatureVariation", F(configuration.Lateral.Weights.CurvatureVariation), ""),
V("上一轨迹", "PreviousTrajectory", F(configuration.Lateral.Weights.PreviousTrajectory), ""),
V("滚动终端", "RollingTerminal", F(configuration.Lateral.Weights.RollingTerminal), "")),
Group("走廊与投影",
V("纵向采样间隔", "LongitudinalSampleSpacingMeters", F(configuration.Corridor.LongitudinalSampleSpacingMeters), "m"),
V("横向采样间隔", "LateralSampleSpacingMeters", F(configuration.Corridor.LateralSampleSpacingMeters), "m"),
V("最大横向偏移", "MaximumLateralOffsetMeters", F(configuration.Corridor.MaximumLateralOffsetMeters), "m"),
V("附加净空储备", "AdditionalClearanceReserveMeters", F(configuration.Corridor.AdditionalClearanceReserveMeters), "m"),
V("最大碰撞检查步长", "MaximumCollisionCheckStepMeters", F(configuration.Corridor.MaximumCollisionCheckStepMeters), "m"),
V("最大投影距离", "MaximumProjectionDistanceMeters", F(configuration.Frenet.MaximumProjectionDistanceMeters), "m"),
V("最小 Frenet 分母", "MinimumFrenetDenominator", F(configuration.Frenet.MinimumFrenetDenominator), ""),
V("边界锚点容差", "BoundaryAnchorToleranceMeters", F(configuration.Frenet.BoundaryAnchorToleranceMeters), "m"),
V("空间容差", "SpatialToleranceMeters", F(configuration.Validation.SpatialToleranceMeters), "m"),
V("运动学容差", "KinematicTolerance", F(configuration.Validation.KinematicTolerance), "")),
Group("地图",
V("障碍快照版本", "ObstacleSnapshotVersion", L(obstacleSnapshotVersion), ""),
V("地图快照编号", "MapSnapshotId", L(map.SnapshotId), ""),
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"),
V("行数", "Rows", I(map.Rows), ""),
V("列数", "Cols", I(map.Cols), "")),
Group("可视化与换向确认",
V("启用网页", "EnableWebVisualization", B(settings.EnableWebVisualization), ""),
V("自动打开网页", "AutoOpenWebVisualization", B(settings.AutoOpenWebVisualization), ""),
V("网页端口", "WebVisualizationPort", I(settings.WebVisualizationPort), ""),
V("网页刷新率", "WebRefreshRateHz", F(settings.WebRefreshRateHz), "Hz"),
V("历史容量", "VisualizationHistoryCycleLimit", I(settings.VisualizationHistoryCycleLimit), "cycles"),
V("启用 Painter", "EnableNativePainterVisualization", B(settings.EnableNativePainterVisualization), ""),
V("方向确认速度", "DirectionConfirmationSpeedMetersPerSecond", F(settings.DirectionConfirmationSpeedMetersPerSecond), "m/s"),
V("方向确认样本", "DirectionConfirmationSamples", I(settings.DirectionConfirmationSamples), ""),
V("换向投影容差", "GearSwitchProjectionToleranceMeters", F(settings.GearSwitchProjectionToleranceMeters), "m"),
V("换向停车保持", "GearSwitchStopHoldSeconds", F(settings.GearSwitchStopHoldSeconds), "s")),
};
}
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");
private static VisualizationConfigurationGroup Group(string title, params VisualizationValue[] values)
{
return new VisualizationConfigurationGroup(title, values);
}
private static VisualizationValue V(string chinese, string raw, string value, string unit)
{
return new VisualizationValue(chinese, raw, value, unit, "normal");
}
}
ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationVisualizationChecks.cs
+364 -6
View File
@@ -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);
}
}
}