ROS 2 通信与 QoS

分层结构（自下而上）：
  DDS 实现（Fast DDS / CycloneDDS / Zenoh）
    —— 提供发布订阅、发现、序列化、传输
  RMW（ROS Middleware）接口层
    —— 把 DDS 的 API 抽象成 ROS 2 需要的统一接口
    —— rmw_fastrtps_cpp / rmw_cyclonedds_cpp / rmw_zenoh_cpp
  rcl（ROS Client Library 通用层）
    —— 话题/服务/动作的通用逻辑，与语言无关
  rclcpp / rclpy
    —— C++ 与 Python 的客户端库，面向用户

关键认知：
  ROS 2 的「话题」概念映射到 DDS 的「Topic」+「DataWriter/DataReader」
  ROS 2 的「服务」映射到 DDS 的两个 Topic（请求与响应）
  ROS 2 的「动作」映射到 DDS 的三个服务（目标、结果、反馈）
  QoS 直接映射到 DDS 的 QoS 策略，ROS 2 只做了少量预设封装

切换 RMW 只需环境变量，不改代码：
  export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp

三个主流 RMW 的定位：
  1. rmw_fastrtps_cpp（Fast DDS，默认）：生态最完整、工具齐全
     （Discovery Server、Monitor），但默认线程模型偏激进，
     节点多时线程数爆炸。适合中小规模、要官方工具
  2. rmw_cyclonedds_cpp（CycloneDDS）：内存与线程占用低、组播可控、
     稳定性口碑好，TRANSIENT_LOCAL 样本数需显式配置。
     适合资源受限平台、大规模节点、长期运行
  3. rmw_zenoh_cpp（Zenoh，较新）：天然支持广域网、路由与穿透。
     适合跨网段、跨机房、云端协同（详见第 7 节）

实测经验（20 节点、100 话题、千兆网、空闲）：
  Fast DDS 默认  ：CPU 约 15%，启动 3 s
  CycloneDDS 默认：CPU 约 6%，启动 1.5 s
  结论：节点数增长时 CycloneDDS 的差距会进一步拉大

线程数对比（每节点默认）：
  Fast DDS：为每个端点可能开多个线程，100 话题可达数百线程
  CycloneDDS：线程模型更省，通常几十个

Reliability（可靠性）：
  RELIABLE     ：保证送达，失败重传（TCP 语义）
  BEST_EFFORT  ：尽力送达，可能丢（UDP 语义）
Durability（持久性）：
  VOLATILE       ：只给当前及之后的订阅者
  TRANSIENT_LOCAL：为晚加入的订阅者保留历史（地图、标定结果）
History（历史）：KEEP_LAST(n) 只保留最近 n 条；KEEP_ALL 全保留
Depth（队列深度）：与 History 配合，决定缓存多少条
Deadline（截止时间）：数据必须在指定周期内更新，否则触发事件
Liveliness（活跃性）：AUTOMATIC 由 DDS 判定存活；MANUAL_BY_TOPIC 显式声明
Lifespan（寿命）：消息超过指定时间自动失效，不再投递
Ownership（所有权）：SHARED 多发布者共存；EXCLUSIVE 配合 OwnershipStrength
DestinationOrder：BY_RECEPTION_TIME / BY_SOURCE_TIMESTAMP
ResourceLimits：限制历史、样本、实例数量

// 自定义 QoS：中等深度 + 可靠 + 保留历史
rclcpp::QoS qos(100);                  // depth = 100
qos.reliable();
qos.transient_local();
qos.deadline(rclcpp::Duration::from_seconds(0.1));  // 100ms 未更新则告警
qos.liveliness(rclcpp::LivelinessPolicy::Automatic);
qos.lifespan(rclcpp::Duration::from_seconds(1.0));  // 1s 后失效

auto pub = create_publisher<sensor_msgs::msg::PointCloud2>("/points", qos);

标准预设（覆盖 90% 场景）：
  rclcpp::SensorDataQoS()    // BEST_EFFORT, KEEP_LAST 5, VOLATILE
  rclcpp::SystemDefaultsQoS()// RELIABLE, KEEP_LAST 10, VOLATILE
  rclcpp::ServicesQoS()      // RELIABLE, KEEP_LAST 10（服务用）
  rclcpp::ParametersQoS()    // RELIABLE + TRANSIENT_LOCAL（参数事件）

兼容规则（发布者的能力必须 ≥ 订阅者的要求）：
  Reliability：RELIABLE 发布可被 BEST_EFFORT 订阅（兼容）
              BEST_EFFORT 发布不能被 RELIABLE 订阅（不兼容！）
  Durability ：TRANSIENT_LOCAL 发布可被 VOLATILE 订阅（兼容）
              VOLATILE 发布不能被 TRANSIENT_LOCAL 订阅（不兼容）
  Deadline   ：发布的 deadline 必须 ≤ 订阅要求的 deadline
  Liveliness ：发布的租约必须 ≤ 订阅要求的租约

