101 lines
4.9 KiB
C#
101 lines
4.9 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using TrajectoryPlanningVisualization;
|
|
|
|
namespace TrajectoryPlanningVisualizationVerificationHost;
|
|
|
|
internal static class SampleSnapshotFactory
|
|
{
|
|
private static readonly DateTimeOffset Origin = DateTimeOffset.Parse("2026-08-06T00:00:00+00:00");
|
|
|
|
public static PlanningVisualizationStaticSnapshot CreateStaticSnapshot()
|
|
{
|
|
return new PlanningVisualizationStaticSnapshot(
|
|
"合成轨迹规划观察",
|
|
new VisualizationBounds(0d, 12d, 0d, 8d),
|
|
new VisualizationOccupancyGrid(new VisualizationBounds(0d, 12d, 0d, 8d), 2d, 4, 6,
|
|
new byte[] { 0b00100001, 0b00000100, 0b00000010 }),
|
|
new[]
|
|
{
|
|
new VisualizationPolyline("global", "全局参考路径", "global", VisualizationLineStyle.Solid,
|
|
Points((0d, 1d), (3d, 1d), (6d, 2d), (8d, 4d), (11d, 6d)))
|
|
},
|
|
new[] { new VisualizationMarker("gear-1", "gear-switch", "换向 1", new VisualizationPoint(6d, 2d)) },
|
|
new[]
|
|
{
|
|
new VisualizationDirectionSegment(0, "forward", false, true, Points((0d, 1d), (3d, 1d), (6d, 2d))),
|
|
new VisualizationDirectionSegment(1, "reverse", true, false, Points((6d, 2d), (8d, 4d), (11d, 6d)))
|
|
},
|
|
new[]
|
|
{
|
|
new VisualizationConfigurationGroup("调度", new[]
|
|
{
|
|
new VisualizationValue("网页刷新率", "RefreshRateHz", "10", "Hz", "normal"),
|
|
new VisualizationValue("历史容量", "HistoryCycleLimit", "60", "cycles", "normal")
|
|
}),
|
|
new VisualizationConfigurationGroup("观察安全边界", new[]
|
|
{
|
|
new VisualizationValue("观察模式", "OBSERVE_ONLY", "true", "", "notice")
|
|
})
|
|
});
|
|
}
|
|
|
|
public static PlanningVisualizationDynamicSnapshot CreateDynamicSnapshot(long sequence)
|
|
{
|
|
double offset = (sequence - 1L) * 0.05d;
|
|
return new PlanningVisualizationDynamicSnapshot(
|
|
sequence,
|
|
Origin.AddMilliseconds(sequence * 100L),
|
|
"合成数据运行中",
|
|
0,
|
|
"forward",
|
|
new VisualizationPose(2d + offset, 1d, 0d),
|
|
new[]
|
|
{
|
|
new VisualizationPolyline("previous", "上一轮轨迹", "previous", VisualizationLineStyle.Dashed, Points((1d, 1d), (2.5d, 1d), (4d, 1.3d))),
|
|
new VisualizationPolyline("current", "当前轨迹", "current", VisualizationLineStyle.Solid, Points((2d + offset, 1d), (3.8d + offset, 1.2d), (5.8d + offset, 1.9d)))
|
|
},
|
|
new[] { new VisualizationMarker("handoff", "handoff", "交接", new VisualizationPoint(2d + offset, 1d)) },
|
|
CreateCharts(offset),
|
|
new[]
|
|
{
|
|
new VisualizationValue("规划耗时", "PlanningElapsedMilliseconds", "4.2", "ms", "normal"),
|
|
new VisualizationValue("轨迹年龄", "TrajectoryAge", "0.1", "s", "normal")
|
|
},
|
|
new VisualizationCycleSummary(sequence, Origin.AddMilliseconds(sequence * 100L), "成功", true, 4.2d, 0,
|
|
"forward", "rolling", "horizon", 1.2d, 0d, ""));
|
|
}
|
|
|
|
private static IReadOnlyList<VisualizationChart> CreateCharts(double offset)
|
|
{
|
|
return new[]
|
|
{
|
|
Chart("ls", "横向偏移", "ReferenceS (m)", "l (m)", Points((0d, .05d), (2d, .08d), (4d, .02d))),
|
|
Chart("st", "时空轨迹", "t (s)", "PathS (m)", Points((0d, 0d), (1d, 1.2d), (2d, 3.0d))),
|
|
Chart("curvature-s", "曲率—距离", "s (m)", "κ (m⁻¹)", Points((0d, .01d), (2d, .04d), (4d, .02d))),
|
|
Chart("curvature-t", "曲率—时间", "t (s)", "κ (m⁻¹)", Points((0d, .01d), (1d, .04d), (2d, .02d))),
|
|
Chart("velocity-t", "速度", "t (s)", "v (m/s)", Points((0d, 0d), (1d, 1d), (2d, 1.2d + offset))),
|
|
Chart("acceleration-t", "加速度", "t (s)", "a (m/s²)", Points((0d, .8d), (1d, .2d), (2d, 0d))),
|
|
Chart("jerk-t", "加加速度", "t (s)", "j (m/s³)", Points((0d, -.6d), (1d, -.2d))),
|
|
Chart("yaw-rate-t", "横摆角速度", "t (s)", "yaw rate (rad/s)", Points((0d, 0d), (1d, .08d), (2d, .04d)))
|
|
};
|
|
}
|
|
|
|
private static VisualizationChart Chart(string id, string title, string xAxis, string yAxis, IReadOnlyList<VisualizationPoint> points)
|
|
{
|
|
return new VisualizationChart(id, title, xAxis, yAxis,
|
|
new[] { new VisualizationSeries(id + "-current", "当前轨迹", VisualizationLineStyle.Solid, points) });
|
|
}
|
|
|
|
private static IReadOnlyList<VisualizationPoint> Points(params (double x, double y)[] values)
|
|
{
|
|
var points = new List<VisualizationPoint>(values.Length);
|
|
foreach ((double x, double y) in values)
|
|
{
|
|
points.Add(new VisualizationPoint(x, y));
|
|
}
|
|
|
|
return points;
|
|
}
|
|
}
|