轨迹规划服务调用与闭环控制器交接指南
本指南展示固定离线场景的完整调用顺序。代码只用于复制阅读和接口交接，不会读取实车定位，也不会向底盘发送命令。
| 边界 | 类型/方法 | 含义 |
|---|---|---|
| EMplanner 正式输出 | EmTrajectory |
带时间、位姿、曲率、速度、加速度、jerk 与方向边界的不可变轨迹 |
| 闭环控制器正式输入 | Trajectory2D |
几何控制器消费的世界位姿、弧长、曲率和有符号参考速度 |
| 唯一适配入口 | EmControlTrajectoryAdapter.Create(EmTrajectory) |
将单方向 EM 轨迹转换为控制器轨迹 |
EmPlanningService 是进程内同步服务。调用方创建冻结的 EmPlanningRequest，调用 Plan(request, cancellationToken)，并仅从本次返回的成功 EmPlanningResult 中索取 Trajectory。
使用边界与固定场景数据流
这是离线、静态、单方向示例：没有实时定位输入，没有底盘命令输出，也不把规划成功等同于可接管车辆。完整链路如下；每个编号都对应 EmPlannerFullPipelineDemo.cs 中同名的 步骤 N。
PlanningGridMap → PlanningRequest → HybridAStarPlanner.Plan
→ PathSmoothingRequest → PathSmoothingService.Smooth → DirectionSegmentView
→ EmPlanningRequest → EmPlanningService.Plan → EmPlanningResult.Trajectory
→ EmControlTrajectoryAdapter.Create → Trajectory2D → TrajectoryTrackingMovement
路径、地图和车辆参数使用 SI：世界位置为 m，航向为 rad，曲率为 1/m。PlanningGridMap 是冻结地图快照；不要把 Clumsy 绘图用的 mm 坐标直接交给规划器。
步骤 1–15
以下各步均可在 EmPlannerFullPipelineDemo.cs 的对应 步骤 N 直接查阅完整固定场景代码。
步骤 1：定义固定世界起点和终点
本步输入：固定场景常量（世界坐标 m、航向 rad）。
调用：构造 Pose2D 起点和目标。
本步输出：供 PlanningRequest.Start、PlanningRequest.Goal 使用的位姿。
失败处理：数值非有限、坐标系或单位不明时停止；不得以 mm 猜测转换。
步骤 2：定义车辆、曲率和搜索预算
本步输入：车辆几何中心尺寸、SafetyMarginMeters 与可验证曲率限制。
调用：构造 VehicleParameters 和 HybridAStarConfiguration。
本步输出：用于粗规划、平滑复核和 EM 走廊的同一车辆约束。
失败处理：长度/宽度/曲率限制无效时不请求规划；不可混用控制点半径与车体几何。
步骤 3：建立静态地图
本步输入：固定障碍物、地图边界与栅格分辨率。
调用：建立并冻结 PlanningGridMap。
本步输出：同一 SnapshotId 的可读地图。
失败处理：地图未就绪、起点或终点越界/碰撞时拒绝，不生成替代移动命令。
步骤 4：建立粗路径请求
本步输入：步骤 1–3 的位姿、地图、车辆和 Hybrid A* 配置。
调用：构造 PlanningRequest。
本步输出：一次性粗路径输入快照。
失败处理：不允许把上一轮的地图、车辆或目标悄悄混入本次请求。
步骤 5：搜索粗路径
本步输入：PlanningRequest。
调用：HybridAStarPlanner.Plan(request, cancellationToken)。
本步输出：仅 PlanningStatus.Success 时可消费的 PlanningResult.Path 和 Segments。
失败处理：SearchTimeout、节点预算、不可达、取消或校验失败均停止；失败结果的集合为空。
步骤 6：请求并执行 Local G2 平滑
本步输入：成功粗路径的 Path、Segments、同一地图/车辆和 PathSmoothingConfiguration。
调用：new PathSmoothingRequest(...)，再调用 PathSmoothingService.Smooth。
本步输出：可发布状态（Complete、PartialImprovement、NotNeeded 或 Unchanged）的完整平滑路径。
失败处理：失败、取消或无效输入的 Path/Segments 为空，不能把局部结果交给 EM。
步骤 7：选择一个方向段
本步输入：平滑结果的单个 SmoothedPathSegment。
调用：建立/取得 DirectionSegmentView。
本步输出：局部 S=0、方向一致的只读参考段和精确边界。
失败处理：不跨 GearSwitch 边界插值或合并；方向段索引、方向或边界不一致即拒绝。
步骤 8：建立初始运动状态
本步输入：固定场景的世界位姿、车体纵向有符号速度、状态时间与序列号。
调用：构造 VehicleMotionState。
本步输出：与地图、平滑路径同版本的起始状态。
失败处理：过期状态或非零速度方向与活动段不符会得到 StaleVehicleState/StateDirectionMismatch。
步骤 9：冻结 EM 配置
本步输入：低速泊车配置树。
调用：EmPlannerConfiguration.CreateDefault() 后在请求前完成本场景修改。
本步输出：供服务复制和验证的一套配置。
失败处理：子配置或权重为空、数值无效时只接受 InvalidInput，不降低安全约束继续运行。
步骤 10：建立 EM 请求
本步输入：平滑成功结果、冻结地图、车辆、状态、配置和活动 segmentIndex。
调用：构造 EmPlanningRequest，给出请求/生效 UTC、输出 ID、参考 ID、EmMotionModel 与 FullDirectionSegment。
本步输出：一次 EM 输入快照。
失败处理：固定演示不传跨段 PreviousTrajectory；身份或方向段不匹配时不调用适配器。
步骤 11：调用 EM 服务
本步输入：EmPlanningRequest 和调用方 CancellationToken。
调用：new EmPlanningService(new OsqpNativeSolver()).Plan(request, cancellationToken)。
本步输出：不可变 EmPlanningResult。
失败处理：这是同步进程内调用，不应把它包装为 HTTP、后台轨迹仓库或异步下发。
步骤 12：判断是否可发布
本步输入：Status、FailureReason 与 Trajectory。
调用：只接受 Success 或 SuccessWithFallback，再检查轨迹非空及控制点数。
本步输出：可适配的完整单段 EmTrajectory。
失败处理：任何其他状态、空轨迹或少于两个不同世界位置的点，保留既有安全策略并记录原因。
步骤 13：读取输出契约
本步输入：成功 EmTrajectory。
调用：读取只读 Metadata 与 Points，用于身份、方向、边界和诊断。
本步输出：原始时间轨迹仍与控制轨迹并存。
失败处理：不得就地修改点列；不得把 VelocityX/VelocityY 当车辆命令。
步骤 14：适配为控制轨迹
本步输入：步骤 12 的同一方向完整 EmTrajectory。
调用：new EmControlTrajectoryAdapter().Create(emTrajectory)。
本步输出：以世界位置重新从零累计弧长的 Trajectory2D，保留航向、车辆曲率和有符号参考速度。
失败处理：适配器拒绝空、单点或没有两个不同位置的轨迹；不自行补点或拼接另一方向。
步骤 15：交给闭环动作
本步输入：Trajectory2D、控制器维护者拥有的 StateProvider 与调参。
调用：赋给 TrajectoryTrackingMovement.Trajectory（或由宿主包装为 TrackMotionPlanSegment）。
本步输出：闭环几何控制器的轨迹输入。
失败处理：只替换轨迹来源；保留状态提供者、控制器调参、执行保护和停止策略的所有权。
服务索取与适配代码
以下是唯一可交接到控制器的状态闸门：
var planningService = new EmPlanningService(new OsqpNativeSolver());
EmPlanningResult result = planningService.Plan(request, cancellationToken);
bool succeeded = result.Status == EmPlanningStatus.Success ||
result.Status == EmPlanningStatus.SuccessWithFallback;
if (!succeeded || result.Trajectory == null)
{
throw new InvalidOperationException(
"EM 规划未返回可交给控制器的完整轨迹：" + result.FailureReason);
}
EmTrajectory emTrajectory = result.Trajectory;
Trajectory2D controllerTrajectory =
new EmControlTrajectoryAdapter().Create(emTrajectory);
输出契约、单位和坐标系
EmPlanningResult
| 成员 | 契约 |
|---|---|
Status |
机器可读最终状态；仅 Success、SuccessWithFallback 可发布。 |
Trajectory |
仅成功状态为完整非空不可变 EmTrajectory；其他状态必为 null，不是部分可执行轨迹。 |
FailureReason |
成功降级或失败诊断；不能取代 Status 判定。 |
EmTrajectoryMetadata
| 成员 | 契约 |
|---|---|
TrajectoryId / PreviousTrajectoryId |
本次发布 ID 与可选前轨迹 ID；后者空字符串表示无关联。 |
GeneratedAtUtc / EffectiveAtUtc |
UTC 生成时刻与计划生效时刻；控制端按自己的调度语义解释。 |
MapSnapshotId / ReferencePathId / VehicleStateSequenceId |
地图、参考路径和状态的来源版本，供追踪和去重。 |
SegmentIndex / Direction |
所属单方向段与已验证前进/倒车方向。 |
TerminalType / LongitudinalMode / PlanningScope |
终端业务语义、纵向终端语义和 FullDirectionSegment/滚动范围。 |
EmTrajectoryPoint
| 字段 | 单位/坐标 | 控制相关语义 |
|---|---|---|
TimeFromStart |
s | 自轨迹开始的非负时间。 |
X, Y, Yaw |
世界 m、世界 rad | Trajectory2D 使用的世界位姿。 |
VehicleCurvature |
1/m | 车辆路径曲率，适配器原样传给控制点。 |
SignedLongitudinalVelocity / Speed |
m/s | 前进为正、倒车为负；Speed 是绝对值。适配器使用前者。 |
VelocityX, VelocityY |
世界 m/s | 由有符号速度和航向导出的预测分量。VelocityX 和 VelocityY 是世界坐标系中的预测速度分量，不是底盘纵向/横向命令；闭环测试不得把它们直接发送给车辆。 |
YawRate |
rad/s | SignedLongitudinalVelocity * VehicleCurvature 导出的预测偏航角速度。 |
SegmentIndex, SegmentLocalS, PathS, Direction, BoundaryType |
索引、m、m、枚举、枚举 | 段归属、局部/全路径弧长、方向及目标/换向边界；用于保证不跨方向消费。 |
LongitudinalAcceleration, LongitudinalJerk |
m/s²、m/s³ | 沿车体前向轴的规划量；当前为程序集内部验证/执行成员，不是 Trajectory2D 输入。 |
VelocityX 和 VelocityY 是世界坐标系中的预测速度分量，不是底盘纵向/横向命令；闭环测试不得把它们直接发送给车辆。
| 概念 | 正确解释 | 常见错误 |
|---|---|---|
| m / mm | 规划与 Trajectory2D 均为 m；外部测试仅绘图时将 m 乘 1000。 |
将 Clumsy Vector2 的 mm 直接用于 Pose2D。 |
| rad / deg | 所有 Yaw、转角、容差为 rad；AngleMath.DegreesToRadians 只用于初始化控制器角度。 |
将 3 或 45 当作 rad。 |
| 1/m、m/s、m/s²、m/s³、rad/s | 分别是曲率、速度、加速度、jerk、偏航角速度。 | 用曲率替代转向角，或把加速度当速度。 |
| 世界/车体 | X/Y/Yaw、VelocityX/Y 在世界系；有符号纵向速度、加速度、jerk 沿车体前向轴。 |
把世界 XY 分量作为车体纵/横向命令。 |
参数索引
表中“演示/当前默认”表示固定 demo 未覆盖时使用当前构造默认值；调大/调小描述主要趋势，任何变更都须重新验证安全与可行性。
粗路径配置
| 参数 | 源码路径 | 单位 | 演示/当前默认 | 含义 | 调大 / 调小影响 |
|---|---|---|---|---|---|
VehicleParameters.LengthMeters, WidthMeters |
CoarsePath/Contracts/VehicleParameters.cs |
m | 场景填入；测试车辆为 0.80 / 0.60 | 几何中心车体长宽 | 大：更保守、可行域小；小：净空风险增大。 |
SafetyMarginMeters |
同上 | m | 场景填入 | 车体四周附加安全余量 | 大：更安全但更易不可达；小：相反。 |
MaximumCurvaturePerMeter / MinimumTurningRadiusMeters |
同上 | 1/m / m | 至少其一；测试为 1/1.20 | 车辆转弯极限 | 曲率大/半径小：机动性增、与实车不符风险增；反之可行域缩小。 |
PrimitiveLengthMeters, IntegrationStepMeters, MaximumCollisionCheckStepMeters |
CoarsePath/Contracts/HybridAStarConfiguration.cs |
m | 0.50 / 0.05 / 0.025 | 原语长度、内部采样、连续碰撞步长 | 长/大：更快但分辨率低；短/小：更精细、成本高。 |
HeadingResolutionRadians, CurvatureLevelCount |
同上 | rad / 个 | π/36 / 5 | 状态航向与曲率离散度 | 分辨率更细/等级更多：质量好、节点增；反之更快粗糙。 |
GoalPositionToleranceMeters, GoalHeadingToleranceRadians |
同上 | m / rad | 0.15 / π/36 | 终点判定容差 | 大：更易成功但末端误差大；小：更严格、可能失败。 |
MaximumExpandedNodes, SearchTimeout |
同上 | 节点 / s | 200000 / 5 | 搜索资源预算 | 大：成功机会高、延迟高；小：更快但易资源失败。 |
HeuristicWeight, ReverseCostMultiplier, GearSwitchPenaltyMeters |
同上 | 无量纲 / 倍 / m | 1 / 1.5 / 1 | 搜索偏好、倒车和换向代价 | 大：更偏启发式/少倒车/少换向；小：更全面但可能慢或频繁换向。 |
CurvatureMagnitudeWeight, CurvatureChangePenaltyMetersPerLevel |
同上 | 无量纲 / m | 0.10 / 0.05 | 曲率与曲率变化代价 | 大：路径更平顺；小：更短但控制负担大。 |
ClearanceCostWeight, ClearanceCostDistanceMeters, AllowReverse |
同上 | 无量纲 / m / bool | 0.20 / 0.50 / true | 净空偏好、安全距离、倒车开关 | 前两项大：偏好更大净空；false：禁止倒车且可行域缩小。 |
Local G2 平滑配置
| 参数 | 源码路径 | 单位 | 演示/当前默认 | 含义 | 调大 / 调小影响 |
|---|---|---|---|---|---|
OutputSpacingMeters, MaximumCollisionCheckStepMeters |
PathSmoothing/Contracts/PathSmoothingConfiguration.cs |
m | 0.025 / 0.025 | 输出采样、扫掠复核步长 | 大：点少/检查粗；小：更细/开销高。 |
MinimumClearanceReserveMeters, CurvatureLimitRadiusToleranceMeters |
同上 | m / m | 0 / 0.002 | 额外净空、曲率半径容差 | 净空大更保守；半径容差大更宽松。 |
MinimumWindowLengthMeters, PreferredWindowLengthMeters, MaximumWindowLengthMeters |
PathSmoothing/Contracts/LocalG2QuinticOptions.cs |
m | 0.20 / 0.50 / 0.80 | 局部 G2 窗口范围 | 大：过渡更缓、影响范围大；小：更局部、改善受限。 |
MaximumDeviationMeters |
同上 | m | 0.10 | 相对原路径最大偏移 | 大：改善空间大但净空风险高；小：更保守。 |
AbsoluteCurvatureJumpFloorPerMeter, CurvatureJumpRatioOfMaximum |
同上 | 1/m / 比例 | 0.001 / 0.05 | 识别曲率跳变阈值 | 大：仅处理明显跳变；小：处理更多区域。 |
MinimumPeakGradientImprovementRatio, MaximumVariationCostRegressionRatio |
同上 | 比例 | 0.20 / 0.02 | 接受候选的改善与退化门槛 | 前者大/后者小：更严格；反之更易接受。 |
MaximumCandidatesPerRegion |
同上 | 个 | 12 | 每区域候选预算 | 大：质量机会高、耗时高；小：更快、可能错过候选。 |
EM 配置
| 参数 | 源码路径 | 单位 | 演示/当前默认 | 含义 | 调大 / 调小影响 |
|---|---|---|---|---|---|
Corridor.LongitudinalSampleSpacingMeters, LateralSampleSpacingMeters, MaximumCollisionCheckStepMeters |
EMPlanner/Configuration/CorridorConfiguration.cs |
m | 0.10 / 0.025 / 0.025 | 走廊纵向、横向和碰撞采样 | 大：更快但走廊/检查变粗；小：更精细、结点增。 |
Corridor.MaximumLateralOffsetMeters, AdditionalClearanceReserveMeters |
同上 | m | 0.30 / 0.02 | 可搜索横偏与额外净空 | 横偏大：绕障空间大；净空大：更安全但更易不可行。 |
Frenet.MaximumProjectionDistanceMeters, MinimumFrenetDenominator, BoundaryAnchorToleranceMeters |
EMPlanner/Configuration/FrenetConfiguration.cs |
m / 无量纲 / m | 0.50 / 0.20 / 1e-8 | 投影范围、1-k*l 安全余量、边界锚定容差 |
前者大更容忍错位；中项大更保守；容差大更宽松。 |
Lateral.MaximumLateralStepPerIterationMeters, MaximumLateralSlope, MaximumLateralSecondDerivativePerMeter, MaximumLateralThirdDerivativePerSquareMeter |
EMPlanner/Configuration/LateralConfiguration.cs |
m / 无 / 1/m / 1/m² | 0.05 / 0.50 / 1 / 2 | LS 横偏更新、斜率及导数硬限 | 大：可操作性增、平顺/可控性风险增；小：更保守或不可行。 |
Lateral.Weights.ReferenceOffset, HeadingDeviation, SecondDerivative, ThirdDerivative |
EMPlanner/Configuration/LateralWeights.cs |
无量纲 | 10 / 1 / 5 / 10 | 偏参考、航向偏差及二/三阶导偏好 | 大：更强惩罚对应量；小：更自由。 |
Lateral.Weights.Curvature, CurvatureVariation, PreviousTrajectory, RollingTerminal |
同上 | 无量纲 | 5 / 20 / 5 / 10 | 曲率、曲率变化、上轮和滚动末端偏好 | 大：对应项更平稳/连续；小：更追求其他目标。 |
Longitudinal.MaximumForwardSpeedMetersPerSecond, MaximumReverseSpeedMetersPerSecond, DesiredForwardSpeedMetersPerSecond, DesiredReverseSpeedMetersPerSecond |
EMPlanner/Configuration/LongitudinalConfiguration.cs |
m/s | 1 / 0.5 / 1 / 0.5 | 正反向速度硬限与目标巡航 | 大：行程快但制动/横向约束更紧；小：更保守。 |
MaximumAccelerationMetersPerSecondSquared, MaximumDecelerationMetersPerSecondSquared, MaximumJerkMetersPerSecondCubed |
同上 | m/s² / m/s² / m/s³ | 0.20 / 0.30 / 0.50 | 纵向舒适性硬限 | 大：响应快但冲击大；小：平顺但易无足够制动距离。 |
MaximumLateralAccelerationMetersPerSecondSquared, MaximumCurvatureRatePerMeterPerSecond, StopSpeedToleranceMetersPerSecond, ZeroSpeedHoldSeconds |
同上 | m/s² / 1/(m·s) / m/s / s | 0.20 / 0.50 / 0.01 / 0.20 | 横向加速、曲率率、停稳阈值和停稳保持 | 前两项大更激进；停速阈值大更易认停；保持长更保守。 |
Longitudinal.Weights.ReferenceSpeed, Acceleration, Jerk, PreviousTrajectory, TerminalAcceleration |
EMPlanner/Configuration/LongitudinalWeights.cs |
无量纲 | 10 / 1 / 10 / 5 / 1 | ST 速度、加速度、jerk、连续性和末端偏好 | 大：更强惩罚对应项；小：让位其他目标。 |
Solver.MaximumOuterIterations, MaximumOsqpIterations |
EMPlanner/Configuration/SolverConfiguration.cs |
次 | 5 / 4000 | 外层与 OSQP 迭代预算 | 大：更可能收敛、耗时高；小：更快、易超时/不收敛。 |
Solver.AbsoluteTolerance, RelativeTolerance, StrictResidualTolerance, WarmStart, Polish, NativeVerbose |
同上 | 无 / bool | 1e-5 / 1e-5 / 1e-5 / true / true / false | 求解精度与运行选项 | 容差大更快但精度低；WarmStart/Polish提高收敛质量；诊断仅影响日志。 |
Scheduling.ReplanPeriodSeconds, TimeHorizonSeconds, DistanceHorizonMeters, OutputTimeStepSeconds |
EMPlanner/Configuration/SchedulingConfiguration.cs |
s / s / m / s | 0.20 / 6 / 500 / 0.05 | 触发周期、预测时空窗口和输出间隔 | 窗口大/步长小：覆盖细、成本高；周期小：更新快。 |
SolverTimeoutSeconds, HandoffLookaheadSeconds, MaximumVehicleStateAgeSeconds |
同上 | s | 0.10 / 0.30 / 0.20 | 求解预算、交接前看、状态时效 | 预算大更易完成；交接/时效大更宽容但更陈旧。 |
MaximumOptimizationTimeStepSeconds, MaximumOptimizationSpatialStepMeters, MaximumOptimizationKnotCount, MaximumPublishedSampleCount |
同上 | s / m / 个 / 个 | 0.20 / 0.10 / 401 / 5001 | 优化离散和资源上限 | 步长大更粗；上限大质量机会高、资源高。 |
Validation.SpatialToleranceMeters, KinematicTolerance, TerminalPositionToleranceMeters, TerminalYawToleranceRadians |
EMPlanner/Configuration/ValidationConfiguration.cs |
m / 无 / m / rad | 1e-8 / 1e-5 / 0 / 0 | 发布前几何、动力学和终端验收容差 | 大更宽松；小更严格、可能拒绝候选。 |
闭环控制配置
| 参数 | 源码路径 | 单位 | 演示/当前默认 | 含义 | 调大 / 调小影响 |
|---|---|---|---|---|---|
Trajectory, StateProvider, CycleObserver |
MultiWheelC/Movements/TrajectoryTrackingMovement.cs（外部测试树） |
类型/回调 | 调用方提供 / null / null | 轨迹、状态来源与诊断所有权 | 仅替换 Trajectory；不要改变状态提供者或诊断归属。 |
StanleyCrossTrackGainPerSecond, StanleyHeadingErrorGain, StanleyMinimumSpeedMetersPerSecond, StanleyUsesActualSpeed |
同上 | 1/s / 无 / m/s / bool | 0.4 / 1 / 0.15 / true | Stanley 横偏、航向、低速保护与速度来源 | 增益大更快纠偏也易振荡；低速保护大更稳但迟钝。 |
MaximumCrossTrackCorrectionRadians, MaximumHeadingCorrectionRadians |
同上 | rad | 10° / 10° | 横偏/航向纠正上限 | 大：纠偏更强；小：更温和、可能跟踪慢。 |
LongitudinalKp, LongitudinalKiPerSecond, LongitudinalKdSeconds |
同上 | 无 / 1/s / s | 0.5 / 0 / 0 | 速度 PID 三项 | 大：响应增强，也可能超调、积分饱和或噪声放大。 |
MaximumIntegralCorrectionMetersPerSecond, LongitudinalSpeedErrorDeadbandMetersPerSecond, MaximumCommandSpeedMetersPerSecond |
同上 | m/s | 0.05 / 0.025 / 0.50 | 积分限幅、死区、命令速度上限 | 限幅/上限大更激进；死区大更稳但有稳态误差。 |
MaximumGcpAngleRadians, MaximumGcpAngleRateRadiansPerSecond |
同上 | rad / rad/s | 45° / 15°/s | GCP 角度与变化率限制 | 大：机动快但执行风险高；小：平稳但转弯跟踪受限。 |
FinishDistanceMeters, FinishSpeedMetersPerSecond, FinishHeadingToleranceRadians |
同上 | m / m/s / rad | 0.03 / 0.02 / 3° | 完成位置、停速和航向阈值 | 大：更易完成、末端误差大；小：更严格。 |
MaximumDistanceToTrajectoryMeters, ExecutionTimeoutSeconds |
同上 | m / s | 0.30 / 120 | 偏离保护与执行超时 | 大：更容忍但风险高；小：更早保护。 |
状态决策与换向
| 场景 | 识别 | 必须处理 |
|---|---|---|
| 无效输入 | InvalidInput、粗路径 Invalid*、平滑失败 |
记录诊断，修正输入；不创建控制轨迹。 |
| 不可行 | NoFeasiblePath、CorridorInfeasible、LateralInfeasible、LongitudinalInfeasible |
停在控制输入边界，保留已有安全策略。 |
| 取消 | Cancelled 或调用方 token 取消 |
丢弃本次结果，不把局部轨迹交给控制器。 |
| 截止/超时 | 粗路径 SearchTimeout、EM SolverTimedOut、CycleDeadlineExpired、资源限制 |
不延用本次半成品；诊断预算与阶段。 |
| 空轨迹 | 成功闸门不成立或 Trajectory == null |
当失败处理；绝不补造直线替代移动。 |
| 方向不匹配 | StateDirectionMismatch，或 Metadata/点方向不同 |
只用当前活动段；先停稳再重新确认。 |
| 适配器拒绝 | 少于两个不同位置点，Create 抛出 |
不修改原始轨迹硬凑点；修复上游输出。 |
本 demo 是单方向。换向需要在当前段的精确边界完整停车，并在安全审查后的换向状态机确认下一方向后，单独请求并执行下一段规划。不得把正、负方向段串接成一个 Trajectory2D，也不得跨换向边界插值。
外部闭环测试中的替换点
当前应替换的人工来源是外部 NewControllerTrackingTests.cs 中的 TestTrajectoryFactory.CreateStraight4Meters（在直线 4 m 测试中构造 trajectory）。下游汇是 TrajectoryTrackingMovement.Trajectory。合并时只把该变量的来源替换为本指南的服务调用和适配器输出；StateProvider、Stanley/PID/GCP 调参、完成/偏离保护、超时与记录器均由控制器测试继续拥有。不要改写它们来迁就规划器。
建议阅读顺序
- 本 README；
- EmPlannerFullPipelineDemo.cs 的步骤 1–15；
EmPlannerTrajectoryReplacementExample.cs的替换前后对照；- 可选阅读现有 trajectory-planning-flow-demo.html。
HTML 只是辅助流程图，不是接口规范；以本 README、源代码契约和完整示例为准。