chore: save current workspace progress
This commit is contained in:
梁薄云
+180
@@ -0,0 +1,180 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Comparison;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Visualization;
|
||||
|
||||
/// <summary>Transforms the raw-path and Local G2 results into shared report-figure data.</summary>
|
||||
public sealed class SmoothingFigureModelBuilder
|
||||
{
|
||||
private const double MarginPoints = 18d;
|
||||
private const double PanelGapPoints = 12d;
|
||||
|
||||
public SmoothingFigureModel Build(
|
||||
PathSmoothingComparisonResult comparison,
|
||||
PlanningGridMap map,
|
||||
Pose2D start,
|
||||
Pose2D goal,
|
||||
string scenarioId,
|
||||
string scenarioLabel)
|
||||
{
|
||||
if (comparison == null) throw new ArgumentNullException(nameof(comparison));
|
||||
if (map == null) throw new ArgumentNullException(nameof(map));
|
||||
if (start == null) throw new ArgumentNullException(nameof(start));
|
||||
if (goal == null) throw new ArgumentNullException(nameof(goal));
|
||||
|
||||
double worldXMin = map.Bounds.XMin / 1000d;
|
||||
double worldXMax = map.Bounds.XMax / 1000d;
|
||||
double worldYMin = map.Bounds.YMin / 1000d;
|
||||
double worldYMax = map.Bounds.YMax / 1000d;
|
||||
double innerWidth = IeeeFigureStyle.FigureWidthPoints - 2d * MarginPoints;
|
||||
double innerHeight = IeeeFigureStyle.FigureHeightPoints - 2d * MarginPoints;
|
||||
double pathWidth = innerWidth * 0.60d;
|
||||
double rightWidth = innerWidth - pathWidth - PanelGapPoints;
|
||||
double rightHeight = (innerHeight - PanelGapPoints) / 2d;
|
||||
|
||||
var model = new SmoothingFigureModel(
|
||||
scenarioId, scenarioLabel, worldXMin, worldXMax, worldYMin, worldYMax,
|
||||
MarginPoints, MarginPoints, pathWidth, innerHeight,
|
||||
MarginPoints + pathWidth + PanelGapPoints, MarginPoints, rightWidth, rightHeight,
|
||||
MarginPoints + pathWidth + PanelGapPoints, MarginPoints + rightHeight + PanelGapPoints, rightWidth, rightHeight,
|
||||
BuildObstacles(map), BuildSeries(comparison, map), BuildMetricRows(comparison),
|
||||
new SmoothingFigurePoint(start.X, start.Y, 0d, 0d), new SmoothingFigurePoint(goal.X, goal.Y, 0d, 0d));
|
||||
|
||||
double worldWidth = worldXMax - worldXMin;
|
||||
double worldHeight = worldYMax - worldYMin;
|
||||
if (worldWidth <= 0d || worldHeight <= 0d)
|
||||
throw new ArgumentOutOfRangeException(nameof(map), "Map bounds must be finite and non-degenerate.");
|
||||
double scale = Math.Min(pathWidth / worldWidth, innerHeight / worldHeight);
|
||||
model.PathScaleX = scale;
|
||||
model.PathScaleY = scale;
|
||||
return model;
|
||||
}
|
||||
|
||||
private static IReadOnlyList<SmoothingFigureObstacle> BuildObstacles(PlanningGridMap map)
|
||||
{
|
||||
var runs = new List<ObstacleRun>();
|
||||
double resolution = map.ResolutionMeters;
|
||||
double xMin = map.Bounds.XMin / 1000d;
|
||||
double yMin = map.Bounds.YMin / 1000d;
|
||||
for (int row = 0; row < map.Rows; row++)
|
||||
{
|
||||
int runStart = -1;
|
||||
for (int column = 0; column <= map.Cols; column++)
|
||||
{
|
||||
bool occupied = column < map.Cols && map.IsOccupied(row, column);
|
||||
if (occupied && runStart < 0) { runStart = column; continue; }
|
||||
if (!occupied && runStart >= 0)
|
||||
{
|
||||
AddOrExtendRun(runs, xMin + runStart * resolution, yMin + row * resolution,
|
||||
(column - runStart) * resolution, resolution);
|
||||
runStart = -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
var obstacles = new List<SmoothingFigureObstacle>(runs.Count);
|
||||
for (int index = 0; index < runs.Count; index++)
|
||||
{
|
||||
ObstacleRun run = runs[index];
|
||||
obstacles.Add(new SmoothingFigureObstacle(run.X, run.Y, run.Width, run.Height));
|
||||
}
|
||||
return obstacles;
|
||||
}
|
||||
|
||||
private static void AddOrExtendRun(IList<ObstacleRun> runs, double x, double y, double width, double height)
|
||||
{
|
||||
for (int index = runs.Count - 1; index >= 0; index--)
|
||||
{
|
||||
ObstacleRun candidate = runs[index];
|
||||
if (NearlyEqual(candidate.X, x) && NearlyEqual(candidate.Width, width) && NearlyEqual(candidate.Y + candidate.Height, y))
|
||||
{
|
||||
candidate.Height += height;
|
||||
return;
|
||||
}
|
||||
}
|
||||
runs.Add(new ObstacleRun(x, y, width, height));
|
||||
}
|
||||
|
||||
private static bool NearlyEqual(double first, double second) => Math.Abs(first - second) < 1e-9d;
|
||||
|
||||
private static IReadOnlyList<SmoothingFigureSeries> BuildSeries(PathSmoothingComparisonResult comparison, PlanningGridMap map)
|
||||
{
|
||||
return new List<SmoothingFigureSeries>(2)
|
||||
{
|
||||
CreateSeries(comparison.RawPathBaseline, null, "raw", "Raw coarse path", IeeeFigureStyle.RawColor, string.Empty, true, map),
|
||||
CreateSeries(FindLocalG2(comparison), SmoothingMethod.LocalG2Quintic, "local-g2", "Local G2", IeeeFigureStyle.LocalG2Color, string.Empty, false, map),
|
||||
};
|
||||
}
|
||||
|
||||
private static IReadOnlyList<SmoothingFigureMetricRow> BuildMetricRows(PathSmoothingComparisonResult comparison)
|
||||
{
|
||||
return new List<SmoothingFigureMetricRow>
|
||||
{
|
||||
CreateRow(comparison.RawPathBaseline, "RawPath", "Raw coarse path"),
|
||||
CreateRow(FindLocalG2(comparison), "LocalG2Quintic", "Local G2"),
|
||||
};
|
||||
}
|
||||
|
||||
private static PathSmoothingComparisonEntry FindLocalG2(PathSmoothingComparisonResult comparison)
|
||||
{
|
||||
for (int index = 0; index < comparison.Entries.Count; index++)
|
||||
{
|
||||
PathSmoothingComparisonEntry entry = comparison.Entries[index];
|
||||
if (entry.Method == SmoothingMethod.LocalG2Quintic) return entry;
|
||||
}
|
||||
return null;
|
||||
}
|
||||
|
||||
private static SmoothingFigureMetricRow CreateRow(PathSmoothingComparisonEntry entry, string method, string label)
|
||||
{
|
||||
return entry == null
|
||||
? new SmoothingFigureMetricRow(method, label, PathSmoothingStatus.Failed, new PathQualityMetrics(), null)
|
||||
: new SmoothingFigureMetricRow(method, label, entry.Status, entry.Metrics, entry.Timing);
|
||||
}
|
||||
|
||||
private static SmoothingFigureSeries CreateSeries(
|
||||
PathSmoothingComparisonEntry entry,
|
||||
SmoothingMethod? method,
|
||||
string key,
|
||||
string label,
|
||||
string color,
|
||||
string dashArray,
|
||||
bool isRawPathBaseline,
|
||||
PlanningGridMap map)
|
||||
{
|
||||
var points = new List<SmoothingFigurePoint>();
|
||||
var violations = new List<SmoothingFigurePoint>();
|
||||
PathSmoothingStatus status = entry == null ? PathSmoothingStatus.Failed : entry.Status;
|
||||
if (entry != null)
|
||||
{
|
||||
for (int index = 0; index < entry.Path.Count; index++)
|
||||
{
|
||||
SmoothedPathPoint point = entry.Path[index];
|
||||
var figurePoint = new SmoothingFigurePoint(point.X, point.Y, point.ArcLength, point.VehicleCurvature);
|
||||
points.Add(figurePoint);
|
||||
if (status == PathSmoothingStatus.Infeasible && map.IsOccupiedWorld(point.X, point.Y)) violations.Add(figurePoint);
|
||||
}
|
||||
}
|
||||
if (status == PathSmoothingStatus.Infeasible && points.Count > 0 && violations.Count == 0)
|
||||
violations.Add(points[points.Count / 2]);
|
||||
return new SmoothingFigureSeries(method, key, label, status, color, dashArray, isRawPathBaseline, points, violations);
|
||||
}
|
||||
|
||||
private sealed class ObstacleRun
|
||||
{
|
||||
public ObstacleRun(double x, double y, double width, double height)
|
||||
{
|
||||
X = x;
|
||||
Y = y;
|
||||
Width = width;
|
||||
Height = height;
|
||||
}
|
||||
|
||||
public double X { get; }
|
||||
public double Y { get; }
|
||||
public double Width { get; }
|
||||
public double Height { get; set; }
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user