ParkingRobot/docs/superpowers/specs/2026-08-04-em-observation-movementtest-design.md

EM 轨迹规划观察模式 MovementTest 设计

目标

在 ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest 新增一个可在实车环境运行的 MovementTest。测试直接读取 MDCS 的实时定位、车速和车辆底盘状态，构造一段完整的 粗路径、平滑路径和 EM 轨迹规划流程，并持续展示规划结果。

该测试是观察模式（observe only）：它能完整生成、采样并打印“控制器此刻会使用的” 轨迹命令，但绝不向底盘、转向、电机或执行器写入任何运动命令。这让真实车辆保持由人工 或已有安全系统控制，同时可以验证定位坐标、地图、路径、速度曲线和时间参数是否一致。

使用方式与可配置项

MovementTest 对外提供以下可编辑字段，单位固定，避免毫米/米和角度/弧度混用：

字段	单位	默认值	含义
GoalXmm	mm	无	终点全局 X 坐标，启动前必须填写。
GoalYmm	mm	无	终点全局 Y 坐标，启动前必须填写。
GoalYawDeg	deg	0	终点航向角。
MapPaddingMeters	m	2.0	起点/终点矩形边界向四周的扩展距离。
MapResolutionMm	mm	50	局部占据栅格分辨率。
ReplanPeriodSeconds	s	0.20	使用最新车辆状态重新调用 EM 的周期。
ObserverPeriodSeconds	s	0.05	采样已发布轨迹、刷新状态信息的周期。
VehicleLengthMeters / VehicleWidthMeters	m	测试配置值	传给规划器的有效车辆外廓。
SafetyMarginMeters	m	测试配置值	车辆外廓之外的规划安全裕度。
MaximumCurvaturePerMeter	1/m	测试配置值	实车允许的最大曲率。

启动后交互式输入障碍物，交互形式沿用现有粗路径 MovementTest：先输入数量，再逐个选择 圆形障碍物或轴对齐矩形障碍物，并输入其中心、尺寸或半径。输入坐标使用全局 mm；障碍物 被转换到该次规划的局部地图坐标。

输入校验必须拒绝非有限值、非正的地图扩展/分辨率/周期、无效车辆尺寸和无效曲率。若起点 与终点无法形成容纳车辆和安全裕度的有效区域，也必须打印原因并不启动规划。

车辆数据与坐标约定

每次启动和重规划均从 MDCS_basictool 读取：

• DetourInterface.getCartLocation()：车辆实时全局位置与航向；其 mm/deg 数据统一转换为 Pose2D 所使用的 m/rad。
• BasicPilotBase.Chassis.GetCarSpeed(true)（或该项目等价的实际底盘读数）：车辆实际车体 纵向速度，用于 MotionState.SignedLongitudinalVelocity 和日志。
• 底盘对象：只用于读取可用性与实际状态；车辆外廓、裕度和曲率限制由测试显式配置， 因为轮组几何本身不能可靠推导车身外形和业务安全距离。

EM 的 VelocityX/VelocityY 是世界坐标速度，不可直接传给 MDCS 的车体坐标 SendXYThSpeed(vx, vy, vth)。本测试也不会调用该接口。若以后单独实现执行模式，非完整横移 模型应使用 SignedLongitudinalVelocity 作为车体 vx、零车体 vy，并把 YawRate 从 rad/s 转换为 MDCS 所需的 deg/s；执行模式还必须经过独立的安全评审。

地图、路径与规划流程

