docs: correct EM controller handoff guide
This commit is contained in:
+39
-9
@@ -41,6 +41,7 @@ public static class EmPlannerTrajectoryReplacementExample
|
||||
// 并在返回前完成 Success/SuccessWithFallback、非空轨迹和可控点的发布闸门检查。
|
||||
private static void WrapperReplacement(
|
||||
ControllerStateProvider stateProvider,
|
||||
bool gearSwitchHandoffIsSafetyReviewedAndAuthorized,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
FixedOfflinePlanningOutput planningOutput =
|
||||
@@ -56,7 +57,9 @@ public static class EmPlannerTrajectoryReplacementExample
|
||||
"固定离线 EM 包装器未返回完整控制轨迹。");
|
||||
}
|
||||
|
||||
RejectGearSwitchForSingleTrackingMovement(emTrajectory);
|
||||
EnsureSingleDirectionTrackingIsAuthorized(
|
||||
emTrajectory,
|
||||
gearSwitchHandoffIsSafetyReviewedAndAuthorized);
|
||||
|
||||
// 原测试已有的状态源继续交给动作;仅 trajectory 的来源由人工数据改为 EM 输出。
|
||||
var movement = new TrajectoryTrackingMovement
|
||||
@@ -68,6 +71,10 @@ public static class EmPlannerTrajectoryReplacementExample
|
||||
// 若原测试使用组合动作计划,则使用同一条已适配的控制轨迹创建段。
|
||||
var segment = new TrackMotionPlanSegment(trajectory);
|
||||
|
||||
// 若本段以 GearSwitch 结束,授权只允许跟踪当前方向段到精确停车。
|
||||
// 动作完成后,外部状态机必须确认已停稳和下一方向,再单独请求/执行下一段。
|
||||
// 绝不把两个方向的点串接起来,也不自动跨越换向边界。
|
||||
|
||||
_ = movement;
|
||||
_ = segment;
|
||||
}
|
||||
@@ -78,6 +85,7 @@ public static class EmPlannerTrajectoryReplacementExample
|
||||
private static void DirectServiceReplacement(
|
||||
EmPlanningRequest request,
|
||||
ControllerStateProvider stateProvider,
|
||||
bool gearSwitchHandoffIsSafetyReviewedAndAuthorized,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
var planningService = new EmPlanningService(new OsqpNativeSolver());
|
||||
@@ -93,7 +101,9 @@ public static class EmPlannerTrajectoryReplacementExample
|
||||
}
|
||||
|
||||
EmTrajectory emTrajectory = result.Trajectory;
|
||||
RejectGearSwitchForSingleTrackingMovement(emTrajectory);
|
||||
EnsureSingleDirectionTrackingIsAuthorized(
|
||||
emTrajectory,
|
||||
gearSwitchHandoffIsSafetyReviewedAndAuthorized);
|
||||
|
||||
// 适配器使用 EM 的世界位姿、车辆曲率和有符号纵向速度,重建 Trajectory2D 的弧长。
|
||||
Trajectory2D trajectory =
|
||||
@@ -110,15 +120,20 @@ public static class EmPlannerTrajectoryReplacementExample
|
||||
// SegmentStarted/TrackingCycleObserver 等观测器以及状态与安全检查仍保持测试所有权。
|
||||
var segment = new TrackMotionPlanSegment(trajectory);
|
||||
|
||||
// GearSwitch 授权不是跨边界授权:只跟踪本次返回的完整方向段到精确停车。
|
||||
// 停稳后由外部状态机确认方向,然后发起一次新请求并创建新的单段动作。
|
||||
// 不得拼接方向点,不得在当前 movement/segment 中自动继续到下一段。
|
||||
|
||||
_ = movement;
|
||||
_ = segment;
|
||||
}
|
||||
|
||||
// 单个 TrajectoryTrackingMovement 只能消费一个方向段。
|
||||
// GearSwitch 表示该段末端必须先停车;除非外层已实现并经安全评审的“停车—方向确认—请求下一段”序列,
|
||||
// 否则拒绝它,绝不能把前进/倒车点拼接进同一个 Trajectory2D 后连续跟踪。
|
||||
private static void RejectGearSwitchForSingleTrackingMovement(
|
||||
EmTrajectory emTrajectory)
|
||||
// 完整方向轨迹以 GearSwitch 结束并不代表轨迹本身跨界:它可安全跟踪到边界精确停车。
|
||||
// 但调用方必须显式传入已通过安全评审的换向交接授权;未授权时在跟踪前拒绝。
|
||||
private static void EnsureSingleDirectionTrackingIsAuthorized(
|
||||
EmTrajectory emTrajectory,
|
||||
bool gearSwitchHandoffIsSafetyReviewedAndAuthorized)
|
||||
{
|
||||
if (emTrajectory == null)
|
||||
{
|
||||
@@ -126,12 +141,27 @@ public static class EmPlannerTrajectoryReplacementExample
|
||||
"EM 规划输出为空,不能创建 TrajectoryTrackingMovement。");
|
||||
}
|
||||
|
||||
if (emTrajectory.Metadata.TerminalType == EmTerminalType.GearSwitch)
|
||||
if (emTrajectory.Metadata.TerminalType != EmTerminalType.GearSwitch)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (emTrajectory.Metadata.PlanningScope != EmPlanningScope.FullDirectionSegment ||
|
||||
emTrajectory.Metadata.LongitudinalMode != EmLongitudinalMode.ExactStopAtBoundary)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"EmTerminalType.GearSwitch 必须由已安全评审的停车、方向确认和下一段规划序列处理;" +
|
||||
"单个 TrajectoryTrackingMovement 不得跨越换向边界。");
|
||||
"GearSwitch 只能作为 FullDirectionSegment 的 ExactStopAtBoundary 末端交给单段跟踪。");
|
||||
}
|
||||
|
||||
if (!gearSwitchHandoffIsSafetyReviewedAndAuthorized)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"GearSwitch 方向段需要调用方显式提供已通过安全评审的换向交接授权;" +
|
||||
"未授权时不得创建跟踪动作。");
|
||||
}
|
||||
|
||||
// 通过检查只授权当前段执行到精确停车。外部状态机仍必须在停稳后
|
||||
// 确认下一方向并另行请求/执行下一段;不得拼接点或自动跨越边界。
|
||||
}
|
||||
|
||||
// CruiseSpeedMetersPerSecond、AccelerationMetersPerSecondSquared、
|
||||
|
||||
Reference in New Issue
Block a user