yiliao2026/test1.md

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，运行时参数为：

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 与时间戳

启动和运行初期曾出现：

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 日志显示：

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 行为所有包含的节点设置：

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

该 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 采样约为：

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 根因。

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

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 权重。