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");
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user