# RDKx5 延迟测试与优化计划

日期：2026-08-06

## 1. 测试范围

- 设备：RDKx5，`192.168.10.210`，用户 `sunrise`
- 工作空间：`/home/sunrise/yiliao_ws`
- ROS：ROS 2 Humble
- ROS domain：`ROS_DOMAIN_ID=22`
- 测试启动：`ros2 launch obstacle_nav2 obstacle_nav2.launch.py`
- 测试结束后已停止本次启动的 launch、底盘、雷达、障碍检测和 Nav2 子进程
- 停止后确认 `/scan` publisher 数量为 0，没有遗留本次测试的雷达或底盘进程

本次没有发送导航目标或非零速度命令。测试期间 `/cmd_vel`、`/cmd_vel_nav` 没有非零输出，里程计速度为 0。由于雷达扇区存在约 0.62 m 近障碍，且系统 CPU 负载较高，没有执行实车运动和底盘 watchdog 的实际动作测试。

## 2. 测试结果

新启动实例的 Nav2 lifecycle 节点成功进入 `active`，并且 TF 检查通过。此前旧实例曾加载过期的 `/tmp/launch_params_*`，导致 local costmap 等待 `odom_combined`；重启后当前 profile 使用 `odom`，该问题消失。

### 2.1 消息链路

独立 rclpy 订阅，稳定窗口约 20 秒：

| 话题 | 平均间隔 | P95 | P99 | 最大间隔 | 时间戳年龄 |
| --- | ---: | ---: | ---: | ---: | ---: |
| `/scan` | 82.1 ms | 87.4 ms | 109.5 ms | 117.5 ms | P99 10.6 ms，最大 113.5 ms |
| `/obstacles` | 82.0 ms | 88.5 ms | 104.7 ms | 115.9 ms | P99 18.2 ms，最大 24.7 ms |
| `/odom_combined` | 50.1 ms | 71.4 ms | 81.5 ms | 112.4 ms | P95 18.8 ms，最大 28.4 ms |

结论：雷达和障碍检测平均链路延迟不高，但存在 100 ms 级别长尾；odom 约 20 Hz，反馈周期本身约 50 ms。

### 2.2 代价地图

当前默认 launch 实际加载 `nav2_profile_10.yaml`，运行时参数为：

```yaml
local_costmap:
  update_frequency: 5.0
  publish_frequency: 2.0
global_costmap:
  update_frequency: 1.0
  publish_frequency: 1.0
```

有效话题为 `/local_costmap/costmap_raw` 和 `/global_costmap/costmap_raw`，类型为 `nav2_msgs/msg/Costmap`。

实测：

- local costmap：约 1.67 Hz，平均间隔 598 ms，最大约 602 ms
- global costmap：约 0.75 Hz，平均间隔约 1337 ms，最大约 2001 ms

源码 `nav2_params.yaml` 中虽然已经是 local `10 Hz / 4 Hz`，但默认 launch 没有使用这个文件。因此此前提高频率的修改没有进入本次实际运行链路。

### 2.3 TF 与时间戳

启动和运行初期曾出现：

```text
Lookup would require extrapolation into the future
```

一个样本中，请求时间比最新 TF 超前约 81 ms。稳定运行后连续约 20 秒没有继续增加 TF 失败计数，但启动阶段曾记录 10 条 `ObstacleArrayLayer: TF failed`。

这说明 20 ms TF 查询约束会暴露雷达时间戳和 odom TF 的长尾。问题不是平均延迟，而是偶发的未来时间查询；仅增大 lookup timeout 不能完全解决请求时间已经超出 TF 缓存的问题。

### 2.4 底盘处理耗时

`origincar_base` 日志显示：

```text
scan_to_odom 平均约 44～47 ms
scan_to_odom 最大约 624.6 ms
```

这仍然不满足平均小于 25 ms、P99 小于 45 ms 的目标。该长尾会直接影响 odom TF、costmap TF 查询和控制闭环。

### 2.5 雷达元数据

8 秒采样约 98 帧，约 375 个 range 点：

- `scan_time`：0 到 122 ms，平均约 75.2 ms
- `time_increment`：0 到 0.327 ms，平均约 0.201 ms
- 实际消息间隔约 82 ms

