121 lines
6.3 KiB
C#
121 lines
6.3 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("coarse-path", "粗路径", "coarse", VisualizationLineStyle.Dashed,
|
|
Points((0d, 1d), (3d, 1.2d), (6d, 1.85d), (8d, 4.2d), (11d, 6d))),
|
|
new VisualizationPolyline("local-g2-path", "完整 Local G2 路径", "local-g2", VisualizationLineStyle.Solid,
|
|
Points((0d, 1d), (3d, 1d), (6d, 2d), (8d, 4d), (11d, 6d)))
|
|
},
|
|
new[]
|
|
{
|
|
new VisualizationMarker("smooth-start", "smooth-start", "平滑路径起点", new VisualizationPoint(0d, 1d)),
|
|
new VisualizationMarker("gear-1", "gear-switch", "换向 1", new VisualizationPoint(6d, 2d)),
|
|
new VisualizationMarker("gear-1-end", "gear-switch-end", "换向点 1 / s_end", new VisualizationPoint(6d, 2d)),
|
|
new VisualizationMarker("final-goal", "final-goal", "终点 / s_end", new VisualizationPoint(11d, 6d))
|
|
},
|
|
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("active-segment", "活动方向段", "active-segment", VisualizationLineStyle.Solid,
|
|
Points((0d, 1d), (3d, 1d), (6d, 2d))),
|
|
new VisualizationPolyline("previous", "上一轮轨迹", "previous", VisualizationLineStyle.Dashed, Points((1d, 1d), (2.5d, 1d), (4d, 1.3d))),
|
|
new VisualizationPolyline("current", "当前轨迹", "current", VisualizationLineStyle.Solid, Points((0d, 1d), (3.8d + offset, 1.2d), (5.8d + offset, 1.9d)))
|
|
},
|
|
new[]
|
|
{
|
|
new VisualizationMarker("vehicle", "vehicle", "车辆", new VisualizationPoint(2d + offset, 1d)),
|
|
new VisualizationMarker("plan-start", "plan-start", "计划起点", new VisualizationPoint(0d, 1d)),
|
|
new VisualizationMarker("gear-switch-end", "gear-switch-end", "换向点 1 / s_end", new VisualizationPoint(6d, 2d))
|
|
},
|
|
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)),
|
|
new[] { new VisualizationChartAnnotation("ls-s-end", "s-end", "s_end", 4d) }),
|
|
Chart("st", "时空轨迹", "t (s)", "PathS (m)", Points((0d, 0d), (1d, 1.2d), (2d, 3.0d)),
|
|
new[] { new VisualizationChartAnnotation("st-s-end", "s-end", "s_end", 2d) }),
|
|
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)", "ω (rad/s)", Points((0d, 0d), (1d, .08d), (2d, .04d)))
|
|
};
|
|
}
|
|
|
|
private static VisualizationChart Chart(string id, string title, string xAxis, string yAxis,
|
|
IReadOnlyList<VisualizationPoint> points,
|
|
IReadOnlyList<VisualizationChartAnnotation> annotations = null)
|
|
{
|
|
return new VisualizationChart(id, title, xAxis, yAxis,
|
|
new[] { new VisualizationSeries(id + "-current", "当前轨迹", VisualizationLineStyle.Solid, points) },
|
|
"", annotations ?? Array.Empty<VisualizationChartAnnotation>());
|
|
}
|
|
|
|
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;
|
|
}
|
|
}
|