70 lines
3.4 KiB
C#
70 lines
3.4 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Threading;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Test;
|
|
|
|
/// <summary>将快速夹具或现有粗路径业务结果转换为独立平滑比较请求。</summary>
|
|
public static class SmoothingScenarioFactory
|
|
{
|
|
/// <summary>不运行 Hybrid A*，从已验证的 JSON 夹具创建八个快速比较请求。</summary>
|
|
public static IReadOnlyList<PathSmoothingComparisonRequest> CreateFixtureRequests(string fixturePath)
|
|
{
|
|
IReadOnlyList<SmoothingScenarioFixture> fixtures = SmoothingScenarioFixtureLoader.LoadAndVerify(fixturePath);
|
|
var requests = new List<PathSmoothingComparisonRequest>(fixtures.Count);
|
|
for (int index = 0; index < fixtures.Count; index++)
|
|
{
|
|
SmoothingScenarioFixture fixture = fixtures[index];
|
|
requests.Add(new PathSmoothingComparisonRequest(
|
|
new PathSmoothingRequest(
|
|
fixture.Path,
|
|
fixture.Segments,
|
|
SmoothingScenarioFixtureLoader.BuildMap(fixture),
|
|
SmoothingScenarioFixtureLoader.BuildVehicle(fixture),
|
|
new PathSmoothingConfiguration())));
|
|
}
|
|
return requests.AsReadOnly();
|
|
}
|
|
|
|
/// <summary>把现有 <see cref="CoarsePathScenarioFactory"/> 已规划成功的结果转换为比较请求。</summary>
|
|
public static PathSmoothingComparisonRequest CreateEndToEndRequest(
|
|
CoarsePathPlanningJob job,
|
|
CoarsePathPlanningJobResult result)
|
|
{
|
|
if (job == null || result == null || result.MapResult == null || result.MapResult.Map == null ||
|
|
result.PlanningResult == null || result.PlanningResult.Status != PlanningStatus.Success)
|
|
throw new ArgumentException("只能从包含成功地图和粗路径的业务结果创建平滑比较请求。", nameof(result));
|
|
|
|
return new PathSmoothingComparisonRequest(new PathSmoothingRequest(
|
|
CopyWithFiniteClearance(result.PlanningResult.Path, result.MapResult.Map),
|
|
result.PlanningResult.Segments,
|
|
result.MapResult.Map,
|
|
job.Vehicle,
|
|
new PathSmoothingConfiguration()));
|
|
}
|
|
|
|
internal static IReadOnlyList<CoarsePathPoint> CopyWithFiniteClearance(
|
|
IReadOnlyList<CoarsePathPoint> source,
|
|
MultiWheelC.TrajectoryPlanning.Mapping.PlanningGridMap map)
|
|
{
|
|
double widthMeters = (map.Bounds.XMax - map.Bounds.XMin) / 1000d;
|
|
double heightMeters = (map.Bounds.YMax - map.Bounds.YMin) / 1000d;
|
|
double finiteEmptyMapClearance = Math.Sqrt(widthMeters * widthMeters + heightMeters * heightMeters);
|
|
var copy = new List<CoarsePathPoint>(source.Count);
|
|
for (int index = 0; index < source.Count; index++)
|
|
{
|
|
CoarsePathPoint point = source[index];
|
|
double clearance = double.IsPositiveInfinity(point.BodyClearance)
|
|
? finiteEmptyMapClearance
|
|
: point.BodyClearance;
|
|
copy.Add(new CoarsePathPoint(
|
|
point.X, point.Y, point.Heading, point.UnwrappedHeading, point.ArcLength, point.Direction,
|
|
point.VehicleCurvature, clearance, point.IsGearSwitchPoint, point.Source));
|
|
}
|
|
return copy.AsReadOnly();
|
|
}
|
|
}
|