171 lines
8.0 KiB
C#
171 lines
8.0 KiB
C#
// 此文件是供“停车机器人-合并测试”闭环测试维护者复制阅读的替换示例，
|
||
// 不属于可执行项目，也不读取实车定位或向底盘发送命令。
|
||
// 只允许替换轨迹来源；状态源、控制参数、观测器和安全策略仍由原测试拥有。
|
||
|
||
using System;
|
||
using System.Threading;
|
||
using ControllerStateProvider = MultiWheelC.StateEstimation.IVehicleStateProvider;
|
||
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(
|
||
ControllerStateProvider stateProvider,
|
||
bool gearSwitchHandoffIsSafetyReviewedAndAuthorized,
|
||
CancellationToken cancellationToken)
|
||
{
|
||
FixedOfflinePlanningOutput planningOutput =
|
||
EmPlannerFullPipelineDemo.CreateFixedOfflineTrajectory(
|
||
cancellationToken);
|
||
|
||
EmTrajectory emTrajectory = planningOutput.EmTrajectory;
|
||
Trajectory2D trajectory = planningOutput.ControllerTrajectory;
|
||
|
||
if (emTrajectory == null || trajectory == null)
|
||
{
|
||
throw new InvalidOperationException(
|
||
"固定离线 EM 包装器未返回完整控制轨迹。");
|
||
}
|
||
|
||
EnsureSingleDirectionTrackingIsAuthorized(
|
||
emTrajectory,
|
||
gearSwitchHandoffIsSafetyReviewedAndAuthorized);
|
||
|
||
// 原测试已有的状态源继续交给动作；仅 trajectory 的来源由人工数据改为 EM 输出。
|
||
var movement = new TrajectoryTrackingMovement
|
||
{
|
||
Trajectory = trajectory,
|
||
StateProvider = stateProvider
|
||
};
|
||
|
||
// 若原测试使用组合动作计划，则使用同一条已适配的控制轨迹创建段。
|
||
var segment = new TrackMotionPlanSegment(trajectory);
|
||
|
||
// 若本段以 GearSwitch 结束，授权只允许跟踪当前方向段到精确停车。
|
||
// 动作完成后，外部状态机必须确认已停稳和下一方向，再单独请求/执行下一段。
|
||
// 绝不把两个方向的点串接起来，也不自动跨越换向边界。
|
||
|
||
_ = movement;
|
||
_ = segment;
|
||
}
|
||
|
||
// 直接服务方案：当测试已按当前地图、车辆、状态和活动方向段构造冻结的 EmPlanningRequest 时使用。
|
||
// “请求”就是进程内同步方法调用：Plan 的返回值就是本次 EmPlanningResult；
|
||
// 没有轮询、HTTP、消息队列或轨迹仓库查询，也不能从其他调用的结果中取轨迹。
|
||
private static void DirectServiceReplacement(
|
||
EmPlanningRequest request,
|
||
ControllerStateProvider stateProvider,
|
||
bool gearSwitchHandoffIsSafetyReviewedAndAuthorized,
|
||
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;
|
||
EnsureSingleDirectionTrackingIsAuthorized(
|
||
emTrajectory,
|
||
gearSwitchHandoffIsSafetyReviewedAndAuthorized);
|
||
|
||
// 适配器使用 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);
|
||
|
||
// GearSwitch 授权不是跨边界授权：只跟踪本次返回的完整方向段到精确停车。
|
||
// 停稳后由外部状态机确认方向，然后发起一次新请求并创建新的单段动作。
|
||
// 不得拼接方向点，不得在当前 movement/segment 中自动继续到下一段。
|
||
|
||
_ = movement;
|
||
_ = segment;
|
||
}
|
||
|
||
// 单个 TrajectoryTrackingMovement 只能消费一个方向段。
|
||
// 完整方向轨迹以 GearSwitch 结束并不代表轨迹本身跨界：它可安全跟踪到边界精确停车。
|
||
// 但调用方必须显式传入已通过安全评审的换向交接授权；未授权时在跟踪前拒绝。
|
||
private static void EnsureSingleDirectionTrackingIsAuthorized(
|
||
EmTrajectory emTrajectory,
|
||
bool gearSwitchHandoffIsSafetyReviewedAndAuthorized)
|
||
{
|
||
if (emTrajectory == null)
|
||
{
|
||
throw new InvalidOperationException(
|
||
"EM 规划输出为空，不能创建 TrajectoryTrackingMovement。");
|
||
}
|
||
|
||
if (emTrajectory.Metadata.TerminalType != EmTerminalType.GearSwitch)
|
||
{
|
||
return;
|
||
}
|
||
|
||
if (emTrajectory.Metadata.PlanningScope != EmPlanningScope.FullDirectionSegment ||
|
||
emTrajectory.Metadata.LongitudinalMode != EmLongitudinalMode.ExactStopAtBoundary)
|
||
{
|
||
throw new InvalidOperationException(
|
||
"GearSwitch 只能作为 FullDirectionSegment 的 ExactStopAtBoundary 末端交给单段跟踪。");
|
||
}
|
||
|
||
if (!gearSwitchHandoffIsSafetyReviewedAndAuthorized)
|
||
{
|
||
throw new InvalidOperationException(
|
||
"GearSwitch 方向段需要调用方显式提供已通过安全评审的换向交接授权；" +
|
||
"未授权时不得创建跟踪动作。");
|
||
}
|
||
|
||
// 通过检查只授权当前段执行到精确停车。外部状态机仍必须在停稳后
|
||
// 确认下一方向并另行请求/执行下一段；不得拼接点或自动跨越边界。
|
||
}
|
||
|
||
// CruiseSpeedMetersPerSecond、AccelerationMetersPerSecondSquared、
|
||
// DecelerationMetersPerSecondSquared 和 PointSpacingMeters 是外部现有测试类的成员；
|
||
// 复制替换时保留其原值和测试所有权，本说明文件不会重新定义或修改它们。
|
||
}
|