fix: complete EM observation visualization contracts
This commit is contained in:
梁薄云
+93
-13
@@ -1,5 +1,6 @@
|
|||||||
using System;
|
using System;
|
||||||
using System.Collections.Generic;
|
using System.Collections.Generic;
|
||||||
|
using System.Globalization;
|
||||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||||
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||||
using TrajectoryPlanningVisualization;
|
using TrajectoryPlanningVisualization;
|
||||||
@@ -10,30 +11,109 @@ public sealed class TrajectoryObservationDynamicSnapshotBuilder
|
|||||||
{
|
{
|
||||||
private readonly TrajectoryObservationKinematicChartBuilder charts = new TrajectoryObservationKinematicChartBuilder();
|
private readonly TrajectoryObservationKinematicChartBuilder charts = new TrajectoryObservationKinematicChartBuilder();
|
||||||
private readonly TrajectoryObservationHandoffAnalyzer handoff = new TrajectoryObservationHandoffAnalyzer();
|
private readonly TrajectoryObservationHandoffAnalyzer handoff = new TrajectoryObservationHandoffAnalyzer();
|
||||||
|
|
||||||
public PlanningVisualizationDynamicSnapshot Build(long sequence, TrajectoryObservationLoopTick tick,
|
public PlanningVisualizationDynamicSnapshot Build(long sequence, TrajectoryObservationLoopTick tick,
|
||||||
DirectionSegmentView activeSegment, EmTrajectory previousTrajectoryForVisualization, EmPlannerConfiguration configuration)
|
DirectionSegmentView activeSegment, EmTrajectory previousTrajectoryForVisualization, EmPlannerConfiguration configuration)
|
||||||
{
|
{
|
||||||
if (tick == null) throw new ArgumentNullException(nameof(tick));
|
if (tick == null) throw new ArgumentNullException(nameof(tick));
|
||||||
if (activeSegment == null) throw new ArgumentNullException(nameof(activeSegment));
|
if (activeSegment == null) throw new ArgumentNullException(nameof(activeSegment));
|
||||||
if (configuration == null) throw new ArgumentNullException(nameof(configuration));
|
if (configuration == null) throw new ArgumentNullException(nameof(configuration));
|
||||||
|
|
||||||
EmTrajectory current = tick.Observation.PublishedTrajectory;
|
EmTrajectory current = tick.Observation.PublishedTrajectory;
|
||||||
var polylines = new List<VisualizationPolyline>();
|
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);
|
AddTrajectory(polylines, "current", "当前轨迹", current, VisualizationLineStyle.Solid);
|
||||||
var status = new List<VisualizationValue>();
|
AddTrajectory(polylines, "current-horizon", "当前规划视界", current, VisualizationLineStyle.Solid);
|
||||||
status.Add(Value("活动段", "ActiveSegmentIndex", activeSegment.SegmentIndex.ToString(), "", "normal"));
|
|
||||||
status.Add(Value("方向", "ActiveDirection", activeSegment.Direction.ToString(), "", "normal"));
|
IReadOnlyList<VisualizationChart> chartList = current == null
|
||||||
IReadOnlyList<VisualizationChart> chartList = current == null ? Array.Empty<VisualizationChart>() : charts.Build(current, activeSegment, configuration);
|
? 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);
|
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];
|
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;
|
var cycle = tick.LatestCycle;
|
||||||
return new PlanningVisualizationDynamicSnapshot(sequence, tick.Observation.ObservedAtUtc, "OBSERVE_ONLY", activeSegment.SegmentIndex, activeSegment.Direction.ToString(),
|
return new PlanningVisualizationDynamicSnapshot(sequence, tick.Observation.ObservedAtUtc, "OBSERVE_ONLY",
|
||||||
new VisualizationPose(tick.Observation.VehicleState.Pose.X, tick.Observation.VehicleState.Pose.Y, tick.Observation.VehicleState.Pose.Heading), polylines, Array.Empty<VisualizationMarker>(), chartList, status,
|
activeSegment.SegmentIndex, activeSegment.Direction.ToString(), new VisualizationPose(
|
||||||
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 ?? ""));
|
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):"不可用";
|
|
||||||
}
|
}
|
||||||
|
|||||||
+134
-11
@@ -1,5 +1,7 @@
|
|||||||
using System;
|
using System;
|
||||||
using System.Collections.Generic;
|
using System.Collections.Generic;
|
||||||
|
using System.Globalization;
|
||||||
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||||
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||||
using TrajectoryPlanningVisualization;
|
using TrajectoryPlanningVisualization;
|
||||||
|
|
||||||
@@ -13,21 +15,142 @@ public sealed class TrajectoryObservationKinematicChartBuilder
|
|||||||
if (trajectory == null) throw new ArgumentNullException(nameof(trajectory));
|
if (trajectory == null) throw new ArgumentNullException(nameof(trajectory));
|
||||||
if (segment == null) throw new ArgumentNullException(nameof(segment));
|
if (segment == null) throw new ArgumentNullException(nameof(segment));
|
||||||
if (configuration == null) throw new ArgumentNullException(nameof(configuration));
|
if (configuration == null) throw new ArgumentNullException(nameof(configuration));
|
||||||
|
|
||||||
|
LsBuildResult ls = BuildLs(trajectory, segment, configuration.Frenet.MaximumProjectionDistanceMeters);
|
||||||
return new[]
|
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("st", "时空轨迹", "t (s)", "PathS (m)", Points(trajectory, p => p.PathS), ""),
|
||||||
Chart("curvature-s", "曲率-距离", "s (m)", "κ (m⁻¹)", Points(trajectory, p => p.VehicleCurvature), ""),
|
WithSymmetricLimit("curvature-s", "曲率-距离", "s (m)", "κ (m⁻¹)",
|
||||||
Chart("curvature-t", "曲率-时间", "t (s)", "κ (m⁻¹)", Points(trajectory, p => p.VehicleCurvature), ""),
|
PointsBySegmentS(trajectory), configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond),
|
||||||
WithLimit("velocity-t", "速度", "t (s)", "v (m/s)", Points(trajectory, p => p.SignedLongitudinalVelocity), configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond),
|
WithSymmetricLimit("curvature-t", "曲率-时间", "t (s)", "κ (m⁻¹)",
|
||||||
WithLimit("acceleration-t", "加速度", "t (s)", "a (m/s²)", Points(trajectory, p => p.LongitudinalAcceleration), configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared),
|
Points(trajectory, p => p.VehicleCurvature), configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond),
|
||||||
|
Velocity(trajectory, configuration),
|
||||||
|
Acceleration(trajectory, configuration),
|
||||||
Jerk(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) }) }, "末点后无时间区间"); }
|
public int CountProjectionFailures(EmTrajectory trajectory, DirectionSegmentView segment,
|
||||||
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);
|
EmPlannerConfiguration configuration)
|
||||||
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; }
|
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; }
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
+120
-20
@@ -1,6 +1,8 @@
|
|||||||
using System;
|
using System;
|
||||||
using System.Collections.Generic;
|
using System.Collections.Generic;
|
||||||
using System.Globalization;
|
using System.Globalization;
|
||||||
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||||
|
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||||||
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||||
using TrajectoryPlanningVisualization;
|
using TrajectoryPlanningVisualization;
|
||||||
|
|
||||||
@@ -19,7 +21,7 @@ public sealed class TrajectoryObservationStaticSnapshotBuilder
|
|||||||
map.Bounds.YMin / 1000d, map.Bounds.YMax / 1000d);
|
map.Bounds.YMin / 1000d, map.Bounds.YMax / 1000d);
|
||||||
return new PlanningVisualizationStaticSnapshot("EM轨迹规划观察", bounds, CreateGrid(map, bounds),
|
return new PlanningVisualizationStaticSnapshot("EM轨迹规划观察", bounds, CreateGrid(map, bounds),
|
||||||
CreatePolylines(bootstrap), CreateMarkers(bootstrap), CreateSegments(bootstrap),
|
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)
|
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];
|
var bits = new byte[(map.Rows * map.Cols + 7) / 8];
|
||||||
for (int row = 0; row < map.Rows; row++)
|
for (int row = 0; row < map.Rows; row++)
|
||||||
for (int column = 0; column < map.Cols; column++)
|
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);
|
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 lines = new List<VisualizationPolyline>();
|
||||||
var coarse = new List<VisualizationPoint>();
|
var coarse = new List<VisualizationPoint>();
|
||||||
foreach (var point in bootstrap.CoarseResult.PlanningResult.Path) coarse.Add(new VisualizationPoint(point.X, point.Y));
|
foreach (var point in bootstrap.CoarseResult.PlanningResult.Path)
|
||||||
if (coarse.Count > 0) lines.Add(new VisualizationPolyline("coarse-path", "粗路径", "coarse", VisualizationLineStyle.Dashed, coarse));
|
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>();
|
var smooth = new List<VisualizationPoint>();
|
||||||
foreach (var point in bootstrap.SmoothedPath.Path) smooth.Add(new VisualizationPoint(point.X, point.Y));
|
foreach (var point in bootstrap.SmoothedPath.Path)
|
||||||
if (smooth.Count > 0) lines.Add(new VisualizationPolyline("local-g2-path", "Local G2路径", "local-g2", VisualizationLineStyle.Solid, smooth));
|
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;
|
return lines;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -62,7 +72,8 @@ public sealed class TrajectoryObservationStaticSnapshotBuilder
|
|||||||
foreach (DirectionSegmentView segment in bootstrap.Segments)
|
foreach (DirectionSegmentView segment in bootstrap.Segments)
|
||||||
{
|
{
|
||||||
var points = new List<VisualizationPoint>();
|
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(),
|
result.Add(new VisualizationDirectionSegment(segment.SegmentIndex, segment.Direction.ToString(),
|
||||||
segment.StartBoundary.BoundaryType == EmBoundaryType.GearSwitchDeparture,
|
segment.StartBoundary.BoundaryType == EmBoundaryType.GearSwitchDeparture,
|
||||||
segment.EndBoundary.BoundaryType == EmBoundaryType.GearSwitchApproach, points));
|
segment.EndBoundary.BoundaryType == EmBoundaryType.GearSwitchApproach, points));
|
||||||
@@ -71,23 +82,112 @@ public sealed class TrajectoryObservationStaticSnapshotBuilder
|
|||||||
}
|
}
|
||||||
|
|
||||||
private static IReadOnlyList<VisualizationConfigurationGroup> CreateConfiguration(EmPlannerConfiguration configuration,
|
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);
|
string F(double value) => value.ToString("R", CultureInfo.InvariantCulture);
|
||||||
var groups = new List<VisualizationConfigurationGroup>();
|
string I(int value) => value.ToString(CultureInfo.InvariantCulture);
|
||||||
groups.Add(Group("调度", V("重规划周期", "ReplanPeriodSeconds", F(configuration.Scheduling.ReplanPeriodSeconds), "s"),
|
string L(long value) => value.ToString(CultureInfo.InvariantCulture);
|
||||||
V("时域", "TimeHorizonSeconds", F(configuration.Scheduling.TimeHorizonSeconds), "s"),
|
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("距离视界", "DistanceHorizonMeters", F(configuration.Scheduling.DistanceHorizonMeters), "m"),
|
||||||
V("输出步长", "OutputTimeStepSeconds", F(configuration.Scheduling.OutputTimeStepSeconds), "s"),
|
V("输出步长", "OutputTimeStepSeconds", F(configuration.Scheduling.OutputTimeStepSeconds), "s"),
|
||||||
V("求解超时", "SolverTimeoutSeconds", F(configuration.Scheduling.SolverTimeoutSeconds), "s")));
|
V("求解超时", "SolverTimeoutSeconds", F(configuration.Scheduling.SolverTimeoutSeconds), "s"),
|
||||||
groups.Add(Group("OSQP", V("最大外层迭代", "MaximumOuterIterations", configuration.Solver.MaximumOuterIterations.ToString(CultureInfo.InvariantCulture), ""),
|
V("交接前视", "HandoffLookaheadSeconds", F(configuration.Scheduling.HandoffLookaheadSeconds), "s"),
|
||||||
V("最大OSQP迭代", "MaximumOsqpIterations", configuration.Solver.MaximumOsqpIterations.ToString(CultureInfo.InvariantCulture), ""),
|
V("状态最大年龄", "MaximumVehicleStateAgeSeconds", F(configuration.Scheduling.MaximumVehicleStateAgeSeconds), "s")),
|
||||||
V("绝对容差", "AbsoluteTolerance", F(configuration.Solver.AbsoluteTolerance), ""), V("相对容差", "RelativeTolerance", F(configuration.Solver.RelativeTolerance), ""), V("严格残差容差", "StrictResidualTolerance", F(configuration.Solver.StrictResidualTolerance), "")));
|
Group("OSQP",
|
||||||
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")));
|
V("最大外层迭代", "MaximumOuterIterations", I(configuration.Solver.MaximumOuterIterations), ""),
|
||||||
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), "")));
|
V("最大 OSQP 迭代", "MaximumOsqpIterations", I(configuration.Solver.MaximumOsqpIterations), ""),
|
||||||
return groups;
|
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 VisualizationConfigurationGroup Group(string title, params VisualizationValue[] values)
|
||||||
private static VisualizationValue V(string chinese, string raw, string value, string unit) => new VisualizationValue(chinese, raw, value, unit, "normal");
|
{
|
||||||
|
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");
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
+364
-6
@@ -1,4 +1,14 @@
|
|||||||
|
using System;
|
||||||
|
using System.Collections.Generic;
|
||||||
|
using System.Globalization;
|
||||||
|
using System.Linq;
|
||||||
|
using System.Reflection;
|
||||||
|
using System.Threading;
|
||||||
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||||
|
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||||||
|
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||||
using MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
using MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
||||||
|
using TrajectoryPlanningVisualization;
|
||||||
|
|
||||||
namespace EMPlannerVerificationHost;
|
namespace EMPlannerVerificationHost;
|
||||||
|
|
||||||
@@ -6,11 +16,359 @@ internal static class TrajectoryObservationVisualizationChecks
|
|||||||
{
|
{
|
||||||
public static void Run()
|
public static void Run()
|
||||||
{
|
{
|
||||||
var staticBuilder = new TrajectoryObservationStaticSnapshotBuilder();
|
VerifiesStaticSnapshotExportsFrozenConfigurationAndGeometry();
|
||||||
var chartBuilder = new TrajectoryObservationKinematicChartBuilder();
|
VerifiesKinematicChartsKeepKnotAndIntervalSemantics();
|
||||||
var dynamicBuilder = new TrajectoryObservationDynamicSnapshotBuilder();
|
VerifiesHandoffUsesSharedReferenceSAndSurvivesProjectionFailure();
|
||||||
var handoffAnalyzer = new TrajectoryObservationHandoffAnalyzer();
|
VerifiesDynamicSnapshotContainsObservationEvidenceWithoutCyclePoints();
|
||||||
Verification.True(staticBuilder != null && chartBuilder != null && dynamicBuilder != null && handoffAnalyzer != null,
|
}
|
||||||
"observation visualization adapters are available");
|
|
||||||
|
private static void VerifiesStaticSnapshotExportsFrozenConfigurationAndGeometry()
|
||||||
|
{
|
||||||
|
var settings = new TrajectoryObservationSettings();
|
||||||
|
TrajectoryObservationBootstrapResult bootstrap = Bootstrap(settings);
|
||||||
|
var controller = new TrajectoryObservationController(bootstrap, settings,
|
||||||
|
new FixedPlanningService(CreateTrajectory("static", DateTimeOffset.UnixEpoch, 2, EmTerminalType.Goal,
|
||||||
|
EmLongitudinalMode.RollingContinuation, TravelDirection.Forward)), "static-configuration");
|
||||||
|
EmPlannerConfiguration effective = ReadEffectiveConfiguration(controller);
|
||||||
|
settings.TimeHorizonSeconds = 99d;
|
||||||
|
|
||||||
|
PlanningVisualizationStaticSnapshot snapshot = new TrajectoryObservationStaticSnapshotBuilder()
|
||||||
|
.Build(bootstrap, effective, settings.CreateValidatedSnapshot(), 17L);
|
||||||
|
|
||||||
|
Verification.Equal(10, snapshot.ConfigurationGroups.Count, "static configuration group count");
|
||||||
|
Verification.Equal("调度", snapshot.ConfigurationGroups[0].ChineseTitle, "static configuration group 0");
|
||||||
|
Verification.Equal("OSQP", snapshot.ConfigurationGroups[1].ChineseTitle, "static configuration group 1");
|
||||||
|
Verification.Equal("车辆与安全", snapshot.ConfigurationGroups[2].ChineseTitle, "static configuration group 2");
|
||||||
|
Verification.Equal("纵向限制", snapshot.ConfigurationGroups[3].ChineseTitle, "static configuration group 3");
|
||||||
|
Verification.Equal("ST 权重", snapshot.ConfigurationGroups[4].ChineseTitle, "static configuration group 4");
|
||||||
|
Verification.Equal("横向限制", snapshot.ConfigurationGroups[5].ChineseTitle, "static configuration group 5");
|
||||||
|
Verification.Equal("LS 权重", snapshot.ConfigurationGroups[6].ChineseTitle, "static configuration group 6");
|
||||||
|
Verification.Equal("走廊与投影", snapshot.ConfigurationGroups[7].ChineseTitle, "static configuration group 7");
|
||||||
|
Verification.Equal("地图", snapshot.ConfigurationGroups[8].ChineseTitle, "static configuration group 8");
|
||||||
|
Verification.Equal("可视化与换向确认", snapshot.ConfigurationGroups[9].ChineseTitle, "static configuration group 9");
|
||||||
|
|
||||||
|
IReadOnlyDictionary<string, VisualizationValue> values = Flatten(snapshot);
|
||||||
|
AssertValue(values, "TimeHorizonSeconds", "2", "effective configuration is frozen before editable settings change");
|
||||||
|
AssertValue(values, "MaximumOsqpIterations", "100000", "effective OSQP iteration limit");
|
||||||
|
AssertEffectiveValues(values, effective, settings, bootstrap);
|
||||||
|
foreach (string rawName in new[]
|
||||||
|
{
|
||||||
|
"AbsoluteTolerance", "RelativeTolerance", "StrictResidualTolerance", "WarmStart", "Polish", "NativeVerbose",
|
||||||
|
"LengthMeters", "WidthMeters", "SafetyMarginMeters", "MaximumCurvaturePerMeter",
|
||||||
|
"MaximumForwardSpeedMetersPerSecond", "MaximumReverseSpeedMetersPerSecond",
|
||||||
|
"MaximumAccelerationMetersPerSecondSquared", "MaximumDecelerationMetersPerSecondSquared",
|
||||||
|
"MaximumJerkMetersPerSecondCubed", "MaximumLateralAccelerationMetersPerSecondSquared",
|
||||||
|
"MaximumCurvatureRatePerMeterPerSecond", "StopSpeedToleranceMetersPerSecond", "ZeroSpeedHoldSeconds",
|
||||||
|
"ReferenceSpeed", "Acceleration", "Jerk", "PreviousTrajectory", "TerminalAcceleration",
|
||||||
|
"ReferenceOffset", "HeadingDeviation", "SecondDerivative", "ThirdDerivative", "Curvature",
|
||||||
|
"CurvatureVariation", "RollingTerminal", "LongitudinalSampleSpacingMeters",
|
||||||
|
"LateralSampleSpacingMeters", "MaximumLateralOffsetMeters", "AdditionalClearanceReserveMeters",
|
||||||
|
"MaximumCollisionCheckStepMeters", "MaximumProjectionDistanceMeters", "MinimumFrenetDenominator",
|
||||||
|
"BoundaryAnchorToleranceMeters", "SpatialToleranceMeters", "KinematicTolerance", "ObstacleSnapshotVersion",
|
||||||
|
"ResolutionMm", "Bounds.XMin", "Bounds.XMax", "Bounds.YMin", "Bounds.YMax", "Rows", "Cols",
|
||||||
|
"EnableWebVisualization", "AutoOpenWebVisualization", "WebVisualizationPort", "WebRefreshRateHz",
|
||||||
|
"VisualizationHistoryCycleLimit", "EnableNativePainterVisualization",
|
||||||
|
"DirectionConfirmationSpeedMetersPerSecond", "DirectionConfirmationSamples",
|
||||||
|
"GearSwitchProjectionToleranceMeters", "GearSwitchStopHoldSeconds",
|
||||||
|
})
|
||||||
|
Verification.True(values.ContainsKey(rawName), "static configuration exports " + rawName);
|
||||||
|
|
||||||
|
Verification.Equal(bootstrap.Segments.Count, snapshot.DirectionSegments.Count, "all direction segments exported");
|
||||||
|
Verification.True(snapshot.StaticPolylines.Any(x => x.Id == "coarse-path"), "coarse path exported");
|
||||||
|
Verification.True(snapshot.StaticPolylines.Any(x => x.Id == "local-g2-path"), "full Local G2 path exported");
|
||||||
|
byte[] occupancy = Convert.FromBase64String(snapshot.OccupancyGrid.OccupancyBitsBase64);
|
||||||
|
Verification.Equal((bootstrap.Map.Rows * bootstrap.Map.Cols + 7) / 8, occupancy.Length,
|
||||||
|
"occupancy bitset compact length");
|
||||||
|
bool verifiedOccupiedCell = false;
|
||||||
|
for (int row = 0; row < bootstrap.Map.Rows; row++)
|
||||||
|
for (int col = 0; col < bootstrap.Map.Cols; col++)
|
||||||
|
if (bootstrap.Map.IsOccupied(row, col))
|
||||||
|
{
|
||||||
|
int bit = row * bootstrap.Map.Cols + col;
|
||||||
|
Verification.True((occupancy[bit >> 3] & (1 << (bit & 7))) != 0,
|
||||||
|
"occupancy uses row-major LSB-first bitset");
|
||||||
|
verifiedOccupiedCell = true;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
Verification.True(verifiedOccupiedCell, "static fixture has an occupied cell");
|
||||||
|
}
|
||||||
|
|
||||||
|
private static void VerifiesKinematicChartsKeepKnotAndIntervalSemantics()
|
||||||
|
{
|
||||||
|
DirectionSegmentView segment = CreateStraightSegment(0, 50d);
|
||||||
|
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||||
|
EmTrajectory trajectory = CreateTrajectory("rolling", DateTimeOffset.UnixEpoch, 21,
|
||||||
|
EmTerminalType.Goal, EmLongitudinalMode.RollingContinuation, TravelDirection.Forward);
|
||||||
|
|
||||||
|
IReadOnlyList<VisualizationChart> charts = new TrajectoryObservationKinematicChartBuilder()
|
||||||
|
.Build(trajectory, segment, configuration);
|
||||||
|
|
||||||
|
Verification.Equal(8, charts.Count, "all eight kinematic charts exported");
|
||||||
|
Verification.Equal(20, FindChart(charts, "jerk-t").Series[0].Points.Count, "jerk has N-1 intervals");
|
||||||
|
Verification.True(FindChart(charts, "jerk-t").NoteChinese.Contains("末点后无时间区间"),
|
||||||
|
"jerk explains no terminal successor interval");
|
||||||
|
Verification.Equal(21, FindChart(charts, "velocity-t").Series[0].Points.Count, "velocity remains knot based");
|
||||||
|
Verification.Equal("ReferenceS (m)", FindChart(charts, "ls").XAxisLabel, "LS uses shared reference station");
|
||||||
|
Verification.Equal("PathS (m)", FindChart(charts, "st").YAxisLabel, "ST uses plan-local PathS");
|
||||||
|
foreach (string id in new[] { "curvature-s", "curvature-t", "velocity-t", "acceleration-t", "jerk-t" })
|
||||||
|
Verification.True(FindChart(charts, id).Series.Any(x => x.LineStyle == VisualizationLineStyle.Limit),
|
||||||
|
id + " has an effective limit line");
|
||||||
|
Verification.True(!FindChart(charts, "yaw-rate-t").Series.Any(x => x.LineStyle == VisualizationLineStyle.Limit),
|
||||||
|
"yaw rate has no invented limit");
|
||||||
|
EmTrajectory unprojectable = CreateTrajectory("unprojectable", DateTimeOffset.UnixEpoch, 21,
|
||||||
|
EmTerminalType.Goal, EmLongitudinalMode.RollingContinuation, TravelDirection.Forward, 100d);
|
||||||
|
Verification.True(FindChart(new TrajectoryObservationKinematicChartBuilder().Build(unprojectable, segment, configuration), "ls")
|
||||||
|
.NoteChinese.Contains("投影失败=21"), "LS chart reports every failed full-segment projection");
|
||||||
|
}
|
||||||
|
|
||||||
|
private static void VerifiesHandoffUsesSharedReferenceSAndSurvivesProjectionFailure()
|
||||||
|
{
|
||||||
|
DirectionSegmentView segment = CreateStraightSegment(0, 100d);
|
||||||
|
DateTimeOffset t0 = DateTimeOffset.UnixEpoch;
|
||||||
|
EmTrajectory previous = CreateTrajectory("previous", t0, 11, EmTerminalType.Goal,
|
||||||
|
EmLongitudinalMode.RollingContinuation, TravelDirection.Forward, 2d, 0d);
|
||||||
|
EmTrajectory current = CreateTrajectory("current", t0.AddSeconds(1d), 2, EmTerminalType.Goal,
|
||||||
|
EmLongitudinalMode.RollingContinuation, TravelDirection.Forward, 4d, 0d);
|
||||||
|
TrajectoryObservationHandoffMetrics metrics = new TrajectoryObservationHandoffAnalyzer().Analyze(current, previous, segment);
|
||||||
|
|
||||||
|
Verification.True(metrics.Available, "handoff samples absolute-time aligned trajectories");
|
||||||
|
Verification.NearlyEqual(1d, metrics.DeltaPositionMeters!.Value, "handoff world delta");
|
||||||
|
Verification.NearlyEqual(1d, metrics.DeltaReferenceSMeters!.Value, "handoff compares shared ReferenceS");
|
||||||
|
Verification.True(metrics.DeltaReferenceSMeters.Value != current.Points[0].PathS - previous.Points[0].PathS,
|
||||||
|
"handoff does not compare local PathS origins");
|
||||||
|
|
||||||
|
EmTrajectory offSegment = CreateTrajectory("off-segment", t0.AddSeconds(1d), 2, EmTerminalType.Goal,
|
||||||
|
EmLongitudinalMode.RollingContinuation, TravelDirection.Forward, 300d, 0d);
|
||||||
|
TrajectoryObservationHandoffMetrics unavailable = new TrajectoryObservationHandoffAnalyzer().Analyze(offSegment, previous, segment);
|
||||||
|
Verification.True(unavailable.Available, "handoff keeps non-projection deltas available");
|
||||||
|
Verification.True(!unavailable.DeltaReferenceSMeters.HasValue, "projection failure makes only ReferenceS unavailable");
|
||||||
|
Verification.True(unavailable.DeltaPositionMeters.HasValue && unavailable.DeltaVelocityMetersPerSecond.HasValue &&
|
||||||
|
unavailable.DeltaAccelerationMetersPerSecondSquared.HasValue, "projection failure preserves world and kinematic deltas");
|
||||||
|
}
|
||||||
|
|
||||||
|
private static void VerifiesDynamicSnapshotContainsObservationEvidenceWithoutCyclePoints()
|
||||||
|
{
|
||||||
|
var settings = new TrajectoryObservationSettings();
|
||||||
|
TrajectoryObservationBootstrapResult bootstrap = Bootstrap(settings);
|
||||||
|
DateTimeOffset now = DateTimeOffset.UnixEpoch.AddMinutes(1);
|
||||||
|
EmTrajectory current = CreateTrajectory("dynamic", now, 21, EmTerminalType.Goal,
|
||||||
|
EmLongitudinalMode.RollingContinuation, TravelDirection.Forward);
|
||||||
|
var controller = new TrajectoryObservationController(bootstrap, settings, new FixedPlanningService(current), "dynamic");
|
||||||
|
var loop = new TrajectoryObservationLoop(controller);
|
||||||
|
var state = new VehicleMotionState(new Pose2D(0d, 0d, 0d), 0.1d, null, now, 1L);
|
||||||
|
controller.StartCycle(now, state, CancellationToken.None).GetAwaiter().GetResult();
|
||||||
|
TrajectoryObservationLoopTick tick = loop.Tick(now.AddSeconds(settings.ObserverPeriodSeconds), state, CancellationToken.None);
|
||||||
|
EmPlannerConfiguration configuration = ReadEffectiveConfiguration(controller);
|
||||||
|
EmTrajectory previous = CreateTrajectory("dynamic-previous", now.AddSeconds(-1d), 21, EmTerminalType.Goal,
|
||||||
|
EmLongitudinalMode.RollingContinuation, TravelDirection.Forward);
|
||||||
|
|
||||||
|
PlanningVisualizationDynamicSnapshot snapshot = new TrajectoryObservationDynamicSnapshotBuilder()
|
||||||
|
.Build(5L, tick, controller.ActiveSegment, previous, configuration);
|
||||||
|
|
||||||
|
Verification.True(snapshot.DynamicPolylines.Any(x => x.Id == "current"), "dynamic snapshot contains current trajectory");
|
||||||
|
Verification.True(snapshot.DynamicPolylines.Any(x => x.Id == "previous"), "dynamic snapshot contains previous trajectory");
|
||||||
|
Verification.True(snapshot.DynamicPolylines.Any(x => x.Id == "active-segment"), "dynamic snapshot contains active segment highlight");
|
||||||
|
Verification.True(snapshot.DynamicPolylines.Any(x => x.Id == "current-horizon"), "dynamic snapshot contains projected horizon");
|
||||||
|
foreach (string rawName in new[] { "ActiveSegmentIndex", "ActiveDirection", "PlanningStatus", "LongitudinalMode",
|
||||||
|
"TerminalType", "PlanningElapsedMilliseconds", "TrajectoryAgeSeconds", "LsProjectionFailures",
|
||||||
|
"RollingTerminalConstraint", "TerminalVelocity", "TerminalAcceleration", "DeltaPositionMeters",
|
||||||
|
"DeltaReferenceSMeters", "DeltaVelocityMetersPerSecond", "DeltaAccelerationMetersPerSecondSquared",
|
||||||
|
"GearConfirmation" })
|
||||||
|
Verification.True(snapshot.StatusValues.Any(x => x.RawName == rawName), "dynamic status includes " + rawName);
|
||||||
|
Verification.True(snapshot.StatusValues.Single(x => x.RawName == "RollingTerminalConstraint").Value.Contains("滚动末端停车硬约束：未启用"),
|
||||||
|
"rolling status describes disabled exact-stop constraint");
|
||||||
|
Verification.NearlyEqual(current.Points[current.Points.Count - 1].SignedLongitudinalVelocity,
|
||||||
|
snapshot.CycleSummary.TerminalVelocity!.Value, "cycle summary uses actual terminal velocity");
|
||||||
|
Verification.NearlyEqual(ReadAcceleration(current.Points[current.Points.Count - 1]),
|
||||||
|
snapshot.CycleSummary.TerminalAcceleration!.Value, "cycle summary uses actual terminal acceleration");
|
||||||
|
Verification.True(!typeof(VisualizationCycleSummary).GetProperties().Any(x =>
|
||||||
|
x.PropertyType == typeof(EmTrajectory) || x.PropertyType == typeof(EmTrajectoryPoint) ||
|
||||||
|
(x.PropertyType.IsGenericType && x.PropertyType.GetGenericArguments().Any(t => t == typeof(EmTrajectoryPoint)))),
|
||||||
|
"cycle summary does not carry complete point arrays");
|
||||||
|
|
||||||
|
EmTrajectory exactStop = CreateTrajectory("exact-stop", now, 21, EmTerminalType.GearSwitch,
|
||||||
|
EmLongitudinalMode.ExactStopAtBoundary, TravelDirection.Forward);
|
||||||
|
var exactController = new TrajectoryObservationController(bootstrap, settings,
|
||||||
|
new FixedPlanningService(exactStop), "exact-stop");
|
||||||
|
exactController.StartCycle(now, state, CancellationToken.None).GetAwaiter().GetResult();
|
||||||
|
TrajectoryObservationLoopTick exactTick = new TrajectoryObservationLoop(exactController)
|
||||||
|
.Tick(now.AddSeconds(settings.ObserverPeriodSeconds), state, CancellationToken.None);
|
||||||
|
PlanningVisualizationDynamicSnapshot exactSnapshot = new TrajectoryObservationDynamicSnapshotBuilder().Build(6L,
|
||||||
|
exactTick, exactController.ActiveSegment, null, ReadEffectiveConfiguration(exactController));
|
||||||
|
Verification.True(exactSnapshot.StatusValues.Single(x => x.RawName == "RollingTerminalConstraint").Value.Contains("精确停车锚点"),
|
||||||
|
"exact-stop status describes the true stop anchor");
|
||||||
|
Verification.NearlyEqual(exactStop.Points[exactStop.Points.Count - 1].SignedLongitudinalVelocity,
|
||||||
|
exactSnapshot.CycleSummary.TerminalVelocity!.Value, "exact-stop terminal velocity is real final knot");
|
||||||
|
Verification.NearlyEqual(ReadAcceleration(exactStop.Points[exactStop.Points.Count - 1]),
|
||||||
|
exactSnapshot.CycleSummary.TerminalAcceleration!.Value, "exact-stop terminal acceleration is real final knot");
|
||||||
|
|
||||||
|
EmTrajectory offSegment = CreateTrajectory("dynamic-off-segment", now, 21, EmTerminalType.Goal,
|
||||||
|
EmLongitudinalMode.RollingContinuation, TravelDirection.Forward, 100d);
|
||||||
|
var offSegmentController = new TrajectoryObservationController(bootstrap, settings,
|
||||||
|
new FixedPlanningService(offSegment), "dynamic-off-segment");
|
||||||
|
offSegmentController.StartCycle(now, state, CancellationToken.None).GetAwaiter().GetResult();
|
||||||
|
TrajectoryObservationLoopTick offSegmentTick = new TrajectoryObservationLoop(offSegmentController)
|
||||||
|
.Tick(now.AddSeconds(settings.ObserverPeriodSeconds), state, CancellationToken.None);
|
||||||
|
PlanningVisualizationDynamicSnapshot recovered = new TrajectoryObservationDynamicSnapshotBuilder().Build(7L,
|
||||||
|
offSegmentTick, offSegmentController.ActiveSegment, null, ReadEffectiveConfiguration(offSegmentController));
|
||||||
|
Verification.Equal("21", recovered.StatusValues.Single(x => x.RawName == "LsProjectionFailures").Value,
|
||||||
|
"projection failure is reported while dynamic snapshot construction remains available");
|
||||||
|
}
|
||||||
|
|
||||||
|
private static TrajectoryObservationBootstrapResult Bootstrap(TrajectoryObservationSettings settings)
|
||||||
|
{
|
||||||
|
CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
|
||||||
|
new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings,
|
||||||
|
new[] { TrajectoryObservationObstacle.Circle(1500d, 1500d, 100d) }, 17L);
|
||||||
|
TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper().Bootstrap(job, CancellationToken.None);
|
||||||
|
Verification.True(bootstrap.Succeeded, "visualization bootstrap succeeds");
|
||||||
|
return bootstrap;
|
||||||
|
}
|
||||||
|
|
||||||
|
private static EmPlannerConfiguration ReadEffectiveConfiguration(TrajectoryObservationController controller)
|
||||||
|
{
|
||||||
|
MethodInfo method = typeof(TrajectoryObservationController).GetMethod("CreateEffectiveConfigurationSnapshot",
|
||||||
|
BindingFlags.Instance | BindingFlags.NonPublic)!;
|
||||||
|
return (EmPlannerConfiguration)method.Invoke(controller, Array.Empty<object>())!;
|
||||||
|
}
|
||||||
|
|
||||||
|
private static IReadOnlyDictionary<string, VisualizationValue> Flatten(PlanningVisualizationStaticSnapshot snapshot)
|
||||||
|
{
|
||||||
|
return snapshot.ConfigurationGroups.SelectMany(x => x.Entries)
|
||||||
|
.GroupBy(x => x.RawName).ToDictionary(x => x.Key, x => x.Last());
|
||||||
|
}
|
||||||
|
|
||||||
|
private static double ReadAcceleration(EmTrajectoryPoint point)
|
||||||
|
{
|
||||||
|
PropertyInfo property = typeof(EmTrajectoryPoint).GetProperty("LongitudinalAcceleration",
|
||||||
|
BindingFlags.Instance | BindingFlags.NonPublic)!;
|
||||||
|
return (double)property.GetValue(point)!;
|
||||||
|
}
|
||||||
|
|
||||||
|
private static void AssertValue(IReadOnlyDictionary<string, VisualizationValue> values, string rawName, string expected, string name)
|
||||||
|
{
|
||||||
|
Verification.Equal(expected, values[rawName].Value, name);
|
||||||
|
}
|
||||||
|
|
||||||
|
private static void AssertEffectiveValues(IReadOnlyDictionary<string, VisualizationValue> values,
|
||||||
|
EmPlannerConfiguration configuration, TrajectoryObservationSettings settings,
|
||||||
|
TrajectoryObservationBootstrapResult bootstrap)
|
||||||
|
{
|
||||||
|
string F(double value) => value.ToString("R", CultureInfo.InvariantCulture);
|
||||||
|
string I(int value) => value.ToString(CultureInfo.InvariantCulture);
|
||||||
|
string B(bool value) => value ? "true" : "false";
|
||||||
|
foreach ((string RawName, string Value) expected in new[]
|
||||||
|
{
|
||||||
|
("ReplanPeriodSeconds", F(configuration.Scheduling.ReplanPeriodSeconds)),
|
||||||
|
("TimeHorizonSeconds", F(configuration.Scheduling.TimeHorizonSeconds)),
|
||||||
|
("DistanceHorizonMeters", F(configuration.Scheduling.DistanceHorizonMeters)),
|
||||||
|
("OutputTimeStepSeconds", F(configuration.Scheduling.OutputTimeStepSeconds)),
|
||||||
|
("SolverTimeoutSeconds", F(configuration.Scheduling.SolverTimeoutSeconds)),
|
||||||
|
("HandoffLookaheadSeconds", F(configuration.Scheduling.HandoffLookaheadSeconds)),
|
||||||
|
("MaximumVehicleStateAgeSeconds", F(configuration.Scheduling.MaximumVehicleStateAgeSeconds)),
|
||||||
|
("MaximumOuterIterations", I(configuration.Solver.MaximumOuterIterations)),
|
||||||
|
("MaximumOsqpIterations", I(configuration.Solver.MaximumOsqpIterations)),
|
||||||
|
("AbsoluteTolerance", F(configuration.Solver.AbsoluteTolerance)),
|
||||||
|
("RelativeTolerance", F(configuration.Solver.RelativeTolerance)),
|
||||||
|
("StrictResidualTolerance", F(configuration.Solver.StrictResidualTolerance)),
|
||||||
|
("WarmStart", B(configuration.Solver.WarmStart)), ("Polish", B(configuration.Solver.Polish)),
|
||||||
|
("NativeVerbose", B(configuration.Solver.NativeVerbose)),
|
||||||
|
("MaximumForwardSpeedMetersPerSecond", F(configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond)),
|
||||||
|
("MaximumReverseSpeedMetersPerSecond", F(configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond)),
|
||||||
|
("MaximumAccelerationMetersPerSecondSquared", F(configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared)),
|
||||||
|
("MaximumDecelerationMetersPerSecondSquared", F(configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared)),
|
||||||
|
("MaximumJerkMetersPerSecondCubed", F(configuration.Longitudinal.MaximumJerkMetersPerSecondCubed)),
|
||||||
|
("MaximumLateralAccelerationMetersPerSecondSquared", F(configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared)),
|
||||||
|
("MaximumCurvatureRatePerMeterPerSecond", F(configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond)),
|
||||||
|
("StopSpeedToleranceMetersPerSecond", F(configuration.Longitudinal.StopSpeedToleranceMetersPerSecond)),
|
||||||
|
("ZeroSpeedHoldSeconds", F(configuration.Longitudinal.ZeroSpeedHoldSeconds)),
|
||||||
|
("ReferenceSpeed", F(configuration.Longitudinal.Weights.ReferenceSpeed)),
|
||||||
|
("Acceleration", F(configuration.Longitudinal.Weights.Acceleration)),
|
||||||
|
("Jerk", F(configuration.Longitudinal.Weights.Jerk)),
|
||||||
|
("TerminalAcceleration", F(configuration.Longitudinal.Weights.TerminalAcceleration)),
|
||||||
|
("ReferenceOffset", F(configuration.Lateral.Weights.ReferenceOffset)),
|
||||||
|
("HeadingDeviation", F(configuration.Lateral.Weights.HeadingDeviation)),
|
||||||
|
("SecondDerivative", F(configuration.Lateral.Weights.SecondDerivative)),
|
||||||
|
("ThirdDerivative", F(configuration.Lateral.Weights.ThirdDerivative)),
|
||||||
|
("Curvature", F(configuration.Lateral.Weights.Curvature)),
|
||||||
|
("CurvatureVariation", F(configuration.Lateral.Weights.CurvatureVariation)),
|
||||||
|
("RollingTerminal", F(configuration.Lateral.Weights.RollingTerminal)),
|
||||||
|
("MaximumLateralStepPerIterationMeters", F(configuration.Lateral.MaximumLateralStepPerIterationMeters)),
|
||||||
|
("MaximumLateralSlope", F(configuration.Lateral.MaximumLateralSlope)),
|
||||||
|
("MaximumLateralSecondDerivativePerMeter", F(configuration.Lateral.MaximumLateralSecondDerivativePerMeter)),
|
||||||
|
("MaximumLateralThirdDerivativePerSquareMeter", F(configuration.Lateral.MaximumLateralThirdDerivativePerSquareMeter)),
|
||||||
|
("LongitudinalSampleSpacingMeters", F(configuration.Corridor.LongitudinalSampleSpacingMeters)),
|
||||||
|
("LateralSampleSpacingMeters", F(configuration.Corridor.LateralSampleSpacingMeters)),
|
||||||
|
("MaximumLateralOffsetMeters", F(configuration.Corridor.MaximumLateralOffsetMeters)),
|
||||||
|
("AdditionalClearanceReserveMeters", F(configuration.Corridor.AdditionalClearanceReserveMeters)),
|
||||||
|
("MaximumCollisionCheckStepMeters", F(configuration.Corridor.MaximumCollisionCheckStepMeters)),
|
||||||
|
("MaximumProjectionDistanceMeters", F(configuration.Frenet.MaximumProjectionDistanceMeters)),
|
||||||
|
("MinimumFrenetDenominator", F(configuration.Frenet.MinimumFrenetDenominator)),
|
||||||
|
("BoundaryAnchorToleranceMeters", F(configuration.Frenet.BoundaryAnchorToleranceMeters)),
|
||||||
|
("SpatialToleranceMeters", F(configuration.Validation.SpatialToleranceMeters)),
|
||||||
|
("KinematicTolerance", F(configuration.Validation.KinematicTolerance)),
|
||||||
|
("LengthMeters", F(bootstrap.Vehicle.LengthMeters)), ("WidthMeters", F(bootstrap.Vehicle.WidthMeters)),
|
||||||
|
("SafetyMarginMeters", F(bootstrap.Vehicle.SafetyMarginMeters)),
|
||||||
|
("MaximumCurvaturePerMeter", F(bootstrap.Vehicle.MaximumCurvaturePerMeter!.Value)),
|
||||||
|
("ObstacleSnapshotVersion", "17"), ("ResolutionMm", F(bootstrap.Map.ResolutionMm)),
|
||||||
|
("Bounds.XMin", F(bootstrap.Map.Bounds.XMin)), ("Bounds.XMax", F(bootstrap.Map.Bounds.XMax)),
|
||||||
|
("Bounds.YMin", F(bootstrap.Map.Bounds.YMin)), ("Bounds.YMax", F(bootstrap.Map.Bounds.YMax)),
|
||||||
|
("Rows", I(bootstrap.Map.Rows)), ("Cols", I(bootstrap.Map.Cols)),
|
||||||
|
("EnableWebVisualization", B(settings.EnableWebVisualization)), ("AutoOpenWebVisualization", B(settings.AutoOpenWebVisualization)),
|
||||||
|
("WebVisualizationPort", I(settings.WebVisualizationPort)), ("WebRefreshRateHz", F(settings.WebRefreshRateHz)),
|
||||||
|
("VisualizationHistoryCycleLimit", I(settings.VisualizationHistoryCycleLimit)),
|
||||||
|
("EnableNativePainterVisualization", B(settings.EnableNativePainterVisualization)),
|
||||||
|
("DirectionConfirmationSpeedMetersPerSecond", F(settings.DirectionConfirmationSpeedMetersPerSecond)),
|
||||||
|
("DirectionConfirmationSamples", I(settings.DirectionConfirmationSamples)),
|
||||||
|
("GearSwitchProjectionToleranceMeters", F(settings.GearSwitchProjectionToleranceMeters)),
|
||||||
|
("GearSwitchStopHoldSeconds", F(settings.GearSwitchStopHoldSeconds)),
|
||||||
|
})
|
||||||
|
AssertValue(values, expected.RawName, expected.Value, "effective configuration value " + expected.RawName);
|
||||||
|
}
|
||||||
|
|
||||||
|
private static VisualizationChart FindChart(IReadOnlyList<VisualizationChart> charts, string id)
|
||||||
|
{
|
||||||
|
return charts.Single(x => x.Id == id);
|
||||||
|
}
|
||||||
|
|
||||||
|
private static DirectionSegmentView CreateStraightSegment(int index, double sourceStart)
|
||||||
|
{
|
||||||
|
var points = new List<MultiWheelC.TrajectoryPlanning.PathSmoothing.SmoothedPathPoint>
|
||||||
|
{
|
||||||
|
new MultiWheelC.TrajectoryPlanning.PathSmoothing.SmoothedPathPoint(0d, 0d, 0d, 0d, 0d,
|
||||||
|
TravelDirection.Forward, 0d, 0d, 1d, false, MultiWheelC.TrajectoryPlanning.PathSmoothing.SmoothedPathPointSource.Anchor),
|
||||||
|
new MultiWheelC.TrajectoryPlanning.PathSmoothing.SmoothedPathPoint(10d, 0d, 0d, 0d, 10d,
|
||||||
|
TravelDirection.Forward, 0d, 0d, 1d, false, MultiWheelC.TrajectoryPlanning.PathSmoothing.SmoothedPathPointSource.Anchor),
|
||||||
|
};
|
||||||
|
return new DirectionSegmentView(index, TravelDirection.Forward, points,
|
||||||
|
new ReferenceBoundary(index, 0d, EmBoundaryType.None, sourceStart),
|
||||||
|
new ReferenceBoundary(index, 10d, EmBoundaryType.Goal, sourceStart + 10d), sourceStart);
|
||||||
|
}
|
||||||
|
|
||||||
|
private static EmTrajectory CreateTrajectory(string id, DateTimeOffset effectiveAt, int count, EmTerminalType terminal,
|
||||||
|
EmLongitudinalMode mode, TravelDirection direction, double startX = 0d, double startPathS = 1000d)
|
||||||
|
{
|
||||||
|
var points = new List<EmTrajectoryPoint>();
|
||||||
|
for (int index = 0; index < count; index++)
|
||||||
|
{
|
||||||
|
double t = index * 0.1d;
|
||||||
|
points.Add(new EmTrajectoryPoint(startX + t, 0d, 0d, 0.1d + t, t, 0.01d, 0,
|
||||||
|
startX + t, startPathS + t, direction, index == count - 1 ? EmBoundaryType.Goal : EmBoundaryType.None,
|
||||||
|
0.2d + t, 0.3d + t));
|
||||||
|
}
|
||||||
|
return new EmTrajectory(new EmTrajectoryMetadata(id, effectiveAt, effectiveAt, 1L, "visualization-reference", 1L,
|
||||||
|
string.Empty, 0, direction, terminal, mode), points);
|
||||||
|
}
|
||||||
|
|
||||||
|
private sealed class FixedPlanningService : IEmPlanningService
|
||||||
|
{
|
||||||
|
private readonly EmTrajectory trajectory;
|
||||||
|
|
||||||
|
public FixedPlanningService(EmTrajectory trajectory) { this.trajectory = trajectory; }
|
||||||
|
|
||||||
|
public EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken)
|
||||||
|
{
|
||||||
|
return new EmPlanningResult(EmPlanningStatus.Success, trajectory, string.Empty);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
Reference in New Issue
Block a user