平均值接近实际雷达周期，但存在 `scan_time=0` 的异常样本。当前障碍检测主要使用 `header.stamp`，暂时没有阻塞运行；后续做去畸变或点时间补偿前必须处理这些异常值。

## 3. DDS UDP 根因分析

### 3.1 直接证据

`obstacle_nav2.launch.py` 在第 144 至 146 行为所有包含的节点设置：

```python
SetEnvironmentVariable(
    name='FASTRTPS_DEFAULT_PROFILES_FILE',
    value=fastdds_profile_path)
```

该 XML 的核心配置为：

```xml
<useBuiltinTransports>false</useBuiltinTransports>
<userTransports>
  <transport_id>udp_transport</transport_id>
</userTransports>
```

这会关闭 FastDDS 内置传输，包含同机进程间通常使用的 shared memory，只保留 UDPv4。于是底盘、TF、Nav2 各 server、costmap 和障碍检测之间的同机通信也通过 UDP loopback 完成。

在完整系统运行期间，对底盘、robot_state_publisher 和 bt_navigator 的热点线程执行 `strace`：

- `sendto` 大量发往 `127.0.0.1:12913`、`12931`、`12933`、`12935`、`12941`、`12945`
- 每次发送前高频调用 `setsockopt(SO_SNDTIMEO)`
- 该调用持续返回 `EDOM (Numerical argument out of domain)`
- 单个底盘进程约 3 秒内出现 9000 级别的 `sendto` 和同量级的 `setsockopt`

稳定窗口的 8 核 CPU 采样约为：

```text
47% user + 31% system
```

主要进程瞬时 CPU 约为：

- `origincar_base`：78%
- `robot_state_publisher`：59%
- `bt_navigator`：59%
- `planner_server`：40%
- `controller_server`：40%

### 3.2 谁导致了 UDP 流量

结论不是“obstacle_scanner 单独导致”，而是：

1. `fastdds_udp_only.xml` 强制整套 launch 使用 UDP-only，禁用了同机 shared memory，这是高 UDP 流量和高系统调用开销的首要放大器。
2. 完整 Nav2/底盘图中的多个节点同时发布和订阅 TF、odom、costmap、障碍和 lifecycle 数据，所有这些进程间通信都被放到了 UDP loopback。
3. `foxglove_bridge` 使用同一 ROS domain，订阅多个高频话题，会增加 DDS reader 数量和数据转发负载，但它不是唯一根因。停止本次 Nav2 后，Foxglove 的瞬时 CPU 采样降到约 0.4%，说明高负载主要随完整 ROS 图出现。
4. 当前独立运行的 `static_transform_publisher` 瞬时 CPU 约 0.2%，不是主要 CPU 根因。

因此，最可能的根因链是：

```text
UDP-only 配置
  -> 同机通信全部走 FastDDS UDP loopback
  -> 多节点 DDS reader/writer 产生大量本地 UDP 包
  -> FastDDS 高频设置 SO_SNDTIMEO 且返回 EDOM
  -> system CPU、上下文切换和调度延迟升高
  -> scan_to_odom、TF 和 Nav2 回调出现长尾
```

仅凭一次运行不能把 `EDOM` 归因到某一个 ROS 节点的业务代码；需要按下面计划做 UDP-only 与 shared-memory 的 A/B 对比。现有证据已经足以把 `fastdds_udp_only.xml` 列为第一优先级排查对象。

## 4. 优化计划

### 阶段 0：建立可重复基线

1. 保持当前 `ROS_DOMAIN_ID=22`，关闭 Foxglove，单独启动底盘、雷达、障碍检测和 Nav2。
2. 固定采样 60 秒：`/scan`、`/obstacles`、`/odom_combined`、`/local_costmap/costmap_raw`、`/tf`。
3. 记录每个话题的平均、P95、P99、最大间隔和消息时间戳年龄。
4. 同时记录 8 核 CPU、上下文切换、FastDDS UDP 端口和 `setsockopt EDOM` 次数。

验收：同一配置重复两次，关键指标差异小于 10%。

### 阶段 1：确认并修复 DDS 传输配置

