diff --git a/ClumsyPilot/ParkrobTrajplanner/Trajplanner_guide/EmPlannerTrajectoryReplacementExample.cs b/ClumsyPilot/ParkrobTrajplanner/Trajplanner_guide/EmPlannerTrajectoryReplacementExample.cs new file mode 100644 index 0000000..c999200 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/Trajplanner_guide/EmPlannerTrajectoryReplacementExample.cs @@ -0,0 +1,140 @@ +// 此文件是供“停车机器人-合并测试”闭环测试维护者复制阅读的替换示例, +// 不属于可执行项目,也不读取实车定位或向底盘发送命令。 +// 只允许替换轨迹来源;状态源、控制参数、观测器和安全策略仍由原测试拥有。 + +using System; +using System.Threading; +using MultiWheelC.StateEstimation; +using MultiWheelC.TrajectoryPlanning.EMPlanner; +using MultiWheelC.TrajectoryPlanning.TrajectoryObservation; +using MyParking.Shared; +using Trajectory2D = MultiWheelC.Trajectory.Trajectory2D; + +namespace MultiWheelC.TrajectoryPlanning.Guide; + +/// +/// 闭环控制测试中从人工轨迹切换到 EM 正式轨迹的逐段对照。 +/// +public static class EmPlannerTrajectoryReplacementExample +{ + // 替换前:NewControllerTrackingTests 中的完整人工测试轨迹来源。 + // 它只生成控制器测试数据,不调用正式规划器、地图构建、粗路径、平滑或 EM 服务。 + private static void ArtificialTrajectorySource( + Pose2D trajectoryStartPose, + CancellationToken cancellationToken) + { + _ = cancellationToken; // 此人工来源不使用取消令牌,保留形参仅便于与替换后位置对照。 + + Trajectory2D trajectory = + TestTrajectoryFactory.CreateStraight4Meters( + trajectoryStartPose, + CruiseSpeedMetersPerSecond, + AccelerationMetersPerSecondSquared, + DecelerationMetersPerSecondSquared, + PointSpacingMeters); + + _ = trajectory; + } + + // 替换后(推荐):复用 Task 2 的固定离线完整管线输出。 + // Task 2 内部同步调用 EmPlanningService.Plan(request, cancellationToken), + // 并在返回前完成 Success/SuccessWithFallback、非空轨迹和可控点的发布闸门检查。 + private static void WrapperReplacement( + IVehicleStateProvider stateProvider, + CancellationToken cancellationToken) + { + FixedOfflinePlanningOutput planningOutput = + EmPlannerFullPipelineDemo.CreateFixedOfflineTrajectory( + cancellationToken); + + EmTrajectory emTrajectory = planningOutput.EmTrajectory; + Trajectory2D trajectory = planningOutput.ControllerTrajectory; + + if (emTrajectory == null || trajectory == null) + { + throw new InvalidOperationException( + "固定离线 EM 包装器未返回完整控制轨迹。"); + } + + RejectGearSwitchForSingleTrackingMovement(emTrajectory); + + // 原测试已有的状态源继续交给动作;仅 trajectory 的来源由人工数据改为 EM 输出。 + var movement = new TrajectoryTrackingMovement + { + Trajectory = trajectory, + StateProvider = stateProvider + }; + + // 若原测试使用组合动作计划,则使用同一条已适配的控制轨迹创建段。 + var segment = new TrackMotionPlanSegment(trajectory); + + _ = movement; + _ = segment; + } + + // 直接服务方案:当测试已按当前地图、车辆、状态和活动方向段构造冻结的 EmPlanningRequest 时使用。 + // “请求”就是进程内同步方法调用:Plan 的返回值就是本次 EmPlanningResult; + // 没有轮询、HTTP、消息队列或轨迹仓库查询,也不能从其他调用的结果中取轨迹。 + private static void DirectServiceReplacement( + EmPlanningRequest request, + IVehicleStateProvider stateProvider, + CancellationToken cancellationToken) + { + 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; + RejectGearSwitchForSingleTrackingMovement(emTrajectory); + + // 适配器使用 EM 的世界位姿、车辆曲率和有符号纵向速度,重建 Trajectory2D 的弧长。 + Trajectory2D trajectory = + new EmControlTrajectoryAdapter().Create(result.Trajectory); + + // 直接单段消费路径:不要在这里覆盖原测试已有的控制器调参、完成容差、观测器或安全检查。 + var movement = new TrajectoryTrackingMovement + { + Trajectory = trajectory, + StateProvider = stateProvider + }; + + // 组合计划消费路径:原测试的 ConfigureTrackingMovement、每段 finish tolerances、 + // SegmentStarted/TrackingCycleObserver 等观测器以及状态与安全检查仍保持测试所有权。 + var segment = new TrackMotionPlanSegment(trajectory); + + _ = movement; + _ = segment; + } + + // 单个 TrajectoryTrackingMovement 只能消费一个方向段。 + // GearSwitch 表示该段末端必须先停车;除非外层已实现并经安全评审的“停车—方向确认—请求下一段”序列, + // 否则拒绝它,绝不能把前进/倒车点拼接进同一个 Trajectory2D 后连续跟踪。 + private static void RejectGearSwitchForSingleTrackingMovement( + EmTrajectory emTrajectory) + { + if (emTrajectory == null) + { + throw new InvalidOperationException( + "EM 规划输出为空,不能创建 TrajectoryTrackingMovement。"); + } + + if (emTrajectory.Metadata.TerminalType == EmTerminalType.GearSwitch) + { + throw new InvalidOperationException( + "EmTerminalType.GearSwitch 必须由已安全评审的停车、方向确认和下一段规划序列处理;" + + "单个 TrajectoryTrackingMovement 不得跨越换向边界。"); + } + } + + // CruiseSpeedMetersPerSecond、AccelerationMetersPerSecondSquared、 + // DecelerationMetersPerSecondSquared 和 PointSpacingMeters 是外部现有测试类的成员; + // 复制替换时保留其原值和测试所有权,本说明文件不会重新定义或修改它们。 +}