406 lines
20 KiB
C#
406 lines
20 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Collections.ObjectModel;
|
|
using System.Drawing;
|
|
using System.Globalization;
|
|
using ClumsyCore;
|
|
using ClumsyCore.DTools;
|
|
using MDCSToolBox.Clumsy.Pilot;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
using MultiWheelC.TrajectoryPlanning.Mapping;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
|
|
|
public static class TrajectoryObservationPresentationText
|
|
{
|
|
public static string Create(TrajectoryObservationObservation observation, TrajectoryObservationCharts charts)
|
|
{
|
|
string projectionFailures = charts == null
|
|
? "unavailable"
|
|
: charts.FailedProjectionCount.ToString(CultureInfo.InvariantCulture);
|
|
if (observation == null || observation.SelectedPoint == null)
|
|
{
|
|
return "OBSERVE_ONLY: no chassis command is sent.\n" +
|
|
"selected trajectory point unavailable\n" +
|
|
"LS projection failures=" + projectionFailures;
|
|
}
|
|
|
|
EmTrajectoryPoint point = observation.SelectedPoint;
|
|
return "OBSERVE_ONLY: no chassis command is sent.\n" +
|
|
"selected t=" + point.TimeFromStart.ToString("F2", CultureInfo.InvariantCulture) +
|
|
" s, path-S=" + point.PathS.ToString("F2", CultureInfo.InvariantCulture) + " m\n" +
|
|
"signed speed=" + point.SignedLongitudinalVelocity.ToString("F2", CultureInfo.InvariantCulture) +
|
|
" m/s, yaw rate=" + point.YawRate.ToString("F2", CultureInfo.InvariantCulture) + " rad/s\n" +
|
|
"LS projection failures=" + projectionFailures;
|
|
}
|
|
}
|
|
|
|
public sealed class TrajectoryObservationLsPresentationPoint
|
|
{
|
|
public TrajectoryObservationLsPresentationPoint(double horizontalPathS, double verticalLateralOffset)
|
|
{
|
|
HorizontalPathS = horizontalPathS;
|
|
VerticalLateralOffset = verticalLateralOffset;
|
|
}
|
|
|
|
public double HorizontalPathS { get; }
|
|
|
|
public double VerticalLateralOffset { get; }
|
|
}
|
|
|
|
public sealed class TrajectoryObservationLsPresentationModel
|
|
{
|
|
private TrajectoryObservationLsPresentationModel(IReadOnlyList<TrajectoryObservationLsPresentationPoint> samples)
|
|
{
|
|
Samples = new ReadOnlyCollection<TrajectoryObservationLsPresentationPoint>(
|
|
new List<TrajectoryObservationLsPresentationPoint>(samples));
|
|
}
|
|
|
|
public string HorizontalAxisLabel => "ReferenceS (m)";
|
|
|
|
public string VerticalAxisLabel => "l (m)";
|
|
|
|
public IReadOnlyList<TrajectoryObservationLsPresentationPoint> Samples { get; }
|
|
|
|
public static TrajectoryObservationLsPresentationModel Create(TrajectoryObservationCharts charts)
|
|
{
|
|
if (charts == null) throw new ArgumentNullException(nameof(charts));
|
|
var samples = new List<TrajectoryObservationLsPresentationPoint>(charts.LsSamples.Count);
|
|
for (int index = 0; index < charts.LsSamples.Count; index++)
|
|
{
|
|
TrajectoryObservationLsSample sample = charts.LsSamples[index];
|
|
samples.Add(new TrajectoryObservationLsPresentationPoint(sample.PathS, sample.LateralOffset));
|
|
}
|
|
return new TrajectoryObservationLsPresentationModel(samples);
|
|
}
|
|
}
|
|
|
|
public sealed class TrajectoryObservationPresentation
|
|
{
|
|
private const float MillimetersPerMeter = 1000f;
|
|
private const int MaximumVisibleGridLines = 80;
|
|
private const float VelocitySeriesBaseMillimeters = -2500f;
|
|
|
|
private readonly Painter worldPainter = UI.GetPainter("TrajectoryObserver.World", true);
|
|
private readonly Painter lsPainter = UI.GetPainter("TrajectoryObserver.LS", true);
|
|
private readonly Painter stPainter = UI.GetPainter("TrajectoryObserver.ST", true);
|
|
|
|
public void DrawWorld(TrajectoryObservationBootstrapResult bootstrap,
|
|
TrajectoryObservationObservation observation, TrajectoryObservationRuntimeState runtimeState,
|
|
string diagnosticText)
|
|
{
|
|
worldPainter.Clear();
|
|
if (bootstrap == null)
|
|
{
|
|
worldPainter.DrawText(Color.LightYellow, "Trajectory bootstrap unavailable.", 0f, 0f);
|
|
return;
|
|
}
|
|
|
|
VehicleParameters vehicle = bootstrap.Vehicle;
|
|
DrawMap(bootstrap.Map);
|
|
DrawPose(bootstrap.Job == null ? null : bootstrap.Job.Start, vehicle, Color.LimeGreen, "计划起点");
|
|
DrawPose(bootstrap.Job == null ? null : bootstrap.Job.Goal, vehicle, Color.Orange, "终点");
|
|
DrawCoarsePath(bootstrap.CoarseResult == null ? null : bootstrap.CoarseResult.PlanningResult.Path);
|
|
DrawLocalG2Path(bootstrap.SmoothedPath == null ? null : bootstrap.SmoothedPath.Path);
|
|
DrawPose(observation == null || observation.VehicleState == null ? null : observation.VehicleState.Pose,
|
|
vehicle, Color.DeepSkyBlue, "车辆");
|
|
if (runtimeState != null && runtimeState.WorldNotice.Length > 0)
|
|
{
|
|
float noticeX = bootstrap.Map == null ? 0f : bootstrap.Map.Bounds.XMin + 100f;
|
|
float noticeY = bootstrap.Map == null ? 0f : bootstrap.Map.Bounds.YMax - 200f;
|
|
worldPainter.DrawText(Color.OrangeRed, runtimeState.WorldNotice, noticeX, noticeY);
|
|
}
|
|
|
|
EmTrajectory trajectory = observation == null ? null : observation.PublishedTrajectory;
|
|
if (trajectory == null || trajectory.Points == null || trajectory.Points.Count == 0)
|
|
{
|
|
float diagnosticX = bootstrap.Map == null ? 0f : bootstrap.Map.Bounds.XMin + 100f;
|
|
float diagnosticY = bootstrap.Map == null ? 0f : bootstrap.Map.Bounds.YMin + 300f;
|
|
DrawBoundaryMarkers(bootstrap.Segments);
|
|
worldPainter.DrawText(Color.LightYellow,
|
|
EmptyChartMessage("No published trajectory available.\n", diagnosticText), diagnosticX, diagnosticY);
|
|
return;
|
|
}
|
|
DrawEmPath(trajectory.Points);
|
|
DrawBoundaryMarkers(bootstrap.Segments);
|
|
}
|
|
|
|
public void DrawLs(TrajectoryObservationCharts charts, string diagnosticText)
|
|
{
|
|
lsPainter.Clear();
|
|
if (charts == null || charts.LsSamples == null || charts.LsSamples.Count == 0)
|
|
{
|
|
lsPainter.DrawText(Color.LightYellow,
|
|
EmptyChartMessage("No published trajectory available for L-S chart.\n", diagnosticText), 0f, 0f);
|
|
return;
|
|
}
|
|
|
|
TrajectoryObservationLsPresentationModel model = TrajectoryObservationLsPresentationModel.Create(charts);
|
|
float maxS = MaximumPathS(charts.LsSamples);
|
|
float maxL = MaximumLateralOffset(charts.LsSamples);
|
|
lsPainter.DrawLine(Color.Gainsboro, 0f, 0f, maxS, 0f, width: 2);
|
|
lsPainter.DrawLine(Color.Gainsboro, 0f, -maxL, 0f, maxL, width: 2);
|
|
lsPainter.DrawText(Color.White, "S-L", maxS + 100f, maxL + 150f);
|
|
lsPainter.DrawText(Color.White, model.HorizontalAxisLabel, maxS + 100f, -200f);
|
|
lsPainter.DrawText(Color.White, model.VerticalAxisLabel, 100f, maxL + 300f);
|
|
for (int index = 1; index < model.Samples.Count; index++)
|
|
{
|
|
TrajectoryObservationLsPresentationPoint previous = model.Samples[index - 1];
|
|
TrajectoryObservationLsPresentationPoint current = model.Samples[index];
|
|
lsPainter.DrawLine(Color.MediumPurple, ToMillimeters(previous.HorizontalPathS),
|
|
ToMillimeters(previous.VerticalLateralOffset), ToMillimeters(current.HorizontalPathS),
|
|
ToMillimeters(current.VerticalLateralOffset), width: 3);
|
|
}
|
|
}
|
|
|
|
public void DrawSt(TrajectoryObservationCharts charts, string diagnosticText)
|
|
{
|
|
stPainter.Clear();
|
|
if (charts == null || charts.StSamples == null || charts.StSamples.Count == 0)
|
|
{
|
|
stPainter.DrawText(Color.LightYellow,
|
|
EmptyChartMessage("No published trajectory available for T-S/T-V charts.\n", diagnosticText), 0f, 0f);
|
|
return;
|
|
}
|
|
|
|
float maxTime = MaximumTime(charts.StSamples, charts.SpeedSamples);
|
|
float maxS = MaximumPathS(charts.StSamples);
|
|
stPainter.DrawLine(Color.Gainsboro, 0f, 0f, maxTime, 0f, width: 2);
|
|
stPainter.DrawLine(Color.Gainsboro, 0f, 0f, 0f, maxS, width: 2);
|
|
stPainter.DrawText(Color.White, "T-S", 0f, maxS + 150f);
|
|
stPainter.DrawText(Color.White, "t (s)", maxTime + 100f, -200f);
|
|
stPainter.DrawText(Color.White, "PathS (m)", 100f, maxS + 300f);
|
|
DrawStSeries(charts.StSamples);
|
|
|
|
stPainter.DrawLine(Color.Gainsboro, 0f, VelocitySeriesBaseMillimeters,
|
|
maxTime, VelocitySeriesBaseMillimeters, width: 2);
|
|
stPainter.DrawLine(Color.Gainsboro, 0f, VelocitySeriesBaseMillimeters, 0f,
|
|
VelocitySeriesBaseMillimeters + MillimetersPerMeter, width: 2);
|
|
stPainter.DrawText(Color.White, "T-V", 0f, VelocitySeriesBaseMillimeters - 200f);
|
|
DrawSpeedSeries(charts.SpeedSamples);
|
|
DrawStLegend(maxTime, maxS);
|
|
}
|
|
|
|
public void ClearAll()
|
|
{
|
|
worldPainter.Clear();
|
|
lsPainter.Clear();
|
|
stPainter.Clear();
|
|
}
|
|
|
|
private static string EmptyChartMessage(string label, string diagnosticText)
|
|
{
|
|
return string.IsNullOrWhiteSpace(diagnosticText)
|
|
? label
|
|
: label + diagnosticText;
|
|
}
|
|
|
|
private void DrawMap(PlanningGridMap map)
|
|
{
|
|
if (map == null) return;
|
|
float xMin = map.Bounds.XMin;
|
|
float xMax = map.Bounds.XMax;
|
|
float yMin = map.Bounds.YMin;
|
|
float yMax = map.Bounds.YMax;
|
|
int gridStride = Math.Max(1, (int)Math.Ceiling(Math.Max(map.Rows, map.Cols) /
|
|
(double)MaximumVisibleGridLines));
|
|
for (int column = 0; column <= map.Cols; column += gridStride)
|
|
{
|
|
float x = Math.Min(xMax, xMin + column * map.ResolutionMm);
|
|
worldPainter.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 * map.ResolutionMm);
|
|
worldPainter.DrawLine(Color.FromArgb(80, Color.SlateGray), xMin, y, xMax, y, width: 1);
|
|
}
|
|
DrawRectangle(Color.Gainsboro, xMin, yMin, xMax, yMax, 3);
|
|
for (int row = 0; row < map.Rows; row++)
|
|
{
|
|
for (int column = 0; column < map.Cols; column++)
|
|
{
|
|
if (!map.IsOccupied(row, column)) continue;
|
|
float x = xMin + column * map.ResolutionMm + map.ResolutionMm / 2f;
|
|
float y = yMin + row * map.ResolutionMm;
|
|
worldPainter.DrawLine(Color.FromArgb(150, Color.Firebrick), x, y, x, y + map.ResolutionMm,
|
|
width: Math.Max(1, (int)Math.Round(map.ResolutionMm)));
|
|
}
|
|
}
|
|
}
|
|
|
|
private void DrawPose(Pose2D pose, VehicleParameters vehicle, Color color, string label)
|
|
{
|
|
if (pose == null) return;
|
|
double lengthMeters = vehicle == null || vehicle.LengthMeters <= 0d ? 0.8d : vehicle.LengthMeters;
|
|
double widthMeters = vehicle == null || vehicle.WidthMeters <= 0d ? 0.6d : vehicle.WidthMeters;
|
|
double heading = pose.Heading;
|
|
double cosHeading = Math.Cos(heading);
|
|
double sinHeading = Math.Sin(heading);
|
|
float x = ToMillimeters(pose.X);
|
|
float y = ToMillimeters(pose.Y);
|
|
float halfLengthMillimeters = (float)(lengthMeters * MillimetersPerMeter / 2d);
|
|
float halfWidthMillimeters = (float)(widthMeters * MillimetersPerMeter / 2d);
|
|
float frontLeftX = x + (float)(cosHeading * halfLengthMillimeters - sinHeading * halfWidthMillimeters);
|
|
float frontLeftY = y + (float)(sinHeading * halfLengthMillimeters + cosHeading * halfWidthMillimeters);
|
|
float frontRightX = x + (float)(cosHeading * halfLengthMillimeters + sinHeading * halfWidthMillimeters);
|
|
float frontRightY = y + (float)(sinHeading * halfLengthMillimeters - cosHeading * halfWidthMillimeters);
|
|
float rearRightX = x + (float)(-cosHeading * halfLengthMillimeters + sinHeading * halfWidthMillimeters);
|
|
float rearRightY = y + (float)(-sinHeading * halfLengthMillimeters - cosHeading * halfWidthMillimeters);
|
|
float rearLeftX = x + (float)(-cosHeading * halfLengthMillimeters - sinHeading * halfWidthMillimeters);
|
|
float rearLeftY = y + (float)(-sinHeading * halfLengthMillimeters + cosHeading * halfWidthMillimeters);
|
|
DrawVehicleOutline(frontLeftX, frontLeftY, frontRightX, frontRightY,
|
|
rearRightX, rearRightY, rearLeftX, rearLeftY, color);
|
|
float headingLengthMillimeters = (float)(lengthMeters * MillimetersPerMeter * 0.25d);
|
|
worldPainter.DrawLine(color, x, y,
|
|
x + (float)cosHeading * headingLengthMillimeters,
|
|
y + (float)sinHeading * headingLengthMillimeters, endArrow: false, width: 1);
|
|
worldPainter.DrawText(color, label, x + 100f, y + 100f);
|
|
}
|
|
|
|
private void DrawVehicleOutline(float frontLeftX, float frontLeftY, float frontRightX, float frontRightY,
|
|
float rearRightX, float rearRightY, float rearLeftX, float rearLeftY, Color color)
|
|
{
|
|
worldPainter.DrawLine(color, frontLeftX, frontLeftY, frontRightX, frontRightY, width: 1);
|
|
worldPainter.DrawLine(color, frontRightX, frontRightY, rearRightX, rearRightY, width: 1);
|
|
worldPainter.DrawLine(color, rearRightX, rearRightY, rearLeftX, rearLeftY, width: 1);
|
|
worldPainter.DrawLine(color, rearLeftX, rearLeftY, frontLeftX, frontLeftY, width: 1);
|
|
}
|
|
|
|
private void DrawBoundaryMarkers(IReadOnlyList<DirectionSegmentView> segments)
|
|
{
|
|
if (segments == null) return;
|
|
for (int index = 0; index < segments.Count; index++)
|
|
{
|
|
DirectionSegmentView segment = segments[index];
|
|
if (segment.Points.Count == 0) continue;
|
|
var point = segment.Points[segment.Points.Count - 1];
|
|
if (segment.EndBoundary.BoundaryType == EmBoundaryType.GearSwitchApproach)
|
|
{
|
|
DrawDiamondMarker(point, Color.Orange,
|
|
"换向点 " + (segment.SegmentIndex + 1).ToString(CultureInfo.InvariantCulture) + " / s_end");
|
|
}
|
|
else if (segment.EndBoundary.BoundaryType == EmBoundaryType.Goal)
|
|
{
|
|
DrawCrossMarker(point, Color.OrangeRed, "终点 / s_end");
|
|
}
|
|
}
|
|
}
|
|
|
|
private void DrawDiamondMarker(SmoothedPathPoint point, Color color, string label)
|
|
{
|
|
float x = ToMillimeters(point.X);
|
|
float y = ToMillimeters(point.Y);
|
|
const float radius = 50f;
|
|
worldPainter.DrawLine(color, x, y + radius, x + radius, y, width: 1);
|
|
worldPainter.DrawLine(color, x + radius, y, x, y - radius, width: 1);
|
|
worldPainter.DrawLine(color, x, y - radius, x - radius, y, width: 1);
|
|
worldPainter.DrawLine(color, x - radius, y, x, y + radius, width: 1);
|
|
worldPainter.DrawText(color, label, x + 100f, y + 100f);
|
|
}
|
|
|
|
private void DrawCrossMarker(SmoothedPathPoint point, Color color, string label)
|
|
{
|
|
float x = ToMillimeters(point.X);
|
|
float y = ToMillimeters(point.Y);
|
|
const float radius = 50f;
|
|
worldPainter.DrawLine(color, x - radius, y - radius, x + radius, y + radius, width: 1);
|
|
worldPainter.DrawLine(color, x - radius, y + radius, x + radius, y - radius, width: 1);
|
|
worldPainter.DrawText(color, label, x + 100f, y + 100f);
|
|
}
|
|
|
|
private void DrawCoarsePath(IReadOnlyList<CoarsePathPoint> path)
|
|
{
|
|
if (path == null || path.Count == 0) return;
|
|
for (int index = 1; index < path.Count; index++)
|
|
worldPainter.DrawLine(Color.LimeGreen, ToMillimeters(path[index - 1].X), ToMillimeters(path[index - 1].Y),
|
|
ToMillimeters(path[index].X), ToMillimeters(path[index].Y), width: 2);
|
|
}
|
|
|
|
private void DrawLocalG2Path(IReadOnlyList<SmoothedPathPoint> path)
|
|
{
|
|
if (path == null || path.Count == 0) return;
|
|
for (int index = 1; index < path.Count; index++)
|
|
worldPainter.DrawLine(Color.Gold, ToMillimeters(path[index - 1].X), ToMillimeters(path[index - 1].Y),
|
|
ToMillimeters(path[index].X), ToMillimeters(path[index].Y), width: 1);
|
|
}
|
|
|
|
private void DrawEmPath(IReadOnlyList<EmTrajectoryPoint> path)
|
|
{
|
|
for (int index = 1; index < path.Count; index++)
|
|
worldPainter.DrawLine(Color.DeepPink, ToMillimeters(path[index - 1].X), ToMillimeters(path[index - 1].Y),
|
|
ToMillimeters(path[index].X), ToMillimeters(path[index].Y), width: 2);
|
|
}
|
|
|
|
private void DrawRectangle(Color color, float xMin, float yMin, float xMax, float yMax, int width)
|
|
{
|
|
worldPainter.DrawLine(color, xMin, yMin, xMax, yMin, width: width);
|
|
worldPainter.DrawLine(color, xMax, yMin, xMax, yMax, width: width);
|
|
worldPainter.DrawLine(color, xMax, yMax, xMin, yMax, width: width);
|
|
worldPainter.DrawLine(color, xMin, yMax, xMin, yMin, width: width);
|
|
}
|
|
|
|
private void DrawStSeries(IReadOnlyList<TrajectoryObservationStSample> samples)
|
|
{
|
|
for (int index = 1; index < samples.Count; index++)
|
|
stPainter.DrawLine(Color.Cyan, ToMillimeters(samples[index - 1].TimeFromStart),
|
|
ToMillimeters(samples[index - 1].PathS), ToMillimeters(samples[index].TimeFromStart),
|
|
ToMillimeters(samples[index].PathS), width: 3);
|
|
}
|
|
|
|
private void DrawSpeedSeries(IReadOnlyList<TrajectoryObservationSpeedSample> samples)
|
|
{
|
|
if (samples == null) return;
|
|
for (int index = 1; index < samples.Count; index++)
|
|
stPainter.DrawLine(Color.Orange, ToMillimeters(samples[index - 1].TimeFromStart),
|
|
VelocitySeriesBaseMillimeters + ToMillimeters(samples[index - 1].SignedLongitudinalVelocity),
|
|
ToMillimeters(samples[index].TimeFromStart),
|
|
VelocitySeriesBaseMillimeters + ToMillimeters(samples[index].SignedLongitudinalVelocity), width: 3);
|
|
}
|
|
|
|
private void DrawStLegend(float maxTime, float maxS)
|
|
{
|
|
float x = maxTime + 250f;
|
|
stPainter.DrawLine(Color.Cyan, x, maxS, x + 200f, maxS, width: 4);
|
|
stPainter.DrawText(Color.Cyan, "t-s", x + 250f, maxS - 50f);
|
|
stPainter.DrawLine(Color.Orange, x, VelocitySeriesBaseMillimeters, x + 200f,
|
|
VelocitySeriesBaseMillimeters, width: 4);
|
|
stPainter.DrawText(Color.Orange, "t-v", x + 250f, VelocitySeriesBaseMillimeters - 50f);
|
|
}
|
|
|
|
private static float MaximumPathS(IReadOnlyList<TrajectoryObservationLsSample> samples)
|
|
{
|
|
double maximum = 1d;
|
|
for (int index = 0; index < samples.Count; index++) maximum = Math.Max(maximum, samples[index].PathS);
|
|
return ToMillimeters(maximum);
|
|
}
|
|
|
|
private static float MaximumPathS(IReadOnlyList<TrajectoryObservationStSample> samples)
|
|
{
|
|
double maximum = 1d;
|
|
for (int index = 0; index < samples.Count; index++) maximum = Math.Max(maximum, samples[index].PathS);
|
|
return ToMillimeters(maximum);
|
|
}
|
|
|
|
private static float MaximumLateralOffset(IReadOnlyList<TrajectoryObservationLsSample> samples)
|
|
{
|
|
double maximum = 1d;
|
|
for (int index = 0; index < samples.Count; index++) maximum = Math.Max(maximum, Math.Abs(samples[index].LateralOffset));
|
|
return ToMillimeters(maximum);
|
|
}
|
|
|
|
private static float MaximumTime(IReadOnlyList<TrajectoryObservationStSample> stSamples,
|
|
IReadOnlyList<TrajectoryObservationSpeedSample> speedSamples)
|
|
{
|
|
double maximum = 1d;
|
|
for (int index = 0; index < stSamples.Count; index++) maximum = Math.Max(maximum, stSamples[index].TimeFromStart);
|
|
if (speedSamples != null)
|
|
for (int index = 0; index < speedSamples.Count; index++)
|
|
maximum = Math.Max(maximum, speedSamples[index].TimeFromStart);
|
|
return ToMillimeters(maximum);
|
|
}
|
|
|
|
private static float ToMillimeters(double meters) => (float)(meters * MillimetersPerMeter);
|
|
}
|