223 lines
10 KiB
Markdown
223 lines
10 KiB
Markdown
# 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 权重。
|