docs: add annotated EM planning pipeline example
This commit is contained in:
梁薄云
@@ -0,0 +1,311 @@
|
||||
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; // 无单位。
|
||||
smoothingConfiguration.LocalG2Quintic.MinimumPeakGradientImprovementRatio = 0.20d; // 无单位。
|
||||
smoothingConfiguration.LocalG2Quintic.MaximumVariationCostRegressionRatio = 0.02d; // 无单位。
|
||||
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; // 固定完整段时间窗口；单位 s。
|
||||
emConfiguration.Scheduling.DistanceHorizonMeters = 10d; // 固定空间窗口；单位 m。
|
||||
emConfiguration.Scheduling.OutputTimeStepSeconds = 0.10d; // 固定发布间隔；单位 s。
|
||||
emConfiguration.Scheduling.SolverTimeoutSeconds = 5d; // 固定求解预算；单位 s。
|
||||
emConfiguration.Scheduling.MaximumVehicleStateAgeSeconds = 1d; // 固定状态时效；单位 s。
|
||||
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; // 固定顺序凸化迭代数。
|
||||
emConfiguration.Solver.MaximumOsqpIterations = 10000; // 固定 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 轨迹。
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user