后果：DDS 不为这对端点建立连接，数据完全不流动，
      ROS 2 层面通常无任何报错（除非显式检查）

# 查看实际生效的 QoS 与端点匹配情况
ros2 topic info /scan --verbose
# Type: sensor_msgs/msg/LaserScan
# Publisher count: 1
#   Node name: lidar_driver
#     Reliability: BEST_EFFORT
#     Durability: VOLATILE
#     History (Depth): KEEP_LAST (5)
# Subscription count: 0        ← 收不到数据时先看这里

# 强制用指定 QoS 订阅，验证是否是兼容性问题
ros2 topic echo /scan --qos-reliability best_effort

调试顺序（固定，不要跳步）：
  1. ros2 topic list                 确认话题存在
  2. ros2 topic info --verbose       比对两端 QoS
  3. ros2 topic hz                   看发布频率
  4. ros2 topic echo                 看内容
  跳过第 2 步直接看内容是最浪费时间的做法

Deadline 事件：
  发布端 deadline 内没发出 → OFFERED_DEADLINE_MISSED
  订阅端 deadline 内没收到 → REQUESTED_DEADLINE_MISSED
  用途：把「传感器卡死」「控制指令超时」变成可观测事件

Liveliness 事件：
  订阅端检测到发布者不再活跃 → LIVELINESS_CHANGED
  用途：检测节点崩溃、网络断开，比心跳话题更底层可靠

Lifespan：消息在队列中超过 lifespan 自动丢弃
  用途：避免过期指令被延迟执行（1 秒前的速度指令不该再执行）

// 订阅端监听 deadline 未命中事件
auto sub = create_subscription<sensor_msgs::msg::Image>(
    "/camera/image", rclcpp::SensorDataQoS(),
    [](sensor_msgs::msg::Image::ConstSharedPtr) {});
auto deadline_qos = sub->get_actual_qos().deadline();
// 通过 rclcpp 的事件机制或 rcl 的 qos_event 监听
// 也可用 ros2 topic 工具查看事件

Simple Discovery Protocol（SDP，默认）：
  节点通过组播周期性宣告自己的端点，节点少时良好，多时组播风暴

节点数与启动时间（实测经验值）：
  5 个 < 1 s；20 个 2~5 s；50 个 10~30 s（开始丢包）；
  100 个分钟级，甚至发现失败

三种应对手段（侵入性递增）：
  1. 限制组播到指定网卡（避免跨网段泄漏）
  2. Discovery Server（集中式发现，节点只与服务器交互）
  3. 静态端点（initialPeersList，拓扑固定时最可靠）

# 方案一：限制组播到指定网卡
export CYCLONEDDS_URI='<CycloneDDS><Domain><General>
  <NetworkInterfaceAddress>eth0</NetworkInterfaceAddress>
  <AllowMulticast>spdp</AllowMulticast>
  </General></Domain></CycloneDDS>'

# 方案二：Discovery Server（ROS 2 内置支持，以 CycloneDDS 为例）
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export ROS_DISCOVERY_SERVER=192.168.1.10:11811
ros2 run rmw_cyclonedds_cpp ddsperf  # 或 fastdds discovery -i 0

# 方案三：静态端点（Fast DDS 的 initialPeersList，写在 XML 里）
export FASTRTPS_DEFAULT_PROFILES_FILE=/etc/ros/fastdds_profile.xml

三种机制的取舍：
  SDP        ：零配置、小规模最好；大规模不可用
  Discovery Server：集中式、可跨网段；引入单点（需冗余）
  静态端点   ：最可靠、无发现开销；新增节点要改配置

Zenoh 相对 DDS 的差异：
  1. 无组播依赖：用 router 做发现与转发，天然穿透 NAT
  2. 跨网络：支持互联网、5G、VPN，节点无需在同一二层网络
  3. 资源占用低：为嵌入式与大规模 IoT 设计
  4. 路由与桥接：Zenoh router 可桥接不同域、不同网络

使用方式：
  export RMW_IMPLEMENTATION=rmw_zenoh_cpp
  ros2 run rmw_zenoh_cpp rmw_zenohd     # 启动 router

取舍：优点是跨网段天然支持、资源省、可路由；
      缺点是生态较新、部分 QoS 语义支持待完善、引入 router 需运维

回调延迟的构成：
  1. DDS 接收线程收到数据（DDS 内部线程，优先级通常不可控）
  2. 放入等待集合（waitset）
  3. 执行器线程取出并调用回调
  4. 回调执行时间

延迟的主要来源：
  DDS 内部线程的调度延迟（可能被低优先级线程抢占）
  执行器的线程模型（单线程 vs 多线程）
  回调组配置（互斥 vs 可重入）
  内存分配与锁竞争（回调内的动态分配）

实时通信的三条纪律：
  1. 控制回调必须绑定到隔离核（CPU 亲和性），而非靠调度碰运气
  2. 控制回调内禁止动态分配、禁止日志、禁止阻塞调用
  3. 用 StaticSingleThreadedExecutor 减少每轮遍历开销
     （订阅关系固定时最快，因为它预建了回调到执行的映射）

