295 lines
18 KiB
Markdown
295 lines
18 KiB
Markdown
# EM 轨迹闭环观察与独立网页可视化设计
|
||
|
||
## 1. 背景与目标
|
||
|
||
现有 `TrajectoryObservationMovementTest` 从 MDCS 读取实时定位和带符号纵向速度，重复执行 EM 规划并显示最新轨迹。它是规划观察闭环，不向底盘、转向、电机、制动或档位接口发送命令。
|
||
|
||
EMPlanner 已修正两类纵向滚动规划语义：
|
||
|
||
1. `RollingContinuation` 的滚动视界末点不再被强制当作静止停车点。
|
||
2. jerk 变量只对应真实相邻时间节点之间的区间，不为滚动轨迹最后一个点之后不存在的后继点施加或展示虚假的 jerk 约束。
|
||
|
||
本次工作修改 MovementTest 观察任务，使实车环境可以直接验证这些语义；同时新增可复用、可独立升级的本机网页可视化类库。网页提供全局路径、方向分段、LS、ST、曲率、速度、加速度、加加速度、规划配置及周期历史。现有测试继续保持 `OBSERVE_ONLY`。
|
||
|
||
## 2. 范围与非目标
|
||
|
||
### 2.1 范围
|
||
|
||
- 保持实车定位/速度输入和滚动重规划，不增加任何车辆控制输出。
|
||
- 支持完整多方向段观察，在可靠确认车辆实际运动方向后顺序推进至下一段。
|
||
- 在鸟瞰图中显示完整全局路径，并高亮当前 EM 规划方向段及本轮时域覆盖区间。
|
||
- 增加独立 `TrajectoryPlanningVisualization` 类库，供当前及未来规划测试复用。
|
||
- 提供只监听本机回环地址的实时网页看板。
|
||
- 保留原生 Painter 作为显式可选的兼容显示方式。
|
||
- 保留用户已调整的 MovementTest 默认运行参数，并同步设置对象、测试与文档：
|
||
- `SolverTimeoutSeconds = 5.0 s`
|
||
- `MaximumOsqpIterations = 100000`
|
||
- `TimeHorizonSeconds = 2.0 s`
|
||
- `OutputTimeStepSeconds = 0.10 s`
|
||
|
||
### 2.2 非目标
|
||
|
||
- 不实现轨迹跟踪、档位控制、底盘写入或自动驾驶闭环。
|
||
- 不开放局域网或公网访问。
|
||
- 不用网页按钮强制切换方向段。
|
||
- 不记录无限历史，不默认写入磁盘，不实现实验回放文件格式。
|
||
- 不修改 EMPlanner 的约束、权重、容差或已确认的滚动规划语义。
|
||
|
||
## 3. 总体架构
|
||
|
||
新增独立类库：
|
||
|
||
```text
|
||
ClumsyPilot/TrajectoryPlanningVisualization/
|
||
├── TrajectoryPlanningVisualization.csproj
|
||
├── Contracts/ # 不可变快照、曲线、配置和状态 DTO
|
||
├── Runtime/ # 回环 HTTP/SSE 服务、最新帧和有界历史
|
||
└── Web/ # 内嵌 HTML/CSS/JavaScript 科研风前端
|
||
```
|
||
|
||
现有观察模块新增 EM 适配层：
|
||
|
||
```text
|
||
ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/
|
||
├── TrajectoryObservationVisualizationAdapter.cs
|
||
├── TrajectoryObservationSegmentTracker.cs
|
||
└── 现有 MovementTest、Pipeline、Contracts、Diagnostics、Presentation
|
||
```
|
||
|
||
依赖方向固定为：
|
||
|
||
```text
|
||
EM / Map / PathSmoothing / MovementTest
|
||
│
|
||
▼
|
||
TrajectoryObservationVisualizationAdapter
|
||
│ 只发布通用不可变快照
|
||
▼
|
||
TrajectoryPlanningVisualization
|
||
│
|
||
▼
|
||
localhost browser
|
||
```
|
||
|
||
可视化类库不得引用 EMPlanner、MDCS、ClumsyCore、Painter 或 MovementTest。MovementTest 负责生命周期和浏览器打开行为；类库只返回本机访问 URI。
|
||
|
||
主项目通过 `ProjectReference` 引用新类库，并排除对子项目源码的重复编译。插件发布流程必须包含新增程序集及其必要依赖。网页资源作为程序集嵌入资源发布，不依赖 CDN、Node.js 或外网。
|
||
|
||
类库和主项目均继续以 `netstandard2.0` 为目标框架并使用 C# 10。JSON 使用仓库现有的 `Newtonsoft.Json 13.0.4`；不引入 ASP.NET Core、Kestrel 或新的前端包。由于 Windows `HttpListener` 可能要求额外的 HTTP.sys URL ACL，运行时使用 `TcpListener(IPAddress.Loopback, port)` 实现本功能所需的受限 HTTP/1.1 GET 与 SSE 子集，不依赖管理员预注册。
|
||
|
||
## 4. 可视化类库公共边界
|
||
|
||
类库只暴露三个主要生命周期操作：
|
||
|
||
```csharp
|
||
visualization.Start(options);
|
||
visualization.Publish(snapshot);
|
||
visualization.Stop();
|
||
```
|
||
|
||
`Start` 返回包含随机会话令牌的完整本机 URI。`Publish` 必须是非阻塞操作：用原子替换保存最新不可变快照，不等待序列化、网络、浏览器或绘图。`Stop` 取消服务任务、关闭连接并释放端口；重复调用安全。
|
||
|
||
快照分为两类：
|
||
|
||
- 静态快照：地图边界、障碍物、全局粗路径、Local G2 路径、全部方向段、换向点和冻结后的生效配置。每次会话只生成并传输一次。
|
||
- 动态快照：实车状态、当前段、当前/上一轮轨迹、当前采样点、本轮规划结果、交接指标和诊断摘要。
|
||
|
||
服务只绑定 `127.0.0.1`。页面和事件接口均校验随机会话令牌。服务最多接受两个浏览器客户端，额外连接被拒绝。前端静态资源通过普通 HTTP 获取，动态帧通过 Server-Sent Events 发送；客户端断开或读取缓慢时丢弃旧帧，不对发布者形成反压。请求解析器只接受 `GET`，请求头上限为 `16 KiB`，首行/请求头读取超时为 `2 s`；其他方法返回 `405`，未知路径返回 `404`，令牌错误返回 `403`。
|
||
|
||
## 5. 资源与性能约束
|
||
|
||
- 默认网页刷新率为 `10 Hz`，与 `ObserverPeriodSeconds` 解耦。
|
||
- 最新动态帧容量固定为 `1`；新帧覆盖旧帧。
|
||
- 只保留当前轮和上一轮的完整轨迹点。
|
||
- 周期历史默认保留最近 `60` 轮，历史项只含摘要，不含完整轨迹。
|
||
- 地图占用信息、全局路径、方向段和配置不进入重复动态帧。
|
||
- 地图占用栅格编码为行优先 bitset（每格 `1 bit`）并由浏览器单个 Canvas 绘制；不得为每个占用格创建 JSON 对象、SVG 节点或 Painter 对象。
|
||
- 没有浏览器客户端时不重复序列化相同动态帧。
|
||
- 网页关闭或变慢不能改变重规划周期、求解器超时或规划结果。
|
||
- `EnableWebVisualization=false` 时不启动服务、不创建网页快照或历史缓冲。
|
||
- 任何网页服务、快照转换或前端异常均被隔离：记录一次中文诊断并关闭本次网页输出，规划观察继续运行。
|
||
|
||
## 6. MovementTest 配置
|
||
|
||
新增公开字段：
|
||
|
||
```text
|
||
EnableWebVisualization = false
|
||
AutoOpenWebVisualization = true
|
||
WebVisualizationPort = 0
|
||
WebRefreshRateHz = 10
|
||
VisualizationHistoryCycleLimit = 60
|
||
EnableNativePainterVisualization = false
|
||
DirectionConfirmationSpeedMetersPerSecond = 0.02
|
||
DirectionConfirmationSamples = 3
|
||
GearSwitchProjectionToleranceMeters = 0.50
|
||
GearSwitchStopHoldSeconds = 0.20
|
||
```
|
||
|
||
校验规则：开关字段不需数值校验；端口为 `0` 或 `1024..65535`；刷新率、历史容量、速度阈值、投影容差和等待时间必须为有限正值；连续确认样本数必须为正整数。
|
||
|
||
端口为 `0` 时选择空闲回环端口。启动成功后，完整带令牌 URI 同时写入 MDCS UI 状态和控制台；`AutoOpenWebVisualization=true` 时由 MovementTest 尝试打开默认浏览器。打开失败只报告地址，不中止观察。
|
||
|
||
原生 Painter 与网页独立控制。`EnableNativePainterVisualization=false` 时不取得或刷新 World、LS、ST Painter，避免与浏览器重复绘图。两种显示都关闭时，规划循环和控制台诊断仍工作。
|
||
|
||
自动打开浏览器只由 MovementTest 主机调用 `Process.Start`，类库本身不读取桌面环境或启动外部进程。该调用必须捕获异常；Windows 服务会话或无桌面环境中只打印完整 URI。
|
||
|
||
## 7. 多方向段观察状态机
|
||
|
||
现有硬编码 `segmentIndex = 0` 改为只允许顺序前进的活动段状态机。
|
||
|
||
### 7.1 正常规划
|
||
|
||
- 活动段初始为第 `0` 段。
|
||
- 每轮请求使用活动段索引、方向及同段上一条已发布轨迹。
|
||
- 只有同一 `SegmentIndex` 和 `TravelDirection` 的上一条轨迹可以作为软参考。
|
||
- 网页与原生 Painter 均从同一个活动段状态读取高亮和 LS/ST 投影对象。
|
||
|
||
### 7.2 换向确认
|
||
|
||
到达 `GearSwitchApproach` 后：
|
||
|
||
1. 车辆位姿必须能在 `GearSwitchProjectionToleranceMeters` 内投影到换向点邻域。
|
||
2. 带符号实际速度绝对值必须不超过 EM 生效配置的 `StopSpeedToleranceMetersPerSecond`，并持续至少 `GearSwitchStopHoldSeconds`。
|
||
3. 状态机进入“等待下一方向实际运动”状态。
|
||
4. 随后实际速度绝对值必须至少为 `DirectionConfirmationSpeedMetersPerSecond`，符号与下一段一致，且车辆可投影到下一段起始区域。
|
||
5. 条件连续满足 `DirectionConfirmationSamples` 次后，活动段从 `N` 切换到 `N+1`。
|
||
|
||
零速时方向不明确，状态机保持原段；瞬时反号、位置不匹配、丢失定位或不连续样本都会清零确认计数。状态机不得跳段、自动回退或跨越两个换向点。
|
||
|
||
切段时创建新的分段规划协调状态，清除可执行的旧段发布轨迹；旧轨迹仅作为网页历史证据保留，不传入新段 EM 请求。新段首次规划前网页显示“已确认换向，等待下一段首条轨迹”。
|
||
|
||
## 8. 网页信息架构与视觉规范
|
||
|
||
界面采用已确认的科研绘图风：纯白背景、浅灰细网格、细坐标轴、`0.8–1.2 px` 曲线、低饱和语义色和论文式多子图。当前轨迹为蓝色实线，上一轮为灰色虚线，交接点和换向点为橙色，红色只表示真实越界或失败。
|
||
|
||
页面说明、状态、告警、按钮和配置名称使用中文。坐标变量、单位和数学符号保留领域惯例，例如 `s (m)`、`t (s)`、`κ (m⁻¹)`、`v (m/s)`、`a (m/s²)`、`j (m/s³)`。
|
||
|
||
### 8.1 路径总览
|
||
|
||
- 地图、障碍物、粗路径、完整 Local G2 全局路径和实车位姿。
|
||
- 已完成方向段为灰色细实线，当前段为蓝色细实线，未来方向段为浅灰虚线。
|
||
- 换向点使用带顺序编号的橙色菱形。
|
||
- 当前 EM 距离/时间视界实际覆盖区间使用更深蓝色叠加。
|
||
- 当前和上一轮 EM 轨迹叠加，并以橙色标出绝对时间交接位置。
|
||
- 显示活动 `SegmentIndex`、前进/倒车方向、纵向模式、终端类型、规划耗时和轨迹年龄。
|
||
|
||
### 8.2 LS / ST
|
||
|
||
- `l(s_ref)`：将当前已发布 EM 轨迹投影到完整活动方向段后，横向偏移随共享方向段 `ReferenceS` 的变化。
|
||
- `PathS(t)`：当前规划时域内的实际局部 `PathS` 随 `TimeFromStart` 的变化，这是 ST 求解结果使用的坐标。
|
||
- 两类横轴必须使用不同标签，网页不得把方向段 `ReferenceS` 与每轮从局部起点累计的 `PathS` 混为一谈。
|
||
- 标明方向段索引、方向、边界类型和投影失败数。
|
||
- LS/ST 只展示当前活动方向段，不跨换向边界拼接。
|
||
|
||
### 8.3 曲率与运动学
|
||
|
||
- `κ(s)` 与 `κ(t)`。
|
||
- `v(t)`、`a(t)`、`j(t)` 与 `yaw rate(t)`。
|
||
- 对应生效限制以细虚线绘制。
|
||
- `j[i]` 表示真实区间 `[tᵢ,tᵢ₊₁)`；jerk 曲线点数严格为轨迹点数减一。最后一个轨迹点不绘制伪造的 `j=0`，而标注“末点后无时间区间”。
|
||
- 同图展示实际带符号车速与当前选中轨迹点速度。
|
||
|
||
纵向加速度与 jerk 由当前主程序集内的 EM Adapter 读取 `EmTrajectoryPoint` 的现有内部字段并转换成通用曲线 DTO；不为可视化修改 EMPlanner 的公共契约。
|
||
|
||
### 8.4 周期历史
|
||
|
||
最近 `VisualizationHistoryCycleLimit` 个周期展示：周期号、时间、成功/失败、是否发布、规划耗时、段索引、方向、纵向模式、终端类型、末端速度、末端加速度和失败原因摘要。
|
||
|
||
### 8.5 生效配置
|
||
|
||
配置页只展示本次冻结后真正传给规划器的值，不读取启动后的可编辑字段。每个条目包含中文名称、原始字段名、数值和单位：
|
||
|
||
- 调度：重规划周期、ST 时域、输出步长、距离视界、求解超时、交接前视时间和状态最大年龄。
|
||
- OSQP：最大外层迭代、最大 OSQP 迭代、绝对/相对/严格残差容差、warm start、polish 和 native verbose。
|
||
- 车辆与安全：长、宽、安全余量、最大曲率或最小转弯半径。
|
||
- 纵向限制与权重：前进/倒车速度、加速度、减速度、jerk、横向加速度、曲率变化率、停车速度容差、零速保持时间及全部 ST 权重。
|
||
- 横向限制、走廊参数和全部 LS 权重。
|
||
- 地图边界、分辨率、障碍物数量和快照版本。
|
||
- 当前地图、参考路径、车辆状态、当前轨迹和上一轨迹 ID。
|
||
- 可视化刷新、历史容量和换向确认参数。
|
||
|
||
## 9. 滚动规划专项诊断
|
||
|
||
网页必须把规划语义与数值结果分开显示：
|
||
|
||
- `RollingContinuation` 或 `ApproachStopBoundary`：显示“滚动末端停车硬约束：未启用”，并展示实际末端 `v/a`。滚动末速为零不单独判定失败，因为车辆状态或最优结果仍可能产生零速；关键证据是纵向模式没有启用精确停车约束。
|
||
- `ExactStopAtBoundary`：显示真实 Goal 或 GearSwitch 停车锚点、稳定段起点和零速稳定区间。
|
||
- jerk 图只显示真实区间，末点显示“不适用”，避免把缺失的末端后继区间解释为加加速度约束。
|
||
- 当前轮和上一轮按 `EffectiveAtUtc + TimeFromStart` 对齐，在交接时刻插值并展示世界坐标 `Δposition`、共享方向段上的 `ΔReferenceS`、`Δv` 和 `Δa`。不得直接比较两轮各自从局部起点累计的 `PathS`；任一轨迹无法投影到共享方向段时，`ΔReferenceS` 显示“不可用”。
|
||
- 交接指标仅作为观察诊断，不改变规划、发布或执行状态。
|
||
|
||
## 10. 错误处理与状态显示
|
||
|
||
- 可视化启动失败：记录一次中文错误，规划观察继续。
|
||
- 快照中单个字段不可用：对应值显示“不可用”，其他图继续刷新。
|
||
- 浏览器超过两个预期刷新周期没有收到新帧：显示“数据已过期”及最后时间戳，不把旧值伪装成实时状态。
|
||
- 换向确认失败：保持当前段，显示位置、停车保持、速度方向或连续样本中未满足的条件。
|
||
- EM 新周期失败：保留上一条成功轨迹，醒目显示本轮状态与未改写的原始失败原因。
|
||
- 服务或快照转换异常：本次网页输出熔断关闭，不自动循环重启。浏览器前端异常只在该标签页显示“页面绘图异常”并停止该页重绘；由于服务只接受 GET 且页面没有反向控制通道，前端异常不通知或关闭主进程服务，规划继续且服务器最终由 `TestStop()` 回收。
|
||
- `TestStop()`：取消规划观察、停止发布、关闭 HTTP/SSE、释放端口；仍连接的页面显示“会话已结束”。
|
||
|
||
## 11. 测试策略
|
||
|
||
### 11.1 独立可视化类库
|
||
|
||
新增独立验证宿主，覆盖：
|
||
|
||
- 类库不引用 MDCS、ClumsyCore、Painter 或 EMPlanner。
|
||
- 快照不可变与 JSON 序列化稳定。
|
||
- `Publish` 非阻塞且最新帧容量为 `1`。
|
||
- 历史严格受容量限制。
|
||
- 服务只监听回环地址并校验令牌。
|
||
- 原始 TCP HTTP 解析器拒绝超大请求头、非 GET 方法、未知路径和错误令牌。
|
||
- 慢客户端不阻塞发布者。
|
||
- 静态快照不重复进入动态帧。
|
||
- 启停、重复停止和端口释放。
|
||
- 内嵌页面包含中文说明、科研坐标单位和所有必需页签。
|
||
|
||
### 11.2 EM Adapter 与观察任务
|
||
|
||
扩展 `EMPlannerVerificationHost` 的 `trajectory-observation` 检查：
|
||
|
||
- 生效配置来自冻结快照，并包含本节规定的全部参数组。
|
||
- MovementTest、settings 默认值和 README 对 `5.0 s / 100000 / 2.0 s / 0.10 s` 保持一致。
|
||
- rolling、approach、exact-stop 三种模式映射正确。
|
||
- jerk 区间数等于轨迹点数减一，末点没有伪造区间。
|
||
- 当前/上一轮绝对时间对齐和 `Δposition/ΔReferenceS/Δv/Δa` 计算正确，且不直接比较两轮局部 `PathS`。
|
||
- 多方向路径完整输出，活动段和当前视界高亮。
|
||
- 换向状态机必须满足位置、停车保持和连续方向样本才顺序推进。
|
||
- 切段后不复用旧方向段轨迹。
|
||
- 可视化关闭时不启动服务或构造快照。
|
||
- 网页异常不会终止观察循环。
|
||
- 源码审计继续确认没有底盘写方法。
|
||
|
||
### 11.3 集成与实车验收
|
||
|
||
运行：
|
||
|
||
```powershell
|
||
dotnet run --project ClumsyPilot/tests/TrajectoryPlanningVisualizationVerificationHost/TrajectoryPlanningVisualizationVerificationHost.csproj
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory-observation
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- em-all
|
||
dotnet build ClumsyPilot/ClumsyPilot.csproj -p:ExcludeLegacyAutoAvoidance=true
|
||
```
|
||
|
||
实车观察清单：
|
||
|
||
1. 开启网页、关闭原生 Painter，确认页面地址、令牌和中文科研风界面。
|
||
2. 确认全局路径所有方向段可见，当前段和本轮规划视界高亮。
|
||
3. 观察 `RollingContinuation -> ApproachStopBoundary -> ExactStopAtBoundary`，确认滚动末点未显示停车硬约束。
|
||
4. 确认 `j(t)` 最后一个区间结束于最后一个轨迹点之前，不出现虚假 point-21 后继区间或 `JerkLimitExceeded`。
|
||
5. 在真实换向场景确认状态机先等待停车，再以连续实际运动方向样本推进至下一段。
|
||
6. 断开或关闭浏览器，确认规划周期和耗时无异常变化。
|
||
7. 停止 MovementTest，确认服务退出、端口释放且无底盘命令。
|
||
|
||
## 12. 完成标准
|
||
|
||
- 独立类库、EM Adapter、多段观察和网页看板均按上述边界实现。
|
||
- 所有自动化验证通过，主项目构建成功。
|
||
- 页面使用中文说明和科研绘图规范，数学符号与单位准确。
|
||
- 滚动末端语义、jerk 区间语义和换向分段高亮均能由页面直接观察。
|
||
- 可视化关闭或失败时不改变 EM 规划行为。
|
||
- 源码和运行时证据均证明 `OBSERVE_ONLY` 不变。
|
||
- 插件包包含 `ClumsyPilot.dll`、`TrajectoryPlanningVisualization.dll`、固定 OSQP 运行时及许可证，且没有遗漏新增托管依赖。
|