init: ROS2 learning suite

2026-08-03 18:09:35 +08:00
commit 5ef38ab508
95 changed files with 13322 additions and 0 deletions
@@ -0,0 +1,635 @@
+# 20 · Topic 深度:pub/sub(完全指南)
+
+> **目标**:吃透 ROS2 pub/sub,涵盖消息定义、QoS、跨语言互通、常见坑,学完直接写工业级代码。
+
+---
+
+## 目录
+
+- [1. 通信模型](#1-通信模型)
+- [2. 消息类型](#2-消息类型)
+- [3. Publisher API(Python / C++)](#3-publisher-apipython--c)
+- [4. Subscriber API(Python / C++)](#4-subscriber-apipython--c)
+- [5. QoS 详解(必备)](#5-qos-详解必备)
+- [6. 跨语言互通(核心特性)](#6-跨语言互通核心特性)
+- [7. 完整实战:写一个传感器数据流](#7-完整实战写一个传感器数据流)
+- [8. 调试命令大全](#8-调试命令大全)
+- [9. 常见坑 + 解决方案](#9-常见坑--解决方案)
+- [10. 在本仓库里跑](#10-在本仓库里跑)
+- [11. 进阶:可靠通信 / 录制 / 跨机](#11-进阶可靠通信--录制--跨机)
+
+---
+
+## 1. 通信模型
+
+### 1.1 一句话
+Topic 是 **异步、多对多、单向**的发布订阅通道。
+
+```
+Publisher ──publish()──> Topic (chatter) ──callback(msg)──> Subscriber
+        ────────────────────────────────────────────────────
+        异步,非阻塞     多对多,单向       事件循环触发
+```
+
+### 1.2 三种通信对比
+
+| 维度 | Topic | Service | Action |
+|---|---|---|---|
+| 同步 | **异步** | 同步(阻塞) | 异步(long-running) |
+| 方向 | 单向 pub→sub | 双向 req/resp | 双向 goal/fb/result |
+| 一对多 | ✅ | ❌ 一对一 | ❌ 一对一 |
+| 取消 | N/A | ❌ | ✅ |
+| 进度反馈 | N/A | ❌ | ✅ |
+| 适合 | 传感器流、状态 | 短查询 | 长任务 |
+
+### 1.3 何时用 Topic
+
+✅ **适合**:
+- 周期性传感器数据(相机、IMU、激光雷达、关节状态)
+- 状态发布(机器人位置、电池电量)
+- 持续监控数据(诊断、log)
+
+❌ **不适合**:
+- 一次性 req/resp(用 Service)
+- 长任务(用 Action)
+- 需要反馈进度(用 Action)
+
+---
+
+## 2. 消息类型
+
+### 2.1 标准消息(ROS2 自带)
+
+| 包 | 常用消息 |
+|---|---|
+| `std_msgs` | `String`, `Bool`, `Int32`, `Float64`, `Header` |
+| `geometry_msgs` | `Point`, `Quaternion`, `Pose`, `Twist`, `Transform`, `Vector3` |
+| `sensor_msgs` | `Image`, `JointState`, `Imu`, `PointCloud2`, `CameraInfo`, `LaserScan` |
+| `nav_msgs` | `Odometry`, `Path`, `OccupancyGrid` |
+| `trajectory_msgs` | `JointTrajectory`, `JointTrajectoryPoint` |
+
+### 2.2 消息结构示例
+
+```python
+# sensor_msgs/msg/Image
+msg = sensor_msgs.msg.Image()
+msg.header.stamp = node.get_clock().now().to_msg()
+msg.header.frame_id = "camera_optical_frame"
+msg.height = 480
+msg.width = 640
+msg.encoding = "bgr8"             # OpenCV 默认
+msg.is_bigendian = 0
+msg.step = 640 * 3                # width * bytes_per_pixel
+msg.data = bgr_array.tobytes()     # numpy → bytes
+```
+
+### 2.3 自定义消息(本仓库不用)
+
+如果需要自定义消息:
+1. 在包内 `msg/MyMsg.msg` 定义字段
+2. `package.xml` 加 `<build_depend>rosidl_default_generators</build_depend>` + `<exec_depend>rosidl_default_runtime</exec_depend>`
+3. `CMakeLists.txt` 调 `rosidl_generate_interfaces(...)`
+4. `colcon build` 后 Python/C++ 自动生成类
+
+本仓库**只使用标准消息**,简化学习曲线。
+
+---
+
+## 3. Publisher API(Python / C++)
+
+### 3.1 Python
+
+```python
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String
+
+class Talker(Node):
+    def __init__(self):
+        super().__init__('talker_py')
+
+        # 1) 声明参数
+        self.declare_parameter('period_ms', 500)
+        self.declare_parameter('topic', 'chatter')
+
+        # 2) 取参数
+        period = self.get_parameter('period_ms').value
+        topic  = self.get_parameter('topic').value
+
+        # 3) 创建 Publisher
+        #    create_publisher(msg_type, topic, qos_depth)
+        self.publisher_ = self.create_publisher(String, topic, 10)
+
+        # 4) 创建定时器,周期性 publish
+        self.timer_ = self.create_timer(period / 1000.0, self.timer_callback)
+
+    def timer_callback(self):
+        msg = String()
+        msg.data = f'Hello, count={self.count}'
+        self.publisher_.publish(msg)            # 异步,不等
+        self.count += 1
+
+def main():
+    rclpy.init()
+    node = Talker()
+    try:
+        rclpy.spin(node)                       # 进入事件循环
+    except KeyboardInterrupt:
+        pass
+    node.destroy_node()
+    rclpy.shutdown()
+```
+
+### 3.2 C++
+
+```cpp
+#include <chrono>
+#include <memory>
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+
+using namespace std::chrono_literals;
+
+class Talker : public rclcpp::Node {
+public:
+  Talker() : rclcpp::Node("talker_cpp"), count_(0) {
+    // 1) 声明 + 读参数
+    this->declare_parameter<int>("period_ms", 500);
+    this->declare_parameter<std::string>("topic", "chatter");
+    int period = this->get_parameter("period_ms").as_int();
+    std::string topic = this->get_parameter("topic").as_string();
+
+    // 2) 创建 Publisher
+    publisher_ = this->create_publisher<std_msgs::msg::String>(topic, 10);
+
+    // 3) 定时器
+    timer_ = this->create_wall_timer(
+      std::chrono::milliseconds(period),
+      std::bind(&Talker::timer_callback, this));
+  }
+
+private:
+  void timer_callback() {
+    auto msg = std_msgs::msg::String();
+    msg.data = "Hello from C++, seq=" + std::to_string(count_++);
+    publisher_->publish(msg);
+  }
+
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  size_t count_;
+};
+
+int main(int argc, char * argv[]) {
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<Talker>());
+  rclcpp::shutdown();
+  return 0;
+}
+```
+
+### 3.3 关键 API 速查
+
+| Python | C++ | 用途 |
+|---|---|---|
+| `create_publisher(MsgType, name, depth)` | `create_publisher<MsgType>(name, depth)` | 创建 publisher |
+| `publisher.publish(msg)` | `publisher->publish(msg)` | 异步发消息 |
+| `publisher.get_subscription_count()` | 同 | 看有几个订阅者 |
+| `destroy_publisher()` | 同 | 销毁 |
+
+---
+
+## 4. Subscriber API(Python / C++)
+
+### 4.1 Python
+
+```python
+class Listener(Node):
+    def __init__(self):
+        super().__init__('listener_py')
+        self.declare_parameter('topic', 'chatter')
+        topic = self.get_parameter('topic').value
+
+        # create_subscription(msg_type, topic, callback, qos_depth)
+        # callback 签名: callback(msg)
+        self.subscription = self.create_subscription(
+            String, topic, self.listener_callback, 10)
+
+    def listener_callback(self, msg):
+        self.get_logger().info(f'recv: "{msg.data}"')
+        # 这里做处理:解析、入队、下发指令、可视化等
+```
+
+### 4.2 C++
+
+```cpp
+class Listener : public rclcpp::Node {
+public:
+  Listener() : rclcpp::Node("listener_cpp") {
+    this->declare_parameter<std::string>("topic", "chatter");
+    std::string topic = this->get_parameter("topic").as_string();
+
+    // create_subscription<T>(topic, depth, callback)
+    // callback 签名: [](const T::SharedPtr msg) { ... }
+    subscription_ = this->create_subscription<std_msgs::msg::String>(
+      topic, 10,
+      [this](const std_msgs::msg::String::SharedPtr msg) {
+        RCLCPP_INFO(this->get_logger(), "recv: \"%s\"", msg->data.c_str());
+      });
+  }
+private:
+  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
+};
+```
+
+### 4.3 关键 API 速查
+
+| Python | C++ | 用途 |
+|---|---|---|
+| `create_subscription(MsgType, name, cb, depth)` | `create_subscription<T>(name, depth, cb)` | 创建订阅者 |
+| callback 签名: `cb(msg)` | `[](const T::SharedPtr msg) { ... }` | 消息回调 |
+
+### 4.4 重要原则
+- **回调里别阻塞**:回调在主线程里跑,长时间阻塞会让 timer / 其他 callback 卡死
+- **回调里别抛异常**:rclcpp 会捕获但仍可能崩
+- **复杂处理入队**:把消息放到 queue,另起线程消费
+
+---
+
+## 5. QoS 详解(必备)
+
+### 5.1 五个维度
+
+| 维度 | 取值 | 默认 | 含义 |
+|---|---|---|---|
+| **Reliability** | RELIABLE / BEST_EFFORT | RELIABLE | 必须投递 / 丢一帧无所谓 |
+| **History** | KEEP_LAST(N) / KEEP_ALL | KEEP_LAST(10) | 队列策略 |
+| **Durability** | VOLATILE / TRANSIENT_LOCAL | VOLATILE | 晚订阅者是否收旧数据 |
+| **Deadline** | Duration | ∞ | 最长多久发一次 |
+| **Lifespan** | Duration | ∞ | 多旧的消息失效 |
+
+### 5.2 常用组合
+
+```python
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
+
+# 视频流:丢一帧没关系,要最新
+sensor_qos = QoSProfile(
+    reliability=ReliabilityPolicy.BEST_EFFORT,
+    history=HistoryPolicy.KEEP_LAST,
+    depth=1
+)
+
+# 控制指令:必须到达
+control_qos = QoSProfile(
+    reliability=ReliabilityPolicy.RELIABLE,
+    history=HistoryPolicy.KEEP_LAST,
+    depth=10
+)
+```
+
+### 5.3 兼容规则(关键!)
+
+| Publisher | Subscriber | 结果 |
+|---|---|---|
+| RELIABLE | RELIABLE | ✅ |
+| BEST_EFFORT | BEST_EFFORT | ✅ |
+| RELIABLE | BEST_EFFORT | ❌ (sub 不发 ACK,pub 报 QoS incompatible) |
+| BEST_EFFORT | RELIABLE | ✅ (sub 容忍丢) |
+
+**报错样例**:
+```
+[WARN] ... New subscription discovered on this topic with incompatible QoS ...
+```
+
+**怎么查**: `ros2 topic info /topic -v` 看 pub/sub 各自 QoS。
+
+### 5.4 本仓库 QoS 策略
+
+所有 demo **都用默认 QoS**(RELIABLE + KEEP_LAST(10))。
+- 优点:跨语言互通零障碍
+- 缺点:高频场景需调优
+
+**实战**: 视频流改 `BEST_EFFORT + depth=1`;关节控制用 `RELIABLE + depth=1`。
+
+---
+
+## 6. 跨语言互通(核心特性)
+
+### 6.1 为什么能互通
+ROS2 用 **DDS** 做底层,Python / C++ / 其他语言只是同一消息的不同"视图"。
+**消息定义在 .msg/.srv/.action 里,所有语言按这个定义自动生成代码**。
+
+### 6.2 互通条件
+
+1. ✅ 消息类型一致(`std_msgs/String` 的 Python/C++ 字段名都是 `data`)
+2. ✅ Topic 名一致
+3. ✅ QoS 兼容
+4. ✅ ROS_DOMAIN_ID 一致(`ROS_DOMAIN_ID` 环境变量)
+
+### 6.3 验证互通
+
+```bash
+# 启 4 节点(pubsub_launch.py)
+ros2 launch bringup pubsub_launch.py
+
+# 看 /chatter 的 pub/sub 列表
+ros2 topic info /chatter -v
+```
+
+**预期**:
+```
+Publication count: 2
+Subscription count: 2
+  Node name: listener_py       Node namespace: /
+    Publisher count: 0
+  Node name: talker_py         Node namespace: /
+    Publisher count: 1
+  Node name: listener_cpp      Node namespace: /
+    Publisher count: 0
+  Node name: talker_cpp        Node namespace: /
+    Publisher count: 1
+```
+
+看到 **talker_py + talker_cpp** 两个 publisher, **listener_py + listener_cpp** 两个 subscriber。
+
+### 6.4 看跨语言消息流
+
+```bash
+# 终端 1
+ros2 launch bringup pubsub_launch.py
+
+# 终端 2
+ros2 topic echo /chatter
+
+# 预期:
+# data: 'Hello from PY, seq=42'
+# data: 'Hello from C++, seq=12'
+# data: 'Hello from PY, seq=43'
+# data: 'Hello from C++, seq=13'
+```
+
+**Python talker 发** → C++ listener 收到 ✅
+**C++ talker 发** → Python listener 收到 ✅
+
+端到端日志: [`docker/bringup_e2e.log`](../docker/bringup_e2e.log)
+
+---
+
+## 7. 完整实战:写一个传感器数据流
+
+假设你做一个激光雷达节点:
+
+### 7.1 Publisher(传感器端)
+
+```python
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import LaserScan
+import random
+
+class FakeLidar(Node):
+    def __init__(self):
+        super().__init__('fake_lidar')
+
+        self.declare_parameter('rate_hz', 10)
+        rate = self.get_parameter('rate_hz').value
+
+        self.pub_ = self.create_publisher(
+            LaserScan, '/scan', 10)
+        self.timer_ = self.create_timer(1.0/rate, self.tick)
+
+    def tick(self):
+        scan = LaserScan()
+        scan.header.stamp = self.get_clock().now().to_msg()
+        scan.header.frame_id = 'laser_frame'
+        scan.angle_min = -3.14159
+        scan.angle_max = 3.14159
+        scan.angle_increment = 0.01
+        scan.time_increment = 0.0
+        scan.range_min = 0.05
+        scan.range_max = 30.0
+        scan.ranges = [random.uniform(0.5, 5.0) for _ in range(629)]
+        self.pub_.publish(scan)
+```
+
+### 7.2 Subscriber(消费端)
+
+```python
+class LidarProcessor(Node):
+    def __init__(self):
+        super().__init__('lidar_processor')
+        self.sub_ = self.create_subscription(
+            LaserScan, '/scan', self.cb, 10)
+
+    def cb(self, msg):
+        # 找最近障碍
+        nearest = min(msg.ranges)
+        self.get_logger().info(f'nearest obstacle: {nearest:.2f}m')
+        # 这里可以做:避障规划、点云处理、可视化
+```
+
+### 7.3 完整 run
+
+```bash
+# 终端 1:传感器
+ros2 run my_pkg fake_lidar
+
+# 终端 2:处理
+ros2 run my_pkg lidar_processor
+
+# 终端 3:验证
+ros2 topic hz /scan         # 10 Hz
+ros2 topic echo /scan       # 看数据(不要全打印,会很乱)
+```
+
+---
+
+## 8. 调试命令大全
+
+```bash
+# 列所有 topic
+ros2 topic list
+
+# 看 topic 元数据(类型 + pub/sub + QoS)
+ros2 topic info /chatter -v
+
+# 实时打印消息内容
+ros2 topic echo /chatter
+
+# 测频率(平均 / 最小 / 最大 / 标准差)
+ros2 topic hz /chatter
+
+# 测带宽(每秒多少 KB)
+ros2 topic bw /chatter
+
+# 发一条测试消息
+ros2 topic pub /chatter std_msgs/String "{data: 'hello'}" --once
+
+# 录包
+ros2 bag record /chatter -o my_bag
+
+# 录包回放
+ros2 bag play my_bag
+
+# 看当前所有节点
+ros2 node list
+
+# 看节点发布的 topic
+ros2 node info /talker_py
+```
+
+---
+
+## 9. 常见坑 + 解决方案
+
+### 9.1 收不到消息(最常见)
+
+**症状**: `ros2 topic echo` 没输出,但 `ros2 topic list` 看得到。
+
+**排查步骤**:
+```bash
+# 1) 看 pub/sub 列表
+ros2 topic info /topic -v
+
+# 2) 看 QoS 是否兼容
+#    如果 QoS incompatible,WARN 日志会打印
+
+# 3) 看节点是否活着
+ros2 node list
+```
+
+**常见原因**:
+- ❌ **节点名重复**: `Node 'x' already exists` → 改名或加 namespace
+- ❌ **Topic 名拼写错误**:大小写、`/` 区分
+- ❌ **QoS 不兼容**:改成 default 或两边匹配
+- ❌ **消息类型不匹配**:Publisher 是 `std_msgs/String`,Subscriber 是 `std_msgs/Int32` → 静默不匹配
+- ❌ **Publisher 还没起来**:等 1-2s DDS discovery
+
+### 9.2 Docker 内 localhost 不互通
+
+**症状**: 两个容器里节点互相看不到。
+
+**解决**: 用 `network_mode: host`(本仓库已配),或 `ROS_STATIC_PEERS` 单播。
+
+### 9.3 DDS QoS 兼容性错
+
+**报错**:
+```
+[WARN] ... Incompatible QoS ... (PolicyKind=RMW_QOS_POLICY_RELIABILITY)
+```
+
+**解决**: 双方 Reliability 一致。
+```python
+from rclpy.qos import QoSProfile, ReliabilityPolicy
+qos = QoSProfile(reliability=ReliabilityPolicy.BEST_EFFORT, depth=1)
+self.pub_ = self.create_publisher(String, 'topic', qos)
+```
+
+### 9.4 Cyclone DDS 没装导致 build 失败
+
+CMake 错误:
+```
+Could not find ROS middleware implementation 'rmw_cyclonedds_cpp'
+```
+
+**解决**:
+- 不要设 `RMW_IMPLEMENTATION=rmw_cyclonedds_cpp`(本仓库默认 fastdds)
+- 切 RMW 时务必 `rm -rf build/ install/ log/`
+
+### 9.5 Callback 阻塞导致节点"卡死"
+
+**症状**: 节点启动后什么都不做,其他 timer / callback 也不响应。
+
+**原因**: callback 里跑同步阻塞代码(long I/O、time.sleep)。
+
+**解决**: 用 `MultiThreadedExecutor`,callback 里只入队 + 另起线程处理。
+
+```python
+from rclpy.executors import MultiThreadedExecutor
+exec_ = MultiThreadedExecutor(num_threads=4)
+exec_.add_node(node)
+exec_.spin()
+```
+
+---
+
+## 10. 在本仓库里跑
+
+### 10.1 启动 4 节点跨语言 demo
+
+```bash
+docker exec ros2_dev bash -lc "cd /root/ros2_ws && source install/setup.bash && ros2 launch bringup pubsub_launch.py"
+```
+
+### 10.2 源码位置
+- Python pub: [`src/py_pubsub/py_pubsub/publisher_member_function.py`](../src/py_pubsub/py_pubsub/publisher_member_function.py)
+- Python sub: [`src/py_pubsub/py_pubsub/subscriber_member_function.py`](../src/py_pubsub/py_pubsub/subscriber_member_function.py)
+- C++ pub: [`src/cpp_pubsub/src/publisher_member_function.cpp`](../src/cpp_pubsub/src/publisher_member_function.cpp)
+- C++ sub: [`src/cpp_pubsub/src/subscriber_member_function.cpp`](../src/cpp_pubsub/src/subscriber_member_function.cpp)
+- launch: [`src/bringup/launch/pubsub_launch.py`](../src/bringup/launch/pubsub_launch.py)
+
+### 10.3 端到端日志
+[`docker/bringup_e2e.log`](../docker/bringup_e2e.log) — 跨语言互通验证。
+
+---
+
+## 11. 进阶:可靠通信 / 录制 / 跨机
+
+### 11.1 Reliable 通信设置
+
+```python
+# 默认就是 RELIABLE,显式写:
+from rclpy.qos import QoSProfile, ReliabilityPolicy
+
+qos = QoSProfile(
+    reliability=ReliabilityPolicy.RELIABLE,
+    history=HistoryPolicy.KEEP_LAST,
+    depth=10
+)
+```
+
+### 11.2 录制 + 回放(bag)
+
+```bash
+ros2 bag record -o my_bag /chatter /tf /joint_states
+# 输出: my_bag_0.db3 + metadata.yaml
+
+ros2 bag play my_bag --loop     # --loop 循环回放
+
+ros2 bag info my_bag           # 看消息数 / 时间 / 类型
+```
+
+**注意**: 回放时,topic 真实 pub 也要在,否则 recorder 找不到对应 publisher。bag 不会保存节点,只保存消息。
+
+### 11.3 跨机 DDS
+
+默认走 UDP multicast,同网段自动发现。
+
+**跨子网**: 用 unicast discovery。
+
+PC:
+```bash
+export ROS_DISCOVERY_SERVER=192.168.1.20:11811
+```
+
+RDK X5: 启 discovery server:
+```bash
+ros2 run discovery_server discovery_server --address 0.0.0.0 --port 11811
+```
+
+**跨 LAN + 防火墙**: 用 `ROS_STATIC_PEERS` 静态发现。
+
+详见 [`doc/100-embedded-deployment.md`](100-embedded-deployment.md) §5。
+
+---
+
+## 接下来读
+
+| 主题 | 文档 |
+|---|---|
+| Service 深度 | [`30-services.md`](30-services.md) |
+| Action 深度 | [`40-actions.md`](40-actions.md) |
+| TF2 坐标变换 | [`50-tf2.md`](50-tf2.md) |
+| 三机部署 | [`100-embedded-deployment.md`](100-embedded-deployment.md) |
+| 具身智能路径 | [`99-embodied-ai.md`](99-embodied-ai.md) |