141 lines
6.4 KiB
C#
141 lines
6.4 KiB
C#
// 此文件是供“停车机器人-合并测试”闭环测试维护者复制阅读的替换示例，
|
||
// 不属于可执行项目，也不读取实车定位或向底盘发送命令。
|
||
// 只允许替换轨迹来源；状态源、控制参数、观测器和安全策略仍由原测试拥有。
|
||
|
||
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;
|
||
|
||
/// <summary>
|
||
/// 闭环控制测试中从人工轨迹切换到 EM 正式轨迹的逐段对照。
|
||
/// </summary>
|
||
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 是外部现有测试类的成员；
|
||
// 复制替换时保留其原值和测试所有权，本说明文件不会重新定义或修改它们。
|
||
}
|