629 lines
28 KiB
C#
629 lines
28 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
using System.Drawing;
|
||
using System.Globalization;
|
||
using System.Threading;
|
||
using System.Threading.Tasks;
|
||
using ClumsyCore;
|
||
using ClumsyCore.DTools;
|
||
using ClumsyCore.Interfaces;
|
||
using ClumsyCore.Pilot;
|
||
using FundamentalLib;
|
||
using MDCSToolBox.Clumsy.Pilot;
|
||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Test;
|
||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||
|
||
namespace MultiWheelC;
|
||
|
||
/// <summary>
|
||
/// 显式空图的粗路径规划测试入口。
|
||
/// 只负责创建纯规划请求；规划、取消与可视化均由共享执行器处理，不会向底盘发送任何命令。
|
||
/// </summary>
|
||
[MovementTest(name = "粗路径规划-显式空图")]
|
||
public sealed class CoarsePathExplicitEmptyTest : MovementTest
|
||
{
|
||
/// <inheritdoc />
|
||
public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.ExplicitEmpty, "显式空图");
|
||
|
||
/// <inheritdoc />
|
||
public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 单矩形绕行的粗路径规划测试入口。
|
||
/// </summary>
|
||
[MovementTest(name = "粗路径规划-单矩形绕行")]
|
||
// TODO：#在该测试下发生了红色矩形栅格碰撞但依然规划成功，需要进一步核实与确认
|
||
public sealed class CoarsePathRectangleDetourTest : MovementTest
|
||
{
|
||
/// <inheritdoc />
|
||
public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.RectangleDetour, "单矩形绕行");
|
||
|
||
/// <inheritdoc />
|
||
public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 手工圆形、矩形和 TwoLeg 快照组合的粗路径规划测试入口。
|
||
/// </summary>
|
||
[MovementTest(name = "粗路径规划-多来源障碍")]
|
||
public sealed class CoarsePathManualAndTwoLegTest : MovementTest
|
||
{
|
||
/// <inheritdoc />
|
||
public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.ManualAndTwoLeg, "多来源障碍");
|
||
|
||
/// <inheritdoc />
|
||
public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 重复输入地图缓存命中的粗路径规划测试入口。
|
||
/// </summary>
|
||
[MovementTest(name = "粗路径规划-缓存命中")]
|
||
public sealed class CoarsePathCacheHitTest : MovementTest
|
||
{
|
||
/// <inheritdoc />
|
||
public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.CacheHit, "缓存命中");
|
||
|
||
/// <inheritdoc />
|
||
public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 前进起步、倒车到达并显示换向点的粗路径规划测试入口。
|
||
/// </summary>
|
||
[MovementTest(name = "粗路径规划-倒车换向")]
|
||
public sealed class CoarsePathReverseGearSwitchTest : MovementTest
|
||
{
|
||
/// <inheritdoc />
|
||
public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.ReverseGearSwitch, "倒车换向");
|
||
|
||
/// <inheritdoc />
|
||
public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 障碍带完全隔开起终点的无解粗路径规划测试入口。
|
||
/// </summary>
|
||
[MovementTest(name = "粗路径规划-无解")]
|
||
public sealed class CoarsePathNoFeasiblePathTest : MovementTest
|
||
{
|
||
/// <inheritdoc />
|
||
public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.NoFeasiblePath, "无解障碍带");
|
||
|
||
/// <inheritdoc />
|
||
public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 使用当前 AMR 车身几何中心位姿和人工终点的粗路径规划演示入口。
|
||
/// 输入的 X/Y 使用世界 mm、航向使用 deg；进入规划核心前由场景工厂一次性转换为 m/rad。
|
||
/// </summary>
|
||
[MovementTest(name = "粗路径规划")]
|
||
public sealed class CoarsePathPlanningTest : MovementTest
|
||
{
|
||
private const int MaximumManualObstacleCount = 20;
|
||
private static long _nextManualObstacleSnapshotVersion;
|
||
|
||
/// <summary>
|
||
/// 读取一次 AMR 当前世界位姿、手动终点和障碍物快照后启动规划。
|
||
/// 注意：getCartLocation 在无定位时可能阻塞；全部输入会在启动后台任务前冻结，不会被规划线程重复读取。
|
||
/// </summary>
|
||
public override void Test()
|
||
{
|
||
try
|
||
{
|
||
var amrPose = DetourInterface.getCartLocation();
|
||
double goalXmm = ReadFiniteInput("粗路径终点 X（世界 mm）");
|
||
double goalYmm = ReadFiniteInput("粗路径终点 Y（世界 mm）");
|
||
double goalHeadingDeg = ReadFiniteInput("粗路径终点航向（世界 deg）");
|
||
TimeSpan searchTimeout = ReadPositiveTimeoutInput("粗路径规划总超时（秒，必须大于 0）");
|
||
IReadOnlyList<ManualCoarsePathObstacle> obstacles = ReadManualObstacles();
|
||
long snapshotVersion = obstacles.Count == 0 ? 0L :
|
||
Interlocked.Increment(ref _nextManualObstacleSnapshotVersion);
|
||
CoarsePathPlanningJob job = CoarsePathScenarioFactory.CreateManualObstacleDemo(
|
||
amrPose.x, amrPose.y, amrPose.th, goalXmm, goalYmm, goalHeadingDeg,
|
||
obstacles, snapshotVersion);
|
||
job.Configuration.SearchTimeout = searchTimeout;
|
||
CoarsePathPlanningTestRunner.Run("AMR 位姿 + 手动终点 + 手动障碍物", job);
|
||
}
|
||
catch (Exception exception)
|
||
{
|
||
CoarsePathPlanningTestRunner.ShowInputFailure(exception);
|
||
}
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
|
||
|
||
private static IReadOnlyList<ManualCoarsePathObstacle> ReadManualObstacles()
|
||
{
|
||
int count = ReadBoundedIntegerInput("手动障碍物数量（0-20）", 0, MaximumManualObstacleCount);
|
||
var obstacles = new List<ManualCoarsePathObstacle>(count);
|
||
for (int index = 0; index < count; index++)
|
||
{
|
||
string label = "障碍物 " + (index + 1);
|
||
int kind = ReadBoundedIntegerInput(label + " 类型（1圆形，2矩形）", 1, 2);
|
||
double centerXmm = ReadFiniteInput(label + " 中心 X（世界 mm）");
|
||
double centerYmm = ReadFiniteInput(label + " 中心 Y（世界 mm）");
|
||
if (kind == 1)
|
||
{
|
||
double radiusMm = ReadPositiveFiniteInput(label + " 半径 r（mm）");
|
||
obstacles.Add(ManualCoarsePathObstacle.Circle(centerXmm, centerYmm, radiusMm));
|
||
}
|
||
else
|
||
{
|
||
double lengthXmm = ReadPositiveFiniteInput(label + " X方向长度（mm）");
|
||
double widthYmm = ReadPositiveFiniteInput(label + " Y方向宽度（mm）");
|
||
obstacles.Add(ManualCoarsePathObstacle.AxisAlignedRectangle(centerXmm, centerYmm,
|
||
lengthXmm, widthYmm));
|
||
}
|
||
}
|
||
return obstacles;
|
||
}
|
||
|
||
private static int ReadBoundedIntegerInput(string prompt, int minimum, int maximum)
|
||
{
|
||
object raw = UI.GetInput(prompt);
|
||
string text = Convert.ToString(raw, CultureInfo.CurrentCulture);
|
||
int value;
|
||
if (!int.TryParse(text, NumberStyles.Integer, CultureInfo.CurrentCulture, out value) &&
|
||
!int.TryParse(text, NumberStyles.Integer, CultureInfo.InvariantCulture, out value))
|
||
throw new ArgumentException("输入必须是整数：" + prompt);
|
||
if (value < minimum || value > maximum)
|
||
throw new ArgumentOutOfRangeException(nameof(prompt), "输入超出允许范围：" + prompt);
|
||
return value;
|
||
}
|
||
|
||
private static double ReadPositiveFiniteInput(string prompt)
|
||
{
|
||
double value = ReadFiniteInput(prompt);
|
||
if (value <= 0d)
|
||
throw new ArgumentOutOfRangeException(nameof(prompt), "输入必须为正数：" + prompt);
|
||
return value;
|
||
}
|
||
|
||
private static TimeSpan ReadPositiveTimeoutInput(string prompt)
|
||
{
|
||
double timeoutSeconds = ReadFiniteInput(prompt);
|
||
if (timeoutSeconds <= 0d)
|
||
throw new ArgumentOutOfRangeException(nameof(prompt), "输入必须为正数：" + prompt);
|
||
try
|
||
{
|
||
return TimeSpan.FromSeconds(timeoutSeconds);
|
||
}
|
||
catch (OverflowException)
|
||
{
|
||
throw new ArgumentOutOfRangeException(nameof(prompt), "输入超出允许范围：" + prompt);
|
||
}
|
||
}
|
||
|
||
private static double ReadFiniteInput(string prompt)
|
||
{
|
||
object raw = UI.GetInput(prompt);
|
||
string text = Convert.ToString(raw, CultureInfo.CurrentCulture);
|
||
double value;
|
||
if (!double.TryParse(text, NumberStyles.Float, CultureInfo.CurrentCulture, out value) &&
|
||
!double.TryParse(text, NumberStyles.Float, CultureInfo.InvariantCulture, out value))
|
||
throw new ArgumentException("输入必须是有限数字：" + prompt);
|
||
if (double.IsNaN(value) || double.IsInfinity(value))
|
||
throw new ArgumentException("输入必须是有限数字：" + prompt);
|
||
return value;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 粗路径 MovementTest 的共享后台会话、停止和绘制实现。
|
||
/// 同一时刻只允许一个会话绘制；启动新会话或停止时会取消旧会话，但不会等待旧任务退出。
|
||
/// </summary>
|
||
internal static class CoarsePathPlanningTestRunner
|
||
{
|
||
private const string PainterLayerName = "CoarsePathPlanningV1";
|
||
private const float MillimetersPerMeter = 1000f;
|
||
private const int MaximumVisibleGridLines = 100;
|
||
|
||
private static readonly object SessionSync = new object();
|
||
private static readonly CoarsePathPlanningService PlanningService = new CoarsePathPlanningService();
|
||
private static readonly Painter Painter = UI.GetPainter(PainterLayerName, true);
|
||
|
||
private static CancellationTokenSource _activeCancellation;
|
||
private static Task<CoarsePathPlanningJobResult> _activeTask;
|
||
private static long _nextSessionId;
|
||
private static long _activeSessionId;
|
||
|
||
/// <summary>
|
||
/// 固定场景启动时冻结的 AMR 位姿。规划后台不会重新读取定位，确保输入一致。
|
||
/// </summary>
|
||
private sealed class AmrPoseSnapshot
|
||
{
|
||
public AmrPoseSnapshot(double xMillimeters, double yMillimeters, double headingDegrees)
|
||
{
|
||
EnsureFiniteAmrValue(xMillimeters, "X");
|
||
EnsureFiniteAmrValue(yMillimeters, "Y");
|
||
EnsureFiniteAmrValue(headingDegrees, "航向");
|
||
XMillimeters = xMillimeters;
|
||
YMillimeters = yMillimeters;
|
||
HeadingDegrees = headingDegrees;
|
||
}
|
||
|
||
public double XMillimeters { get; }
|
||
public double YMillimeters { get; }
|
||
public double HeadingDegrees { get; }
|
||
|
||
public string DisplayText
|
||
{
|
||
get
|
||
{
|
||
return "AMR 起点：X=" + XMillimeters.ToString("F0", CultureInfo.InvariantCulture) +
|
||
" mm，Y=" + YMillimeters.ToString("F0", CultureInfo.InvariantCulture) +
|
||
" mm，航向=" + HeadingDegrees.ToString("F1", CultureInfo.InvariantCulture) + " deg";
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 创建指定固定场景并将其提交给共享后台服务。
|
||
/// </summary>
|
||
internal static void RunScenario(CoarsePathTestScenario scenario, string scenarioName)
|
||
{
|
||
try
|
||
{
|
||
var pose = DetourInterface.getCartLocation();
|
||
if (ReferenceEquals(pose, null)) throw new ArgumentException("AMR 位姿为空。");
|
||
var snapshot = new AmrPoseSnapshot(pose.x, pose.y, pose.th);
|
||
CoarsePathPlanningJob job = CoarsePathScenarioFactory.Create(scenario,
|
||
snapshot.XMillimeters, snapshot.YMillimeters, snapshot.HeadingDegrees);
|
||
Run(scenarioName, job, snapshot);
|
||
}
|
||
catch (Exception exception)
|
||
{
|
||
ShowInputFailure(new ArgumentException("AMR 位姿不可用：" + exception.Message, exception));
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 提交已经冻结输入的一次规划请求。调用立即返回，结果只会由对应会话的完成回调绘制。
|
||
/// </summary>
|
||
internal static void Run(string scenarioName, CoarsePathPlanningJob job)
|
||
{
|
||
Run(scenarioName, job, null);
|
||
}
|
||
|
||
private static void Run(string scenarioName, CoarsePathPlanningJob job, AmrPoseSnapshot amrPose)
|
||
{
|
||
if (job == null) throw new ArgumentNullException(nameof(job));
|
||
|
||
var cancellation = new CancellationTokenSource();
|
||
CancellationTokenSource previousCancellation;
|
||
long sessionId;
|
||
lock (SessionSync)
|
||
{
|
||
previousCancellation = _activeCancellation;
|
||
_activeCancellation = cancellation;
|
||
_activeTask = null;
|
||
sessionId = ++_nextSessionId;
|
||
_activeSessionId = sessionId;
|
||
}
|
||
|
||
// 先发布新会话编号，再取消旧任务，避免旧完成回调覆盖新画面。
|
||
if (previousCancellation != null) previousCancellation.Cancel();
|
||
Painter.Clear();
|
||
DrawPending(scenarioName, job, amrPose);
|
||
|
||
Task<CoarsePathPlanningJobResult> task = Task.Run(() => PlanningService.Plan(job, cancellation.Token));
|
||
lock (SessionSync)
|
||
{
|
||
if (_activeSessionId == sessionId) _activeTask = task;
|
||
}
|
||
|
||
_ = task.ContinueWith(completed => Finish(sessionId, scenarioName, job, amrPose, cancellation, completed),
|
||
TaskScheduler.Default);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 取消当前会话并清空专用图层；不等待后台任务结束。
|
||
/// 已取消任务的完成回调只释放资源，不再记录或绘制结果。
|
||
/// </summary>
|
||
internal static void Stop()
|
||
{
|
||
CancellationTokenSource cancellation;
|
||
lock (SessionSync)
|
||
{
|
||
cancellation = _activeCancellation;
|
||
_activeCancellation = null;
|
||
_activeTask = null;
|
||
_activeSessionId = 0;
|
||
}
|
||
|
||
if (cancellation != null) cancellation.Cancel();
|
||
Painter.Clear();
|
||
Hedingben.ToastText("粗路径规划已请求停止。", PainterLayerName);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 显示输入读取或校验失败，且不改变现有规划任务。
|
||
/// </summary>
|
||
internal static void ShowInputFailure(Exception exception)
|
||
{
|
||
string message = exception == null ? "未知输入错误。" : exception.Message;
|
||
Painter.DrawText(Color.LightYellow, "粗路径规划未启动：" + message, 0f, 0f);
|
||
Hedingben.ToastText("粗路径规划未启动：" + message, PainterLayerName);
|
||
}
|
||
|
||
private static void Finish(long sessionId, string scenarioName, CoarsePathPlanningJob job, AmrPoseSnapshot amrPose,
|
||
CancellationTokenSource cancellation, Task<CoarsePathPlanningJobResult> completed)
|
||
{
|
||
try
|
||
{
|
||
CoarsePathPlanningJobResult result = completed.GetAwaiter().GetResult();
|
||
bool isCurrent;
|
||
lock (SessionSync)
|
||
{
|
||
isCurrent = _activeSessionId == sessionId;
|
||
if (isCurrent)
|
||
{
|
||
_activeTask = null;
|
||
_activeCancellation = null;
|
||
}
|
||
}
|
||
|
||
if (!isCurrent) return;
|
||
DrawResult(scenarioName, job, amrPose, result);
|
||
Hedingben.ToastText(BuildToastMessage(scenarioName, result), PainterLayerName);
|
||
}
|
||
catch (Exception exception)
|
||
{
|
||
bool isCurrent;
|
||
lock (SessionSync)
|
||
{
|
||
isCurrent = _activeSessionId == sessionId;
|
||
if (isCurrent)
|
||
{
|
||
_activeTask = null;
|
||
_activeCancellation = null;
|
||
}
|
||
}
|
||
|
||
if (isCurrent)
|
||
Hedingben.ToastText("粗路径规划任务异常：" + exception.GetType().Name + "。" + exception.Message,
|
||
PainterLayerName);
|
||
}
|
||
finally
|
||
{
|
||
cancellation.Dispose();
|
||
}
|
||
}
|
||
|
||
private static void DrawPending(string scenarioName, CoarsePathPlanningJob job, AmrPoseSnapshot amrPose)
|
||
{
|
||
DrawPose(job.Start, Color.LimeGreen, "起点");
|
||
DrawPose(job.Goal, Color.Orange, "终点");
|
||
Painter.DrawText(Color.LightGray, "场景：" + scenarioName + "（规划中）", 0f, 0f);
|
||
if (amrPose != null) Painter.DrawText(Color.LightGray, amrPose.DisplayText, 0f, -120f);
|
||
}
|
||
|
||
private static void DrawResult(string scenarioName, CoarsePathPlanningJob job, AmrPoseSnapshot amrPose,
|
||
CoarsePathPlanningJobResult result)
|
||
{
|
||
Painter.Clear();
|
||
PlanningGridMap map = result.MapResult.Map;
|
||
if (map != null) DrawMap(map);
|
||
|
||
DrawPose(job.Start, Color.LimeGreen, "起点");
|
||
DrawGoal(job.Goal, job.Configuration, Color.Orange);
|
||
if (result.PlanningResult.Status == PlanningStatus.Success)
|
||
DrawPath(result.PlanningResult, job.Vehicle);
|
||
|
||
DrawLegend(map);
|
||
DrawStatus(scenarioName, job, result, map, amrPose);
|
||
}
|
||
|
||
private static void DrawMap(PlanningGridMap map)
|
||
{
|
||
float xMin = map.Bounds.XMin;
|
||
float xMax = map.Bounds.XMax;
|
||
float yMin = map.Bounds.YMin;
|
||
float yMax = map.Bounds.YMax;
|
||
float resolution = map.ResolutionMm;
|
||
int gridStride = Math.Max(1, (int)Math.Ceiling(Math.Max(map.Rows, map.Cols) / (double)MaximumVisibleGridLines));
|
||
|
||
// 真实 ResolutionMm 决定网格位置，gridStride 只影响显示抽稀。
|
||
for (int col = 0; col <= map.Cols; col += gridStride)
|
||
{
|
||
float x = Math.Min(xMax, xMin + col * resolution);
|
||
Painter.DrawLine(Color.FromArgb(80, Color.SlateGray), x, yMin, x, yMax, width: 1);
|
||
}
|
||
for (int row = 0; row <= map.Rows; row += gridStride)
|
||
{
|
||
float y = Math.Min(yMax, yMin + row * resolution);
|
||
Painter.DrawLine(Color.FromArgb(80, Color.SlateGray), xMin, y, xMax, y, width: 1);
|
||
}
|
||
|
||
DrawRectangle(Color.Gainsboro, xMin, yMin, xMax, yMax, 3);
|
||
if (xMin <= 0f && 0f < xMax) Painter.DrawLine(Color.DimGray, 0f, yMin, 0f, yMax, width: 2);
|
||
if (yMin <= 0f && 0f < yMax) Painter.DrawLine(Color.DimGray, xMin, 0f, xMax, 0f, width: 2);
|
||
|
||
for (int row = 0; row < map.Rows; row++)
|
||
{
|
||
for (int col = 0; col < map.Cols; col++)
|
||
{
|
||
if (!map.IsOccupied(row, col)) continue;
|
||
float x = xMin + col * resolution + resolution / 2f;
|
||
float y = yMin + row * resolution;
|
||
Painter.DrawLine(Color.FromArgb(150, Color.Firebrick), x, y, x, y + resolution,
|
||
width: Math.Max(1, (int)Math.Round(resolution)));
|
||
}
|
||
}
|
||
}
|
||
|
||
private static void DrawPose(Pose2D pose, Color color, string label)
|
||
{
|
||
if (pose == null) return;
|
||
float x = ToMillimeters(pose.X);
|
||
float y = ToMillimeters(pose.Y);
|
||
Painter.DrawCircle(color, x, y, 80f);
|
||
DrawHeadingArrow(x, y, pose.Heading, color, 260f);
|
||
Painter.DrawText(color, label, x + 100f, y + 100f);
|
||
}
|
||
|
||
private static void DrawGoal(Pose2D goal, HybridAStarConfiguration configuration, Color color)
|
||
{
|
||
DrawPose(goal, color, "终点");
|
||
if (goal == null || configuration == null) return;
|
||
Painter.DrawCircle(Color.FromArgb(150, color), ToMillimeters(goal.X), ToMillimeters(goal.Y),
|
||
ToMillimeters(configuration.GoalPositionToleranceMeters));
|
||
}
|
||
|
||
private static void DrawPath(PlanningResult planningResult, VehicleParameters vehicle)
|
||
{
|
||
if (planningResult.Path == null || planningResult.Path.Count == 0) return;
|
||
|
||
int frameStride = Math.Max(1, planningResult.Path.Count / 10);
|
||
for (int index = 1; index < planningResult.Path.Count; index++)
|
||
{
|
||
CoarsePathPoint previous = planningResult.Path[index - 1];
|
||
CoarsePathPoint current = planningResult.Path[index];
|
||
Color color = current.Direction == TravelDirection.Forward ? Color.LimeGreen : Color.DeepSkyBlue;
|
||
Painter.DrawLine(color, ToMillimeters(previous.X), ToMillimeters(previous.Y),
|
||
ToMillimeters(current.X), ToMillimeters(current.Y), width: 4);
|
||
|
||
if (index % frameStride == 0 || current.IsGearSwitchPoint || index == planningResult.Path.Count - 1)
|
||
DrawVehicleFrame(current, vehicle);
|
||
if (index % Math.Max(1, frameStride / 2) == 0)
|
||
DrawHeadingArrow(ToMillimeters(current.X), ToMillimeters(current.Y),
|
||
current.Heading + (current.Direction == TravelDirection.Reverse ? Math.PI : 0d), color, 140f);
|
||
if (!current.IsGearSwitchPoint) continue;
|
||
|
||
float x = ToMillimeters(current.X);
|
||
float y = ToMillimeters(current.Y);
|
||
Painter.DrawCircle(Color.MediumPurple, x, y, 100f);
|
||
Painter.DrawText(Color.MediumPurple, "换向", x + 110f, y - 110f);
|
||
}
|
||
|
||
DrawVehicleFrame(planningResult.Path[0], vehicle);
|
||
}
|
||
|
||
private static void DrawVehicleFrame(CoarsePathPoint point, VehicleParameters vehicle)
|
||
{
|
||
if (point == null || vehicle == null) return;
|
||
float halfLength = ToMillimeters(vehicle.LengthMeters / 2d + vehicle.SafetyMarginMeters);
|
||
float halfWidth = ToMillimeters(vehicle.WidthMeters / 2d + vehicle.SafetyMarginMeters);
|
||
float centerX = ToMillimeters(point.X);
|
||
float centerY = ToMillimeters(point.Y);
|
||
double cos = Math.Cos(point.Heading);
|
||
double sin = Math.Sin(point.Heading);
|
||
|
||
TransformVehicleCorner(centerX, centerY, cos, sin, halfLength, halfWidth, out float frontLeftX, out float frontLeftY);
|
||
TransformVehicleCorner(centerX, centerY, cos, sin, halfLength, -halfWidth, out float frontRightX, out float frontRightY);
|
||
TransformVehicleCorner(centerX, centerY, cos, sin, -halfLength, -halfWidth, out float rearRightX, out float rearRightY);
|
||
TransformVehicleCorner(centerX, centerY, cos, sin, -halfLength, halfWidth, out float rearLeftX, out float rearLeftY);
|
||
Painter.DrawLine(Color.Gold, frontLeftX, frontLeftY, frontRightX, frontRightY, width: 2);
|
||
Painter.DrawLine(Color.Gold, frontRightX, frontRightY, rearRightX, rearRightY, width: 2);
|
||
Painter.DrawLine(Color.Gold, rearRightX, rearRightY, rearLeftX, rearLeftY, width: 2);
|
||
Painter.DrawLine(Color.Gold, rearLeftX, rearLeftY, frontLeftX, frontLeftY, width: 2);
|
||
}
|
||
|
||
private static void TransformVehicleCorner(float centerX, float centerY, double cos, double sin,
|
||
float longitudinal, float lateral, out float x, out float y)
|
||
{
|
||
x = centerX + (float)(cos * longitudinal - sin * lateral);
|
||
y = centerY + (float)(sin * longitudinal + cos * lateral);
|
||
}
|
||
|
||
private static void DrawHeadingArrow(float x, float y, double headingRadians, Color color, float length)
|
||
{
|
||
float endX = x + (float)Math.Cos(headingRadians) * length;
|
||
float endY = y + (float)Math.Sin(headingRadians) * length;
|
||
Painter.DrawLine(color, x, y, endX, endY, endArrow: true, width: 3);
|
||
}
|
||
|
||
private static void DrawLegend(PlanningGridMap map)
|
||
{
|
||
float x = map == null ? 0f : map.Bounds.XMin + 150f;
|
||
float y = map == null ? 250f : map.Bounds.YMax - 180f;
|
||
Painter.DrawText(Color.White, "图例", x, y);
|
||
DrawLegendItem(x, y - 130f, Color.Gainsboro, "边界 / 栅格");
|
||
DrawLegendItem(x, y - 260f, Color.Firebrick, "占据格");
|
||
DrawLegendItem(x, y - 390f, Color.LimeGreen, "起点 / 前进");
|
||
DrawLegendItem(x, y - 520f, Color.Orange, "终点 / 容差");
|
||
DrawLegendItem(x, y - 650f, Color.DeepSkyBlue, "倒车");
|
||
DrawLegendItem(x, y - 780f, Color.MediumPurple, "换向");
|
||
DrawLegendItem(x, y - 910f, Color.Gold, "扩大车体检查框");
|
||
}
|
||
|
||
private static void DrawLegendItem(float x, float y, Color color, string text)
|
||
{
|
||
Painter.DrawLine(color, x, y, x + 90f, y, width: 5);
|
||
Painter.DrawText(color, text, x + 120f, y - 30f);
|
||
}
|
||
|
||
private static void DrawStatus(string scenarioName, CoarsePathPlanningJob job,
|
||
CoarsePathPlanningJobResult result, PlanningGridMap map, AmrPoseSnapshot amrPose)
|
||
{
|
||
float x = map == null ? 0f : map.Bounds.XMin + 150f;
|
||
float y = map == null ? -250f : map.Bounds.YMin + 150f;
|
||
string snapshot = map == null ? "无" : map.SnapshotId.ToString(CultureInfo.InvariantCulture);
|
||
string resolution = map == null ? "无" : map.ResolutionMm.ToString("F0", CultureInfo.InvariantCulture) + " mm";
|
||
PlanningDiagnostics diagnostics = result.PlanningResult.Diagnostics;
|
||
string reason = diagnostics.TerminationReason ?? string.Empty;
|
||
string turningRadius = "无";
|
||
if (job != null && VehicleKinematics.TryGetMaximumCurvaturePerMeter(job.Vehicle, out double maximumCurvaturePerMeter))
|
||
turningRadius = (1d / maximumCurvaturePerMeter).ToString("F2", CultureInfo.InvariantCulture) + " m";
|
||
Painter.DrawText(Color.White, "场景：" + scenarioName, x, y);
|
||
float detailOffset = 0f;
|
||
if (amrPose != null)
|
||
{
|
||
Painter.DrawText(Color.White, amrPose.DisplayText, x, y + 120f);
|
||
detailOffset = 120f;
|
||
}
|
||
Painter.DrawText(Color.White, "地图：" + result.MapResult.Status + "，缓存：" + result.MapResult.CacheHit + "，快照：" + snapshot,
|
||
x, y + 120f + detailOffset);
|
||
Painter.DrawText(Color.White, "栅格：" + resolution + "，规划：" + result.PlanningResult.Status + "，总耗时：" +
|
||
diagnostics.Elapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) + " ms，路径搜索：" +
|
||
diagnostics.PathSearchElapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) + " ms", x, y + 240f + detailOffset);
|
||
Painter.DrawText(Color.White, "节点：扩展=" + diagnostics.ExpandedNodeCount.ToString(CultureInfo.InvariantCulture) +
|
||
"，生成=" + diagnostics.GeneratedNodeCount.ToString(CultureInfo.InvariantCulture) + "，Open List峰值=" +
|
||
diagnostics.PeakOpenListCount.ToString(CultureInfo.InvariantCulture), x, y + 360f + detailOffset);
|
||
if (job != null && job.Vehicle != null)
|
||
{
|
||
Painter.DrawText(Color.White, "演示车辆：长=" + job.Vehicle.LengthMeters.ToString("F2", CultureInfo.InvariantCulture) +
|
||
" m，宽=" + job.Vehicle.WidthMeters.ToString("F2", CultureInfo.InvariantCulture) + " m，余量=" +
|
||
job.Vehicle.SafetyMarginMeters.ToString("F2", CultureInfo.InvariantCulture) + " m，最小转弯半径=" +
|
||
turningRadius, x, y + 480f + detailOffset);
|
||
}
|
||
if (!string.IsNullOrEmpty(reason))
|
||
Painter.DrawText(Color.LightYellow, "原因：" + reason, x, y + 600f + detailOffset);
|
||
}
|
||
|
||
private static string BuildToastMessage(string scenarioName, CoarsePathPlanningJobResult result)
|
||
{
|
||
PlanningDiagnostics diagnostics = result.PlanningResult.Diagnostics;
|
||
string message = "粗路径[" + scenarioName + "]：地图=" + result.MapResult.Status + "，规划=" +
|
||
result.PlanningResult.Status + "，总耗时=" +
|
||
diagnostics.Elapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) + "ms，路径搜索=" +
|
||
diagnostics.PathSearchElapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) + "ms";
|
||
if (result.PlanningResult.Status != PlanningStatus.Success && !string.IsNullOrEmpty(diagnostics.TerminationReason))
|
||
message += "，原因=" + diagnostics.TerminationReason;
|
||
return message + "。";
|
||
}
|
||
|
||
private static void DrawRectangle(Color color, float xMin, float yMin, float xMax, float yMax, int width)
|
||
{
|
||
Painter.DrawLine(color, xMin, yMin, xMax, yMin, width: width);
|
||
Painter.DrawLine(color, xMax, yMin, xMax, yMax, width: width);
|
||
Painter.DrawLine(color, xMax, yMax, xMin, yMax, width: width);
|
||
Painter.DrawLine(color, xMin, yMax, xMin, yMin, width: width);
|
||
}
|
||
|
||
private static float ToMillimeters(double meters) => (float)(meters * MillimetersPerMeter);
|
||
|
||
private static void EnsureFiniteAmrValue(double value, string name)
|
||
{
|
||
if (double.IsNaN(value) || double.IsInfinity(value))
|
||
throw new ArgumentException(name + " 必须是有限数。");
|
||
}
|
||
}
|