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