1. 读取当前定位作为本次测试的起点，读取配置的终点。
2. 以起点和终点为对角线构造轴对齐矩形，四个方向均扩展 MapPaddingMeters；将障碍物栅格化 到此局部地图。
3. 用现有 Hybrid A* 生成粗路径，再以现有 Local G2 平滑器生成平滑参考路径。任何一个上游 环节失败时打印状态、保留世界视图和失败原因，不进入 EM 循环。
4. 从平滑路径建立 ReferencePathSegmenter。首次成功后创建 EmPlanningCoordinator、EmPlanningService 和 TrajectoryExecutor。
5. 每个 ReplanPeriodSeconds 读取最新车辆状态，以同一地图、参考路径和终点构造 EmPlanningRequest。协调器在上一条有效轨迹基础上滚动发布下一条 EmTrajectory；新的 失败不会覆盖上一条已经验证成功的轨迹。
6. 每个 ObserverPeriodSeconds 按 EmTrajectory.Metadata.EffectiveAtUtc 和每个点的 TimeFromStart 采样当前时刻的目标点，并调用 TrajectoryExecutor.UpdateCommand 得到 仅供显示的 TrajectoryControlCommand。

EmTrajectoryPoint 已携带 TimeFromStart、SignedLongitudinalVelocity 与 YawRate，因此不 需要外部猜测轨迹时间，也不应通过相邻 yaw 差分计算角速度。

初版只观察当前方向段。若轨迹到达 GearSwitch，界面和终端会明确显示“等待真实档位/方向 确认”，不会假设车辆已经换向，也不会自动推进到下一方向段。这样可避免静止观察测试在倒车 段产生虚假的完成状态。

UI 与终端输出

测试在生命周期内维护三组独立 painter；停止、取消或异常时清除对应图层。

图层	内容
TrajectoryObserver.World	局部地图边界、障碍物、实时车辆位姿、起点、终点、Hybrid A* 粗路径、G2 平滑路径和最新 EM 轨迹。
TrajectoryObserver.LS	将最新 EM 点投影到当前 DirectionSegmentView，绘制参考弧长 s 与横向偏移 l 的曲线，附上投影失败数。
TrajectoryObserver.ST	绘制 t-s 曲线，并在同一图层标注或以第二条曲线展示 t-v，用于检查时间推进、纵向进度和速度。

LS/ST 由最终 EmTrajectory 派生，不修改当前 EM 的公共结果接口：LS 的 s/l 来自 FrenetProjector.TryProject，ST 的 t 来自 TimeFromStart、s 来自 PathS，速度来自 SignedLongitudinalVelocity。这保证展示数据正是控制器将要消费的发布轨迹。

终端启动时打印起终点、地图尺寸、配置和障碍物摘要；上游每步打印成功/失败和耗时；运行中按 节流周期打印规划序号、定位、实际纵向速度、轨迹状态、选中点、目标速度、目标 yaw-rate、 LS 投影统计以及重规划耗时。每一段运行日志都带有：

OBSERVE_ONLY: no chassis command is sent.

明确的安全边界

该 MovementTest 中禁止调用下列或等价的硬件写接口：

• MultiWheelChassis.SendXYThSpeed
• MultiWheelChassis.SendMotion
• 任何轮速、轮角、制动释放或执行器 adapter 写接口
• 任何会启动现有闭环几何控制器并输出到底盘的接口

TrajectoryControlCommand 仅用于控制器适配验证、屏幕展示和终端打印。运动状态不正常、定位 不可用、地图外、规划失败或取消时，测试仅停止/保留观察信息并报错；不尝试“补救性”控制车辆。

验证标准

实现完成后至少验证：

1. 项目可编译，测试类可以被现有 TestRunner 发现和启动。
2. 在不连接可写底盘的环境中，以可复现的定位/地图数据运行一次，确认粗路径、平滑路径和 EM 轨迹均可显示。
3. 每条发布轨迹的 TimeFromStart 单调、ST 图与 PathS 一致，LS 图能报告可投影点和失败点。
4. 源码检查与运行日志都证明观察循环未触发任一底盘写方法。
5. 取消测试后后台循环退出，三个 painter 图层被清理，不残留定时任务。

非目标

本次不实现实车自动驾驶、档位切换、底盘写命令、安全冗余、急停策略或把 EM 轨迹转换为旧的 AbstractTrack 几何控制器输入。这些属于后续独立、可评审的执行模式工作。