195 lines
14 KiB
C#
195 lines
14 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Globalization;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
|
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
using TrajectoryPlanningVisualization;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
|
|
|
public sealed class TrajectoryObservationStaticSnapshotBuilder
|
|
{
|
|
public PlanningVisualizationStaticSnapshot Build(TrajectoryObservationBootstrapResult bootstrap,
|
|
EmPlannerConfiguration effectiveConfiguration, TrajectoryObservationSettings settings, long obstacleSnapshotVersion)
|
|
{
|
|
if (bootstrap == null) throw new ArgumentNullException(nameof(bootstrap));
|
|
if (effectiveConfiguration == null) throw new ArgumentNullException(nameof(effectiveConfiguration));
|
|
if (settings == null) throw new ArgumentNullException(nameof(settings));
|
|
var map = bootstrap.Map ?? throw new ArgumentException("Bootstrap map is required.", nameof(bootstrap));
|
|
var bounds = new VisualizationBounds(map.Bounds.XMin / 1000d, map.Bounds.XMax / 1000d,
|
|
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, bootstrap.Vehicle, map, obstacleSnapshotVersion));
|
|
}
|
|
|
|
private static VisualizationOccupancyGrid CreateGrid(Mapping.PlanningGridMap map, VisualizationBounds bounds)
|
|
{
|
|
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))
|
|
{
|
|
int bitIndex = row * map.Cols + column;
|
|
bits[bitIndex >> 3] |= (byte)(1 << (bitIndex & 7));
|
|
}
|
|
return new VisualizationOccupancyGrid(bounds, map.ResolutionMeters, map.Rows, map.Cols, bits);
|
|
}
|
|
|
|
private static IReadOnlyList<VisualizationPolyline> CreatePolylines(TrajectoryObservationBootstrapResult bootstrap)
|
|
{
|
|
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));
|
|
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));
|
|
return lines;
|
|
}
|
|
|
|
private static IReadOnlyList<VisualizationMarker> CreateMarkers(TrajectoryObservationBootstrapResult bootstrap)
|
|
{
|
|
var markers = new List<VisualizationMarker>();
|
|
foreach (DirectionSegmentView segment in bootstrap.Segments)
|
|
if (segment.EndBoundary.BoundaryType == EmBoundaryType.GearSwitchApproach)
|
|
{
|
|
var point = segment.Points[segment.Points.Count - 1];
|
|
markers.Add(new VisualizationMarker("gear-switch-" + segment.SegmentIndex, "gear-switch",
|
|
"换向 " + (segment.SegmentIndex + 1), new VisualizationPoint(point.X, point.Y)));
|
|
}
|
|
return markers;
|
|
}
|
|
|
|
private static IReadOnlyList<VisualizationDirectionSegment> CreateSegments(TrajectoryObservationBootstrapResult bootstrap)
|
|
{
|
|
var result = new List<VisualizationDirectionSegment>();
|
|
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));
|
|
result.Add(new VisualizationDirectionSegment(segment.SegmentIndex, segment.Direction.ToString(),
|
|
segment.StartBoundary.BoundaryType == EmBoundaryType.GearSwitchDeparture,
|
|
segment.EndBoundary.BoundaryType == EmBoundaryType.GearSwitchApproach, points));
|
|
}
|
|
return result;
|
|
}
|
|
|
|
private static IReadOnlyList<VisualizationConfigurationGroup> CreateConfiguration(EmPlannerConfiguration configuration,
|
|
TrajectoryObservationSettings settings, VehicleParameters vehicle, Mapping.PlanningGridMap map,
|
|
long obstacleSnapshotVersion)
|
|
{
|
|
string F(double value) => value.ToString("R", CultureInfo.InvariantCulture);
|
|
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("规划范围", "PlanningScope", settings.PlanningScope.ToString(), ""),
|
|
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)
|
|
{
|
|
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");
|
|
}
|
|
}
|