test: add path smoothing scenarios and fixtures
This commit is contained in:
梁薄云
@@ -0,0 +1,260 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Globalization;
|
||||
using System.IO;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Test;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using Newtonsoft.Json;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Test;
|
||||
|
||||
/// <summary>显式开发入口：从已成功的 Hybrid A* 粗路径结果生成八个稳定的快速比较夹具。</summary>
|
||||
public static class SmoothingFixtureGenerator
|
||||
{
|
||||
// 仅用于开发时固化真实规划输出；不会改变业务场景或运行时服务的预算。
|
||||
private static readonly TimeSpan FixtureGenerationSearchTimeout = TimeSpan.FromSeconds(120d);
|
||||
|
||||
/// <summary>生成夹具 JSON；目标已存在且未明确允许覆盖时拒绝写入。</summary>
|
||||
public static void Generate(string outputPath, bool overwrite)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(outputPath)) throw new ArgumentException("输出路径不能为空。", nameof(outputPath));
|
||||
if (File.Exists(outputPath) && !overwrite)
|
||||
throw new IOException("夹具目标已存在；必须显式允许覆盖。");
|
||||
|
||||
SmoothingFixtureDocument document = CreateDocument();
|
||||
Directory.CreateDirectory(Path.GetDirectoryName(Path.GetFullPath(outputPath)));
|
||||
var settings = new JsonSerializerSettings { Culture = CultureInfo.InvariantCulture, Formatting = Formatting.Indented };
|
||||
File.WriteAllText(outputPath, JsonConvert.SerializeObject(document, settings), new System.Text.UTF8Encoding(false));
|
||||
}
|
||||
|
||||
private static SmoothingFixtureDocument CreateDocument()
|
||||
{
|
||||
var document = new SmoothingFixtureDocument { SchemaVersion = 1 };
|
||||
var planner = new CoarsePathPlanningService();
|
||||
IReadOnlyList<FixturePlan> plans = CreatePlans();
|
||||
for (int index = 0; index < plans.Count; index++)
|
||||
{
|
||||
FixturePlan plan = plans[index];
|
||||
CoarsePathPlanningJob job = plan.CreateJob();
|
||||
job.Configuration.SearchTimeout = FixtureGenerationSearchTimeout;
|
||||
CoarsePathPlanningJobResult result = planner.Plan(job, CancellationToken.None);
|
||||
document.Scenarios.Add(CreateRecord(plan.Id, job, result));
|
||||
}
|
||||
|
||||
for (int index = 0; index < document.Scenarios.Count; index++)
|
||||
document.Scenarios[index].ConfigurationFingerprint = SmoothingScenarioFixtureLoader.ComputeFingerprint(document.Scenarios[index]);
|
||||
return document;
|
||||
}
|
||||
|
||||
private static IReadOnlyList<FixturePlan> CreatePlans()
|
||||
{
|
||||
return new List<FixturePlan>
|
||||
{
|
||||
new FixturePlan("straight", () => CoarsePathScenarioFactory.Create(CoarsePathTestScenario.ExplicitEmpty)),
|
||||
new FixturePlan("single-turn", () => CoarsePathScenarioFactory.CreateManualGoalDemo(1000d, 1000d, 0d, 3000d, 2000d, 45d)),
|
||||
new FixturePlan("s-bend", () => CoarsePathScenarioFactory.Create(CoarsePathTestScenario.ManualAndTwoLeg, 1000d, 1000d, 45d)),
|
||||
new FixturePlan("large-heading-change", () => CoarsePathScenarioFactory.CreateManualGoalDemo(1000d, 1000d, 0d, 3000d, 3000d, 90d)),
|
||||
new FixturePlan("rectangle-detour", () => CoarsePathScenarioFactory.Create(CoarsePathTestScenario.RectangleDetour)),
|
||||
new FixturePlan("multi-obstacle-detour", () => CoarsePathScenarioFactory.Create(CoarsePathTestScenario.ManualAndTwoLeg)),
|
||||
new FixturePlan("narrow-corridor", CreateNarrowCorridor),
|
||||
new FixturePlan("forward-reverse-switch", () => CoarsePathScenarioFactory.Create(CoarsePathTestScenario.ReverseGearSwitch)),
|
||||
};
|
||||
}
|
||||
|
||||
private static CoarsePathPlanningJob CreateNarrowCorridor()
|
||||
{
|
||||
return CreateFixedObstacleJob("narrow-corridor", new IMapObstacle[]
|
||||
{
|
||||
new AxisAlignedRectangleObstacle(1500f, 4500f, 500f, 1200f),
|
||||
new AxisAlignedRectangleObstacle(1500f, 4500f, 2800f, 3500f),
|
||||
});
|
||||
}
|
||||
|
||||
private static CoarsePathPlanningJob CreateFixedObstacleJob(string id, IReadOnlyList<IMapObstacle> obstacles)
|
||||
{
|
||||
var job = new CoarsePathPlanningJob
|
||||
{
|
||||
MapRequest = new PlanningMapRequest
|
||||
{
|
||||
Bounds = new MapBoundsMm(0f, 6000f, 0f, 4000f),
|
||||
ResolutionMm = 50f,
|
||||
AllowExplicitEmptyMap = false,
|
||||
ObstacleSources = new IMapObstacleSource[]
|
||||
{
|
||||
new ManualObstacleSource("fixture-" + id, 1L, true, obstacles),
|
||||
},
|
||||
},
|
||||
Start = new Pose2D(1d, 2d, 0d),
|
||||
Goal = new Pose2D(5d, 2d, 0d),
|
||||
Vehicle = new VehicleParameters
|
||||
{
|
||||
LengthMeters = 0.80d,
|
||||
WidthMeters = 0.60d,
|
||||
SafetyMarginMeters = 0.05d,
|
||||
MaximumCurvaturePerMeter = 1d / 1.20d,
|
||||
},
|
||||
Configuration = new HybridAStarConfiguration(),
|
||||
GoalDirection = GoalDirectionConstraint.Forward,
|
||||
};
|
||||
job.Configuration.SearchTimeout = TimeSpan.FromSeconds(30d);
|
||||
return job;
|
||||
}
|
||||
|
||||
private static SmoothingFixtureRecord CreateRecord(string id, CoarsePathPlanningJob job, CoarsePathPlanningJobResult result)
|
||||
{
|
||||
if (job == null || result == null || result.MapResult == null || !result.MapResult.Succeeded ||
|
||||
result.MapResult.Map == null || result.PlanningResult == null || result.PlanningResult.Status != PlanningStatus.Success)
|
||||
{
|
||||
string status = result == null || result.PlanningResult == null ? "no-result" : result.PlanningResult.Status.ToString();
|
||||
string reason = result == null || result.PlanningResult == null || result.PlanningResult.Diagnostics == null
|
||||
? string.Empty
|
||||
: result.PlanningResult.Diagnostics.TerminationReason;
|
||||
throw new InvalidOperationException("夹具场景未产生成功的粗路径：" + id + "；状态=" + status + "；原因=" + reason);
|
||||
}
|
||||
|
||||
PlanningGridMap map = result.MapResult.Map;
|
||||
IReadOnlyList<CoarsePathPoint> smoothingPath = SmoothingScenarioFactory.CopyWithFiniteClearance(result.PlanningResult.Path, map);
|
||||
var record = new SmoothingFixtureRecord
|
||||
{
|
||||
Id = id,
|
||||
FixtureVersion = 1,
|
||||
Map = CreateMapRecord(map, result.MapResult.SourceResults),
|
||||
Vehicle = CreateVehicleRecord(job.Vehicle),
|
||||
PlanningConfiguration = CreatePlanningConfigurationRecord(job.Configuration),
|
||||
};
|
||||
|
||||
for (int index = 0; index < smoothingPath.Count; index++)
|
||||
record.Path.Add(CreatePathPointRecord(smoothingPath[index]));
|
||||
for (int index = 0; index < result.PlanningResult.Segments.Count; index++)
|
||||
record.Segments.Add(CreateSegmentRecord(result.PlanningResult.Segments[index]));
|
||||
return record;
|
||||
}
|
||||
|
||||
private static SmoothingFixtureMap CreateMapRecord(PlanningGridMap map, IReadOnlyList<ObstacleProjectionResult> sourceResults)
|
||||
{
|
||||
var record = new SmoothingFixtureMap
|
||||
{
|
||||
XMinMm = map.Bounds.XMin,
|
||||
XMaxMm = map.Bounds.XMax,
|
||||
YMinMm = map.Bounds.YMin,
|
||||
YMaxMm = map.Bounds.YMax,
|
||||
ResolutionMm = map.ResolutionMm,
|
||||
};
|
||||
for (int sourceIndex = 0; sourceIndex < sourceResults.Count; sourceIndex++)
|
||||
{
|
||||
IReadOnlyList<IMapObstacle> obstacles = sourceResults[sourceIndex].Obstacles;
|
||||
for (int obstacleIndex = 0; obstacleIndex < obstacles.Count; obstacleIndex++)
|
||||
record.Obstacles.Add(CreateObstacleRecord(obstacles[obstacleIndex]));
|
||||
}
|
||||
return record;
|
||||
}
|
||||
|
||||
private static SmoothingFixtureObstacle CreateObstacleRecord(IMapObstacle obstacle)
|
||||
{
|
||||
if (obstacle is CircleObstacle circle)
|
||||
{
|
||||
return new SmoothingFixtureObstacle
|
||||
{
|
||||
Kind = "circle",
|
||||
CenterXMm = circle.CenterX,
|
||||
CenterYMm = circle.CenterY,
|
||||
RadiusMm = circle.RadiusMm,
|
||||
};
|
||||
}
|
||||
if (obstacle is AxisAlignedRectangleObstacle rectangle)
|
||||
{
|
||||
return new SmoothingFixtureObstacle
|
||||
{
|
||||
Kind = "axis-aligned-rectangle",
|
||||
XMinMm = rectangle.XMin,
|
||||
XMaxMm = rectangle.XMax,
|
||||
YMinMm = rectangle.YMin,
|
||||
YMaxMm = rectangle.YMax,
|
||||
};
|
||||
}
|
||||
throw new InvalidOperationException("夹具仅支持圆形和轴对齐矩形障碍物。" + obstacle?.GetType().FullName);
|
||||
}
|
||||
|
||||
private static SmoothingFixtureVehicle CreateVehicleRecord(VehicleParameters vehicle)
|
||||
{
|
||||
if (!VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvaturePerMeter))
|
||||
throw new InvalidOperationException("夹具车辆缺少有效最大曲率限制。");
|
||||
return new SmoothingFixtureVehicle
|
||||
{
|
||||
LengthMeters = vehicle.LengthMeters,
|
||||
WidthMeters = vehicle.WidthMeters,
|
||||
SafetyMarginMeters = vehicle.SafetyMarginMeters,
|
||||
MaximumCurvaturePerMeter = maximumCurvaturePerMeter,
|
||||
};
|
||||
}
|
||||
|
||||
private static SmoothingFixturePlanningConfiguration CreatePlanningConfigurationRecord(HybridAStarConfiguration configuration)
|
||||
{
|
||||
return new SmoothingFixturePlanningConfiguration
|
||||
{
|
||||
PrimitiveLengthMeters = configuration.PrimitiveLengthMeters,
|
||||
IntegrationStepMeters = configuration.IntegrationStepMeters,
|
||||
MaximumCollisionCheckStepMeters = configuration.MaximumCollisionCheckStepMeters,
|
||||
HeadingResolutionRadians = configuration.HeadingResolutionRadians,
|
||||
CurvatureLevelCount = configuration.CurvatureLevelCount,
|
||||
GoalPositionToleranceMeters = configuration.GoalPositionToleranceMeters,
|
||||
GoalHeadingToleranceRadians = configuration.GoalHeadingToleranceRadians,
|
||||
MaximumExpandedNodes = configuration.MaximumExpandedNodes,
|
||||
SearchTimeoutSeconds = configuration.SearchTimeout.TotalSeconds,
|
||||
HeuristicWeight = configuration.HeuristicWeight,
|
||||
ReverseCostMultiplier = configuration.ReverseCostMultiplier,
|
||||
GearSwitchPenaltyMeters = configuration.GearSwitchPenaltyMeters,
|
||||
CurvatureMagnitudeWeight = configuration.CurvatureMagnitudeWeight,
|
||||
CurvatureChangePenaltyMetersPerLevel = configuration.CurvatureChangePenaltyMetersPerLevel,
|
||||
ClearanceCostWeight = configuration.ClearanceCostWeight,
|
||||
ClearanceCostDistanceMeters = configuration.ClearanceCostDistanceMeters,
|
||||
AllowReverse = configuration.AllowReverse,
|
||||
};
|
||||
}
|
||||
|
||||
private static SmoothingFixturePathPoint CreatePathPointRecord(CoarsePathPoint point)
|
||||
{
|
||||
return new SmoothingFixturePathPoint
|
||||
{
|
||||
XMeters = point.X,
|
||||
YMeters = point.Y,
|
||||
HeadingRadians = point.Heading,
|
||||
UnwrappedHeadingRadians = point.UnwrappedHeading,
|
||||
ArcLengthMeters = point.ArcLength,
|
||||
Direction = point.Direction,
|
||||
VehicleCurvaturePerMeter = point.VehicleCurvature,
|
||||
BodyClearanceMeters = point.BodyClearance,
|
||||
IsGearSwitchPoint = point.IsGearSwitchPoint,
|
||||
Source = point.Source,
|
||||
};
|
||||
}
|
||||
|
||||
private static SmoothingFixtureSegment CreateSegmentRecord(PathSegment segment)
|
||||
{
|
||||
return new SmoothingFixtureSegment
|
||||
{
|
||||
SegmentIndex = segment.SegmentIndex,
|
||||
Direction = segment.Direction,
|
||||
StartIndex = segment.StartIndex,
|
||||
EndIndex = segment.EndIndex,
|
||||
StartsAtGearSwitch = segment.StartsAtGearSwitch,
|
||||
EndsAtGearSwitch = segment.EndsAtGearSwitch,
|
||||
};
|
||||
}
|
||||
|
||||
private sealed class FixturePlan
|
||||
{
|
||||
public FixturePlan(string id, Func<CoarsePathPlanningJob> createJob)
|
||||
{
|
||||
Id = id;
|
||||
CreateJob = createJob;
|
||||
}
|
||||
|
||||
public string Id { get; }
|
||||
public Func<CoarsePathPlanningJob> CreateJob { get; }
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user