// 实时控制节点的执行器配置
rclcpp::NodeOptions opts;
opts.use_intra_process_comms(true);     // 同进程零拷贝
auto node = std::make_shared<ControlNode>(opts);

rclcpp::executors::StaticSingleThreadedExecutor exec;
exec.add_node(node);
// 回调组：控制相关用 MutuallyExclusive，避免并发访问共享状态
// 感知相关用 Reentrant 提升吞吐（见架构一篇）
exec.spin();

三个层次的「零拷贝」：
  1. 进程内通信（Intra-process）：同进程节点绕过 DDS 直接传指针
     要求 use_intra_process_comms(true) + unique_ptr 发布，
     durability 必须为 VOLATILE
  2. Loaned Messages：从 DDS 预分配内存池借一块填充后发布，
     避免构造→序列化→拷贝的多次分配，适合高频定长消息
  3. 共享内存传输：跨进程但同机时用共享内存而非网络回环
     Iceoryx / Fast DDS SHM，适合大消息（图像、点云）跨进程高频传输

// 进程内通信：必须用 unique_ptr 发布，否则退化为拷贝
auto msg = std::make_unique<sensor_msgs::msg::PointCloud2>();
fillPointCloud(*msg);
pub_->publish(std::move(msg));    // 移动语义，零拷贝

// 借出消息（Fast DDS 风格伪码）
auto loaned = pub_->borrow_loaned_message();
fill(*loaned);
pub_->publish(std::move(loaned));

验证零拷贝是否生效：
  ros2 topic info /points --verbose
  看端点是否标记为 intra-process；intra-process 下 Publisher count
  可能显示为 0 但数据仍在流动——这是最容易误判的一点
  用 ros2 topic hz 确认数据实际到达更可靠

多机部署的三个必配项：
  ROS_DOMAIN_ID          ：区分逻辑域，不同机器人不同 ID，避免互相发现
  ROS_LOCALHOST_ONLY=1   ：单机调试时避免污染共享网络
  ROS_STATIC_PEERS / Discovery Server：跨网段通信

网络拓扑的三种形态：
  1. 扁平二层网络（同一交换机）：组播可用，SDP 即可，< 50 节点
  2. 路由网络（跨网段）：组播通常不可用，须 Discovery Server/静态端点/Zenoh
  3. 星型（中心 + 边缘）：中心做 Discovery Server 与汇聚，适合车队与云端

带宽与延迟的预算：
  同交换机 < 1 ms；跨交换机 1~5 ms；WiFi 5~50 ms 且抖动大；
  蜂窝/广域几十到几百 ms
  无线策略：传感器用 BEST_EFFORT + 大 depth，感知尽量下沉到本地

常用诊断工具：
  ros2 topic info --verbose ：端点、QoS、类型、匹配情况
  ros2 topic hz / bw        ：发布频率 / 带宽占用
  ros2 topic delay          ：端到端延迟（需时间戳）
  ros2 doctor --report      ：整体健康检查
  ddsperf（CycloneDDS）     ：底层 DDS 吞吐与延迟基准

ros2 topic bw /points       # 逐话题看带宽，定位「谁吃满网络」
ros2 topic delay /scan      # 看端到端延迟分布
ros2 doctor --report        # 整体诊断
ddsperf -c -n 10 -s 1024 -r 1Hz pub sub   # 底层 DDS 基准

关键监控指标：
  每个话题的实际接收频率（对比发布频率，看丢帧率）
  端到端延迟的 p50/p99（不是均值）
  带宽占用（是否接近网卡上限）
  DDS 线程数与 CPU 占用（发现线程泄漏）、发现耗时

RMW 选型：
  节点 < 20、要官方工具 → rmw_fastrtps_cpp（默认）
  节点 > 50 或资源受限  → rmw_cyclonedds_cpp
  跨网段 / 云端协同     → rmw_zenoh_cpp

QoS 配置：
  传感器数据 → SensorDataQoS（BEST_EFFORT, KEEP_LAST 5）
  状态与命令 → RELIABLE + KEEP_LAST(10)
  地图与标定 → RELIABLE + TRANSIENT_LOCAL + KEEP_LAST(1)
  静态变换   → RELIABLE + TRANSIENT_LOCAL + KEEP_ALL

发现机制：< 20 节点用 SDP；> 50 用 Discovery Server；
          拓扑固定用静态端点；跨网段用 Discovery Server 或 Zenoh

性能：同进程大消息用 intra-process + unique_ptr；跨进程同机用共享内存；
      实时控制用隔离核 + 静态单线程执行器 + 回调内无分配

调参优先级（从高到低）：
  1. RMW 实现（影响最大，改环境变量即可）
  2. QoS 匹配（连不上先查这里）
  3. 发现机制（规模上不去先查这里）
  4. 零拷贝与共享内存（大消息吞吐）
  5. 执行器与回调组（实时性）