315 lines
19 KiB
C#
315 lines
19 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
using System.Threading;
|
||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;
|
||
using MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
||
using Trajectory2D = MultiWheelC.Trajectory.Trajectory2D;
|
||
|
||
namespace MultiWheelC.TrajectoryPlanning.Guide;
|
||
|
||
/// <summary>固定离线示例的只读输出，同时保留 EM 时间轨迹和控制器几何轨迹。</summary>
|
||
public sealed class FixedOfflinePlanningOutput
|
||
{
|
||
public FixedOfflinePlanningOutput(
|
||
EmTrajectory emTrajectory, // 来源：EM 发布闸门后的非空时间轨迹。
|
||
Trajectory2D controllerTrajectory) // 来源：同一 EM 轨迹经控制适配器生成的 SI 轨迹。
|
||
{
|
||
EmTrajectory = emTrajectory ??
|
||
throw new ArgumentNullException(nameof(emTrajectory));
|
||
ControllerTrajectory = controllerTrajectory ??
|
||
throw new ArgumentNullException(nameof(controllerTrajectory));
|
||
}
|
||
|
||
public EmTrajectory EmTrajectory { get; }
|
||
|
||
public Trajectory2D ControllerTrajectory { get; }
|
||
}
|
||
|
||
/// <summary>
|
||
/// 展示固定、静态、单方向的离线规划数据流；本类不读取实时定位，也不发送底盘命令。
|
||
/// </summary>
|
||
public static class EmPlannerFullPipelineDemo
|
||
{
|
||
public static FixedOfflinePlanningOutput CreateFixedOfflineTrajectory(
|
||
CancellationToken cancellationToken)
|
||
{
|
||
// 步骤 1：定义固定世界起点和终点
|
||
var startPose = new Pose2D(
|
||
1.50d, // 来源：固定场景起点世界 X；单位 m。
|
||
1.50d, // 来源：固定场景起点世界 Y；单位 m。
|
||
0d); // 来源：固定场景起点车头航向；单位 rad。
|
||
var goalPose = new Pose2D(
|
||
6.50d, // 来源：固定场景终点世界 X；单位 m。
|
||
1.50d, // 来源：固定场景终点世界 Y；单位 m。
|
||
0d); // 来源：固定场景终点车头航向；单位 rad。
|
||
|
||
// 步骤 2：定义车辆、曲率和搜索预算
|
||
const double vehicleLengthMeters = 1.00d;
|
||
const double vehicleWidthMeters = 0.60d;
|
||
const double safetyMarginMeters = 0.10d;
|
||
const double maximumCurvaturePerMeter = 0.80d;
|
||
var vehicle = new VehicleParameters
|
||
{
|
||
LengthMeters = vehicleLengthMeters, // 来源：固定车辆本体长度；单位 m。
|
||
WidthMeters = vehicleWidthMeters, // 来源：固定车辆本体宽度；单位 m。
|
||
SafetyMarginMeters = safetyMarginMeters, // 来源：固定碰撞安全余量；单位 m。
|
||
MaximumCurvaturePerMeter = maximumCurvaturePerMeter, // 来源：固定运动学上限；单位 1/m。
|
||
MinimumTurningRadiusMeters = null, // 来源：本示例已直接给出最大曲率；单位 m。
|
||
};
|
||
var coarseConfiguration = new HybridAStarConfiguration
|
||
{
|
||
PrimitiveLengthMeters = 0.40d, // 来源：固定搜索原语长度；单位 m。
|
||
IntegrationStepMeters = 0.05d, // 来源：固定原语积分间距；单位 m。
|
||
MaximumCollisionCheckStepMeters = 0.025d, // 来源：固定连续碰撞检查步长；单位 m。
|
||
HeadingResolutionRadians = Math.PI / 36d, // 来源：固定航向离散；单位 rad。
|
||
CurvatureLevelCount = 5, // 来源：固定曲率离散级数；无单位。
|
||
GoalPositionToleranceMeters = 0.15d, // 来源：固定终点位置容差；单位 m。
|
||
GoalHeadingToleranceRadians = Math.PI / 36d, // 来源：固定终点航向容差；单位 rad。
|
||
MaximumExpandedNodes = 100000, // 来源：固定节点预算；单位为节点数。
|
||
SearchTimeout = TimeSpan.FromSeconds(5d), // 来源：固定搜索时间预算；单位 s。
|
||
HeuristicWeight = 1d, // 来源：固定启发式权重；无单位。
|
||
ReverseCostMultiplier = 1.5d, // 来源：固定倒车代价倍率；无单位。
|
||
GearSwitchPenaltyMeters = 1d, // 来源：固定换向惩罚；单位 m 等价代价。
|
||
CurvatureMagnitudeWeight = 0.10d, // 来源：固定曲率代价权重；无单位。
|
||
CurvatureChangePenaltyMetersPerLevel = 0.05d, // 来源：固定曲率变化惩罚；单位 m/级。
|
||
ClearanceCostWeight = 0.20d, // 来源：固定净空代价权重；无单位。
|
||
ClearanceCostDistanceMeters = 0.50d, // 来源：固定净空代价作用距离；单位 m。
|
||
AllowReverse = false, // 来源：固定场景只允许前进；无单位。
|
||
};
|
||
|
||
// 步骤 3：建立静态地图
|
||
var bounds = new MapBoundsMm(
|
||
0f, // 来源：固定地图 X 下界；单位 mm。
|
||
8000f, // 来源：固定地图 X 上界；单位 mm。
|
||
0f, // 来源：固定地图 Y 下界；单位 mm。
|
||
5000f); // 来源：固定地图 Y 上界；单位 mm。
|
||
const float resolutionMm = 50f;
|
||
IReadOnlyList<IMapObstacle> obstacles = new IMapObstacle[]
|
||
{
|
||
new CircleObstacle(
|
||
4000f, // 来源：固定圆障碍圆心 X；单位 mm。
|
||
3500f, // 来源：固定圆障碍圆心 Y；单位 mm。
|
||
400f), // 来源：固定圆障碍半径；单位 mm。
|
||
};
|
||
var obstacleSource = new ManualObstacleSource(
|
||
"fixed-offline-obstacles", // 来源：固定障碍快照身份；无单位。
|
||
1L, // 来源：固定障碍快照版本；非负版本号。
|
||
true, // 来源：障碍来源必须成功投影；无单位。
|
||
obstacles); // 来源：上方固定世界障碍列表；几何坐标单位 mm。
|
||
var mapRequest = new PlanningMapRequest
|
||
{
|
||
Bounds = bounds, // 来源：固定地图边界；单位 mm。
|
||
ResolutionMm = resolutionMm, // 来源：固定栅格边长；单位 mm。
|
||
ObstacleSources = new IMapObstacleSource[] { obstacleSource }, // 来源：固定障碍来源快照。
|
||
AllowExplicitEmptyMap = false, // 来源：本场景要求障碍物真实参与建图；无单位。
|
||
};
|
||
PlanningMapBuildResult mapResult = new PlanningMapFactory().Create(mapRequest);
|
||
if (!mapResult.Succeeded || mapResult.Map == null || !mapResult.Map.PlanningReady)
|
||
{
|
||
string reason = mapResult.Map != null && !mapResult.Map.PlanningReady
|
||
? mapResult.Map.PlanningBlockReason
|
||
: mapResult.FailureReason;
|
||
throw new InvalidOperationException("固定离线地图不可用于规划：" + reason);
|
||
}
|
||
PlanningGridMap map = mapResult.Map;
|
||
|
||
// 步骤 4：建立粗路径请求
|
||
var coarseRequest = new PlanningRequest
|
||
{
|
||
Map = map, // 来源：步骤 3 的冻结地图快照。
|
||
Start = startPose, // 来源：步骤 1 的起点；位置 m、航向 rad。
|
||
Goal = goalPose, // 来源：步骤 1 的终点；位置 m、航向 rad。
|
||
Vehicle = vehicle, // 来源：步骤 2 的车辆；尺寸 m、曲率 1/m。
|
||
Configuration = coarseConfiguration, // 来源：步骤 2 的固定搜索配置。
|
||
StartVehicleCurvature = 0d, // 来源：固定起步直行状态；单位 1/m。
|
||
StartDirection = TravelDirection.Forward, // 来源：固定场景仅前进；无单位。
|
||
GoalDirection = GoalDirectionConstraint.Forward, // 来源：固定终点以前进方向到达；无单位。
|
||
};
|
||
|
||
// 步骤 5：搜索粗路径
|
||
PlanningResult coarseResult = new HybridAStarPlanner().Plan(
|
||
coarseRequest,
|
||
cancellationToken);
|
||
if (coarseResult.Status != PlanningStatus.Success)
|
||
{
|
||
throw new InvalidOperationException(
|
||
"Hybrid A* 粗路径失败：" +
|
||
coarseResult.Diagnostics.TerminationReason);
|
||
}
|
||
|
||
// 步骤 6：请求并执行 Local G2 平滑
|
||
var smoothingConfiguration = new PathSmoothingConfiguration
|
||
{
|
||
OutputSpacingMeters = 0.025d, // 来源：固定平滑输出间距；单位 m。
|
||
MaximumCollisionCheckStepMeters = 0.025d, // 来源：固定扫掠复核步长；单位 m。
|
||
MinimumClearanceReserveMeters = 0.02d, // 来源：固定额外净空；单位 m。
|
||
CurvatureLimitRadiusToleranceMeters = 0.002d, // 来源：固定曲率半径容差；单位 m。
|
||
};
|
||
smoothingConfiguration.LocalG2Quintic.MinimumWindowLengthMeters = 0.20d; // 最短窗口避免过短过渡放大曲率变化；单位 m。
|
||
smoothingConfiguration.LocalG2Quintic.PreferredWindowLengthMeters = 0.50d; // 优先用半米窗口平衡平滑范围与局部性；单位 m。
|
||
smoothingConfiguration.LocalG2Quintic.MaximumWindowLengthMeters = 0.80d; // 限制窗口不侵入过远路径几何；单位 m。
|
||
smoothingConfiguration.LocalG2Quintic.MaximumDeviationMeters = 0.10d; // 候选最多偏离粗路径十厘米；单位 m。
|
||
smoothingConfiguration.LocalG2Quintic.AbsoluteCurvatureJumpFloorPerMeter = 0.001d; // 忽略低于数值噪声量级的曲率跳变；单位 1/m。
|
||
smoothingConfiguration.LocalG2Quintic.CurvatureJumpRatioOfMaximum = 0.05d; // 以车辆最大曲率的 5% 识别相对跳变；无单位。
|
||
smoothingConfiguration.LocalG2Quintic.MinimumPeakGradientImprovementRatio = 0.20d; // 候选至少改善 20% 峰值曲率梯度；无单位。
|
||
smoothingConfiguration.LocalG2Quintic.MaximumVariationCostRegressionRatio = 0.02d; // 最多容许 2% 曲率变化代价退化；无单位。
|
||
smoothingConfiguration.LocalG2Quintic.MaximumCandidatesPerRegion = 12; // 限制每个区域的搜索工作量；单位为候选数。
|
||
var smoothingRequest = new PathSmoothingRequest(
|
||
coarseResult.Path, // 来源：成功粗路径；位置与弧长单位 m、航向 rad。
|
||
coarseResult.Segments, // 来源：成功粗路径的方向分段；无单位。
|
||
map, // 来源：步骤 3 的同一冻结地图快照。
|
||
vehicle, // 来源：步骤 2 的同一车辆约束；尺寸 m、曲率 1/m。
|
||
smoothingConfiguration); // 来源：本步骤显式固定的 Local G2 配置。
|
||
PathSmoothingResult smoothingResult = new PathSmoothingService().Smooth(
|
||
smoothingRequest,
|
||
cancellationToken);
|
||
bool smoothingSucceeded =
|
||
smoothingResult.Status == PathSmoothingStatus.Complete ||
|
||
smoothingResult.Status == PathSmoothingStatus.PartialImprovement ||
|
||
smoothingResult.Status == PathSmoothingStatus.NotNeeded ||
|
||
smoothingResult.Status == PathSmoothingStatus.Unchanged;
|
||
if (!smoothingSucceeded ||
|
||
smoothingResult.Path == null || smoothingResult.Path.Count == 0 ||
|
||
smoothingResult.Segments == null || smoothingResult.Segments.Count == 0)
|
||
{
|
||
throw new InvalidOperationException(
|
||
"Local G2 平滑没有发布完整路径：" +
|
||
smoothingResult.Diagnostics.TerminationReason);
|
||
}
|
||
|
||
// 步骤 7：选择一个方向段
|
||
IReadOnlyList<DirectionSegmentView> directionSegments =
|
||
ReferencePathSegmenter.Create(smoothingResult);
|
||
if (directionSegments.Count != 1)
|
||
{
|
||
throw new InvalidOperationException(
|
||
"固定控制器演示不得跨越换挡边界，平滑路径必须恰好包含一个方向段。");
|
||
}
|
||
DirectionSegmentView activeSegment = directionSegments[0];
|
||
if (activeSegment.Direction != TravelDirection.Forward)
|
||
{
|
||
throw new InvalidOperationException("固定离线演示只接受前进方向段。");
|
||
}
|
||
|
||
// 步骤 8：建立初始运动状态
|
||
DateTimeOffset planningTimeUtc = DateTimeOffset.UtcNow;
|
||
var vehicleState = new VehicleMotionState(
|
||
startPose, // 来源：步骤 1 的固定起点；位置 m、航向 rad。
|
||
0d, // 来源：固定静止起步状态；有符号纵向速度单位 m/s。
|
||
0d, // 来源：固定静止起步状态；纵向加速度单位 m/s²。
|
||
planningTimeUtc, // 来源：本次离线调用的 UTC 快照时刻。
|
||
0L); // 来源：固定离线状态版本；非负序列号。
|
||
|
||
// 步骤 9：冻结 EM 配置
|
||
EmPlannerConfiguration emConfiguration = EmPlannerConfiguration.CreateDefault();
|
||
emConfiguration.Scheduling.TimeHorizonSeconds = 20d; // 仅供 RollingHorizon 裁剪预测时间；单位 s；FullDirectionSegment 使用完整方向段与自适应时间表，不以此截断。
|
||
emConfiguration.Scheduling.DistanceHorizonMeters = 10d; // 仅供 RollingHorizon 裁剪参考距离；单位 m；完整方向段范围不受此值截断。
|
||
emConfiguration.Scheduling.OutputTimeStepSeconds = 0.10d; // 固定发布间隔；单位 s。
|
||
emConfiguration.Scheduling.SolverTimeoutSeconds = 5d; // 给离线 OSQP 足够的求解预算上限；单位 s。
|
||
emConfiguration.Scheduling.MaximumVehicleStateAgeSeconds = 1d; // 固定状态时效；单位 s。
|
||
emConfiguration.Scheduling.MaximumOptimizationTimeStepSeconds = 0.20d; // 自适应全段时间结点的最大间隔；单位 s。
|
||
emConfiguration.Scheduling.MaximumOptimizationSpatialStepMeters = 0.10d; // 自适应全段空间结点的最大间隔；单位 m。
|
||
emConfiguration.Scheduling.MaximumOptimizationKnotCount = 401; // 限制完整方向段自适应离散的资源上限；单位为结点数。
|
||
emConfiguration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 0.60d; // 限制固定停车场景的前进峰值速度；单位 m/s。
|
||
emConfiguration.Longitudinal.DesiredForwardSpeedMetersPerSecond = 0.40d; // 采用低于上限的低速巡航目标；单位 m/s。
|
||
emConfiguration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 0.40d; // 限制起步加速以保持低速平顺；单位 m/s²。
|
||
emConfiguration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 0.40d; // 为终点精确停车限制减速度幅值；单位 m/s²。
|
||
emConfiguration.Longitudinal.MaximumJerkMetersPerSecondCubed = 1.00d; // 限制加速度变化以避免突变；单位 m/s³。
|
||
emConfiguration.Solver.MaximumOuterIterations = 5; // 限制 LS/ST 顺序凸化的外层资源；单位为迭代次数。
|
||
emConfiguration.Solver.MaximumOsqpIterations = 10000; // 为每次 QP 收敛提供明确上限；单位为 OSQP 迭代次数。
|
||
|
||
// 步骤 10：建立 EM 请求
|
||
var emRequest = new EmPlanningRequest(
|
||
smoothingResult, // 来源：步骤 6 的完整平滑路径；位置与弧长单位 m。
|
||
map, // 来源：步骤 3 的冻结地图快照。
|
||
vehicle, // 来源：步骤 2 的车辆约束；尺寸 m、曲率 1/m。
|
||
vehicleState, // 来源：步骤 8 的固定运动状态；SI 单位、UTC。
|
||
emConfiguration, // 来源：步骤 9 的显式低速泊车配置。
|
||
0, // 来源：步骤 7 已验证的唯一方向段；零基索引。
|
||
null, // 来源：首次固定离线规划没有上一条轨迹。
|
||
planningTimeUtc, // 来源：本次请求发起 UTC 时刻。
|
||
planningTimeUtc, // 来源：本次轨迹生效 UTC 时刻。
|
||
"fixed-offline-em-trajectory", // 来源：固定非空输出轨迹身份。
|
||
"fixed-offline-reference-path", // 来源：固定非空参考路径身份。
|
||
string.Empty, // 来源：没有上一条轨迹，因此没有前序身份。
|
||
EmMotionModel.NonholonomicForwardReverse, // 来源：当前 EM 支持的非完整运动模型。
|
||
EmPlanningScope.FullDirectionSegment, // 来源：固定示例规划完整单方向段。
|
||
null, // 来源：离线示例不另设循环截止剩余时间；单位 s。
|
||
cancellationToken, // 来源：方法调用方的取消令牌。
|
||
CancellationToken.None); // 来源：离线示例没有独立循环截止令牌。
|
||
|
||
// 步骤 11：调用 EM 服务
|
||
var emPlanningService = new EmPlanningService(
|
||
new OsqpNativeSolver()); // 来源：仓库现有原生 OSQP 求解器；无单位。
|
||
EmPlanningResult emResult = emPlanningService.Plan(
|
||
emRequest,
|
||
cancellationToken);
|
||
|
||
// 步骤 12：判断是否可发布
|
||
bool emSucceeded = emResult.Status == EmPlanningStatus.Success ||
|
||
emResult.Status == EmPlanningStatus.SuccessWithFallback;
|
||
if (!emSucceeded ||
|
||
emResult.Trajectory == null ||
|
||
emResult.Trajectory.Points.Count < 2)
|
||
{
|
||
throw new InvalidOperationException(
|
||
"EM 规划没有返回完整可控轨迹：" + emResult.FailureReason);
|
||
}
|
||
EmTrajectory emTrajectory = emResult.Trajectory;
|
||
|
||
// 步骤 13：读取输出契约
|
||
// 以下局部变量只用于逐字段检查与诊断示范，不是车辆命令，也绝不能直接下发到底盘。
|
||
string trajectoryId = emTrajectory.Metadata.TrajectoryId;
|
||
string referencePathId = emTrajectory.Metadata.ReferencePathId;
|
||
TravelDirection publishedDirection = emTrajectory.Metadata.Direction;
|
||
EmTerminalType terminalType = emTrajectory.Metadata.TerminalType;
|
||
EmPlanningScope publishedScope = emTrajectory.Metadata.PlanningScope;
|
||
_ = trajectoryId;
|
||
_ = referencePathId;
|
||
_ = publishedDirection;
|
||
_ = terminalType;
|
||
_ = publishedScope;
|
||
foreach (EmTrajectoryPoint point in emTrajectory.Points)
|
||
{
|
||
double timeFromStartSeconds = point.TimeFromStart;
|
||
double worldXMeters = point.X;
|
||
double worldYMeters = point.Y;
|
||
double yawRadians = point.Yaw;
|
||
double vehicleCurvaturePerMeter = point.VehicleCurvature;
|
||
double pathSMeters = point.PathS;
|
||
double signedSpeedMetersPerSecond = point.SignedLongitudinalVelocity;
|
||
double velocityXMetersPerSecond = point.VelocityX;
|
||
double velocityYMetersPerSecond = point.VelocityY;
|
||
double longitudinalAccelerationMetersPerSecondSquared = point.LongitudinalAcceleration;
|
||
double longitudinalJerkMetersPerSecondCubed = point.LongitudinalJerk;
|
||
double yawRateRadiansPerSecond = point.YawRate;
|
||
_ = timeFromStartSeconds;
|
||
_ = worldXMeters;
|
||
_ = worldYMeters;
|
||
_ = yawRadians;
|
||
_ = vehicleCurvaturePerMeter;
|
||
_ = pathSMeters;
|
||
_ = signedSpeedMetersPerSecond;
|
||
_ = velocityXMetersPerSecond;
|
||
_ = velocityYMetersPerSecond;
|
||
_ = longitudinalAccelerationMetersPerSecondSquared;
|
||
_ = longitudinalJerkMetersPerSecondCubed;
|
||
_ = yawRateRadiansPerSecond;
|
||
}
|
||
|
||
// 步骤 14：适配为控制轨迹
|
||
Trajectory2D controllerTrajectory =
|
||
new EmControlTrajectoryAdapter().Create(emTrajectory);
|
||
|
||
// 步骤 15：交给闭环动作
|
||
// 本示例只返回轨迹输入；StateProvider、控制器参数、执行保护和停车策略仍由宿主拥有。
|
||
return new FixedOfflinePlanningOutput(
|
||
emTrajectory, // 来源：步骤 12 的完整 EM 时间轨迹。
|
||
controllerTrajectory); // 来源：步骤 14 的控制器 SI 轨迹。
|
||
}
|
||
}
|