1. A 组：当前 `fastdds_udp_only.xml`。
2. B 组：移除 `FASTRTPS_DEFAULT_PROFILES_FILE`，使用 FastDDS 默认 shared memory + UDP。
3. C 组：显式配置 shared memory + UDP，仅在需要跨主机时保留 UDP，不关闭内置传输。
4. 每组重复阶段 0 的 60 秒采样。
5. 统计 `strace -c` 中 `sendto`、`setsockopt`、`futex`，确认 `SO_SNDTIMEO -> EDOM` 是否消失。
6. Foxglove 单独做一组开启/关闭对比，避免把桥接器负载误判为 Nav2 根因。

优先实现：默认 launch 不再强制 UDP-only。若确实需要跨主机通信，再使用同时启用 shared memory 和 UDP 的 profile，并限制网卡/接口范围。

验收：

- `SO_SNDTIMEO -> EDOM` 为 0
- 完整图空闲运行时系统 CPU 显著下降
- `/scan`、`/obstacles`、`/odom_combined` P99 不恶化
- `scan_to_odom` 长尾不再因 DDS 配置放大

### 阶段 2：统一 Nav2 参数来源并提高 costmap 频率

1. 明确 `nav2_params.yaml`、`nav2_profile_10.yaml`、`nav2_profile_11.yaml` 的职责。
2. 默认 launch 不要继续硬编码与实际调参目标不一致的 profile。
3. 如果 profile 10 是默认实车配置，将 local costmap 的 update/publish 调整到目标值；建议先验证 `10 Hz / 8～10 Hz`，不要一次直接追求更高。
4. global costmap 保持较低频率，避免把 CPU 消耗在不影响即时避障的全局地图上。
5. 重新测 `/local_costmap/costmap_raw` 的实际频率，而不是只检查 YAML。

验收：local costmap 发布 P95 间隔接近 100～125 ms，且 CPU 不出现持续饱和；costmap update 不产生 deadline 或 missed-cycle 日志。

### 阶段 3：修复 TF 时间戳长尾

1. 对比 `/scan` header、`/obstacles` header、odom TF 发布时间和当前 ROS 时间。
2. 对未来时间请求做明确策略：短暂等待、丢弃异常帧或使用最新可用 TF，不能让单个回调阻塞整个障碍链。
3. 保持 `odom`、`base_footprint`、`laser_link` 帧名统一，禁止旧配置回退到 `odom_combined`。
4. 对雷达时间元数据做校验：`scan_time <= 0` 时使用最近有效周期，并限制异常跳变；`time_increment` 必须与 range 数量一致。

验收：连续 60 秒无 `extrapolation into the future`；障碍消息丢弃率为 0；TF 时间戳年龄 P99 小于 20 ms，或对超限帧有明确计数和降级行为。

### 阶段 4：优化底盘处理时序

1. 将 `scan_to_odom` 计算和串口收发的耗时分开统计。
2. 找出 624 ms 长尾对应的线程、锁等待、日志和内存分配。
3. 检查 DDS 高负载修复后 `origincar_base` 的 CPU 和回调耗时是否恢复。
4. 保持底盘 TX 周期 20 ms、PC watchdog 150 ms；随后与 STM32 固件 watchdog 联调。

验收：平均处理耗时小于 25 ms，P99 小于 45 ms，不允许持续出现大于 50 ms 的控制周期；命令停止后 150～200 ms 内归零。

### 阶段 5：Nav2 控制和实车验证

1. 仅在阶段 1～4通过后启用 MPPI 运动测试。
2. 先在开阔区域发送短距离、低速度、带终点姿态的目标。
3. 记录 MPPI 循环耗时、`cmd_vel_nav -> cmd_vel -> odom` 响应和实际舵角响应。
4. 再测试近障碍和急转弯，不同时修改多个 critic 参数。

验收：MPPI 平均循环小于 25 ms，P99 小于 45 ms；障碍从 `/scan` 到 costmap 的端到端延迟满足目标；无持续恢复行为、振荡或控制周期丢失。

## 5. 当前结论

当前还不能宣称系统达到 50～100 ms 障碍反应目标。雷达到障碍检测的平均链路已经接近 80～90 ms，但代价地图约 600 ms 发布一次，DDS UDP-only 引起的高 CPU/高系统调用负载，以及底盘 `scan_to_odom` 的 624 ms 长尾仍是主要阻塞项。下一步应优先做 DDS A/B，对比结果出来前不建议继续调 MPPI critic 权重。