261 lines
12 KiB
C#
261 lines
12 KiB
C#
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; }
|
|
}
|
|
}
|