xiaoshuai/ROS2_learn
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

init: ROS2 learning suite

Browse Source
This commit is contained in:
xs
2026-08-03 18:09:35 +08:00
commit 5ef38ab508
95 changed files with 13322 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.flake8
+22
View File
@@ -0,0 +1,22 @@
# =============================================================================
# .flake8 —— flake8 lint 配置(仅在用 flake8 时生效;主力已切到 ruff)。
# 与 pyproject.toml [tool.ruff] / [tool.black] 配合,IDE / CI 三选一即可。
# =============================================================================
[flake8]
max-line-length = 100
extend-ignore =
E203, # slice 空格冲突(black 友好)
E501, # 长行(black 不管)
W503, # line break before operator(black 友好)
exclude =
.venv,
build,
install,
log,
src/*/build,
src/*/install,
__pycache__,
*/__pycache__
per-file-ignores =
src/**/test_*.py: B011
.gitignore
+42
View File
@@ -0,0 +1,42 @@
# ROS2 构建产物
install/
build/
log/
# Python venv(本机开发 + 容器内一致)
.venv/
venv/
env/
*.egg-info/
# Python 编译缓存
__pycache__/
*.py[cod]
*$py.class
# 测试产物
.pytest_cache/
.coverage
htmlcov/
.tox/
.nox/
*.cover
# 类型检查 / lint
.mypy_cache/
.ruff_cache/
# IDE
.idea/
.vscode/
.DS_Store
*.swp
*.swo
# 临时文件(本地调试用,不进 git)
*.tmp
*.bak
.cache/
# OS / 杂项
Thumbs.db
AGENTS.md
+207
View File
@@ -0,0 +1,207 @@
# ROS2 子项目 Agent 铁律
## 铁律 (Hard Rules)
**违反任何一条,所有变更立刻回滚。**
1. **只允许在本目录 `D:\xs\ros2` 下创建/修改/删除文件。**
- 任何系统临时目录(`%TEMP%`、`/tmp` 等) 一律禁止落盘。
- 在 Windows 下临时目录会残留垃圾文件且不清理,绝对禁止。
- 创建临时数据用 PowerShell 内存对象或 stdout,不写盘。
2. **禁止用 shell(PowerShell / cmd)编辑文件。**
- 改 / 写 / 读文件一律用内置工具:Read / Edit / Write / Glob / Grep。
- shell(本会话的 `bash` 工具)只允许运行可执行命令,例如 `docker ...`、`colcon ...`,**不允许用 `Set-Content`、`Out-File`、`>`、`>>` 写文件**。
- 禁止 `cd` / `Set-Location` 切换工作目录;需要别的目录时,在 `bash` 工具里用绝对路径,或者在 GUI / 编辑器里用内置工具定位。
3. **禁止到处创建文件/目录。**
- 不擅自创建 `.cache/`、`.tmp/`、`.bak/` 等隐藏目录。
- 不擅自创建 `test/xxx` 临时目录、"out/`、`日志/。
- 所有产出(镜像 `ros2-humble-dev:latest`、容器 `ros2_dev`、构建目录 `install/`、`build/`、`log/`)放在本项目内或其默认位置,不放别处。
4. **禁止问与思考循环。**
- 用户拒绝连续"要不要 / 要不要这样"。给出明确方案,直接开干。
- 必须问时,一次问清,不要反问。
5. **测试必须 100% 通过才能停手。**
- `docker compose build` + `colcon build` + `colcon test` + 节点启动 + `ros2 topic echo` 全绿才能汇报"完成"。
- 任何环节失败 → 自动修 → 再跑,直到全过。
## 子项目结构
```
D:\xs\ros2\
├── AGENTS.md # 本文件
├── README.md # 项目入口 + 架构 + 启动命令
├── pyproject.toml # PEP 621 workspace metadata(IDE 入口)
├── requirements.txt # venv runtime 依赖
├── requirements-dev.txt # venv 开发工具(ruff/black/mypy/pytest)
├── .flake8 / pyrightconfig.json # lint / 类型检查配置
├── .gitignore # 包含 .venv/ build/ install/ log/
│
├── docker/
│ ├── Dockerfile # ROS2 Humble 镜像
│ ├── docker-compose.yml # 容器编排
│ ├── bringup_e2e.log # 4 节点跨语言端到端验证日志(31KB)
│ ├── srv_e2e.log # Service 端到端(12+30=42,664B)
│ ├── robot_e2e.log # URDF + TF 端到端(11KB)
│ ├── vision_e2e.log # sensor_msgs/Image 端到端(49KB)
│ └── full_demo_e2e.log # 11 节点全开端到端(46KB)
│
├── tools/
│ ├── setup_venv.sh # Linux/WSL/Docker 一键 venv
│ └── setup_venv.ps1 # Windows 一键 venv(不污染系统 Python)
│
├── build.sh # 容器内 colcon build 一键脚本
├── start.sh / start.ps1 # 一键启动 + 构建 + 进开发终端
│
├── src/
│ ├── py_pubsub/ # ament_python — Topic pub/sub (Python)
│ │ ├── package.xml
│ │ ├── setup.py / setup.cfg
│ │ ├── py_pubsub/publisher_member_function.py
│ │ ├── py_pubsub/subscriber_member_function.py
│ │ ├── launch/pubsub_launch.py
│ │ ├── resource/py_pubsub
│ │ └── test/test_pubsub_launch.py # 4 pytest 用例
│ ├── cpp_pubsub/ # ament_cmake — Topic pub/sub (C++) + gtest
│ │ ├── package.xml
│ │ ├── CMakeLists.txt
│ │ ├── src/publisher_member_function.cpp
│ │ ├── src/subscriber_member_function.cpp
│ │ ├── launch/pubsub_launch.py
│ │ └── test/test_pub_sub.cpp # 2 gtest 用例
│ ├── py_srv/ # ament_python — Service demo
│ │ ├── package.xml
│ │ ├── setup.py / setup.cfg
│ │ ├── py_srv/add_two_ints_server.py
│ │ ├── py_srv/add_two_ints_client.py
│ │ ├── launch/srv_launch.py
│ │ └── test/test_srv.py # 1 pytest 用例
│ ├── py_action_demo/ # ament_python — Action 三件套 demo
│ │ ├── package.xml
│ │ ├── setup.py / setup.cfg
│ │ ├── py_action_demo/fibonacci_server.py
│ │ ├── py_action_demo/fibonacci_client.py
│ │ ├── launch/action_launch.py
│ │ └── test/test_action.py # 1 pytest 用例
│ ├── cpp_robot_tf2/ # ament_cmake — URDF + TF2 + JointState (C++)
│ │ ├── package.xml
│ │ ├── CMakeLists.txt
│ │ ├── urdf/simple_arm.urdf # 3 关节机械臂
│ │ ├── src/joint_state_publisher.cpp
│ │ ├── src/tf2_listener.cpp
│ │ ├── launch/robot_tf2_launch.py
│ │ └── test/test_tf2_lookup.cpp # 2 gtest 用例
│ ├── py_vision_demo/ # ament_python — sensor_msgs/Image (Python)
│ │ ├── package.xml
│ │ ├── setup.py / setup.cfg
│ │ ├── py_vision_demo/fake_camera.py
│ │ ├── py_vision_demo/image_processor.py
│ │ ├── launch/vision_launch.py
│ │ └── test/test_vision.py # 2 pytest 用例
│ └── bringup/ # ament_python — 顶层 launch 聚合
│ ├── package.xml
│ ├── setup.py / setup.cfg
│ ├── launch/
│ │ ├── pubsub_launch.py # 4 节点 Topic(pubsub)
│ │ ├── service_launch.py # AddTwoInts server
│ │ ├── action_launch.py # Fibonacci server
│ │ ├── robot_launch.py # URDF TF2(嵌套 cpp_robot_tf2)
│ │ ├── vision_launch.py # 嵌套 py_vision_demo
│ │ └── full_demo_launch.py # 11 节点一起
│ └── resource/bringup
│
└── doc/ # 14 篇深度文档
├── 00-overview.md # 架构 + 设计取舍
├── 01-quickstart.md # 5 分钟上手
├── 02-virtualenv.md # venv 工作流(本机不污染)
├── 10-concepts.md # Node/Topic/Service/Action/Parameter/TF
├── 20-topics.md # Topic pub/sub 深度
├── 30-services.md # Service 深度
├── 40-actions.md # Action 三件套深度
├── 50-tf2.md # TF2 坐标变换
├── 60-urdf.md # URDF 机器人模型
├── 70-launch.md # launch 文件系统
├── 80-package-build.md # colcon / ament 包构建
├── 85-docker.md # Docker 容器化开发
├── 90-testing.md # 测试金字塔策略
├── 99-embodied-ai.md # VLA / 机器人 / 具身智能路径
└── 100-embedded-deployment.md # 三机嵌入式部署(PC + RDK X5 + RK3506)
```
## 子项目工作流
| 步骤 | 命令 |
|---|---|
| 构建镜像(首次 5-10min) | `docker compose -f docker/docker-compose.yml build` |
| 启动容器 | `docker compose -f docker/docker-compose.yml up -d` |
| 容器内构建所有 7 个包 | `docker exec ros2_dev bash -lc "source /opt/ros/humble/setup.bash && cd /root/ros2_ws && colcon build --symlink-install --packages-select py_pubsub cpp_pubsub py_srv py_action_demo cpp_robot_tf2 py_vision_demo bringup"` |
| 跑所有单元/集成测试 | `docker exec ros2_dev bash -lc "source /opt/ros/humble/setup.bash && cd /root/ros2_ws && colcon test --packages-select py_pubsub cpp_pubsub py_srv py_action_demo cpp_robot_tf2 py_vision_demo bringup"` |
| 启动 4 节点 Topic | `docker exec ros2_dev bash -lc "source /root/ros2_ws/install/setup.bash && ros2 launch bringup pubsub_launch.py"` |
| 启动 Service 端到端 | `docker exec ros2_dev bash -lc "source install/setup.bash && ros2 launch bringup service_launch.py"` + `ros2 service call /add_two_ints example_interfaces/srv/AddTwoInts '{a: 12, b: 30}'` |
| 启动 Action 端到端 | `ros2 launch bringup action_launch.py` + `ros2 action send_goal /fibonacci example_interfaces/action/Fibonacci '{order: 6}' --feedback` |
| 启动 TF2 演示 | `ros2 launch bringup robot_launch.py` |
| 启动 Vision 演示 | `ros2 launch bringup vision_launch.py` |
| 启动 11 节点 Full demo | `ros2 launch bringup full_demo_launch.py` |
| 进入开发终端 | `docker exec -it ros2_dev bash -lc "source /root/ros2_ws/install/setup.bash && exec bash"` |
| 本机 venv 初始化(不污染系统 Python) | `powershell .\tools\setup_venv.ps1` |
| 本机 venv 激活 | `.\.venv\Scripts\Activate.ps1` |
## 测试覆盖 (100% 通过)
| 包 | 测试类型 | 用例数 | 结果 |
|---|---|---|---|
| py_pubsub | pytest + in-process spin | 4/4 | PASSED |
| cpp_pubsub | gtest | 2/2 | PASSED |
| py_srv | pytest + Service in-process | 1/1 | PASSED |
| py_action_demo | pytest + Action in-process | 1/1 | PASSED |
| cpp_robot_tf2 | gtest | 2/2 | PASSED |
| py_vision_demo | pytest + Image in-process | 2/2 | PASSED |
| bringup | launch 6 文件就绪 | OK | PASSED |
**总计: 10/10 单元/集成测试 + 5 个端到端 demo 全部通过 100%。**
### 端到端日志(固化在 `docker/`)
- `bringup_e2e.log`:4 节点 Topic 跨语言互通(listener_cpp 同时收到 PY + CPP 消息)
- `srv_e2e.log`:Service 12+30=42
- `robot_e2e.log`:3 关节机械臂 + TF 实时打印(gripper 在 base_link 下位置)
- `vision_e2e.log`:fake_camera → image_processor 图像流(cv_bridge 解码 + 平均亮度)
- `full_demo_e2e.log`:11 节点同时运行(Topic + Service + Action + Robot + Vision)
## 子项目约定
### Python 包用 `ament_python`(`py_pubsub` / `py_srv` / `py_action_demo` / `py_vision_demo` / `bringup`)。
### C++ 包用 `ament_cmake`(`cpp_pubsub` / `cpp_robot_tf2`)。
### 跨包 launch 收纳到 `bringup` 包,包名**不能**叫 `launch`(与 ROS2 自带包同名,ament 索引冲突)。
### 所有 ROS2 节点默认参数 `period_ms=500`、`topic=chatter`,可在 launch 文件里覆盖。
### 源码注释一律中文,顶部 docstring 先写"用途 / 关键概念 / 运行方式",关键 API 旁写 inline。
### 跨语言互通演示:talker_py / talker_cpp 都用 `std_msgs/String`,见 `ros2 topic info chatter -v`。
## 嵌入式部署硬件清单(典型)
| 设备 | 角色 | ROS2 适配 |
|---|---|---|
| PC (x86) | 主控 | ✅ 完整 ROS2 + MoveIt2 + Nav2 + RViz |
| RDK X5 (ARM + 5 TOPS NPU) | 边缘 AI | ✅ 完整 ROS2 (视觉 / 语音 / SLAM) |
| RK3506 × 2 (ARM 3核 + 512MB RAM + 8GB eMMC) | 实时控制 | ✅ **精简 ROS2** (`ros-humble-ros-base`,不要 desktop) |
**关键提示**:
- RK3506 是 Linux 应用处理器(不是 MCU),**直接 apt 装 `ros-humble-ros-base`**,不用 micro-ROS
- 三机同 LAN 同 `ROS_DOMAIN_ID`,通过 FastDDS multicast 自动发现(或 `ROS_STATIC_PEERS` 单播)
- RK3506 用精简包 + 关 daemon + 关 GUI,可省 ~300MB RAM
- 详细步骤 + 故障排查见 `doc/100-embedded-deployment.md`
## RMW / 网络注意事项
- Docker Desktop on Windows + host network 模式下,默认 `rmw_fastrtps_cpp` 工作良好。
- **不要在 docker-compose.yml 里设 `RMW_IMPLEMENTATION=rmw_cyclonedds_cpp`**:镜像没装 cyclone dds,
CMake 配置阶段会失败。切换 RMW 时务必 `rm -rf build/ install/ log/` 再 build。
- `ros2 launch` 调 `IncludeLaunchDescription` 时,被包含的子 launch 文件必须在被包含包的 `share/<pkg>/launch/` 下被 colcon 实际安装 — 检查 `data_files` 里 `glob('launch/*.py')` 是否覆盖到。
## 调试速查
| 症状 | 排查 |
|---|---|
| `rcl_xxx not found` | `source /opt/ros/humble/setup.bash` |
| `Command ['cat', path]` 空格丢了 | launch 里别用 cat,改用 `open().read()` |
| `rcl_shutdown already called` | 测试 fixture 别在 callback 里 shutdown |
| `frame not exist` | robot_state_publisher 还没算完,等 1-2s |
| `Cannot connect to Docker daemon` | 启动 Docker Desktop |
| Windows venv import rclpy 飘红 | 正常,rclpy 没 Windows wheels,在容器里跑 |
README.md
+223
View File
@@ -0,0 +1,223 @@
# ROS2 Learning Suite — 具身智能入门实战
> 一套 **从零到具身智能开发** 的 ROS2 Humble 全栈实战:Topics / Services / Actions /
> TF2 / URDF / Vision。每个 demo 都可独立运行,跨包跨语言互通,所有测试 **100% 通过**。
> 为后续 VLA(Vision-Language-Action) / 机器人 / 具身智能开发铺路。
---
## 🎯 适合谁
- 第一次学 ROS2,想从 0 到能搭一个完整机器人项目
- 想**深耕具身智能**(机器人 + VLA),需要把 ROS2 通信栈 + TF2 + URDF + Vision 一次打通
- 想在 Windows 本机用 venv + VSCode/PyCharm 写 Python,在 Docker/WSL Linux 里跑 ROS2
## 📦 仓库提供什么
7 个 ROS2 包 + 端到端 demo + 深度文档:
| 包 | 类型 | 通信模式 | 语言 | 测试 |
|---|---|---|---|---|
| [`py_pubsub`](src/py_pubsub/) | ament_python | Topic pub/sub | Python | pytest 4/4 ✓ |
| [`cpp_pubsub`](src/cpp_pubsub/) | ament_cmake | Topic pub/sub | C++ | gtest 2/2 ✓ |
| [`py_srv`](src/py_srv/) | ament_python | Service req/resp | Python | pytest 1/1 ✓ |
| [`py_action_demo`](src/py_action_demo/) | ament_python | Action 三件套 | Python | pytest 1/1 ✓ |
| [`cpp_robot_tf2`](src/cpp_robot_tf2/) | ament_cmake | TF2 + URDF + JointState | C++ | gtest 2/2 ✓ |
| [`py_vision_demo`](src/py_vision_demo/) | ament_python | sensor_msgs/Image | Python | pytest 2/2 ✓ |
| [`bringup`](src/bringup/) | ament_python | launch 聚合 | Python | OK |
**合计 10/10 测试 100% 通过**;5 个端到端日志固化在 [`docker/`](docker/)。
---
## 🚀 30 秒上手
### Windows 本机(开发)
```bash
# 一键创建 venv(不污染系统 Python)
powershell .\tools\setup_venv.ps1
.\.venv\Scripts\Activate.ps1
# 打开 VSCode / PyCharm
code D:\xs\ros2
```
### Docker 容器(运行 + 测试)
```powershell
# 第一次:构建 + 启动 + 编译 + 进入开发终端
powershell D:\xs\ros2\start.ps1
# 后续:重启即用
docker compose -f D:\xs\ros2\docker\docker-compose.yml up -d
# 在容器内构建 + 跑测试
docker exec ros2_dev bash -lc "cd /root/ros2_ws && bash build.sh"
docker exec ros2_dev bash -lc "source /opt/ros/humble/setup.bash && cd /root/ros2_ws && colcon test --packages-select py_pubsub cpp_pubsub py_srv py_action_demo cpp_robot_tf2 py_vision_demo bringup"
```
---
## 🧱 架构
```
┌──────────────────────────────────────────┐
│ 本机 Windows / Linux │
│ (venv: ruff/black/mypy/pytest/numpy) │
└─────────────────┬────────────────────────┘
│ 共享源码目录 (bind mount)
┌─────────────────▼────────────────────────┐
│ Docker (osrf/ros:humble-desktop) │
│ ┌────────── ROS2 apt ───────────┐ │
│ │ rclcpp rclpy tf2 cv_bridge │ │
│ │ ros-humble-desktop-full │ │
│ └───────────────────────────────┘ │
│ ┌──── colcon build/test ───────┐ │
│ │ py_pubsub cpp_pubsub │ │
│ │ py_srv py_action_demo │ │
│ │ cpp_robot_tf2 py_vision_demo│ │
│ │ bringup │ │
│ └─────────────────────────────┘ │
└──────────────────────────────────────────┘
```
**两层解耦**:
- **本机层**:venv 装开发工具(runtime 隔离),IDE 直接读源码
- **容器层**:colcon 装 ROS2 节点(apt 来源,共享给所有用户)
详细架构见 [`doc/00-overview.md`](doc/00-overview.md)。
---
## 🎬 6 种端到端 demo
| Demo | 命令 | 看什么 |
|---|---|---|
| Topic 跨包跨语言 | `ros2 launch bringup pubsub_launch.py` | `listener_cpp` 收 `talker_py` 和 `talker_cpp` 的消息 |
| Service | `ros2 launch bringup service_launch.py` + `ros2 service call /add_two_ints ...` | `12+30=42` |
| Action | `ros2 launch bringup action_launch.py` + `ros2 action send_goal /fibonacci ...` | Fibonacci(6) 边跑边反馈 |
| Robot TF2 | `ros2 launch bringup robot_launch.py` | gripper 在 base_link 下的实时位姿 |
| Vision | `ros2 launch bringup vision_launch.py` | fake_camera → image_processor 图像流 |
| Full demo | `ros2 launch bringup full_demo_launch.py` | **11 个节点同时运行** |
固化日志:[`docker/bringup_e2e.log`](docker/bringup_e2e.log) · [`docker/srv_e2e.log`](docker/srv_e2e.log) · [`docker/robot_e2e.log`](docker/robot_e2e.log) · [`docker/vision_e2e.log`](docker/vision_e2e.log) · [`docker/full_demo_e2e.log`](docker/full_demo_e2e.log)
---
## 📚 文档导航
### 上手
- [`doc/00-overview.md`](doc/00-overview.md) — 项目架构 + 设计取舍
- [`doc/01-quickstart.md`](doc/01-quickstart.md) — 5 分钟跑通
- [`doc/02-virtualenv.md`](doc/02-virtualenv.md) — venv 工作流(本机不污染)
### ROS2 核心概念
- [`doc/10-concepts.md`](doc/10-concepts.md) — Node / Topic / Service / Action / Parameter / TF / Time
- [`doc/20-topics.md`](doc/20-topics.md) — Topic pub/sub 深度
- [`doc/30-services.md`](doc/30-services.md) — Service req/resp 深度
- [`doc/40-actions.md`](doc/40-actions.md) — Action 三件套深度
### 机器人专属
- [`doc/50-tf2.md`](doc/50-tf2.md) — 坐标变换(VLA/抓取/对齐的基石)
- [`doc/60-urdf.md`](doc/60-urdf.md) — 机器人模型描述
### 工程实践
- [`doc/70-launch.md`](doc/70-launch.md) — launch 文件系统
- [`doc/80-package-build.md`](doc/80-package-build.md) — colcon / ament 包构建
- [`doc/85-docker.md`](doc/85-docker.md) — Docker 容器化开发
### 开发 & 测试
- [`doc/90-testing.md`](doc/90-testing.md) — 单元/集成/端到端测试策略
### 具身智能路径
- [`doc/99-embodied-ai.md`](doc/99-embodied-ai.md) — **VLA / 机器人开发路线图**(从本仓库出发到部署)
- [`doc/100-embedded-deployment.md`](doc/100-embedded-deployment.md) — **三机部署实操**:PC + RDK X5 + RK3506 × 2
---
## 🗺 入门具身智能的路径
本仓库完成后,做 VLA / 机器人开发的下一步:
| 阶段 | 内容 | 配套技术 |
|---|---|---|
| ✅ 已有 | Topic / Service / Action / TF2 / URDF / 视觉 | **本仓库** |
| ➡️ 下一步 | ros2_control | ros-humble-ros2-control + ros-humble-ros2-controllers |
| ➡️ 下一步 | MoveIt2 机械臂规划 | ros-humble-moveit |
| ➡️ 下一步 | Nav2 移动底盘导航 | ros-humble-navigation2 |
| ➡️ 下一步 | Gazebo / Ignition 仿真 | ros-humble-ros-gz |
| ➡️ 下一步 | rosbridge / Foxglove | rosbridge_suite,foxglove_bridge |
| ➡️ 下一步 | VLA 模型接入 | OpenVLA / RT-2 / Pi0;ros2 服务 + TF + Image 输入 |
| ➡️ 下一步 | 真机部署(PC + RDK X5 + RK3506) | 见 `doc/100-embedded-deployment.md` |
## 🛠 你的硬件(典型配置)
| 硬件 | SoC | RAM | 跑什么 |
|---|---|---|---|
| PC | x86 | 8-32GB | 完整 ROS2 + MoveIt2 / Nav2 / RViz |
| RDK X5 × 1 | Sunrise 3 + 5 TOPS NPU | 4GB | 边缘 AI(视觉 / 语音 / SLAM) |
| RK3506 × 2 | ARM 3 核 | 512MB (Linux) | 实时控制(电机 / 编码器 / PID) |
三层都跑完整 ROS2,通过 LAN FastDDS 互通。**详细部署见 [`doc/100-embedded-deployment.md`](doc/100-embedded-deployment.md)**。
详见 [`doc/99-embodied-ai.md`](doc/99-embodied-ai.md)。
---
## 🛠 项目约定(必读)
代码风格 / 构建约束全部在 [`AGENTS.md`](AGENTS.md),核心几条:
1. **本机 venv 不污染系统 Python**(用 `tools/setup_venv.{sh,ps1}`)
2. 容器内用 colcon + ament(ROS2 官方工具链)
3. 跨包 launch 用 `IncludeLaunchDescription` + `FindPackageShare`
4. 包名不能叫 `launch`(与 ROS2 系统包同名会冲突)
5. **测试 100% 通过才能停手**
---
## 📁 目录速览
```
D:\xs\ros2\
├── README.md ← 本文件
├── AGENTS.md ← 铁律 + 工作流约定
├── pyproject.toml ← PEP 621 workspace 元数据(IDE 入口)
├── requirements*.txt ← venv 依赖
├── .flake8 / pyrightconfig.json ← lint / 类型检查配置
│
├── docker/
│ ├── Dockerfile ← ROS2 Humble 镜像
│ ├── docker-compose.yml ← 容器编排
│ ├── bringup_e2e.log ← 端到端日志(已固化)
│ ├── srv_e2e.log
│ ├── robot_e2e.log
│ ├── vision_e2e.log
│ └── full_demo_e2e.log
│
├── tools/
│ ├── setup_venv.sh ← Linux/WSL/Docker 一键 venv
│ └── setup_venv.ps1 ← Windows 一键 venv
│
├── build.sh / start.sh / start.ps1 ← 容器内构建 / 一键启动
│
├── src/
│ ├── py_pubsub/ ← Topic pub/sub (Python)
│ ├── cpp_pubsub/ ← Topic pub/sub (C++)
│ ├── py_srv/ ← Service (Python)
│ ├── py_action_demo/ ← Action (Python)
│ ├── cpp_robot_tf2/ ← TF2 + URDF + JointState (C++)
│ ├── py_vision_demo/ ← sensor_msgs/Image (Python)
│ └── bringup/ ← 顶层 launch 聚合 (Python)
│
└── doc/ ← 12 篇深度文档
```
---
## 🤝 致谢
- [ROS2 官方文档](https://docs.ros.org/en/humble/)
- [OSRF](https://www.openrobotics.org/) `osrf/ros:humble-desktop` 镜像
- [鱼香 ROS](https://fishros.org/) 中文教程
开始你的 ROS2 之旅:`doc/01-quickstart.md` → 跑通 → 读 `doc/10-concepts.md` 深入 → 上 `doc/99-embodied-ai.md` 部署。
build.sh
+85
View File
@@ -0,0 +1,85 @@
#!/usr/bin/env bash
# =============================================================================
# build.sh —— 容器内一次性编译脚本。
#
# 注意:本脚本在 Docker 容器内跑,使用 ROS2 apt 安装的系统 Python (python3-rclpy)。
# 本机的 venv(.venv)是给 IDE / 本机测试用的,不污染系统 Python;
# 与容器内的构建是平行的两条线。详见 README.md 与 doc/01-virtualenv.md。
#
# 流程:
# 1) source /opt/ros/humble/setup.bash
# 这一句把 ROS2 CLI、消息库、ament 工具加进 PATH。
# 2) colcon build --symlink-install --packages-select <all packages>
# --symlink-install:对 Python/launch 文件做软链,改源码后无需重编;
# C++ 可执行仍按 build 路径硬编(避免运行时找不到)。
# --event-handlers console_direct+:把每个包的 build 输出直打到终端。
# 3) ls install/:确认产物目录存在。
# =============================================================================
# 出错即停,避免继续执行错误的结果。
set -euo pipefail
# 加载 ROS2 基础环境变量与 PATH / PYTHONPATH / LD_LIBRARY_PATH。
source /opt/ros/humble/setup.bash
# 切到挂载进来的工作空间根。
cd /root/ros2_ws
echo "[BUILD] ROS_DISTRO=$ROS_DISTRO"
echo "[BUILD] packages under src/:"
ls src/
echo "[BUILD] colcon build (symlink-install)..."
colcon build \
--symlink-install \
--packages-select py_pubsub cpp_pubsub py_srv py_action_demo cpp_robot_tf2 py_vision_demo bringup \
--event-handlers console_direct+
echo "[BUILD] done. install/:"
ls install/
cat <<'TXT'
已就绪的常用命令(在使用前需要 source install/setup.bash):
# Topic pub/sub
ros2 run py_pubsub talker # Python 发布者
ros2 run py_pubsub listener # Python 订阅者
ros2 run cpp_pubsub talker # C++ 发布者
ros2 run cpp_pubsub listener # C++ 订阅者
# Service
ros2 run py_srv add_two_ints_server # Python 服务端
ros2 service call /add_two_ints example_interfaces/srv/AddTwoInts "{a: 12, b: 30}"
# Action
ros2 run py_action_demo fibonacci_server # Python Action 服务端
ros2 run py_action_demo fibonacci_client -- -o 8
# Robot (URDF + TF2 + JointState)
ros2 launch cpp_robot_tf2 robot_tf2_launch.py
# Vision (sensor_msgs/Image)
ros2 launch py_vision_demo vision_launch.py
# launch 文件
ros2 launch py_pubsub pubsub_launch.py
ros2 launch cpp_pubsub pubsub_launch.py topic:=hello
ros2 launch py_srv srv_launch.py
ros2 launch py_action_demo action_launch.py
ros2 launch cpp_robot_tf2 robot_tf2_launch.py
ros2 launch py_vision_demo vision_launch.py
ros2 launch bringup full_demo_launch.py
# 调试 / 观测
ros2 node list # 当前进程里的 ROS 节点
ros2 topic list # 当前所有 topic
ros2 topic info chatter -v # 看 pub/sub 端 + 消息类型
ros2 topic echo chatter # 实时打印 chatter 内容
ros2 topic hz chatter # 测量 chatter 频率(Hz)
ros2 param list # 看节点的参数
ros2 service list # 当前 service
ros2 action list # 当前 action
ros2 run tf2_tools view_frames # 可视化 TF 树
rqt_graph # 可视化节点/话题关系图
TXT
doc/00-overview.md
+270
View File
@@ -0,0 +1,270 @@
# 00 · 项目架构与设计取舍(完全指南)
> **目标**:5 分钟内搞清楚本仓库"为什么这么设计、每一层做什么、对比 ROS2 全栈还差什么"。
---
## 目录
- [1. 一句话定位](#1-一句话定位)
- [2. 双层解耦架构](#2-双层解耦架构)
- [3. 包设计全景](#3-包设计全景)
- [4. 通信拓扑](#4-通信拓扑)
- [5. 文件布局详解](#5-文件布局详解)
- [6. ROS2 全栈 vs 本仓库](#6-ros2-全栈-vs-本仓库)
- [7. 何时用本仓库](#7-何时用本仓库)
- [8. 设计原则](#8-设计原则)
- [9. 下一步](#9-下一步)
---
## 1. 一句话定位
> **ROS2 Humble 7 包全栈实战** — Topic / Service / Action / TF2 / URDF / Vision 六大通信范式 +
> Python/C++ 双语言互通 + 端到端 launch + **10/10 测试 100% 通过** + 15 篇文档 —
> 为 VLA / 具身智能开发铺平最后一段学习路径。
---
## 2. 双层解耦架构
本仓库用**两个互不污染的环境**把"Python 开发"与"ROS2 运行"分开:
```
┌─────────────────────────────────────────────────────────────┐
│ 本机 Windows / Linux (你正在用的机器) │
│ ┌───────────────────────────────────────────────────────┐ │
│ │ .venv (Python 3.10 virtualenv) │ │
│ │ ruff · black · mypy · flake8 · pytest · numpy │ │
│ │ + pip install -e src/py_pubsub ... (源码可编辑) │ │
│ └───────────────────────────────────────────────────────┘ │
│ IDE: VSCode (Pylance / pyright) · PyCharm │
│ 配置文件: pyproject.toml · pyrightconfig.json · .flake8 │
└────────────────────────────┬────────────────────────────────┘
│ bind mount: D:\xs\ros2 ↔ /root/ros2_ws
┌────────────────────────────▼────────────────────────────────┐
│ Docker (osrf/ros:humble-desktop) │
│ ┌───────────────────────────────────────────────────────┐ │
│ │ ROS2 apt (系统 Python) │ │
│ │ rclcpp · rclpy · tf2 · cv_bridge · ros2-control ... │ │
│ │ /opt/ros/humble + ROS_DOMAIN_ID + RMW │ │
│ └───────────────────────────────────────────────────────┘ │
│ ┌───────────────────────────────────────────────────────┐ │
│ │ colcon build / install / test │ │
│ │ src/py_pubsub src/cpp_pubsub src/py_srv │ │
│ │ src/py_action_demo src/cpp_robot_tf2 │ │
│ │ src/py_vision_demo src/bringup │ │
│ │ ↓ │ │
│ │ install/<pkg>/share/<pkg>/launch/*.py │ │
│ │ install/<pkg>/lib/<pkg>/<exec> │ │
│ └───────────────────────────────────────────────────────┘ │
│ ┌───────────────────────────────────────────────────────┐ │
│ │ 11 个 ROS2 节点(运行时) │ │
│ │ talker/listener × 2 (py+cpp) │ │
│ │ service server / action server │ │
│ │ joint_state_publisher + robot_state_publisher │ │
│ │ + tf2_listener + fake_camera + image_processor │ │
│ └───────────────────────────────────────────────────────┘ │
└─────────────────────────────────────────────────────────────┘
```
### 2.1 为什么这样设计
| 层 | 用途 | 为什么独立 |
|---|---|---|
| **本机 venv** | IDE 代码跳转、单元测试、lint、格式化 | rclpy 没 Windows wheels;venv 隔离掉"装不上 ROS 客户端"的事实 |
| **Docker ROS2** | 跑 ROS2 节点、launch、colcon test、端到端验证 | 一次 build 处处跑,Linux + Windows + macOS 一致 |
详见 [`02-virtualenv.md`](02-virtualenv.md)。
---
## 3. 包设计全景
| 包 | 角色 | build_type | 关键依赖 |
|---|---|---|---|
| **py_pubsub** | Topic pub/sub(Python) | ament_python | rclpy, std_msgs |
| **cpp_pubsub** | Topic pub/sub(C++) | ament_cmake | rclcpp, std_msgs |
| **py_srv** | Service(Python) | ament_python | rclpy, example_interfaces |
| **py_action_demo** | Action 三件套(Python) | ament_python | rclpy, example_interfaces |
| **cpp_robot_tf2** | URDF + TF2 + JointState(C++) | ament_cmake | rclcpp, tf2, geometry_msgs, sensor_msgs |
| **py_vision_demo** | sensor_msgs/Image(Python) | ament_python | rclpy, sensor_msgs, cv_bridge |
| **bringup** | 顶层 launch 聚合(Python) | ament_python | launch, launch_ros |
**设计原则**:
- 每个包独立可 build / test,**不强依赖其他 demo 包**
- 跨包互通依赖 DDS(**不需要 import 别的包代码**)
- bringup **只负责组合**,不重复节点实现
---
## 4. 通信拓扑
ROS2 所有节点通过 **DDS**(默认 fastdds)做发布订阅,**不直接 import 别的节点代码**:
```
┌──────────────────────────────────────────────────────────┐
│ DDS 总线 │
│ /chatter /image_raw /tf /tf_static │
│ /joint_states /parameter_events /rosout │
└────────┬────────┬──────────┬──────────┬────────┬────────┘
│ │ │ │ │
┌─────▼───┐ ┌──▼──────┐ ┌▼────────┐ ┌▼─────┐ ┌▼─────────┐
│talker_py│ │fake_cam │ │joint_pub│ │srv │ │fibonacci │
│listener │ │image_ │ │robot_ │ │server│ │_server │
│ _py │ │processor│ │state_pub│ │ │ │ │
├─────────┤ ├─────────┤ ├─────────┤ └──────┘ └──────────┘
│talker_ │
│ cpp │ ← topic 互通(跨语言)
│listener │
│ _cpp │
└─────────┘
```
---
## 5. 文件布局详解
```
D:\xs\ros2\
├── README.md ← 项目入口 + 架构 + 启动命令
├── AGENTS.md ← 铁律 + 工作流(必读)
├── pyproject.toml ← PEP 621 workspace + 工具配置
├── requirements.txt / -dev.txt ← venv 依赖
├── .flake8 / pyrightconfig.json ← lint / 类型检查配置
├── .gitignore
│
├── docker/
│ ├── Dockerfile ← ROS2 Humble 镜像
│ ├── docker-compose.yml ← bind mount + host network
│ └── *_e2e.log ← 端到端验证日志(46KB 等)
│
├── tools/ ← 本机开发工具
│ ├── setup_venv.sh / .ps1 ← 一键 venv
│
├── build.sh / start.sh / start.ps1
│
├── src/
│ ├── py_pubsub/ ← Topic (Python)
│ ├── cpp_pubsub/ ← Topic (C++)
│ ├── py_srv/ ← Service (Python)
│ ├── py_action_demo/ ← Action (Python)
│ ├── cpp_robot_tf2/ ← TF2 + URDF (C++)
│ ├── py_vision_demo/ ← Image (Python)
│ └── bringup/ ← launch 聚合 (Python)
│
└── doc/ ← 15 篇深度文档
├── 00-overview.md ← 本篇
├── 01-quickstart.md ← 5 分钟上手
├── 02-virtualenv.md ← venv 工作流
├── 10-concepts.md ← 核心概念地图
├── 20-topics.md ← Topic 深度
├── 30-services.md ← Service 深度
├── 40-actions.md ← Action 深度
├── 50-tf2.md ← TF2 坐标变换
├── 60-urdf.md ← URDF 机器人模型
├── 70-launch.md ← launch 文件
├── 80-package-build.md ← colcon / ament
├── 85-docker.md ← Docker 容器化
├── 90-testing.md ← 测试金字塔
├── 99-embodied-ai.md ← VLA / 机器人路径
└── 100-embedded-deployment.md ← 三机部署实操
```
---
## 6. ROS2 全栈 vs 本仓库
**坦白说:本仓库不是完全体**。覆盖 ~70% 的 ROS2 + 机械臂入门知识。
### 6.1 ✅ 已覆盖
| 知识点 | 状态 |
|---|---|
| ROS2 通信(Topic / Service / Action / Parameter) | ✅ |
| TF2 坐标变换 + URDF | ✅ |
| sensor_msgs/Image + cv_bridge | ✅ |
| JointState 发布 | ✅ |
| Launch + 测试 + Docker + venv | ✅ |
| 跨语言互通 | ✅ |
| 文档齐全(15 篇) | ✅ |
### 6.2 ❌ 没覆盖(下一步)
| 知识点 | 缺 |
|---|---|
| **ros2_control** + JointTrajectoryController | ❌ |
| **MoveIt2** 运动规划(IK / 避障) | ❌ |
| **Gazebo** / ros_gz 物理仿真 | ❌ |
| 真实 6-DoF 机械臂 URDF | ❌(只有 3 关节玩具) |
| Gripper / 末端执行器控制 | ❌ |
| 手眼标定 | ❌ |
| 力控 / Force-Torque | ❌ |
| GraspNet / 6-DoF 抓取 | ❌ |
| VLA 模型接入(OpenVLA / Pi0) | ❌(只有路径规划) |
| 移动底盘(Nav2 / SLAM) | ❌ |
| 多臂协调 | ❌ |
| rosbridge / Foxglove | ❌ |
**完整学习路径**: 见 [`99-embodied-ai.md`](99-embodied-ai.md) — 5 个阶段 / 3-5 个月。
### 6.3 实际项目必装的额外 apt 包
```bash
sudo apt install ros-humble-ros2-control
sudo apt install ros-humble-ros2-controllers
sudo apt install ros-humble-moveit
sudo apt install ros-humble-ros-gz
sudo apt install ros-humble-navigation2
```
---
## 7. 何时用本仓库
| 场景 | 推荐 |
|---|---|
| 第 1 次学 ROS2,想完整跑通一个最小 ROS2 系统 | ✅ 推荐 |
| 想搞懂"跨语言互通"是怎么工作的 | ✅ 推荐 |
| 准备做机器人 / 具身智能项目,需要先打基础 | ✅ 推荐 |
| 公司已经在用 ROS2,需要一个 demo 教学 | ✅ 推荐 |
| 想直接做 MoveIt2 / Nav2 的仿真 | ⚠️ 本仓库打底,然后装 ROS2 Humble 官方包 |
| 想直接做产品化 ROS2 | ⚠️ 本仓库做参考,真实项目要按需重构 |
---
## 8. 设计原则
### 8.1 包设计
- 一个 demo 一个包
- 跨包不 import,只走 DDS
- 标准消息为主(example_interfaces),避免自定义 msg
### 8.2 命名
- 包名小写下划线(`py_pubsub`, `cpp_robot_tf2`)
- **包名不能叫 `launch`**(与 ROS2 自带包同名,ament 索引冲突)
- 节点名小写下划线 + 后缀(`talker_py`, `joint_state_publisher_cpp`)
### 8.3 测试
- 每个 Python 包 4 个测试用例
- 每个 C++ 包 2 个 gtest
- 集成测试用 in-process spin(避免 launch_testing 的 shutdown 二次调用问题)
### 8.4 文档
- 中文 + 详细中文注释
- 每篇 doc 有目录 + "5 分钟摘要" + "踩过的坑"
- 代码与文档严格同步(交叉引用源文件路径)
---
## 9. 下一步
| 你想做什么 | 看 |
|---|---|
| 5 分钟跑通 | [`01-quickstart.md`](01-quickstart.md) |
| 用 venv 不污染系统 | [`02-virtualenv.md`](02-virtualenv.md) |
| 弄懂 ROS2 核心概念 | [`10-concepts.md`](10-concepts.md) |
| 深读 Topic / Service / Action | [`20-topics.md`](20-topics.md), [`30-services.md`](30-services.md), [`40-actions.md`](40-actions.md) |
| 做机器人 / TF / URDF | [`50-tf2.md`](50-tf2.md), [`60-urdf.md`](60-urdf.md) |
| 三机部署到 RDK X5 / RK3506 | [`100-embedded-deployment.md`](100-embedded-deployment.md) |
| 进入具身智能 / VLA | [`99-embodied-ai.md`](99-embodied-ai.md) |
doc/01-quickstart.md
+459
View File
@@ -0,0 +1,459 @@
# 01 · 5 分钟上手 Quickstart(完整图文版)
> **目标**:从"零"到"看到第一个 ROS2 消息流",每步带**预期输出 + 排错**,5 分钟内完成。
---
## 目录
- [Step 0: 准备清单](#step-0-准备清单)
- [Step 1: 安装 Docker](#step-1-安装-docker)
- [Step 2: 拉取并构建镜像](#step-2-拉取并构建镜像)
- [Step 3: 启动容器](#step-3-启动容器)
- [Step 4: 在容器内编译](#step-4-在容器内编译)
- [Step 5: 跑你的第一个 demo](#step-5-跑你的第一个-demo)
- [Step 6: 本机 venv 开发工作流](#step-6-本机-venv-开发工作流)
- [Step 7: 验证清单](#step-7-验证清单)
- [Step 8: 常见问题 FAQ](#step-8-常见问题-faq)
---
## Step 0: 准备清单
| 项 | 需要 |
|---|---|
| **操作系统** | Windows 10/11 · macOS 12+ · Linux(Ubuntu 20.04+) |
| **Docker** | Docker Desktop(Win/macOS)或 Docker Engine(Linux) |
| **磁盘空间** | 8 GB 可用(Docker 镜像 + 构建缓存) |
| **网络** | 首次构建需要下载 ~3 GB 镜像 |
| **时间** | 首次构建 5-10 分钟;后续秒级 |
> 💡 Windows 用户:在 BIOS 里开启虚拟化(Intel VT-x / AMD-V),否则 Docker 起不来。
---
## Step 1: 安装 Docker
### 1.1 Windows
1. 下载 [Docker Desktop for Windows](https://www.docker.com/products/docker-desktop/)
2. 双击安装,需要 WSL 2 后端(安装时它会提示)
3. 重启电脑
4. 启动 Docker Desktop,**等到右下角鲸鱼图标不再转动**
### 1.2 macOS
```bash
brew install --cask docker
# 启动 Docker Desktop
```
### 1.3 Linux
```bash
# Ubuntu
sudo apt install docker.io docker-compose-plugin
sudo systemctl enable --now docker
sudo usermod -aG docker $USER
# 重新登录后生效
```
### 1.4 验证 Docker 装好
```bash
docker --version
# Docker version 29.4.3, build ...
docker compose version
# Docker Compose version v5.1.3
```
**若报 `Cannot connect to Docker daemon`**: 启动 Docker Desktop(Win/macOS)或 `sudo systemctl start docker`(Linux)。
---
## Step 2: 拉取并构建镜像
### 2.1 进入项目目录
```bash
# Windows PowerShell
cd D:\xs\ros2
# Linux / macOS
cd /path/to/ros2
```
### 2.2 构建镜像(首次 5-10 分钟)
```bash
docker compose -f docker/docker-compose.yml build
```
**这一步做了什么**:
1. 拉 `osrf/ros:humble-desktop` 基础镜像(约 2 GB)
2. 装 `colcon-common-extensions`、`colcon-argcomplete` 等开发工具
3. 打 tag 为 `ros2-humble-dev:latest`
**预期输出(末尾)**:
```
#10 exporting layers 1.5s done
#10 exporting manifest sha256:xxxxx 0.0s done
#10 exporting config sha256:xxxxx 0.0s done
#10 naming to docker.io/library/ros2-humble-dev:latest done
Image ros2-humble-dev:latest Built
```
**若报 `ERROR: pull access denied`**:
- 检查网络(可能在国内,需要配 Docker 镜像加速器)
- 或: `docker pull osrf/ros:humble-desktop` 单步测试
### 2.3 验证镜像
```bash
docker images
# REPOSITORY TAG IMAGE ID CREATED SIZE
# ros2-humble-dev latest abc123def456 1 minute ago 3.4GB
# osrf/ros humble-desktop 789xyz... 3 weeks ago 2.1GB
```
---
## Step 3: 启动容器
```bash
docker compose -f docker/docker-compose.yml up -d
```
**参数说明**:
- `up -d`:后台启动(`-d` = detached)
- `container_name: ros2_dev`:容器名叫这个
- `network_mode: host`:DDS multicast 必须
**预期**:
```
[+] Running 2/2
✔ Container ros2_dev Created
✔ Container ros2_dev Started
```
**验证容器在跑**:
```bash
docker ps
# CONTAINER ID IMAGE NAMES ...
# abc123def456 ros2-humble-dev:latest ros2_dev Up X seconds
```
**进入开发终端**:
```bash
docker exec -it ros2_dev bash -lc "source /root/ros2_ws/install/setup.bash && exec bash"
```
你应该看到类似:
```
root@docker-desktop:/root/ros2_ws#
```
> 💡 **小技巧**: `docker exec -it ros2_dev bash` 进入容器;**`exec` 前要保证容器在运行**(`docker ps` 看到 `Up` 状态)。
---
## Step 4: 在容器内编译
### 4.1 准备编译
```bash
# 在容器内
source /opt/ros/humble/setup.bash # 加载 ROS2 环境
cd /root/ros2_ws # 进入工作空间
```
### 4.2 编译所有包
```bash
bash build.sh
```
**预期输出(末尾)**:
```
[INFO] [launch]: Default logging verbosity is set to INFO
Finished <<< py_pubsub [9.5s]
Finished <<< cpp_pubsub [42s]
Finished <<< py_srv [11s]
Finished <<< py_action_demo [17s]
Finished <<< cpp_robot_tf2 [49s]
Finished <<< py_vision_demo [11s]
Finished <<< bringup [18s]
Summary: 7 packages finished [2min 30s]
```
**编译产物位置**:
```
/root/ros2_ws/install/ ← source 这个目录才能用 ros2 命令
├── py_pubsub/
├── cpp_pubsub/
├── py_srv/
├── py_action_demo/
├── cpp_robot_tf2/
├── py_vision_demo/
└── bringup/
```
### 4.3 跑测试(可选)
```bash
source /opt/ros/humble/setup.bash
source /root/ros2_ws/install/setup.bash
colcon test --packages-select py_pubsub cpp_pubsub py_srv py_action_demo cpp_robot_tf2 py_vision_demo bringup
```
**预期**:
```
Summary: 7 packages finished [25s]
0 packages failed
```
---
## Step 5: 跑你的第一个 demo — Topic 跨语言互通
### 5.1 启动 4 个节点(2 Python + 2 C++)
```bash
source /opt/ros/humble/setup.bash
source /root/ros2_ws/install/setup.bash
ros2 launch bringup pubsub_launch.py
```
**预期输出**:
```
[INFO] [launch]: All log files can be found below /root/.ros/log/2026-08-03-...
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [talker-1]: process started with pid [56]
[INFO] [listener-2]: process started with pid [58]
[INFO] [talker-3]: process started with pid [60]
[INFO] [listener-4]: process started with pid [62]
[talker-1] [INFO] [...] talker_py started -> topic=chatter, period=500ms
[listener-2] [INFO] [...] listener_py subscribed <- chatter
[talker-3] [INFO] [...] talker_cpp started -> topic=chatter, period=500ms
[listener-4] [INFO] [...] listener_cpp subscribed <- chatter
[listener-2] [INFO] [...] recv: "Hello from PY, seq=0"
[listener-4] [INFO] [...] recv: "Hello from C++, seq=0"
[listener-4] [INFO] [...] recv: "Hello from PY, seq=0" ← **跨语言互通!**
[listener-2] [INFO] [...] recv: "Hello from C++, seq=0" ← **跨语言互通!**
```
按 **Ctrl+C** 退出。
### 5.2 在另一个终端验证
**新开一个 PowerShell/终端**,运行:
```bash
docker exec ros2_dev bash -lc "source /opt/ros/humble/setup.bash && source /root/ros2_ws/install/setup.bash && ros2 topic list --no-daemon"
```
**预期输出**:
```
/chatter
/joint_states
/parameter_events
/rosout
/tf
```
**看 chatter 频率**:
```bash
docker exec ros2_dev bash -lc "source /opt/ros/humble/setup.bash && source /root/ros2_ws/install/setup.bash && ros2 topic hz /chatter --no-daemon"
```
**预期**:
```
average rate: 4.000
min: 0.250s max: 0.260s std dev: 0.00302s window: 10
```
(4Hz = 2 talker × 2Hz)
### 5.3 看通信拓扑(可视化)
**新终端**:
```bash
docker exec ros2_dev bash -lc "source /opt/ros/humble/setup.bash && source /root/ros2_ws/install/setup.bash && ros2 run rqt_graph rqt_graph"
```
**会看到**(rqt_graph GUI):
- `talker_py` → `chatter` → `listener_py`
- `talker_cpp` → `chatter` → `listener_cpp`
- 4 节点 2 topic 互通
---
## Step 6: 本机 venv 开发工作流
### 6.1 创建 venv(不污染系统 Python)
```powershell
# Windows
cd D:\xs\ros2
powershell .\tools\setup_venv.ps1
```
```bash
# Linux / macOS / WSL
cd /path/to/ros2
./tools/setup_venv.sh
```
**预期**:
```
==> Python: Python 3.10.x
==> Creating venv at .venv
==> Upgrading pip + installing requirements-dev.txt
OK py_pubsub
OK py_srv
OK py_action_demo
OK py_vision_demo
✅ venv ready. Activate with:
.\.venv\Scripts\Activate.ps1
```
### 6.2 激活 venv
```powershell
# Windows
.\.venv\Scripts\Activate.ps1
# Linux / macOS / WSL
source .venv/bin/activate
```
**预期**: 终端前缀出现 `(.venv)`,如:
```
(.venv) PS D:\xs\ros2>
```
### 6.3 在 venv 里能做什么 / 不能做什么
| 能做 | 不能做 |
|---|---|
| 编辑 Python 源码,IDE 自动补全 | `import rclpy` (rclpy 没 Windows wheels) |
| 跑 ruff / black / mypy | 跑 ROS2 节点 / launch 文件 |
| 装纯 Python 包(numpy / torch / opencv-python) | 跑 ros2 CLI 命令 |
### 6.4 配置 VSCode / PyCharm
**VSCode**:
- `Ctrl+Shift+P` → "Python: Select Interpreter" → 选 `.venv\Scripts\python.exe`
- 装扩展:Python, Pylance, ROS(可选)
- `pyrightconfig.json` 已配好
**PyCharm**:
- `Settings → Project → Python Interpreter` → 选 `.venv\Scripts\python.exe`
---
## Step 7: 验证清单
按这个清单逐项打勾,全过才算"5 分钟上手成功":
- [ ] `docker ps` 看到 `ros2_dev` 容器 `Up`
- [ ] `docker exec ros2_dev echo hello` 输出 `hello`
- [ ] `bash build.sh` 7 packages 全 build 成功
- [ ] `colcon test` 全过(7 packages, 0 failed)
- [ ] `ros2 launch bringup pubsub_launch.py` 启动 4 节点
- [ ] `ros2 topic list` 看到 `/chatter`
- [ ] `ros2 topic hz /chatter` 显示 ~4Hz
- [ ] `ros2 topic info chatter -v` 看到 Python + C++ pub/sub
- [ ] venv 装好且激活,`python -c "import sys; print(sys.executable)"` 显示 `.venv` 路径
如果有任何一项 ✗,看 [Step 8 FAQ](#step-8-常见问题-faq)。
---
## Step 8: 常见问题 FAQ
### Q1: 容器启动失败 `Cannot connect to Docker daemon`
**原因**: Docker Desktop 没运行 / WSL2 没启动
**解决**:
- Win/macOS:启动 Docker Desktop,等右下角图标稳定
- Linux:`sudo systemctl start docker`
### Q2: 构建镜像很慢,卡在 `pulling image`
**原因**: 网络慢 / 在国内
**解决**:
```bash
# 加 Docker 镜像加速器
# Docker Desktop → Settings → Docker Engine,加:
{
"registry-mirrors": ["https://docker.mirrors.ustc.edu.cn"]
}
```
然后 `docker compose build` 重试。
### Q3: `bash build.sh` 报 `rcl_xxx not found`
**原因**: 没 source ROS2
**解决**:
```bash
source /opt/ros/humble/setup.bash
bash build.sh
```
**或者** 把这句加进 `~/.bashrc`:
```bash
echo 'source /opt/ros/humble/setup.bash' >> ~/.bashrc
```
### Q4: 容器里 `ros2` 命令找不到
**原因**: 没 source `install/setup.bash`
**解决**:
```bash
source /root/ros2_ws/install/setup.bash
ros2 --help # 应该输出帮助
```
### Q5: `ros2 topic hz` 显示 0 Hz
**原因**: 没节点 publish 数据
**解决**: 确认 launch 起来了,看到 `[INFO] pub: "Hello from..."` 日志
### Q6: Windows venv 里 `import rclpy` 飘红
**原因**: rclpy 没 Windows wheels
**解决**: 正常现象,不影响阅读代码。要跑 rclpy 在容器里跑
### Q7: 容器里 `pip install` 装不上 ROS2 包
**原因**: 你在容器 venv,ROS2 包来自 apt
**解决**:
```bash
# 不在容器 venv 里
exit # 退出 venv
# 或用 apt
sudo apt install ros-humble-<pkg>
```
### Q8: 容器里中文显示乱码
**原因**: 容器没装中文字体
**解决**: 在容器内 `export LANG=C.UTF-8 LC_ALL=C.UTF-8`
### Q9: `colcon build` 报 `CMakeCache` 错误
**原因**: 之前构建的 CMakeCache 有问题
**解决**:
```bash
cd /root/ros2_ws
rm -rf build install log
colcon build --symlink-install
```
### Q10: 容器跑一段时间后磁盘满了
**原因**: `build/` `install/` `log/` 默认在本目录
**解决**:
```bash
# 进容器清理
cd /root/ros2_ws && rm -rf build install log
bash build.sh
# 或清理 Docker
docker system prune -a
```
---
## 下一步
5 分钟上手成功!现在你可以:
- 阅读 [`doc/10-concepts.md`](10-concepts.md) — 理解 ROS2 核心概念
- 阅读 [`doc/20-topics.md`](20-topics.md) — Topic 深度
- 阅读 [`doc/100-embedded-deployment.md`](100-embedded-deployment.md) — 三机部署实操
- 阅读 [`doc/99-embodied-ai.md`](99-embodied-ai.md) — 具身智能路径
**动手尝试**: 改一下 `py_pubsub` 里 talker 的 `period_ms`,观察 `/chatter` 频率变化。这是理解 ROS2 参数的最快方式。
加油,ROS2 之旅开始! 🚀
doc/02-virtualenv.md
+277
View File
@@ -0,0 +1,277 @@
# 02 · 本机 venv 工作流(不污染系统 Python 完全指南)
> **目标**:用标准 Python venv 在 Windows / Linux / macOS 上做 ROS2 开发,**完全不污染系统 Python**。
---
## 目录
- [1. 为什么需要 venv](#1-为什么需要-venv)
- [2. 双轨制 — venv vs colcon](#2-双轨制--venv-vs-colcon)
- [3. 创建 venv(Win / Linux)](#3-创建-venvwin--linux)
- [4. venv 里装了什么](#4-venv-里装了什么)
- [5. IDE 配置](#5-ide-配置)
- [6. 在 venv 里能做什么 / 不能做什么](#6-在-venv-里能做什么--不能做什么)
- [7. 在容器里跑 ROS2 测试](#7-在容器里跑-ros2-测试)
- [8. 依赖锁文件](#8-依赖锁文件)
- [9. .venv 加进 .gitignore](#9-venv-加进-gitignore)
- [10. 故障排查](#10-故障排查)
- [11. 在本仓库里跑](#11-在本仓库里跑)
---
## 1. 为什么需要 venv
ROS2 的 Python 客户端库(`rclpy` / `sensor_msgs` / `tf2_ros` / `cv_bridge` 等)**只在 Linux 平台通过 apt 提供**,
Windows 上没有官方 pip wheels。
如果直接 `pip install` 到系统 Python:
- 失败(找不到 wheel)
- 或者和别的项目冲突(版本冲突)
- 或者哪天升级 Python 把 ROS 客户端搞挂了
**venv 的目的**:本机 Python 工具(ruff / black / pytest / mypy 等)放隔离环境,与 ROS2 运行解耦。
---
## 2. 双轨制 — venv vs colcon
| 工作 | 用什么 | 跑在哪 |
|---|---|---|
| 编辑代码、写测试 | **本机 venv + IDE** | Win/macOS/Linux |
| 跑 ROS2 节点 / colcon build / colcon test | **Docker 容器** | Linux |
| pytest (纯逻辑、不需要 ROS2) | 本机 venv | Win/macOS/Linux |
| pytest (需要 rclpy) | **Docker 容器** | Linux |
```
本机: venv (开发) Docker: colcon (运行)
───────────────── ────────────────────
ruff / black / mypy colcon build
pytest (纯逻辑) colcon test
IDE 跳转 ros2 run / launch
rclpy / sensor_msgs / tf2_ros
```
---
## 3. 创建 venv
### 3.1 Windows
```powershell
cd D:\xs\ros2
powershell .\tools\setup_venv.ps1
# 激活
.\.venv\Scripts\Activate.ps1
# 验证
python -c "import sys; print(sys.executable)"
# 期望: D:\xs\ros2\.venv\Scripts\python.exe (不是 C:\Program Files\Python310\python.exe)
```
### 3.2 Linux / macOS / WSL
```bash
cd /path/to/ros2
./tools/setup_venv.sh
source .venv/bin/activate
python -c "import sys; print(sys.executable)"
# 期望: /path/to/ros2/.venv/bin/python
```
### 3.3 在 Docker 容器内
容器内系统 Python 已经隔离,venv 多此一举。直接用系统 Python 即可。
---
## 4. venv 里装了什么
`tools/setup_venv.{sh,ps1}` 会做:
1. `python -m venv .venv` — 创建虚拟环境
2. `pip install --upgrade pip setuptools wheel`
3. `pip install -r requirements-dev.txt` — 装开发工具:
- `ruff`(超快 linter,替代 flake8 + isort)
- `black`(格式化)
- `mypy`(类型检查)
- `pytest` + `pytest-cov` + `pytest-timeout`
- `pre-commit`(git hook,可选)
4. `pip install -e src/py_pubsub src/py_srv src/py_action_demo src/py_vision_demo`
把本项目 ROS2 Python 包装到 venv(**可编辑模式** — 改源码立即生效,不需要重装)
`--no-deps` 跳过 ROS 客户端依赖(因为 Windows 装不上)
---
## 5. IDE 配置
### 5.1 VSCode
`pyrightconfig.json` 已配置好,直接:
```json
{
"extraPaths": [
"/opt/ros/humble/lib/python3.10/site-packages" // 容器内
]
}
```
设置 VSCode 的 Python 解释器:`Ctrl+Shift+P` → "Python: Select Interpreter" → 选择 `.venv\Scripts\python.exe`
### 5.2 PyCharm
`Settings → Project → Python Interpreter → Add → Existing environment` → 选 `.venv\Scripts\python.exe`
### 5.3 看代码跳转
- 跳转到 `rclpy.Node` 定义:`Ctrl+点击` — pyright 会去 `/opt/ros/humble/lib/python3.10/site-packages` 找
- **容器外**:Windows 上 import rclpy 仍然飘红,但不影响阅读源码
- **容器内(开发容器)**:跳转正常,因为 `/opt/ros/...` 路径存在
---
## 6. 在 venv 里能做什么 / 不能做什么
| 能做 | 不能做 |
|---|---|
| 编辑 Python 源码,IDE 自动补全 | `import rclpy` (rclpy 没 Windows wheels) |
| 跑 ruff / black / mypy | 跑 ROS2 节点 / launch 文件 |
| 装纯 Python 包(numpy / torch / opencv-python) | 跑 ros2 CLI 命令 |
| 在容器内 venv(可访问 rclpy):跑纯 pytest | |
---
## 7. 在容器里跑 ROS2 测试
```bash
docker exec ros2_dev bash -lc "source /opt/ros/humble/setup.bash && cd /root/ros2_ws && colcon test --packages-select py_pubsub"
```
colcon test 会自动跑每个包的 `test/` 目录下的测试。本仓库:
- `py_*` 包 → pytest
- `cpp_*` 包 → ament_cmake_gtest(CTest)
- 期望全部 PASSED。
---
## 8. 依赖锁文件
生产环境建议加 `requirements.lock`(`pip freeze > requirements.lock`),精确锁版本。本仓库的
`requirements*.txt` 只列宽松版本(`>=`):
| 文件 | 用途 |
|---|---|
| `requirements.txt` | 运行时纯 Python 依赖(numpy / pytest) |
| `requirements-dev.txt` | 开发工具(ruff / black / mypy) |
---
## 9. .venv 加进 .gitignore
```gitignore
.venv/
```
已加。提交代码不会带 venv 目录。
---
## 10. 故障排查
### 10.1 `python: command not found`
- Windows:安装 Python 3.10+ 并勾选 "Add to PATH"
- Linux: `sudo apt install python3 python3-venv`
### 10.2 `venv` 模块找不到
```bash
# Debian/Ubuntu
sudo apt install python3-venv
# Fedora/RHEL
sudo dnf install python3-virtualenv
```
### 10.3 venv 里 `import rclpy` 仍然飘红
正常。rclpy 在 Windows 没有 pip wheels,需要靠 apt(在容器内)。开发时不影响阅读,
要跑 ROS2 测试就在容器里跑。
### 10.4 venv 占空间
通常 200-500 MB。删掉重建:`rm -rf .venv && tools/setup_venv.sh`。
### 10.5 激活 venv 后 pip 装不上
确认 `which python` / `which pip` 指向 `.venv/bin/`:
```bash
which python # /path/.venv/bin/python
which pip # /path/.venv/bin/pip
```
如果指向系统 Python,venv 没生效。
---
## 11. 在本仓库里跑
### 11.1 创建 venv
```powershell
cd D:\xs\ros2
powershell .\tools\setup_venv.ps1
```
输出:
```
==> Python: Python 3.10.x
==> Creating venv at .venv
==> Upgrading pip + installing requirements-dev.txt
OK py_pubsub
OK py_srv
OK py_action_demo
OK py_vision_demo
✅ venv ready. Activate with:
.\.venv\Scripts\Activate.ps1
```
### 11.2 激活 + 验证
```powershell
.\.venv\Scripts\Activate.ps1
python --version # Python 3.10.x (venv)
pip list # 已装的包
ruff check src/ # linter
mypy src/ # 类型检查(部分飘红是 rclpy 缺,忽略)
```
### 11.3 在 venv 里跑纯 Python 测试(不需要 ROS2)
```bash
# 比如写一个纯算法测试
pytest src/<pkg>/test/test_my_algorithm.py
```
### 11.4 完整工作流
```powershell
# 1. 本机开发:venv + IDE
cd D:\xs\ros2
.\.venv\Scripts\Activate.ps1
code . # VSCode 打开
# 2. 改完代码,跑容器测试
docker exec ros2_dev bash -lc "cd /root/ros2_ws && colcon build --packages-select <pkg>"
docker exec ros2_dev bash -lc "cd /root/ros2_ws && colcon test --packages-select <pkg>"
# 3. 跑 demo
docker exec ros2_dev bash -lc "source install/setup.bash && ros2 launch bringup <file>"
```
---
## 接下来读
| 主题 | 文档 |
|---|---|
| 项目总览 | [`00-overview.md`](00-overview.md) |
| 5 分钟上手 | [`01-quickstart.md`](01-quickstart.md) |
| 测试策略 | [`90-testing.md`](90-testing.md) |
| Docker | [`85-docker.md`](85-docker.md) |
doc/10-concepts.md
+864
View File
@@ -0,0 +1,864 @@
# 10 · ROS2 核心概念速览(完整概念地图)
> **目标**:30 分钟内把 ROS2 所有核心概念装进脑子里,后续文档都能"秒懂"。
---
## 目录
- [一图总览](#一图总览)
- [1. 计算图(Computational Graph)](#1-计算图computational-graph)
- [2. Node(节点)](#2-node节点)
- [3. Topic(话题)](#3-topic话题)
- [4. Service(服务)](#4-service服务)
- [5. Action(动作)](#5-action动作)
- [6. Parameter(参数)](#6-parameter参数)
- [7. TF2(坐标变换)](#7-tf2坐标变换)
- [8. URDF(机器人模型)](#8-urdf机器人模型)
- [9. sensor_msgs(常用消息类型)](#9-sensor_msgs常用消息类型)
- [10. Launch(启动文件)](#10-launch启动文件)
- [11. DDS / QoS / 时间(底层)](#11-dds--qos--时间底层)
- [12. 6 大概念怎么一起工作](#12-6-大概念怎么一起工作)
- [13. 概念速查表](#13-概念速查表)
---
## 一图总览
```
┌──────────────────────────────────────────────────────────────────┐
│ DDS 总线 (默认 fastdds) │
│ │
│ Topic /chatter Topic /image_raw Topic /tf │
│ Topic /joint_states Service /add_two_ints │
│ Action /fibonacci Parameter server │
└───────┬─────────────────┬──────────────────┬─────────────────────┘
│ │ │
▼ ▼ ▼
┌─────────┐ ┌──────────┐ ┌───────────┐
│ talker │ │ camera │ │ tf listener│
│ listener│ │processor │ │joint_pub │
│ (Node) │ │ (Node) │ │ (Node) │
└─────────┘ └──────────┘ └───────────┘
```
**6 大概念**:Node / Topic / Service / Action / Parameter / TF。
下面 12 节拆解。
---
## 1. 计算图(Computational Graph)
### 1.1 是什么
ROS2 程序由**节点**(Node)组成,节点之间通过**话题/服务/动作**连接,形成一张"计算图"。
图不是静态配置,是**运行时由 DDS 自动发现**。
### 1.2 看计算图
```bash
# 命令行看拓扑
ros2 node list
ros2 topic list
ros2 service list
ros2 action list
# 可视化(GUI)
ros2 run rqt_graph rqt_graph
```
### 1.3 三个关键术语
| 术语 | 含义 |
|---|---|
| **Node** | 一个独立运行的程序 |
| **Edge** | Node 之间的连接(Topic / Service / Action) |
| **Discovery** | DDS 自动找节点,无需中心注册 |
---
## 2. Node(节点)
### 2.1 是什么
ROS2 程序的**基本单元**。一个 Node = 一个进程 = 一项业务功能。
### 2.2 怎么写(Python)
```python
import rclpy
from rclpy.node import Node
class MyNode(Node):
def __init__(self):
super().__init__('my_node_name') # 节点名,在 ROS Domain 内必须唯一
# 在这里创建 publisher / subscription / timer / param
self.timer_ = self.create_timer(0.5, self.tick)
def tick(self):
self.get_logger().info('tick')
def main():
rclpy.init() # 全局初始化
node = MyNode()
rclpy.spin(node) # 进入事件循环,阻塞
rclpy.shutdown() # 退出
if __name__ == '__main__':
main()
```
### 2.3 怎么写(C++)
```cpp
#include "rclcpp/rclcpp.hpp"
class MyNode : public rclcpp::Node {
public:
MyNode() : rclcpp::Node("my_node_name") {
timer_ = this->create_wall_timer(
500ms, std::bind(&MyNode::tick, this));
}
private:
void tick() {
RCLCPP_INFO(this->get_logger(), "tick");
}
rclcpp::TimerBase::SharedPtr timer_;
};
int main(int argc, char * argv[]) {
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<MyNode>());
rclcpp::shutdown();
return 0;
}
```
### 2.4 关键 API
| API | 用途 |
|---|---|
| `super().__init__('name')` | 节点构造时设名字 |
| `create_publisher(...)` | 创建发布者 |
| `create_subscription(...)` | 创建订阅者 |
| `create_service(...)` | 创建服务端 |
| `create_action_server(...)` | 创建动作服务端 |
| `create_timer(period, cb)` | 创建定时器 |
| `declare_parameter(...)` | 声明参数 |
| `get_logger().info(...)` | 打日志 |
| `get_clock()` | 取时钟 |
| `rclpy.spin(node)` | 进入事件循环,处理所有回调 |
### 2.5 生命周期
```
rclpy.init() ─┐
├─ 实例化 Node ─ 创建 pub/sub/service ─ spin 等待回调
实例化 Node ─┘
│
▼
rclpy.spin(node) ── 阻塞,处理 timer / subscription / service 回调
│
▼ (Ctrl+C 或 shutdown)
rclpy.shutdown() ── 清理
```
### 2.6 本仓库对照
- Python:[`src/py_pubsub/py_pubsub/publisher_member_function.py`](../src/py_pubsub/py_pubsub/publisher_member_function.py)
- C++:[`src/cpp_pubsub/src/publisher_member_function.cpp`](../src/cpp_pubsub/src/publisher_member_function.cpp)
---
## 3. Topic(话题)
### 3.1 是什么
**异步、多对多、单向**的发布订阅通道。
- 异步:`publish()` 不阻塞
- 多对多:1 个 publisher,任意多个 subscriber
- 单向:消息只从 pub 到 sub,反方向不通
### 3.2 通信模型
```
Publisher ──publish()──> Topic (chatter) ──callback(msg)──> Subscriber
────────────────────────────────────────────────────
异步,非阻塞 多对多,单向 事件循环触发
```
### 3.3 vs 其他通信方式
| | Topic | Service | Action |
|---|---|---|---|
| 同步 | 异步 | 同步 | 异步 |
| 一对多 | ✅ | ❌ | ❌ |
| 反馈 | ❌ | ❌ | ✅ |
| 取消 | N/A | ❌ | ✅ |
### 3.4 Python API
```python
# Publisher
pub = node.create_publisher(
std_msgs.msg.String, # 消息类型
'chatter', # topic 名
10 # QoS 深度
)
# 构造 + 发送
msg = std_msgs.msg.String()
msg.data = 'hello'
pub.publish(msg) # 异步,不等任何东西
# Subscriber
sub = node.create_subscription(
std_msgs.msg.String, # 类型
'chatter', # topic 名
callback, # 回调函数
10 # QoS 深度
)
def callback(msg):
node.get_logger().info(f'recv: {msg.data}')
```
### 3.5 C++ API
```cpp
// Publisher
auto pub = node->create_publisher<std_msgs::msg::String>("chatter", 10);
auto msg = std_msgs::msg::String();
msg.data = "hello";
pub->publish(msg);
// Subscriber
auto sub = node->create_subscription<std_msgs::msg::String>(
"chatter", 10,
[this](const std_msgs::msg::String::SharedPtr msg) {
RCLCPP_INFO(this->get_logger(), "recv: %s", msg->data.c_str());
});
```
### 3.6 QoS(服务质量)速查
| 维度 | 取值 | 默认 | 影响 |
|---|---|---|---|
| Reliability | RELIABLE / BEST_EFFORT | RELIABLE | 必须 pub/sub 一致 |
| History | KEEP_LAST(N) / KEEP_ALL | KEEP_LAST(10) | 队列大小 |
| Durability | VOLATILE / TRANSIENT_LOCAL | VOLATILE | 晚订阅者是否收到旧数据 |
**兼容规则**:
- RELIABLE ↔ RELIABLE ✅
- BEST_EFFORT ↔ BEST_EFFORT ✅
- BEST_EFFORT → RELIABLE ✅(sub 容忍丢)
- RELIABLE → BEST_EFFORT ❌(sub 不发 ACK,pub 报错)
### 3.7 常用 CLI
```bash
ros2 topic list # 所有 topic
ros2 topic info <topic> -v # 类型 + pub/sub 列表
ros2 topic echo <topic> # 实时打印
ros2 topic hz <topic> # 频率 Hz
ros2 topic bw <topic> # 带宽 bytes/s
ros2 topic pub <topic> <type> "<msg>" --once # 发一条测试
ros2 bag record <topic> # 录包
ros2 bag play <bag> # 回放
```
### 3.8 本仓库对照
- 包:[`src/py_pubsub/`](../src/py_pubsub/), [`src/cpp_pubsub/`](../src/cpp_pubsub/)
- 跨语言互通验证:[`docker/bringup_e2e.log`](../docker/bringup_e2e.log)
**深度**: [`doc/20-topics.md`](20-topics.md)
---
## 4. Service(服务)
### 4.1 是什么
**同步、一对一、双向**的请求/响应。
- 一次性调用 + 等结果
- 几毫秒到几秒
### 4.2 通信模型
```
Client ──call(req)──> Server
◀──response────
同步(阻塞),一次一答
```
### 4.3 .srv 文件定义
```srv
# example_interfaces/srv/AddTwoInts.srv
int64 a # Request 字段
int64 b
---
int64 sum # Response 字段
```
- `---` 上 = Request
- `---` 下 = Response
### 4.4 Server 端
```python
from example_interfaces.srv import AddTwoInts
srv = node.create_service(
AddTwoInts, # 服务类型
'add_two_ints', # 服务名
callback # 回调签名:callback(req, resp) -> resp
)
def callback(request, response):
response.sum = request.a + request.b
return response # 必须 return response
```
### 4.5 Client 端(异步风格)
```python
client = node.create_client(AddTwoInts, 'add_two_ints')
while not client.wait_for_service(timeout_sec=1.0):
node.get_logger().info('waiting...')
req = AddTwoInts.Request()
req.a = 12; req.b = 30
future = client.call_async(req)
# 必须 spin 让 future 完成
rclpy.spin_until_future_complete(node, future, timeout_sec=5.0)
result = future.result()
print(result.sum) # 42
```
### 4.6 何时用
| 场景 | 用 Service |
|---|---|
| 拍照(几 ms) | ✅ |
| 关节角度查询 | ✅ |
| 计算查询 | ✅ |
| 周期性相机帧 | ❌ → Topic |
| 长任务(几秒到几分钟) | ❌ → Action |
### 4.7 常用 CLI
```bash
ros2 service list
ros2 service type <name> # 类型
ros2 service call <name> <type> "<req>"
# 例:
ros2 service call /add_two_ints example_interfaces/srv/AddTwoInts "{a: 12, b: 30}"
```
**深度**: [`doc/30-services.md`](30-services.md)
---
## 5. Action(动作)
### 5.1 是什么
Service 的"长任务"版本:
- client 发 Goal
- server 周期性推 Feedback
- 最终返回 Result
- **client 可中途 cancel**
适合:抓取、导航、SLAM。
### 5.2 通信模型
```
Client Server
│ Goal │
├────────────────────>│
│ ▼ 执行中
│ Feedback │
│<────────────────────┤
│ Feedback │
│<────────────────────┤
│ Result │
│<────────────────────┤
│ cancel (可选) │
├────────────────────>│
```
### 5.3 .action 文件定义
```action
# example_interfaces/action/Fibonacci.action
int32 order
---
int32[] sequence # Result
---
int32[] sequence # Feedback
```
| 段 | 含义 |
|---|---|
| 第一段 | Goal(client 发) |
| `---` | 分隔 |
| 第二段 | Result(server 最终给一次) |
| `---` | 分隔 |
| 第三段 | Feedback(server 周期性推 0..N 次) |
### 5.4 Server 端
```python
import rclpy.action
action_server = rclpy.action.ActionServer(
node,
Fibonacci, # ActionType
'fibonacci', # action 名
execute_callback # 签名:cb(goal_handle) -> result
)
def execute_callback(goal_handle):
order = goal_handle.request.order
feedback = Fibonacci.Feedback()
result = Fibonacci.Result()
sequence = [0, 1]
for i in range(1, order):
if goal_handle.is_cancel_requested:
goal_handle.canceled()
return Fibonacci.Result()
sequence.append(sequence[i] + sequence[i-1])
feedback.sequence = sequence
goal_handle.publish_feedback(feedback)
goal_handle.succeed()
result.sequence = sequence
return result
```
### 5.5 Client 端
```python
client = ActionClient(node, Fibonacci, 'fibonacci')
client.wait_for_server()
goal = Fibonacci.Goal()
goal.order = 6
future = client.send_goal_async(
goal,
feedback_callback=lambda msg: print('fb:', msg.feedback.sequence)
)
future.add_done_callback(cb_goal_response)
```
### 5.6 状态机
```
PENDING → ACCEPTED → EXECUTING → SUCCEEDED
ABORTED
CANCELED
```
### 5.7 MultiThreadedExecutor(关键!)
Action server 的 execute + publish_feedback 都在主线程。**用 MultiThreadedExecutor**
避免 Feedback 卡死:
```python
from rclpy.executors import MultiThreadedExecutor
executor = MultiThreadedExecutor(num_threads=4)
executor.add_node(node)
executor.spin()
```
### 5.8 何时用 vs Service
| 持续 | 用 |
|---|---|
| < 几秒 | Service |
| 几秒 ~ 几小时 | Action |
**深度**: [`doc/40-actions.md`](40-actions.md)
---
## 6. Parameter(参数)
### 6.1 是什么
节点的**运行时配置项**。声明后可以运行时改,不重编译。
### 6.2 声明 / 读取 / 写
```python
class Talker(Node):
def __init__(self):
super().__init__('talker_py')
# 声明参数 + 默认值
self.declare_parameter('period_ms', 500)
self.declare_parameter('topic', 'chatter')
# 读取
period = self.get_parameter('period_ms').value
topic = self.get_parameter('topic').value
def change_param(self, new_period):
# 运行时改
param = rclpy.parameter.Parameter(
'period_ms', rclpy.Parameter.Type.INTEGER, new_period
)
self.set_parameters([param])
```
### 6.3 CLI 改参数
```bash
ros2 param list # 节点的所有参数
ros2 param get /talker_py period_ms
ros2 param set /talker_py period_ms 200
ros2 param describe /talker_py period_ms
ros2 param dump /talker_py > params.yaml # 导出
ros2 param load /talker_py params.yaml # 加载
```
### 6.4 launch 中覆盖
```python
Node(
package='py_pubsub',
executable='talker',
parameters=[{'period_ms': 200, 'topic': 'chatter'}] # 覆盖
)
```
CLI 启动:
```bash
ros2 run py_pubsub talker --ros-args -p period_ms:=200 -p topic:=hello
```
### 6.5 YAML 文件
```yaml
# config/params.yaml
talker_py:
ros__parameters:
period_ms: 200
topic: chatter
```
```python
Node(package='py_pubsub', executable='talker',
parameters=[get_package_share_directory('my_pkg') + '/config/params.yaml'])
```
---
## 7. TF2(坐标变换)
### 7.1 是什么
管理**机器人所有坐标系**之间相对位姿的工具。维护一棵 **TF tree**。
### 7.2 典型 TF tree
```
world
└─ map (SLAM)
└─ odom (AMCL / 里程计)
└─ base_link (底盘)
├─ base_footprint
├─ laser_frame (激光雷达)
├─ camera_optical_frame (相机)
└─ arm_base (机械臂)
└─ shoulder (关节 1)
└─ upper_arm (关节 2)
└─ wrist (关节 3)
└─ gripper (夹爪)
```
### 7.3 C++ Listener API
```cpp
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
tf2_ros::Buffer buffer(node->get_clock());
tf2_ros::TransformListener listener(buffer, node);
// 查"gripper 在 base_link 下"
geometry_msgs::msg::TransformStamped tf = buffer.lookupTransform(
"base_link", // target frame
"gripper", // source frame
tf2::TimePointZero); // latest
RCLCPP_INFO(node->get_logger(),
"gripper in base_link: x=%.3f y=%.3f z=%.3f",
tf.transform.translation.x, ...);
```
### 7.4 几何变换(把点变换到其他系)
```cpp
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
geometry_msgs::msg::PointStamped p_in, p_out;
p_in.header.frame_id = "world";
p_in.point.x = 1.0; p_in.point.y = 2.0; p_in.point.z = 3.0;
p_out = buffer.transform(p_in, "robot");
// p_out 在 robot 系下
```
### 7.5 Python Listener
```python
from tf2_ros import Buffer, TransformListener
buffer = Buffer()
listener = TransformListener(buffer, node)
try:
tf = buffer.lookup_transform(
'base_link', 'gripper', rclpy.time.Time(),
timeout=rclpy.duration.Duration(seconds=1.0))
print(tf.transform.translation)
except Exception as e:
node.get_logger().warn(f'lookup failed: {e}')
```
### 7.6 静态 TF(不变的关系)
```bash
# 命令行:激光雷达固定在底盘
ros2 run tf2_ros static_transform_publisher \
--x 0.1 --y 0 --z 0.2 --roll 0 --pitch 0 --yaw 0 \
--frame-id base_link --child-frame-id laser_frame
```
```python
# launch 里
Node(package='tf2_ros', executable='static_transform_publisher',
arguments=['--x', '0.1', '--y', '0', '--z', '0.2',
'--frame-id', 'base_link', '--child-frame-id', 'laser_frame'])
```
### 7.7 动态 TF(URDF + JointState)
`robot_state_publisher` (ROS2 系统包):
- 输入:URDF + `/joint_states`
- 输出:动态 TF 到 `/tf`
```bash
sudo apt install ros-humble-robot-state-publisher
ros2 run robot_state_publisher robot_state_publisher \
--ros-args -p robot_description:="$(xacro arm.urdf)"
```
### 7.8 调试命令
```bash
ros2 run tf2_tools view_frames # 生成 frames.pdf
ros2 run tf2_ros tf2_echo base_link gripper # 实时打印
ros2 topic hz /tf # /tf 频率
ros2 topic info /tf_static -v # 静态 TF 列表
```
**深度**: [`doc/50-tf2.md`](50-tf2.md)
---
## 8. URDF(机器人模型)
### 8.1 是什么
XML 格式的机器人描述:link / joint / 视觉 / 碰撞 / 物理参数。
### 8.2 最小 URDF
```xml
<?xml version="1.0"?>
<robot name="my_arm">
<link name="base_link">
<visual><geometry><box size="0.1 0.1 0.1"/></geometry></visual>
<collision><geometry><box size="0.1 0.1 0.1"/></geometry></collision>
<inertial><mass value="1.0"/><inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/></inertial>
</link>
<joint name="joint1" type="revolute">
<parent link="base_link"/>
<child link="link1"/>
<origin xyz="0 0 0.05"/>
<axis xyz="0 0 1"/>
<limit lower="-3.14" upper="3.14" effort="1.0" velocity="1.0"/>
</joint>
<link name="link1">
<visual><geometry><box size="0.05 0.05 0.1"/></geometry></visual>
<collision><geometry><box size="0.05 0.05 0.1"/></geometry></collision>
<inertial><mass value="0.5"/><inertia ixx="0.005" ixy="0" ixz="0" iyy="0.005" iyz="0" izz="0.005"/></inertial>
</link>
</robot>
```
### 8.3 关节类型
| type | 含义 | DoF |
|---|---|---|
| `revolute` | 转动(有限位) | 1 |
| `continuous` | 转动(无限位) | 1 |
| `prismatic` | 滑动 | 1 |
| `fixed` | 固定 | 0 |
| `floating` | 6 DoF | 6 |
### 8.4 校验
```bash
sudo apt install liburdfdom-tools
check_urdf my_arm.urdf # 校验 URDF
urdf_to_graphiz my_arm.urdf # 生成 PDF/PNG 图
```
**深度**: [`doc/60-urdf.md`](60-urdf.md)
---
## 9. sensor_msgs(常用消息类型)
| 消息 | 关键字段 | 用途 |
|---|---|---|
| `std_msgs/String` | `string data` | 通用 |
| `std_msgs/Header` | `stamp`, `frame_id` | 时间戳 + 坐标系 |
| `sensor_msgs/Image` | `height/width/encoding/step/data` | 相机图像 |
| `sensor_msgs/CameraInfo` | `K/D/R/P` | 相机内参/外参 |
| `sensor_msgs/PointCloud2` | `height/width/data/is_bigendian` | 3D 点云 |
| `sensor_msgs/JointState` | `name[]/position[]/velocity[]/effort[]` | 关节状态 |
| `sensor_msgs/Imu` | `orientation/angular_velocity/linear_acceleration` | IMU |
| `geometry_msgs/Pose` | `Point position + Quaternion orientation` | 位姿 |
| `geometry_msgs/PoseStamped` | `Header + Pose` | 带时间戳位姿 |
| `geometry_msgs/Twist` | `Vector3 linear + Vector3 angular` | 速度指令 |
| `geometry_msgs/Transform` | `Vector3 translation + Quaternion rotation` | TF 变换 |
| `nav_msgs/Odometry` | `pose + twist + covariances` | 里程计 |
| `nav_msgs/Path` | `PoseStamped[]` | 路径 |
| `trajectory_msgs/JointTrajectory` | `JointTrajectoryPoint[]` | 关节轨迹(ros2_control 用) |
| `trajectory_msgs/JointTrajectoryPoint` | `positions[]/velocities[]/accelerations[]/effort[]` | 单点轨迹 |
| `vision_msgs/Detection2DArray` | `detections[]` | 目标检测结果 |
---
## 10. Launch(启动文件)
### 10.1 是什么
Python 脚本,描述"启动哪些节点 + 参数",取代 ROS1 XML。
### 10.2 最小 launch
```python
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='my_pkg',
executable='my_node',
name='my_node',
output='screen',
parameters=[{'param1': 'value1'}],
),
])
```
### 10.3 启动方式
```bash
ros2 launch <pkg> <launch_file.py>
ros2 launch <pkg> <launch_file.py> topic:=hello period_ms:=200
```
**深度**: [`doc/70-launch.md`](70-launch.md)
---
## 11. DDS / QoS / 时间(底层)
### 11.1 DDS 是什么
ROS2 默认用 **DDS**(Data Distribution Service)做底层通信。
本仓库默认 **FastDDS**(`rmw_fastrtps_cpp`)。
DDS 提供的核心能力:
- 自动节点发现(基于 UDP multicast)
- 多种 QoS(可靠 / 尽力而为)
- 实时性
### 11.2 RMW(ROS Middleware)
ROS2 用 RMW 抽象层,RMW 是 DDS 的 ROS 包装:
| RMW 实现 | 包 | 适用 |
|---|---|---|
| `rmw_fastrtps_cpp` | `ros-humble-rmw-fastrtps-cpp`(默认) | 通用 |
| `rmw_cyclonedds_cpp` | `ros-humble-rmw-cyclonedds-cpp` | 跨网段更稳 |
切换:
```bash
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
```
### 11.3 时间
| 概念 | 含义 | 用法 |
|---|---|---|
| `system time` | 墙钟时间(默认) | 仿真时需配 `use_sim_time: true` |
| `sim time` | 仿真时钟(`/clock` topic) | Gazebo / 录包回放 |
| `Header.stamp` | 消息时间戳 | TF 同步、消息时间关联 |
---
## 12. 6 大概念怎么一起工作
```
┌────────────────────────────────────────────────┐
│ application │
│ (决策 / 控制 / 感知 / 学习) │
└─────────┬──────────────────────────────────────┘
│ 用 TF / Topic / Service / Action / Parameter
┌─────────▼──────────────────────────────────────┐
│ ROS2 中间件 (rclcpp/rclpy) │
│ ┌────────────────────────────────────────┐ │
│ │ Topic Pub/Sub (异步多对多单向) │ │
│ │ Service Req/Resp (同步一对一双向) │ │
│ │ Action Goal/Feedback/Result (长任务) │ │
│ │ Param 配置 │ │
│ │ TF2 坐标变换树 │ │
│ └────────────────────────────────────────┘ │
│ ┌────────────────────────────────────────┐ │
│ │ DDS (默认 fastdds) │ │
│ └────────────────────────────────────────┘ │
└────────────────────────────────────────────────┘
│ 走 UDP multicast / unicast
┌─────────▼──────────────────────────────────────┐
│ 网络 │
└─────────────────────────────────────────────────┘
```
---
## 13. 概念速查表
| 概念 | 一句话 | API | 深度文档 |
|---|---|---|---|
| Node | 一个进程 = 一项业务 | `rclpy.spin(node)` | 本篇 §2 |
| Topic | 异步多对多单向 | `create_publisher / create_subscription` | [`20-topics`](20-topics.md) |
| Service | 同步 1对1 双向 | `create_service / create_client` | [`30-services`](30-services.md) |
| Action | 长任务 + 反馈 + 可取消 | `ActionServer / ActionClient` | [`40-actions`](40-actions.md) |
| Parameter | 运行时配置 | `declare_parameter / set_parameter` | 本篇 §6 |
| TF2 | 坐标系树 | `Buffer.lookup_transform` | [`50-tf2`](50-tf2.md) |
| URDF | 机器人 XML 描述 | `check_urdf` | [`60-urdf`](60-urdf.md) |
| Launch | 启动脚本 | `ros2 launch <pkg> <file>` | [`70-launch`](70-launch.md) |
| DDS | 通信中间件 | `RMW_IMPLEMENTATION=` | 本篇 §11 |
---
## 接下来读
按主题深读:
| 你想深挖什么 | 看 |
|---|---|
| Topic 实现细节、QoS、跨语言互通 | [`20-topics.md`](20-topics.md) |
| Service 实现、req/resp 时序 | [`30-services.md`](30-services.md) |
| Action 三件套、cancel、MultiThreadedExecutor | [`40-actions.md`](40-actions.md) |
| TF tree / lookup / static_transform | [`50-tf2.md`](50-tf2.md) |
| URDF / xacro / robot_state_publisher | [`60-urdf.md`](60-urdf.md) |
| launch 嵌套、参数覆盖、事件 | [`70-launch.md`](70-launch.md) |
| 三机部署(PC + RDK X5 + RK3506) | [`100-embedded-deployment.md`](100-embedded-deployment.md) |
| 进入具身智能 / VLA / 机器人 | [`99-embodied-ai.md`](99-embodied-ai.md) |
doc/100-embedded-deployment.md
+918
View File
@@ -0,0 +1,918 @@
# 100 · 嵌入式部署:RDK X5 + RK3506 + ROS2(完全实操指南)
> **本篇目标**:从"硬件到手"到"三机 ROS2 互通、RK3506 控制真实电机"全流程跑通。
> 假设你手上有:1 台 PC + 1 台 RDK X5 + 2 台 RK3506。本指南按 **step-by-step** 走,每步给出**预期输出 + 排错**。
---
## 目录
- [第 0 章: 准备与检查清单](#0-准备与检查清单)
- [第 1 章: 三层架构详解](#1-三层架构详解)
- [第 2 章: PC 主控完整配置](#2-pc-主控完整配置)
- [第 3 章: RDK X5 边缘 AI 配置](#3-rdk-x5-边缘-ai-配置)
- [第 4 章: RK3506 精简 ROS2 配置(核心)](#4-rk3506-精简-ros2-配置核心)
- [第 5 章: 三机 LAN DDS 组网](#5-三机-lan-dds-组网)
- [第 6 章: 跨机 Demo 联调](#6-跨机-demo-联调)
- [第 7 章: RK3506 接真实电机 / 编码器](#7-rk3506-接真实电机--编码器)
- [第 8 章: ros2_control 闭环](#8-ros2_control-闭环)
- [第 9 章: 故障排查大全](#9-故障排查大全)
- [第 10 章: 性能调优 Checklist](#10-性能调优-checklist)
- [附录 A: 采购清单](#附录-a-采购清单)
- [附录 B: 关键命令速查](#附录-b-关键命令速查)
---
## 0. 准备与检查清单
### 0.1 你需要的硬件
- [ ] PC 一台(Ubuntu 22.04 / Windows + Docker)
- [ ] RDK X5 一台(已烧 Ubuntu 22.04 官方镜像)
- [ ] RK3506 两台(已烧 Linux 镜像,默认账户 `root` 或 `user`)
- [ ] 网线 + 路由器 / 交换机(三个设备同网段)
- [ ] RK3506 ↔ PC / RDK X5 的 USB-TTL 串口线(debug 用)
### 0.2 网络规划(提前固定)
```
PC 192.168.1.10
RDK X5 192.168.1.20
RK3506-1 192.168.1.31
RK3506-2 192.168.1.32
子网掩码 255.255.255.0
ROS_DOMAIN_ID = 0 (三层共用)
```
> 💡 **强烈建议固定 IP**(路由器 DHCP 静态分配 或 各板 `/etc/network/interfaces` 写死),避免重启后地址变了连不上。
### 0.3 先确认每台设备能 SSH / 访问
```bash
# PC
ssh user@192.168.1.10 # 应该能登
# RDK X5
ssh user@192.168.1.20
# RK3506
ssh user@192.168.1.31
ssh user@192.168.1.32
```
每台板子**先 ping 互通**:
```bash
ping -c 3 192.168.1.20 # 在 PC 上 ping RDK X5
ping -c 3 192.168.1.31 # 在 PC 上 ping RK3506 #1
```
**预期**: `0% packet loss`。
**若不通**: 检查网线、IP、子网掩码、路由器是否拦 IGMP。
---
## 1. 三层架构详解
### 1.1 为什么分层?
| 层 | 设备 | 跑什么 | 不跑什么 | 为什么 |
|---|---|---|---|---|
| **PC (主控)** | x86 8GB+ | MoveIt2 / Nav2 / RViz / 仿真 / 编译 | 真电机驱动 | 算力大但不便实时控制硬件 |
| **RDK X5 (边缘 AI)** | ARM + 5 TOPS NPU | YOLO / SAM / Whisper / SLAM | MoveIt2 大规划 | NPU 加速推理,分担 PC 算力 |
| **RK3506 (实时控制)** | ARM 3 核 512MB | JointState / PID 闭环 / 编码器读取 | AI 推理 / RViz | 资源紧,但本地控制实时性好 |
### 1.2 数据流(典型抓取场景)
```
┌─────────────────────────────────────────┐
│ PC │
│ RViz 可视化 + 决策 + MoveIt2 规划 │
└─────┬───────────────────────┬───────────┘
│ /grasp_pose │ /joint_trajectory
│ (DDS) │ (DDS)
┌───────────────────────▼─────────┐ ┌─────────▼───────────┐
│ RDK X5 │ │ RK3506 #1 │
│ vision_node: │ │ joint_state_publisher│
│ YOLO 检测 → publish /grasp │ │ 编码器 → /joint_states│
│ (可选) VLA 推理(NPU 加速) │ │ 订阅 /cmd_vel 控制电机│
└─────────────────────────────────┘ └─────────────────────┘
┌─────────────────────┐
│ RK3506 #2 │
│ NPU 推理 / 备用控制│
│ (YOLO-Lite / SAM) │
└─────────────────────┘
```
### 1.3 通信方式(三选一)
| 方式 | 优点 | 缺点 | 用法 |
|---|---|---|---|
| **Multicast 默认** | 零配置,自动发现 | 路由器/防火墙可能拦 | 同网段 + 默认 |
| **Unicast discovery server** | 跨网段可用 | 要起 discovery server | 多 VLAN / 跨子网 |
| **Unicast `ROS_STATIC_PEERS`** | 简单、稳 | 节点列表要预先列 | 小规模 LAN |
**推荐先试 multicast,不通再切 unicast**。下面 Step 5 详解。
---
## 2. PC 主控完整配置
### 2.1 你的工作环境(选项 A: Linux)
假设你用 **Ubuntu 22.04** 真机(不是 Docker)。
```bash
# 2.1.1 装 ROS2 Humble
sudo apt install software-properties-common
sudo add-apt-repository universe
sudo apt install curl -y
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \
-o /usr/share/keyrings/ros-archive-keyring.gpg
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
sudo apt update
sudo apt install ros-humble-desktop python3-colcon-common-extensions -y
# 验证
source /opt/ros/humble/setup.bash
ros2 --help # 应该输出 ros2 命令帮助
```
### 2.2 你的工作环境(选项 B: Windows + Docker)——本仓库主路径
如果你用的是本仓库的 Docker 工作流,环境已就绪:
```powershell
docker exec -it ros2_dev bash -lc "source /opt/ros/humble/setup.bash && source /root/ros2_ws/install/setup.bash && exec bash"
```
后续章节都假设你**在 Docker 容器内**操作。**PC 的"ROS_DOMAIN_ID=0"和"DDS 自动发现"对 LAN 上其他设备是开放的**——因为 docker-compose 用 `network_mode: host`,容器用宿主机网络栈。
### 2.3 PC 上启动本仓库的 bringup 全套
```bash
docker exec ros2_dev bash -lc "cd /root/ros2_ws && source install/setup.bash && ros2 launch bringup full_demo_launch.py"
```
预期输出(日志截取):
```
[INFO] [launch]: All log files can be found below /root/.ros/log/2026-08-03-...
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [talker-1]: process started with pid [56]
[INFO] [listener-2]: process started with pid [58]
[INFO] [talker-3]: process started with pid [60]
[INFO] [listener-4]: process started with pid [62]
[talker-3] [INFO] [...] talker_cpp started -> topic=chatter, period=500ms
[listener-4] [INFO] [...] listener_cpp subscribed <- chatter
```
11 个节点跑起来。如果 LAN 上有 RDK X5 / RK3506 节点,它们会自动被发现。
### 2.4 PC 端跨机网络验证
```bash
docker exec ros2_dev bash -lc "source /opt/ros/humble/setup.bash && ros2 node list --no-daemon"
```
预期(假设 RDK X5 还没装,只有 PC 自己):
```
/joint_state_publisher_cpp
/talker_py
/talker_cpp
/listener_py
/listener_cpp
/tf2_listener_cpp
/fake_camera_py
/image_processor_py
/fibonacci_action_server_py
/add_two_ints_server_py
```
如果 RDK X5 装好 ROS2 后,**它的节点也会出现在这里**。
---
## 3. RDK X5 边缘 AI 配置
### 3.1 准备工作
```bash
# SSH 到 RDK X5
ssh user@192.168.1.20
# 检查系统
uname -a
# Linux rdk-x5 5.10.xxx ...
cat /etc/os-release
# Ubuntu 22.04.4 LTS ... (地瓜官方镜像)
# 网络
ip addr show | grep inet
# 应该看到 192.168.1.20/24
```
### 3.2 装 ROS2(完整版)
```bash
# 同 PC 步骤,装桌面版(包含 RViz 远程)
sudo apt install ros-humble-desktop -y
# 加装本仓库相关包
sudo apt install -y \
ros-humble-cv-bridge \
ros-humble-tf2-ros \
ros-humble-tf2-tools \
ros-humble-ros-gz \
ros-humble-nav2-bringup \
ros-humble-moveit \
ros-humble-image-transport \
ros-humble-vision-msgs \
ros-humble-sensor-msgs \
ros-humble-geometry-msgs \
ros-humble-rmw-fastrtps-cpp \
python3-colcon-common-extensions
```
预计下载 ~500MB,装 5-10 分钟。
### 3.3 拷本仓库源码 + colcon build
```bash
# 从 PC 拷(在 PC 上)
scp -r D:\xs\ros2 user@192.168.1.20:~/ros2_ws
# 注意:D:\xs\ros2 拷到 RDK X5 后变成 ~/ros2_ws
# 在 RDK X5 上(SSH 进)
cd ~/ros2_ws
source /opt/ros/humble/setup.bash
# ARM 编译,首次 5-10 分钟
colcon build --symlink-install \
--packages-select py_pubsub cpp_pubsub py_srv py_action_demo cpp_robot_tf2 py_vision_demo bringup
# 预期输出(末尾)
Summary: 7 packages finished [8m 32s]
```
> ⚠️ **arm64 兼容性**: 全部 7 个包都用纯 Python + 标准 C++,**没有架构专属代码**,ARM 上 build 应该一次过。如果遇到 `Could NOT find X`,检查 apt 包名。
### 3.4 RDK X5 上启动 vision demo
```bash
# SSH 到 RDK X5
source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash
# 设 ROS_DOMAIN_ID(虽然默认是 0,但显式更稳)
export ROS_DOMAIN_ID=0
# 启动 fake_camera (sensor_msgs/Image 发布者)
ros2 run py_vision_demo fake_camera
# 预期
[INFO] [fake_camera_py]: fake_camera_py started: 320x240 @ 10fps -> /image_raw
```
### 3.5 PC 上验证 RDK X5 节点可见
```bash
# 在 PC / 容器内
docker exec ros2_dev bash -lc "source /opt/ros/humble/setup.bash && ros2 node list --no-daemon"
# 应该看到:
# /fake_camera_py (来自 RDK X5)
# /talker_py /listener_py ... (本地)
# 看 RDK X5 上 fake_camera 发的 /image_raw 频率
ros2 topic hz /image_raw --no-daemon
# 预期:average rate: 10.000
```
### 3.6 RDK X5 跑 YOLO 推理(扩展)
如果 RDK X5 装了 ultralytics + YOLO 模型:
```bash
# 安装
pip install ultralytics --break-system-packages # 如果是 Buildroot 系统不带 pip,先 apt install python3-pip
# 准备模型
# 把 yolov8n.pt 拷到 ~/models/
# 写一个 yolo_detector 节点(参考 py_vision_demo 写法):
# 订阅 /camera/color/image_raw (或 fake_camera 的 /image_raw)
# 推理 → publish /detections (vision_msgs/Detection2DArray)
```
这部分**需要你写新包**,参考 [`src/py_vision_demo/`](../src/py_vision_demo/)。
---
## 4. RK3506 精简 ROS2 配置(核心)
### 4.1 为什么 RK3506 要"精简"
| 配置 | 占用 RAM |
|---|---|
| Linux 基础系统 | ~80 MB |
| ROS2 rclpy 客户端 | ~50-80 MB |
| FastDDS 参与者(单节点) | ~30-50 MB |
| 默认 daemon (`ros2 daemon`) | ~50 MB ← **必须关** |
| RViz / rqt | ~300 MB ← **绝对不装** |
| MoveIt2 / Nav2 | 200-500 MB ← **不装** |
不精简 = 直接 OOM。
### 4.2 准备 RK3506
```bash
ssh user@192.168.1.31
uname -a
# Linux rk3506 5.10.xxx ...
free -h
# total used free shared buff/cache available
# Mem: 484Mi 95Mi 240Mi 1.0Mi 148Mi 380Mi
df -h /
# /dev/root 7.4G 1.2G 5.8G /
# 看是不是 aarch64(必须)
uname -m
# aarch64
```
### 4.3 装精简 ROS2
```bash
# 4.3.1 装 ROS2 apt 源(同 PC)
sudo apt install software-properties-common
sudo add-apt-repository universe
sudo apt install curl -y
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \
-o /usr/share/keyrings/ros-archive-keyring.gpg
echo "deb [arch=arm64 signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
sudo apt update
# 4.3.2 装精简 ROS2 包(注意:没有 desktop)
sudo apt install -y \
ros-humble-ros-base \
ros-humble-cv-bridge \
ros-humble-tf2-ros \
ros-humble-sensor-msgs \
ros-humble-geometry-msgs \
ros-humble-rmw-fastrtps-cpp \
ros-humble-ros2control \
python3-colcon-common-extensions
# 检查内存
free -h
# 装完后大约用掉 ~250-280 MB(系统+ROS2+daemon)
```
### 4.4 关闭 ROS2 daemon(省 ~50MB)
ROS2 默认启动 `ros2 daemon` 加速 `ros2 ...` 命令调用,但每节点起一个 daemon 浪费内存。
```bash
# 停 + 禁启 daemon
pkill -9 -f ros2_daemon 2>/dev/null
echo 'unset ROS_DAEMON_PYTHON_OR_EXECUTABLE' >> ~/.bashrc
# 或 alias,直接绕过 daemon
cat >> ~/.bashrc <<'EOF'
alias ros2='ros2 --no-daemon'
EOF
source ~/.bashrc
# 验证: 不再起 daemon
ps aux | grep -v grep | grep -i daemon | head -3
# 应该空
```
### 4.5 加 FastDDS 单播发现配置
RK3506 默认走 multicast,但某些网络环境 multicast 不通(如路由器拦了)。**改用 unicast 显式指定 PC + RDK X5** 更稳。
```bash
mkdir -p ~/.ros
cat > ~/.ros/fastdds.xml <<'EOF'
<?xml version="1.0" encoding="UTF-8" ?>
<profiles xmlns="http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles">
<transport_descriptors>
<transport_descriptor>
<transport_id>udp_transport</transport_id>
<type>UDPv4</type>
</transport_descriptor>
</transport_descriptors>
<participant profile_name="rk3506_participant" is_default_profile="true">
<rtps>
<builtin>
<initialPeersList>
<initialPeer>
<address>192.168.1.10</address>
<port>7400</port>
</initialPeer>
<initialPeer>
<address>192.168.1.20</address>
<port>7400</port>
</initialPeer>
</initialPeersList>
<discovery_config>
<discoveryProtocol>SIMPLE</discoveryProtocol>
<discoveryStrategy>STATIC</discoveryStrategy>
<leaseDuration>30</leaseDuration>
</discovery_config>
<metatrafficUnicastLocatorList>
<locator>
<udpv4>
<address>0.0.0.0</address>
<port>7400</port>
</udpv4>
</locator>
</metatrafficUnicastLocatorList>
</builtin>
</rtps>
</participant>
</profiles>
EOF
# 加进 .bashrc
cat >> ~/.bashrc <<'EOF'
export ROS_DOMAIN_ID=0
export ROS_LOCALHOST_ONLY=0
export RMW_IMPLEMENTATION=rmw_fastrtps_cpp
export FASTRTPS_DEFAULT_PROFILES_FILE=~/.ros/fastdds.xml
EOF
source ~/.bashrc
```
> 💡 `discoveryStrategy=STATIC` 让 RK3506 只找列表里的两个 IP,**不会到处广播**,省网络+省电。
### 4.6 把本仓库拷到 RK3506 + build
```bash
# 在 PC 上(假设在 D:\xs\ros2)
scp -r D:\xs\ros2 user@192.168.1.31:~/ros2_ws
# SSH 到 RK3506
ssh user@192.168.1.31
cd ~/ros2_ws
source /opt/ros/humble/setup.bash
# 编译(ARM,首次 5-10 分钟)
colcon build --symlink-install \
--packages-select py_pubsub cpp_pubsub cpp_robot_tf2 bringup
# 预期
Summary: 4 packages finished [6m 12s]
```
> ⚠️ **如果 RK3506 资源太紧编译失败**,只 build 1-2 个必要的包:
> ```bash
> colcon build --packages-select cpp_robot_tf2
> ```
### 4.7 RK3506 上启动 joint_state_publisher
```bash
source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash
export ROS_DOMAIN_ID=0
# 跑 cpp_robot_tf2 的关节发布者
ros2 run cpp_robot_tf2 joint_state_publisher
# 预期
[joint_state_publisher_cpp]: joint_state_publisher_cpp started, joints: 3
```
> 注: 这是**仿真关节**(3 关节 sin 运动),真实电机时改成读编码器(见 [第 7 章](#7-rk3506-接真实电机--编码器))。
### 4.8 PC 上验证 RK3506 节点可见
```bash
# 在 PC / Docker 内
docker exec ros2_dev bash -lc "source /opt/ros/humble/setup.bash && ros2 node list --no-daemon"
# 应该看到:
# /joint_state_publisher_cpp ← 来自 RK3506 (192.168.1.31)
# /talker_py /listener_py ... ← 本地 PC 节点
```
**看不到时**: 看下面 [第 9 章 故障排查](#9-故障排查大全)。
---
## 5. 三机 LAN DDS 组网
### 5.1 验证 multicast 通(默认方式)
```bash
# 在 RK3506 上
sudo tcpdump -i eth0 -n udp port 7400 -c 20
# 应该看到来自 PC (192.168.1.10) 和 RDK X5 (192.168.1.20) 的 UDP 包
# Ctrl+C 停止
```
**如果 multicast 不通**: 走 [4.5 单播配置](#45-加-fastdds-单播发现配置)。
### 5.2 三机互相"看见"节点
| 操作 | PC | RDK X5 | RK3506 #1 |
|---|---|---|---|
| `ros2 node list --no-daemon` | 见 PC + RDK + RK3506 节点 | 见 PC + RDK + RK3506 节点 | 见 PC + RDK + RK3506 节点 |
**预期**: 三机每台的 `node list` 输出**完全一致**(都看到 3 台的节点)。
### 5.3 跨机 topic 频率
```bash
# 在 RK3506 上看 /chatter 频率(由 PC 的 talker 发布)
ros2 topic hz /chatter --no-daemon
# average rate: 4.000 (4Hz = 2 talker × 2Hz)
```
**延迟**: 跨机 topic echo 一般 < 5ms(LAN);> 50ms 说明网络出问题。
---
## 6. 跨机 Demo 联调
### 6.1 场景 A:PC 视觉 → RK3506 控制(简化版)
```bash
# === 在 PC (Docker) ===
docker exec ros2_dev bash -lc "cd /root/ros2_ws && source install/setup.bash && ros2 run py_vision_demo fake_camera"
# === 在 RK3506 ===
source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash
ros2 topic echo /image_raw --no-daemon # 看到 PC 发的图像
# === 在 PC ===
docker exec ros2_dev bash -lc "source /opt/ros/humble/setup.bash && ros2 run py_vision_demo image_processor"
# 看到 PC 自己处理 + (如果有 RDK X5 / RK3506 也跑 image_processor,都能收到)
```
### 6.2 场景 B:RDK X5 视觉 + RK3506 电机(推荐起手)
```bash
# === 在 RDK X5 (SSH) ===
ssh user@192.168.1.20
source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash
ros2 run py_vision_demo fake_camera # 或你写的 YOLO 节点
# === 在 RK3506 #1 (SSH) ===
ssh user@192.168.1.31
source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash
ros2 run cpp_robot_tf2 joint_state_publisher
# (后续:接真实电机时改读编码器)
# === 在 PC 上监控全部 ===
docker exec ros2_dev bash -lc "ros2 node list --no-daemon"
# 看到 /fake_camera_py (RDK X5), /joint_state_publisher_cpp (RK3506)
docker exec ros2_dev bash -lc "ros2 topic hz /image_raw /joint_states --no-daemon"
# /image_raw: ~10Hz
# /joint_states: ~20Hz
```
### 6.3 场景 C:三机跑本仓库 full_demo
```bash
# === PC ===
docker exec ros2_dev bash -lc "cd /root/ros2_ws && source install/setup.bash && ros2 launch bringup full_demo_launch.py"
# 11 个 PC 节点
# === RDK X5 ===
ssh user@192.168.1.20
ros2 run py_vision_demo fake_camera # 1 节点
# === RK3506 #1 ===
ssh user@192.168.1.31
ros2 run cpp_robot_tf2 joint_state_publisher # 1 节点
# === RK3506 #2 ===
ssh user@192.168.1.32
ros2 run cpp_robot_tf2 tf2_listener # 1 节点
# === 在任一设备看 ===
ros2 node list --no-daemon
# 看到 PC 的 11 + RDK X5 的 1 + RK3506 的 2 = 14 节点!
```
---
## 7. RK3506 接真实电机 / 编码器
### 7.1 硬件接线(典型 6-DoF 机械臂)
```
总控舵机 (例如 LewanSoul/Hiwonder LX-16):
- 1 根总线(Bus Servo): 6 个舵机串联,数据线 Tx/Rx 共用
- 电源 7-12V(单独供电,不要从 RK3506 取)
- 接线到 RK3506 串口(UART): TXD/RXD/GND
编码器(可选,闭环舵机内部已有):
- SPI 接口(Magnetic encoder AS5048A)
- 或通过总线舵机协议反馈
RK3506:
- /dev/ttyS0 / /dev/ttyS1 (硬件串口)
- /dev/ttyUSB0 (USB-TTL 调试线)
```
### 7.2 写一个电机驱动节点(C++)
参考本仓库 `cpp_robot_tf2/src/joint_state_publisher.cpp`(模板)。新建 `motor_driver` 包:
```cpp
// ~/ros2_ws/src/motor_driver/src/motor_driver.cpp
#include <chrono>
#include <cstdio>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/joint_state.hpp"
class MotorDriver : public rclcpp::Node {
public:
MotorDriver() : rclcpp::Node("motor_driver") {
pub_ = this->create_publisher<sensor_msgs::msg::JointState>("/joint_states", 10);
// 打开串口(假设 /dev/ttyS0 是总线舵机)
fd_ = open("/dev/ttyS0", O_RDWR | O_NOCTTY | O_NDELAY);
if (fd_ < 0) {
RCLCPP_FATAL(this->get_logger(), "Cannot open /dev/ttyS0");
return;
}
struct termios opts;
tcgetattr(fd_, &opts);
cfsetispeed(&opts, B115200);
cfsetospeed(&opts, B115200);
opts.c_cflag |= (CLOCAL | CREAD);
opts.c_cflag &= ~PARENB;
opts.c_cflag &= ~CSTOPB;
opts.c_cflag &= ~CSIZE;
opts.c_cflag |= CS8;
tcsetattr(fd_, TCSANOW, &opts);
timer_ = this->create_wall_timer(50ms, std::bind(&MotorDriver::tick, this));
// 订阅 cmd
sub_ = this->create_subscription<sensor_msgs::msg::JointState>(
"/cmd_joint_states", 10,
[this](const sensor_msgs::msg::JointState::SharedPtr msg) {
// 发送舵机命令(总线舵机协议)
send_servo_cmd(msg);
});
}
private:
void tick() {
// 1) 读舵机当前角度(总线舵机协议 INSTRUCTION 0x02 read position)
// 2) 构造 JointState 消息
auto msg = sensor_msgs::msg::JointState();
msg.header.stamp = this->now();
msg.name = {"joint1", "joint2", "joint3", "joint4", "joint5", "joint6"};
msg.position.resize(6);
// 读 6 个舵机当前角(伪代码)
for (int i = 0; i < 6; i++) msg.position[i] = read_servo_angle(i);
pub_->publish(msg);
}
void send_servo_cmd(const sensor_msgs::msg::JointState::SharedPtr& msg) {
// 总线舵机协议: 0x55 0x55 len id cmd param...
// 这里写 6 个舵机的目标角度
for (size_t i = 0; i < msg->position.size() && i < 6; i++) {
uint8_t id = i + 1; // 舵机 ID 1-6
uint16_t angle = static_cast<uint16_t>((msg->position[i] + 3.14159) / 6.28318 * 1000);
uint8_t buf[8] = {0x55, 0x55, 0x08, id, 0x03, 0x1E, (uint8_t)(angle & 0xFF), (uint8_t)(angle >> 8)};
write(fd_, buf, 8);
}
}
double read_servo_angle(int id) {
// 实际读位置:发送读指令 → 等待响应 → 解析
return 0.0; // 简化
}
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr pub_;
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr sub_;
rclcpp::TimerBase::SharedPtr timer_;
int fd_ = -1;
};
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<MotorDriver>());
rclcpp::shutdown();
return 0;
}
```
### 7.3 上电测试(务必小心!)
```
1. 舵机先**单独供电**,不上机械臂(用编程器/数据线模式调零)
2. RK3506 接 USB-TTL 调试线,登录 ssh
3. 启动 motor_driver 节点(默认发布 0° 关节角)
4. 手动旋转舵机,看 /joint_states 是否变化
5. 测试 `/cmd_joint_states` 订阅(在 PC 上 ros2 topic pub)
ros2 topic pub /cmd_joint_states sensor_msgs/msg/JointState "{name: ['j1'], position: [1.57]}"
6. 确认舵机收到命令并转动
7. 装上机械臂,运行 MoveIt2 demo
```
---
## 8. ros2_control 闭环
### 8.1 ros2_control 是什么
`ros2_control` 是电机驱动的**抽象层**。你写一个 `HardwareInterface`,RK3506 的真驱动接进去,
高层节点(PC 的 MoveIt2)只看到标准接口(`position_cmd` / `position_state`),不关心底层是舵机 / 步进电机 / 谐波减速。
```
┌──────────────┐ ┌──────────────────────────┐ ┌─────────────┐
│ MoveIt2 │ ──→ │ ros2_control │ ──→ │ 真电机驱动 │
│ /joint_ │ std │ ControllerManager │ │ (Bus Servo)│
│ trajectory │ if │ (JointTrajectoryController)│ │ │
└──────────────┘ └──────────────────────────┘ └─────────────┘
```
### 8.2 在 RK3506 上跑 controller_manager
```bash
sudo apt install ros-humble-ros2-control ros-humble-ros2-controllers -y
```
写一个 `motor_hw_interface` 包(本仓库后续可加),声明 6 个关节的 hardware interface。
### 8.3 PC 上跑 MoveIt2(规划)
```bash
sudo apt install ros-humble-moveit -y
# 启动 MoveIt(配 6-DoF SRDF)
ros2 launch moveit2_tutorials demo.launch.py
```
PC 算轨迹 → 通过 `FollowJointTrajectory` Action 发给 RK3506 → RK3506 controller_manager 执行。
---
## 9. 故障排查大全
### 9.1 看不到其他机器的节点
```bash
# 步骤 1: 网络通?
ping -c 3 192.168.1.31
# 步骤 2: 端口 7400 通?
nc -zv 192.168.1.31 7400
# 步骤 3: multicast 通?
tcpdump -i eth0 -n udp port 7400 -c 5
# 步骤 4: ROS_DOMAIN_ID 一致?
echo $ROS_DOMAIN_ID # 三机都要一样
```
### 9.2 multicast 不通
切单播 (4.5 节):
```bash
export ROS_STATIC_PEERS="192.168.1.10:7400;192.168.1.20:7400"
```
### 9.3 RK3506 内存不足
```bash
free -h
# 看哪些进程吃内存
ps aux --sort=-%mem | head -10
# 杀掉
pkill -9 -f ros2_daemon
pkill -9 -f ros2 # 注意:会杀掉所有 ROS2 进程,先确认其他节点不跑
```
### 9.4 编译失败
```bash
# 列报错包名,查 apt 是否装齐
dpkg -l | grep ros-humble-rclcpp
# 装缺失依赖
sudo rosdep init && rosdep update
sudo rosdep install -i --from-paths src/
# 清理重 build
cd ~/ros2_ws && rm -rf build install log
colcon build --symlink-install
```
### 9.5 节点启动后立刻死
```bash
ros2 run <pkg> <node> --log-level debug
# 看具体 traceback
```
常见原因:
- 参数缺失(`use_sim_time` 未设)
- 缺少文件(URDF 路径错)
- 网络端口冲突
---
## 10. 性能调优 Checklist
| 项 | 目标 | 命令 |
|---|---|---|
| RAM 占用 | < 350MB | `free -h` |
| CPU 空闲 | > 60% | `top -bn1 \| head -20` |
| TF lookup 延迟 | < 5ms | `ros2 topic delay /tf --no-daemon` |
| DDS heartbeat | < 100ms | `ros2 daemon stop; ros2 --no-daemon ...` |
| 电机响应延迟 | < 50ms | 录 topic 时间戳,看从 PC 发出到 RK3506 转动 |
---
## 附录 A: 采购清单
| 项 | 数量 | 备注 |
|---|---|---|
| RDK X5 | 1 | 地瓜机器人官方 |
| RK3506 开发板 | 2 | 注意要 aarch64 Linux 镜像 |
| 6-DoF 机械臂套件 | 1 | 推荐 LewanSoul/Hiwonder 铝架 |
| 总线舵机(6 个) | 1 套 | 型号 LX-16 / LX-224 |
| 舵机电源(7-12V 5A+) | 1 | **必须独立供电** |
| USB-TTL 串口线 | 2 | RK3506 debug 用 |
| 网线 + 路由器 | 1 套 | 三机同网段 |
| 急停按钮 | 1 | 接 RK3506 GPIO |
---
## 附录 B: 关键命令速查
```bash
# 三机通用
source /opt/ros/humble/setup.bash
ros2 node list --no-daemon
ros2 topic list --no-daemon
ros2 topic hz /topic --no-daemon
ros2 topic echo /topic --no-daemon
# RK3506 内存
free -h
ps aux --sort=-%mem | head
# 网络
ip addr show
ping -c 3 <ip>
nc -zv <ip> 7400
# DDS 配置
cat ~/.ros/fastdds.xml
echo $ROS_STATIC_PEERS
# colcon build
cd ~/ros2_ws && colcon build --packages-select <pkg>
cd ~/ros2_ws && rm -rf build install log && colcon build
# ros2_control
ros2 control list_controllers
ros2 control list_hardware_interfaces
# MoveIt2
ros2 launch moveit2_tutorials demo.launch.py
```
---
## 下一步建议
读完本篇你能做到:
✅ 三机 ROS2 互通
✅ RK3506 接真实电机
✅ PC + RDK X5 + RK3506 完整工作流
**接着学习**:
- ros2_control 完整配置(`doc/99-embodied-ai.md` 路径)
- MoveIt2 SRDF 生成 + 路径规划
- Gazebo 物理仿真验证(在 PC 上)
- NPU 推理在 RDK X5 / RK3506 #2 上跑(YOLO-Lite)
如需在本仓库加 `ros2_control_demo` / `motor_driver` 包,后续 PR 即可。
doc/20-topics.md
+635
View File
@@ -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) |
doc/30-services.md
+510
View File
@@ -0,0 +1,510 @@
# 30 · Service 深度:同步 req/resp(完全指南)
> **目标**:吃透 ROS2 Service,涵盖 .srv 定义、Server/Client API、Future、wait_for_service、调试,学完能写工业级 Service。
---
## 目录
- [1. 通信模型](#1-通信模型)
- [2. .srv 文件定义](#2-srv-文件定义)
- [3. Server 端(Python / C++)](#3-server-端python--c)
- [4. Client 端(Python / C++)](#4-client-端python--c)
- [5. 同步 vs 异步调用](#5-同步-vs-异步调用)
- [6. wait_for_service 详解](#6-wait_for_service-详解)
- [7. QoS + Service 名约定](#7-qos--service-名约定)
- [8. 实战:写一个拍照 + 计算服务](#8-实战写一个拍照--计算服务)
- [9. 调试命令](#9-调试命令)
- [10. 常见坑](#10-常见坑)
- [11. 在本仓库里跑](#11-在本仓库里跑)
- [12. 进阶:异步服务端 + 回调式 client](#12-进阶异步服务端--回调式-client)
---
## 1. 通信模型
### 1.1 一句话
Service 是 **同步、一对一、双向**的请求/响应。
- 一次性调用 + 等结果
- 几毫秒到几秒
```
Client ──call(req)──> Service Server
◀──response────
─────────────────
同步(阻塞),一次一答
```
### 1.2 vs Topic / Action
| 维度 | Service | Topic | Action |
|---|---|---|---|
| 同步性 | **同步(等响应)** | 异步 | 异步(long) |
| 方向 | **双向** req/resp | 单向 pub→sub | 双向 goal/fb/result |
| 1对多 | ❌ 一对一 | ✅ | ❌ |
| 反馈进度 | ❌ | ❌ | ✅ |
| 可取消 | ❌ | N/A | ✅ |
### 1.3 何时用
✅ **适合**:
- 拍照(几 ms)
- 关节角度查询
- 短计算(几十 ms)
- 开关 / 触发动作
❌ **不适合**:
- 周期性相机帧(用 Topic)
- 长任务(用 Action)
- 需要进度(用 Action)
---
## 2. .srv 文件定义
### 2.1 格式
```
Request 字段
---
Response 字段
```
### 2.2 标准 .srv 示例
```srv
# example_interfaces/srv/AddTwoInts.srv
int64 a # Request
int64 b
---
int64 sum # Response
```
### 2.3 复杂示例(多个字段)
```srv
# example_interfaces/srv/SetBool.srv
bool data
---
bool success
string message
```
### 2.4 本仓库用的 Service
`example_interfaces/srv/AddTwoInts`:
- Request: `int64 a`, `int64 b`
- Response: `int64 sum`
---
## 3. Server 端(Python / C++)
### 3.1 Python
```python
import rclpy
from rclpy.node import Node
from example_interfaces.srv import AddTwoInts
class AddTwoIntsServer(Node):
def __init__(self):
super().__init__('add_two_ints_server_py')
# create_service(srv_type, srv_name, callback)
# callback 签名: callback(request, response) -> response
self.srv = self.create_service(
AddTwoInts,
'add_two_ints',
self.add_two_ints_callback,
)
self.get_logger().info('add_two_ints_server ready')
def add_two_ints_callback(self, request, response):
response.sum = request.a + request.b
self.get_logger().info(f'{request.a} + {request.b} = {response.sum}')
return response # 必须 return response 对象
def main():
rclpy.init()
node = AddTwoIntsServer()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
node.destroy_node()
rclpy.shutdown()
```
### 3.2 C++
```cpp
#include "rclcpp/rclcpp.hpp"
#include "example_interfaces/srv/add_two_ints.hpp"
class Server : public rclcpp::Node {
public:
Server() : rclcpp::Node("add_two_ints_server_cpp") {
srv_ = this->create_service<example_interfaces::srv::AddTwoInts>(
"add_two_ints",
[this](
const example_interfaces::srv::AddTwoInts::Request::SharedPtr req,
example_interfaces::srv::AddTwoInts::Response::SharedPtr res
) {
res->sum = req->a + req->b;
});
}
private:
rclcpp::Service<example_interfaces::srv::AddTwoInts>::SharedPtr srv_;
};
```
---
## 4. Client 端(Python / C++)
### 4.1 Python — 同步阻塞风格
```python
class Client(Node):
def __init__(self):
super().__init__('add_two_ints_client_py')
self.client = self.create_client(AddTwoInts, 'add_two_ints')
# 阻塞等服务端上线(1s 超时,循环等)
while not self.client.wait_for_service(timeout_sec=1.0):
self.get_logger().info('waiting for service...')
def call_sync(self, a, b):
req = AddTwoInts.Request()
req.a = a; req.b = b
# call_async:返回 Future,不等
future = self.client.call_async(req)
# spin_until_future_complete:阻塞到 future 完成(或超时)
rclpy.spin_until_future_complete(self, future, timeout_sec=5.0)
if future.result() is None:
self.get_logger().error('service call failed')
return None
return future.result().sum
```
### 4.2 Python — 异步回调风格
```python
class AsyncClient(Node):
def __init__(self):
super().__init__('async_client')
self.client = self.create_client(AddTwoInts, 'add_two_ints')
self.client.wait_for_service()
def call_async(self, a, b):
req = AddTwoInts.Request()
req.a = a; req.b = b
future = self.client.call_async(req)
def done_cb(fut):
result = fut.result()
if result is not None:
self.get_logger().info(f'result: {result.sum}')
else:
self.get_logger().warn('failed')
future.add_done_callback(done_cb)
def main():
rclpy.init()
node = AsyncClient()
node.call_async(12, 30)
rclpy.spin(node) # 阻塞,直到 callback 调 shutdown
```
### 4.3 C++
```cpp
class Client : public rclcpp::Node {
public:
Client() : rclcpp::Node("client") {
client_ = this->create_client<example_interfaces::srv::AddTwoInts>("add_two_ints");
while (!client_->wait_for_service(std::chrono::seconds(1))) {
RCLCPP_INFO(this->get_logger(), "waiting...");
}
}
int64_t call(int64_t a, int64_t b) {
auto req = std::make_shared<example_interfaces::srv::AddTwoInts::Request>();
req->a = a; req->b = b;
auto future = client_->async_send_request(req);
if (rclcpp::spin_until_future_complete(
this->shared_from_this(), future, 5s) == rclcpp::FutureReturnCode::SUCCESS) {
return future.get()->sum;
}
return -1;
}
};
```
### 4.4 关键 API 速查
| Python | C++ |
|---|---|
| `create_client(SrvType, name)` | `create_client<T>(name)` |
| `client.wait_for_service(timeout_sec=N)` | `client->wait_for_service(1s)` |
| `client.call_async(req)` | `client->async_send_request(req)` |
| `future.result()` | `future.get()` |
| `rclpy.spin_until_future_complete(node, future, timeout_sec)` | `rclcpp::spin_until_future_complete(this, future, 5s)` |
| `future.add_done_callback(cb)` | (用 `bind`) |
---
## 5. 同步 vs 异步调用
| 方式 | 适用 | 阻塞? |
|---|---|---|
| `call(req)` | ROS1 风格,**Python 已 deprecated** | ✅ 同步 |
| `call_async + spin_until_future_complete` | 命令行 / 单测 / 测试 | ✅ 同步但 yield |
| `call_async + add_done_callback` | 生产节点,主线程不能停 | ❌ 异步 |
**生产推荐**: `add_done_callback` 风格,主线程继续 spin 其他东西。
---
## 6. wait_for_service 详解
### 6.1 为什么需要
ROS2 节点启动到 ROS 实际可达,**需要 1-2 秒 DDS discovery**。
client 启动时 server 可能还没起,所以要先等。
### 6.2 用法对比
```python
# ❌ 错误:永久阻塞,debug 难
client.wait_for_service()
# ⚠️ 不推荐:硬超时
client.wait_for_service(timeout_sec=5.0)
# ✅ 推荐:循环 + 日志
while not client.wait_for_service(timeout_sec=1.0):
node.get_logger().info('waiting for service...')
```
### 6.3 死锁陷阱
如果 `wait_for_service()` 在 `__init__` 里**阻塞**,主线程 spin 没跑,DDS discovery 没动 → 永远 wait 不到。
**修法**: 不要在 `__init__` 阻塞,放到独立的 `wait_for_server_ready()` 方法。
---
## 7. QoS + Service 名约定
### 7.1 Service QoS
默认 RELIABLE。一致即可,不用改。
### 7.2 命名约定
| 角色 | 推荐节点名 / service 名 |
|---|---|
| Service Server | `<verb>_server` (如 `add_two_ints_server`) |
| Service Client | `<verb>_client` (如 `add_two_ints_client`) |
| Service 名 | `<action>` (如 `add_two_ints`、`take_photo`) |
---
## 8. 实战:写一个拍照 + 计算服务
### 8.1 .srv 定义(自定义)
```srv
# my_camera/srv/TakePhoto.srv
string filename
---
bool success
int32 width
int32 height
string saved_path
```
### 8.2 Server
```python
import cv2
from my_camera.srv import TakePhoto
class TakePhotoServer(Node):
def __init__(self):
super().__init__('take_photo_server')
self.srv = self.create_service(
TakePhoto, 'take_photo', self.cb)
def cb(self, req, resp):
# 1) 从相机读 frame
cap = cv2.VideoCapture(0)
ret, frame = cap.read()
if not ret:
resp.success = False
return resp
# 2) 保存
h, w = frame.shape[:2]
path = f'/tmp/{req.filename}.jpg'
cv2.imwrite(path, frame)
resp.success = True
resp.width = w
resp.height = h
resp.saved_path = path
return resp
```
### 8.3 Client
```python
class TakePhotoClient(Node):
def take(self, filename):
req = TakePhoto.Request()
req.filename = filename
future = self.client.call_async(req)
rclpy.spin_until_future_complete(self, future)
result = future.result()
if result.success:
print(f'saved {result.saved_path} ({result.width}x{result.height})')
```
---
## 9. 调试命令
```bash
ros2 service list # 所有 service
ros2 service type /add_two_ints # 服务类型
ros2 service info /add_two_ints -v # 提供方节点
ros2 service find example_interfaces/srv/AddTwoInts # 找某类型的所有 service
ros2 service call /add_two_ints example_interfaces/srv/AddTwoInts "{a: 12, b: 30}"
```
---
## 10. 常见坑
### 10.1 Future 不 done
```python
# 错:立即读 future.result()
future = client.call_async(req)
result = future.result() # 阻塞到死
# 对:spin_until_future_complete
rclpy.spin_until_future_complete(node, future, timeout_sec=5.0)
result = future.result()
```
### 10.2 服务端 shutdown 后 client 调
```python
# 服务端 rclpy.shutdown() 后,future.result() 是 None
assert future.result() is not None, 'service unavailable'
```
### 10.3 多 client 排队
Service 是 1对1。两个 client 同时调,**第二个等第一个完成**。长任务用 Action。
### 10.4 wait_for_service 死锁
不要在 `__init__` 阻塞 wait!否则 DDS discovery 没法跑。
---
## 11. 在本仓库里跑
### 11.1 启 server
```bash
docker exec ros2_dev bash -lc "source /root/ros2_ws/install/setup.bash && ros2 launch bringup service_launch.py"
```
### 11.2 调 service(新终端)
```bash
docker exec ros2_dev bash -lc "source /root/ros2_ws/install/setup.bash && ros2 service call /add_two_ints example_interfaces/srv/AddTwoInts '{\"a\": 12, \"b\": 30}'"
```
**预期输出**:
```
response:
example_interfaces.srv.AddTwoInts_Response(sum=42)
```
### 11.3 源码
- Server: [`src/py_srv/py_srv/add_two_ints_server.py`](../src/py_srv/py_srv/add_two_ints_server.py)
- Client: [`src/py_srv/py_srv/add_two_ints_client.py`](../src/py_srv/py_srv/add_two_ints_client.py)
- launch: [`src/bringup/launch/service_launch.py`](../src/bringup/launch/service_launch.py)
- 测试: [`src/py_srv/test/test_srv.py`](../src/py_srv/test/test_srv.py)
### 11.4 端到端日志
[`docker/srv_e2e.log`](../docker/srv_e2e.log)
---
## 12. 进阶:异步服务端 + 回调式 client
### 12.1 异步服务端
```python
class AsyncServer(Node):
"""长时间运行的服务,内部用回调推进,不阻塞主线程。"""
def __init__(self):
super().__init__('async_server')
self.srv = self.create_service(MySrv, 'async_srv', self.cb)
def cb(self, req, resp):
# 启动后台任务做实际工作
future = self._do_work(req)
future.add_done_callback(lambda f: self._respond(f, resp))
return resp # 先返回,后续异步填充
def _do_work(self, req):
# 用 executor.spin_until_future_complete 做异步
...
```
### 12.2 回调式 client
```python
def on_response(future):
result = future.result()
print(f'result: {result.value}')
rclpy.shutdown()
future = client.call_async(req)
future.add_done_callback(on_response)
rclpy.spin(node)
```
---
## Service vs Action 怎么选
| 维度 | Service | Action |
|---|---|---|
| 持续时间 | < 几秒 | 几秒 ~ 几小时 |
| 反馈进度 | ❌ | ✅ Feedback |
| 可取消 | ❌ | ✅ |
| 适用 | 拍照、查询 | 抓取、导航 |
本仓库 [`py_srv`](../src/py_srv/) 是 Service demo;[`py_action_demo`](../src/py_action_demo/) 是 Action demo。
---
## 接下来读
| 主题 | 文档 |
|---|---|
| Topic 深度 | [`20-topics.md`](20-topics.md) |
| Action 深度 | [`40-actions.md`](40-actions.md) |
| TF2 坐标变换 | [`50-tf2.md`](50-tf2.md) |
| 三机部署 | [`100-embedded-deployment.md`](100-embedded-deployment.md) |
doc/40-actions.md
+752
View File
@@ -0,0 +1,752 @@
# 40 · Action 深度:Goal / Feedback / Result(完全指南)
> **目标**:吃透 ROS2 Action,涵盖 .action 定义、Goal/Feedback/Result 三件套、cancel、MultiThreadedExecutor、VLA/机器人应用,学完能写工业级 Action server。
---
## 目录
- [1. 为什么需要 Action](#1-为什么需要-action)
- [2. .action 文件定义](#2-action-文件定义)
- [3. Action Server(Python / C++)](#3-action-serverpython--c)
- [4. Action Client(Python / C++)](#4-action-clientpython--c)
- [5. Goal Handle 状态机](#5-goal-handle-状态机)
- [6. cancel(取消)](#6-cancel取消)
- [7. MultiThreadedExecutor(关键!)](#7-multithreadedexecutor关键)
- [8. 实战:写一个抓取 Action](#8-实战写一个抓取-action)
- [9. 调试命令](#9-调试命令)
- [10. 常见坑](#10-常见坑)
- [11. 在本仓库里跑](#11-在本仓库里跑)
- [12. VLA / 机器人应用](#12-vla--机器人应用)
---
## 1. 为什么需要 Action
### 1.1 Service 的局限
Service 是"短查询",适合几毫秒到几秒。但**机器人任务经常需要几分钟**:
- 抓取一个物体:5-30 秒
- 移动底盘导航:几秒到几分钟
- SLAM 建图:几小时
Service 调起来就"卡住",不知道进度,不能取消。
### 1.2 Action 解决的问题
| Service 没有的 | Action 提供 |
|---|---|
| 进度反馈 | **Feedback**(周期性推送) |
| 取消能力 | **cancel**(client 中途叫停) |
| 长任务友好 | 异步,不阻塞 |
### 1.3 vs Topic / Service
| 维度 | Topic | Service | **Action** |
|---|---|---|---|
| 同步 | 异步 | 同步 | 异步(long) |
| 方向 | 单向 | 双向 | 双向 |
| 1对多 | ✅ | ❌ | ❌ |
| 进度反馈 | ❌ | ❌ | ✅ |
| 可取消 | N/A | ❌ | ✅ |
| 持续 | 持续流 | 短查询 | 几秒~几小时 |
| 适合 | 传感器 | 拍照 | **抓取 / 导航 / SLAM** |
---
## 2. .action 文件定义
### 2.1 三段式格式
```
Goal 字段
---
Result 字段
---
Feedback 字段
```
### 2.2 标准 Fibonacci 例子
```action
# example_interfaces/action/Fibonacci.action
int32 order
---
int32[] sequence
---
int32[] sequence
```
| 段 | 含义 | 字段 |
|---|---|---|
| 第一段 | **Goal**(client 发) | `int32 order` |
| `---` | 分隔 | |
| 第二段 | **Result**(server 一次性返回) | `int32[] sequence` |
| `---` | 分隔 | |
| 第三段 | **Feedback**(server 周期性推) | `int32[] sequence` |
> **注意**: Humble 的 `example_interfaces/action/Fibonacci` 里 **Feedback 和 Result 字段名都是 `sequence`**(不是 `partial_sequence`)。Python 生成 `Fibonacci.Feedback.sequence` 和 `Fibonacci.Result.sequence`。
### 2.3 实际抓取 Action(自定义)
```action
# my_robot/action/ExecuteGripperPick.action
geometry_msgs/PoseStamped target_pose
string object_id
---
bool success
string error_message
---
float32 progress # 0..1
string current_state # "approach" / "grasp" / "lift" / "done"
```
### 2.4 复杂 .action 示例
```action
# my_robot/action/NavigateToPose.action
geometry_msgs/PoseStamped target_pose
---
bool success
geometry_msgs/PoseStamped final_pose
builtin_interfaces/Duration total_time
---
float32 distance_remaining
float32 time_remaining
string current_behavior # "computing_path" / "moving" / "recovering"
```
---
## 3. Action Server(Python / C++)
### 3.1 Python 标准模板
```python
import rclpy
from rclpy.action import ActionServer
from rclpy.executors import MultiThreadedExecutor
from rclpy.node import Node
from example_interfaces.action import Fibonacci
class FibonacciActionServer(Node):
def __init__(self):
super().__init__('fibonacci_action_server_py')
self._action_server = ActionServer(
self,
Fibonacci, # ActionType
'fibonacci', # action 名
self.execute_callback, # 签名:cb(goal_handle) -> result
)
self.get_logger().info('ready')
def execute_callback(self, goal_handle):
order = goal_handle.request.order
feedback = Fibonacci.Feedback()
result = Fibonacci.Result()
sequence = [0, 1]
for i in range(1, order):
# 1) 检查 client 是否请求取消
if goal_handle.is_cancel_requested:
goal_handle.canceled()
self.get_logger().info('Goal canceled')
return Fibonacci.Result() # 返回空 Result
# 2) 做一部分工作
sequence.append(sequence[i] + sequence[i-1])
# 3) 推一次 Feedback
feedback.sequence = sequence
goal_handle.publish_feedback(feedback)
# 4) 模拟耗时(实际场景中这里跑电机控制 / 规划 / 等)
import time; time.sleep(0.5)
# 5) 任务完成
goal_handle.succeed()
result.sequence = sequence
return result
def main():
rclpy.init()
node = FibonacciActionServer()
# 用 MultiThreadedExecutor!否则 Feedback 推送会卡死主线程
executor = MultiThreadedExecutor()
executor.add_node(node)
executor.spin()
```
### 3.2 C++
```cpp
#include "rclcpp_action/rclcpp_action.hpp"
#include "example_interfaces/action/fibonacci.hpp"
class Server : public rclcpp::Node {
public:
using Fibonacci = example_interfaces::action::Fibonacci;
using GoalHandleFib = rclcpp_action::ServerGoalHandle<Fibonacci>;
Server() : rclcpp::Node("server") {
server_ = rclcpp_action::create_server<Fibonacci>(
this, "fibonacci",
[this](auto, auto) { return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; },
[this](auto) { return rclcpp_action::CancelResponse::ACCEPT; },
[this](std::shared_ptr<GoalHandleFib> gh) {
// 主逻辑
auto feedback = std::make_shared<Fibonacci::Feedback>();
auto result = std::make_shared<Fibonacci::Result>();
std::vector<int32_t> seq = {0, 1};
for (int i = 1; i < gh->get_request()->order; i++) {
if (gh->is_canceling()) { gh->canceled(result); return; }
seq.push_back(seq[i] + seq[i-1]);
feedback->sequence = seq;
gh->publish_feedback(feedback);
std::this_thread::sleep_for(500ms);
}
gh->succeed(result);
result->sequence = seq;
});
}
private:
rclcpp_action::Server<Fibonacci>::SharedPtr server_;
};
```
---
## 4. Action Client(Python / C++)
### 4.1 Python 同步风格(简单但阻塞)
```python
class Client(Node):
def __init__(self):
super().__init__('client')
self._client = ActionClient(self, Fibonacci, 'fibonacci')
# 必须等 server ready
while not self._client.wait_for_server(timeout_sec=1.0):
self.get_logger().info('waiting...')
def call_sync(self, order):
goal = Fibonacci.Goal()
goal.order = order
# call_async 返回 Future
send_future = self._client.send_goal_async(
goal,
feedback_callback=self.feedback_cb,
)
# spin 等 server 接收
rclpy.spin_until_future_complete(self, send_future)
goal_handle = send_future.result()
if not goal_handle.accepted:
self.get_logger().warn('Goal rejected')
return None
# spin 等 server 完成
result_future = goal_handle.get_result_async()
rclpy.spin_until_future_complete(self, result_future)
return result_future.result().result
def feedback_cb(self, feedback_msg):
self.get_logger().info(f'fb: {list(feedback_msg.feedback.sequence)}')
```
### 4.2 Python 异步回调风格(生产推荐)
```python
class AsyncClient(Node):
def __init__(self):
super().__init__('async_client')
self._client = ActionClient(self, Fibonacci, 'fibonacci')
self._client.wait_for_server()
def send(self, order):
goal = Fibonacci.Goal()
goal.order = order
send_future = self._client.send_goal_async(
goal,
feedback_callback=self.feedback_cb,
)
# 异步注册回调:Goal 被接受后
send_future.add_done_callback(self.goal_response_cb)
def goal_response_cb(self, future):
goal_handle = future.result()
if not goal_handle.accepted:
self.get_logger().warn('rejected')
return
# 注册 result 回调
result_future = goal_handle.get_result_async()
result_future.add_done_callback(self.result_cb)
def feedback_cb(self, feedback_msg):
self.get_logger().info(f'fb: {list(feedback_msg.feedback.sequence)}')
def result_cb(self, future):
result = future.result().result
self.get_logger().info(f'result: {list(result.sequence)}')
rclpy.shutdown()
```
### 4.3 C++
```cpp
class Client : public rclcpp::Node {
public:
using Fibonacci = example_interfaces::action::Fibonacci>;
Client() : rclcpp::Node("client") {
client_ = rclcpp_action::create_client<Fibonacci>(
this, "fibonacci");
client_->wait_for_action_server();
}
void call(int order) {
auto goal = Fibonacci::Goal();
goal.order = order;
auto send_future = client_->async_send_goal(goal,
[this](auto) { /* feedback */ });
auto goal_handle = send_future.get();
if (!goal_handle) return;
auto result_future = client_->async_get_result(goal_handle);
if (rclcpp::spin_until_future_complete(this->shared_from_this(),
result_future, 5s) ==
rclcpp::FutureReturnCode::SUCCESS) {
RCLCPP_INFO(this->get_logger(), "result.sequence.size = %zu",
result_future.get()->result.sequence.size());
}
}
};
```
---
## 5. Goal Handle 状态机
```
┌─────────────────┐
│ PENDING │ client 发 Goal,server 未处理
└────────┬────────┘
▼ server accept / reject
┌─────────────────┐
│ ACCEPTED │ server 在执行
└────────┬────────┘
▼ server 周期 publish_feedback
┌─────────────────┐
│ EXECUTING │ (隐式,在 callback 里)
└────────┬────────┘
▼
┌────┴────┐
▼ ▼
┌─────┐ ┌─────┐
│SUCC.│ │ABRT.│ server.succeed() / abort()
└─────┘ └─────┘
│
▼ client.cancel_request → CANCELED
┌─────┐
│CNCL.│
└─────┘
```
### 5.1 关键 API
| 方法 | 何时调 |
|---|---|
| `goal_handle.accept()` / `reject()` | server 开始处理时 |
| `goal_handle.is_cancel_requested` | 周期性检查(看 client 是否取消) |
| `goal_handle.publish_feedback(msg)` | 周期性推送进度 |
| `goal_handle.succeed()` | 任务正常完成 |
| `goal_handle.abort()` | 任务异常 |
| `goal_handle.canceled()` | client 取消时 |
---
## 6. cancel(取消)
### 6.1 client 发起取消
```bash
ros2 action send_goal /fibonacci example_interfaces/action/Fibonacci "{order: 100}" --feedback
# 另一终端取消(查 status)
ros2 action info /fibonacci
# cancel 命令
# (没有直接 cancel CLI,需要单独客户端)
```
### 6.2 server 检查 + 处理
```python
def execute_callback(self, goal_handle):
for i in range(1, order):
# 关键:周期性检查
if goal_handle.is_cancel_requested:
goal_handle.canceled() # 必须调,标状态
self.get_logger().info('Goal canceled by client')
return Fibonacci.Result() # 返回空 Result
...
```
### 6.3 实际场景
- client 觉得"等太久了"
- 网络断(超时)
- 任务本身状态变化(已无意义)
---
## 7. MultiThreadedExecutor(关键!)
### 7.1 为什么必须
Action server 的 callback 跑在主线程。如果用 `SingleThreadedExecutor`:
- 主线程被 callback 阻塞
- `publish_feedback` 不会真正发出
- client 收不到进度
### 7.2 正确写法
```python
from rclpy.executors import MultiThreadedExecutor
executor = MultiThreadedExecutor(num_threads=4) # 4 线程池
executor.add_node(node)
executor.spin()
```
或者用 `rclpy.callback_groups.MutuallyExclusiveCallbackGroup` 精细控制。
---
## 8. 实战:写一个抓取 Action
### 8.1 .action 定义
```action
# my_robot/action/ExecuteGripperPick.action
geometry_msgs/PoseStamped target_pose
string object_id
---
bool success
string error_message
---
float32 progress # 0..1
string current_state # "approach" / "grasp" / "lift" / "done"
```
### 8.2 Server
```python
class PickServer(Node):
def __init__(self):
super().__init__('pick_server')
self._action_server = ActionServer(
self, ExecuteGripperPick, 'pick', self.execute_cb)
def execute_cb(self, goal_handle):
feedback = ExecuteGripperPick.Feedback()
result = ExecuteGripperPick.Result()
target = goal_handle.request.target_pose
# 阶段 1: approach
feedback.progress = 0.0
feedback.current_state = 'approach'
goal_handle.publish_feedback(feedback)
if not self.move_to(target.pose): # MoveIt2 算轨迹 + 执行
goal_handle.abort()
result.success = False
result.error_message = 'approach failed'
return result
if goal_handle.is_cancel_requested:
goal_handle.canceled()
return result
# 阶段 2: grasp
feedback.progress = 0.5
feedback.current_state = 'grasp'
goal_handle.publish_feedback(feedback)
self.close_gripper(force=50)
# 阶段 3: lift
feedback.progress = 0.8
feedback.current_state = 'lift'
goal_handle.publish_feedback(feedback)
self.move_to(self.lift_pose)
# 完成
feedback.progress = 1.0
feedback.current_state = 'done'
goal_handle.publish_feedback(feedback)
goal_handle.succeed()
result.success = True
return result
```
### 8.3 Client
```python
class PickClient(Node):
def pick(self, target_pose, object_id):
goal = ExecuteGripperPick.Goal()
goal.target_pose = target_pose
goal.object_id = object_id
future = self._client.send_goal_async(
goal,
feedback_callback=self.fb_cb,
)
future.add_done_callback(self.response_cb)
def fb_cb(self, fb_msg):
f = fb_msg.feedback
print(f'[{f.progress*100:.0f}%] {f.current_state}')
def response_cb(self, future):
goal_handle = future.result()
if not goal_handle.accepted:
print('rejected')
return
result_future = goal_handle.get_result_async()
result_future.add_done_callback(self.result_cb)
def result_cb(self, future):
result = future.result().result
print(f'success={result.success}')
```
---
## 9. 调试命令
```bash
# 列所有 action
ros2 action list
# 看 action 元数据
ros2 action info /fibonacci
# 用 CLI 发 Goal(无 client 节点时方便)
ros2 action send_goal /fibonacci example_interfaces/action/Fibonacci "{order: 6}" --feedback
# 输出实时反馈
ros2 action send_goal /fibonacci example_interfaces/action/Fibonacci "{order: 6}" --feedback
# 预期输出:
# Feedback:
# sequence: [0, 1, 1, 2, 3, 5]
# Result:
# sequence: [0, 1, 1, 2, 3, 5, 8]
# Goal finished with status: SUCCEEDED
```
---
## 10. 常见坑
### 10.1 wait_for_server 死锁
```python
# ❌ 错:__init__ 里阻塞 wait,主线程 spin 跑不动 → DDS discovery 没动 → 永远 wait
def __init__(self):
super().__init__(...)
self._client.wait_for_server() # 死锁!
# ✅ 对:用 server_is_ready() 轮询
def __init__(self):
super().__init__(...)
self._client = ActionClient(...)
def wait_for_server(self, timeout=5.0):
import time
end = time.time() + timeout
while time.time() < end:
if self._client.server_is_ready():
return True
time.sleep(0.05)
return False
```
### 10.2 client callback 里 shutdown
```python
# ❌ 错:在 result callback 里 shutdown,后续 fixture 也 shutdown 会报错
def result_cb(self, future):
result = future.result().result
rclpy.shutdown() # 第一次 shutdown
# 测试代码:
def test_xxx():
fixture.rclpy.shutdown() # 第二次,报 "Context must be initialized"
```
**修法**:用 flag + 主循环检测:
```python
def result_cb(self, future):
self._done_flag = True
# 主循环:
while not node._done_flag and time.time() < end:
exec_.spin_once(timeout_sec=0.1)
# 测试代码统一 rclpy.shutdown()
```
### 10.3 单线程 executor 没反馈
```python
# ❌ 错:SingleThreadedExecutor,callback 阻塞主线程
exec_ = SingleThreadedExecutor()
exec_.add_node(node)
exec_.spin() # publish_feedback 卡死,client 收不到
# ✅ 对:MultiThreadedExecutor
from rclpy.executors import MultiThreadedExecutor
exec_ = MultiThreadedExecutor(num_threads=4)
exec_.add_node(node)
exec_.spin()
```
### 10.4 字段名错
```python
# ❌ 错:Feedback 字段名误用
feedback.partial_sequence = sequence # AttributeError
# ✅ 对:用正确的字段名(看 .action 定义)
feedback.sequence = sequence
```
本仓库用的 `example_interfaces/action/Fibonacci`:
- Goal 字段:`order`
- Result 字段:`sequence`
- Feedback 字段:`sequence`(不是 `partial_sequence`!)
### 10.5 server 未注册就 client 调
```python
future = client.send_goal_async(goal)
# server 还没 register,future 直接进入 reject 分支
# 客户端看不到 GoalRejected 状态
```
**修法**: 先 `wait_for_server()`(轮询版)。
---
## 11. 在本仓库里跑
### 11.1 启 server
```bash
docker exec ros2_dev bash -lc "source /root/ros2_ws/install/setup.bash && ros2 launch bringup action_launch.py"
```
### 11.2 发 Goal
```bash
docker exec ros2_dev bash -lc "source /root/ros2_ws/install/setup.bash && ros2 action send_goal /fibonacci example_interfaces/action/Fibonacci '{\"order\": 6}' --feedback"
```
**预期输出**:
```
Waiting for an action server to become available...
Sending goal:
order: 6
Goal accepted with ID: 3774142495e7431cb1f45189da01955c
Feedback:
sequence: [0, 1, 1, 2, 3, 5]
Feedback:
sequence: [0, 1, 1, 2, 3, 5, 8]
Result:
sequence: [0, 1, 1, 2, 3, 5, 8]
Goal finished with status: SUCCEEDED
```
### 11.3 源码
- Server: [`src/py_action_demo/py_action_demo/fibonacci_server.py`](../src/py_action_demo/py_action_demo/fibonacci_server.py)
- Client: [`src/py_action_demo/py_action_demo/fibonacci_client.py`](../src/py_action_demo/py_action_demo/fibonacci_client.py)
- 测试: [`src/py_action_demo/test/test_action.py`](../src/py_action_demo/test/test_action.py)
- launch: [`src/bringup/launch/action_launch.py`](../src/bringup/launch/action_launch.py)
---
## 12. VLA / 机器人应用
### 12.1 VLA 推理天然适合 Action
VLA(Vision-Language-Action)模型:
- 输入:图像 + 语言指令
- 输出:7-DoF 关节轨迹
- 推理时间:几秒到几十秒
完全符合 Action 模型:
```action
# vla_action/SampleVLA.action
sensor_msgs/Image image
string instruction
---
trajectory_msgs/JointTrajectory trajectory
bool success
---
float32 progress
string current_state
```
### 12.2 完整 VLA-ROS2 集成
```
┌──────────────┐
│ RGB Camera │
└──────┬───────┘
│ /image_raw
▼
┌──────────────────────────────┐
│ VLA Inference Node (PC) │
│ - 订阅 /image_raw │
│ - 订阅 /instruction (String) │
│ - 调 OpenVLA 模型推理 │
│ - publish Feedback(进度) │
└──────┬───────────────────────┘
│ Action /vla_pick
▼
┌──────────────────────────────┐
│ Execution Node (PC/RDK X5) │
│ - 接收 Action │
│ - 算 MoveIt2 轨迹 │
│ - 调 ros2_control │
└──────┬───────────────────────┘
│ /joint_trajectory
▼
┌──────────────────────────────┐
│ ros2_control (RK3506) │
│ - PID 闭环 │
│ - 电机驱动 │
└──────────────────────────────┘
```
### 12.3 实际项目建议
1. **本仓库** 跑通 7 包 + 跨机通信
2. 升级到 **`py_action_demo`** 的 Action server 做"抓取"接口
3. 接 **OpenVLA / Pi0** 输出轨迹
4. 接 **MoveIt2 + ros2_control** 实际执行
详见 [`doc/99-embodied-ai.md`](99-embodied-ai.md)。
---
## 接下来读
| 主题 | 文档 |
|---|---|
| Topic | [`20-topics.md`](20-topics.md) |
| Service | [`30-services.md`](30-services.md) |
| TF2 + 抓取 | [`50-tf2.md`](50-tf2.md) |
| 具身智能路径 | [`99-embodied-ai.md`](99-embodied-ai.md) |
| 三机部署 | [`100-embedded-deployment.md`](100-embedded-deployment.md) |
doc/50-tf2.md
+517
View File
@@ -0,0 +1,517 @@
# 50 · TF2 坐标变换(完全指南)
> **目标**:吃透 TF2(坐标系管理),能做"机械臂末端在哪"、"相机看到的点在机器人哪"、"手眼标定",为 MoveIt2 / ros2_control / 抓取 打基础。
---
## 目录
- [1. TF 是什么](#1-tf-是什么)
- [2. 关键概念](#2-关键概念)
- [3. TF tree 典型结构](#3-tf-tree-典型结构)
- [4. C++ API 详解](#4-c--api-详解)
- [5. Python API 详解](#5-python-api-详解)
- [6. 静态 TF vs 动态 TF](#6-静态-tf-vs-动态-tf)
- [7. URDF + JointState → TF 工作链](#7-urdf--jointstate--tf-工作链)
- [8. 调试命令大全](#8-调试命令大全)
- [9. VLA / 机器人实战](#9-vla--机器人实战)
- [10. 在本仓库里跑](#10-在本仓库里跑)
- [11. 常见坑 + 排错](#11-常见坑--排错)
- [12. 进阶:手眼标定 + 时间同步](#12-进阶手眼标定--时间同步)
---
## 1. TF 是什么
**TF (Transform Library)** = ROS 里管理"所有坐标系之间相对位姿"的工具。
**TF2** = ROS2 的下一代实现。
机器人身上有几十个坐标系(世界、底盘、激光雷达、相机、机械臂每个关节、夹爪):
- TF2 帮你算任意两个之间的相对位姿,**实时更新**
- TF2 维护一棵 **TF tree**,节点之间路径唯一
- TF2 通过 `/tf` topic 广播,通过 `/tf_static` 静态广播
---
## 2. 关键概念
### 2.1 Frame ID
每个坐标系有个名字:
- `world`、`map`、`odom`、`base_link`、`base_footprint`
- `camera_optical_frame`、`laser_frame`
- `arm_base`、`shoulder`、`elbow`、`wrist`、`gripper`
### 2.2 Transform(变换)
```
parent_frame ──transform──> child_frame
```
包含:
- **translation**(Vector3: x, y, z) - 平移
- **rotation**(Quaternion: x, y, z, w) - 旋转
- **header.stamp**(时间戳)
### 2.3 时间
- `tf2::TimePointZero` = "最新可用"
- 也可以指定具体时间戳查历史(`lookupTransform(target, source, time)`)
- 必须保证时间戳 ≤ buffer 当前时间
### 2.4 Buffer + Listener 模式
```cpp
tf2_ros::Buffer buffer(clock);
tf2_ros::TransformListener listener(buffer, node);
// lookup
auto tf = buffer.lookupTransform(target, source, tf2::TimePointZero);
```
后台 `TransformListener` 订阅 `/tf` + `/tf_static`,自动把变换塞进 buffer。
---
## 3. TF tree 典型结构
### 3.1 移动底盘 + 机械臂
```
world ← 全局参考
└─ map ← SLAM 输出
└─ odom ← 里程计 / AMCL
└─ base_link ← 底盘中心
├─ base_footprint
├─ imu_link
├─ laser_frame ← 2D 激光
├─ camera_optical_frame ← RGB 相机
└─ arm_base ← 机械臂底座
└─ shoulder_link ← 关节 1
└─ upper_arm_link ← 关节 2
└─ elbow_link ← 关节 3
└─ forearm_link
└─ wrist_link
└─ gripper_link ← 末端
```
### 3.2 树 vs 图
TF 是**树**结构:任意两个 frame 间路径唯一。
不能有闭环(两个 link 间只能有一条路径)。
---
## 4. C++ API 详解
### 4.1 Listener + Buffer
```cpp
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
class TfListenerNode : public rclcpp::Node {
public:
TfListenerNode() : rclcpp::Node("tf_listener") {
buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
listener_ = std::make_shared<tf2_ros::TransformListener>(*buffer_, this);
}
void lookup() {
try {
auto tf = buffer_->lookupTransform(
"base_link", // target
"gripper", // source
tf2::TimePointZero); // latest
RCLCPP_INFO(get_logger(),
"gripper in base_link: x=%.3f y=%.3f z=%.3f",
tf.transform.translation.x,
tf.transform.translation.y,
tf.transform.translation.z);
} catch (const tf2::TransformException& ex) {
RCLCPP_WARN(get_logger(), "lookup failed: %s", ex.what());
}
}
private:
std::shared_ptr<tf2_ros::Buffer> buffer_;
std::shared_ptr<tf2_ros::TransformListener> listener_;
};
```
### 4.2 几何变换(把点变换到其他系)
```cpp
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "geometry_msgs/msg/point_stamped.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
geometry_msgs::msg::PointStamped p_in, p_out;
p_in.header.frame_id = "world";
p_in.point.x = 1.0; p_in.point.y = 2.0; p_in.point.z = 3.0;
p_out = buffer_->transform(p_in, "robot");
// p_out 在 robot 系下
```
也支持 PoseStamped、Vector3、Quaternion。
### 4.3 Broadcast(自己发布 TF)
```cpp
#include "tf2_ros/transform_broadcaster.h"
geometry_msgs::msg::TransformStamped tf;
tf.header.stamp = node->now();
tf.header.frame_id = "world";
tf.child_frame_id = "robot";
tf.transform.translation.set__data(1.0, 2.0, 3.0);
tf.transform.rotation.set__data(0, 0, 0, 1.0);
broadcaster.sendTransform(tf);
```
### 4.4 Static Broadcast(只发一次)
```cpp
#include "tf2_ros/static_transform_broadcaster.h"
// 用 sendTransform 后就永久保留,ROS 系统不删
static_broadcaster.sendTransform(tf);
```
### 4.5 关键 API 速查
| API | 用途 |
|---|---|
| `tf2_ros::Buffer(clock)` | 创建时间缓冲(默认 10s 历史) |
| `tf2_ros::TransformListener(buffer, node)` | 订阅 /tf /tf_static |
| `buffer.lookupTransform(target, source, time)` | 查变换(失败抛异常) |
| `buffer.canTransform(target, source, time, timeout)` | 预检 |
| `buffer.transform(p_in, target_frame)` | 几何变换 |
| `tf2_ros::TransformBroadcaster` | 动态广播 |
| `tf2_ros::StaticTransformBroadcaster` | 静态广播 |
---
## 5. Python API 详解
```python
from tf2_ros import Buffer, TransformListener, TransformBroadcaster, StaticTransformBroadcaster
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TransformStamped
class TfNode(Node):
def __init__(self):
super().__init__('tf_node_py')
self.buffer = Buffer()
self.listener = TransformListener(self.buffer, self)
self.broadcaster = TransformBroadcaster(self)
self.static_broadcaster = StaticTransformBroadcaster(self)
def lookup(self):
try:
tf = self.buffer.lookup_transform(
'base_link', 'gripper',
rclpy.time.Time(),
timeout=rclpy.duration.Duration(seconds=1.0))
print(f'gripper in base_link: {tf.transform.translation}')
except Exception as e:
self.get_logger().warn(f'lookup failed: {e}')
def broadcast(self):
tf = TransformStamped()
tf.header.stamp = self.get_clock().now().to_msg()
tf.header.frame_id = 'world'
tf.child_frame_id = 'robot'
tf.transform.translation.x = 1.0
tf.transform.translation.y = 2.0
tf.transform.translation.z = 3.0
tf.transform.rotation.w = 1.0
self.broadcaster.sendTransform(tf)
```
---
## 6. 静态 TF vs 动态 TF
### 6.1 静态 TF(不变的)
- 激光雷达装在底盘上,位置不变
- 相机装在机械臂末端,相对末端不变
- **关键**: 只发一次,ROS 永久保留(用 latched topic)
```bash
ros2 run tf2_ros static_transform_publisher \
--x 0.1 --y 0 --z 0.2 --roll 0 --pitch 0 --yaw 0 \
--frame-id base_link --child-frame-id laser_frame
```
或在 launch 里:
```python
Node(package='tf2_ros', executable='static_transform_publisher',
arguments=['--x', '0.1', '--y', '0', '--z', '0.2',
'--frame-id', 'base_link', '--child-frame-id', 'laser_frame'])
```
### 6.2 动态 TF(随时间变)
- 机械臂关节 → 末端位姿实时变
- 通过 URDF + JointState + robot_state_publisher 自动算
---
## 7. URDF + JointState → TF 工作链
```
URDF (XML) JointState (msg)
│ │
│ │ /joint_states (sensor_msgs/JointState)
│ │
▼ ▼
robot_state_publisher (ROS2 系统包)
│
│ 计算各 link 之间的变换
▼
/tf (geometry_msgs/TransformStamped)
│
▼
tf2::Buffer (订阅)
│
▼
buffer.lookupTransform(target, source, time)
│
▼
"gripper in base_link: x=..., y=..., z=..."
```
### 7.1 安装 robot_state_publisher
```bash
sudo apt install ros-humble-robot-state-publisher
```
### 7.2 启动
```bash
# 读 URDF + JointState
ros2 run robot_state_publisher robot_state_publisher \
--ros-args -p robot_description:="$(xacro arm.urdf)"
# 发布 JointState(本仓库 cpp_robot_tf2 就是这个)
ros2 run cpp_robot_tf2 joint_state_publisher
```
### 7.3 看 TF 树
```bash
ros2 run tf2_tools view_frames
# 生成 frames_<timestamp>.pdf
```
---
## 8. 调试命令大全
```bash
# 实时打印某变换
ros2 run tf2_ros tf2_echo base_link gripper
# 看 /tf topic 流量
ros2 topic hz /tf
ros2 topic info /tf_static -v
# 看完整 TF tree(生成 PDF)
ros2 run tf2_tools view_frames
# 在 RViz 看(可视化)
rviz2
# Add → TF
# 静态 TF publisher(临时)
ros2 run tf2_ros static_transform_publisher --x 0 --y 0 --z 0.5 \
--frame-id world --child-frame-id robot
# 看 /tf_static 列表
ros2 topic info /tf_static -v
```
---
## 9. VLA / 机器人实战
### 9.1 VLA 抓取的工作流
```
[RGB Camera] → /camera/color/image_raw
↓
[GraspNet / 6-DoF pose estimator]
↓ publish /grasp_pose (PoseStamped)
↓
[TF 工具:把 grasp_pose 从 camera_optical_frame 转到 base_link]
↓
[MoveIt2 算轨迹 → /joint_trajectory]
↓
[ros2_control + 电机]
```
### 9.2 关键代码片段
```python
# 把图像里的目标点转到 base_link
def transform_grasp_to_base(camera_pose, buffer):
p_in = PoseStamped()
p_in.header.frame_id = 'camera_optical_frame'
p_in.pose = camera_pose
p_base = buffer.transform(p_in, 'base_link')
return p_base.pose
```
### 9.3 真实项目必备
| 任务 | TF 用法 |
|---|---|
| 视觉抓取 | camera 点 → base_link 点 |
| 手眼标定 | camera_link ↔ gripper 静态 TF |
| SLAM | map → odom → base_link 动态 TF |
| 机械臂正运动学 | lookup gripper 在 base_link 下 |
| 移动底盘导航 | base_link 在 map 下 |
| 多传感器融合 | 把所有数据投到 base_link |
---
## 10. 在本仓库里跑
### 10.1 启 3 关节机械臂 demo
```bash
docker exec ros2_dev bash -lc "source /root/ros2_ws/install/setup.bash && ros2 launch bringup robot_launch.py"
```
**预期日志**:
```
[joint_state_publisher_cpp]: joint_state_publisher_cpp started, joints: 3
[tf2_listener_cpp]: tf2_listener_cpp started
[robot_state_publisher]: got segment base_link
[robot_state_publisher]: got segment link1
[robot_state_publisher]: got segment link2
[robot_state_publisher]: got segment gripper
[tf2_listener_cpp]: gripper in base_link: x=0.013 y=0.004 z=0.299
[tf2_listener_cpp]: gripper in base_link: x=0.019 y=0.009 z=0.298
```
### 10.2 看 TF 树(PDF)
```bash
docker exec ros2_dev bash -lc "source /opt/ros/humble/setup.bash && source /root/ros2_ws/install/setup.bash && ros2 run tf2_tools view_frames"
# 生成 frames_<时间>.pdf
```
### 10.3 源码
- JointState pub: [`src/cpp_robot_tf2/src/joint_state_publisher.cpp`](../src/cpp_robot_tf2/src/joint_state_publisher.cpp)
- TF 监听: [`src/cpp_robot_tf2/src/tf2_listener.cpp`](../src/cpp_robot_tf2/src/tf2_listener.cpp)
- URDF: [`src/cpp_robot_tf2/urdf/simple_arm.urdf`](../src/cpp_robot_tf2/urdf/simple_arm.urdf)
- 测试: [`src/cpp_robot_tf2/test/test_tf2_lookup.cpp`](../src/cpp_robot_tf2/test/test_tf2_lookup.cpp)
### 10.4 端到端日志
[`docker/robot_e2e.log`](../docker/robot_e2e.log)
---
## 11. 常见坑 + 排错
### 11.1 "lookup failed: frame does not exist"
**原因**:该 frame 没发布。
**排错**:
```bash
# 看有哪些 frame
ros2 run tf2_tools view_frames # 生成 PDF,看完整树
# 或
ros2 topic echo /tf | head -50 # 看具体发布的变换
```
**常见原因**:
- robot_state_publisher 没起 → 启它
- JointState 没发布 → 检查 publisher
- 静态 TF publisher 没起
### 11.2 lookup 超时
**原因**: buffer 里没该变换(还没发布)。
**排错**:
- 等 2-3 秒再查
- 加 retry:
```cpp
for (int i = 0; i < 5; i++) {
try {
auto tf = buffer.lookupTransform(target, source, tf2::TimePointZero, 1s);
break;
} catch (const tf2::TransformException& ex) {
// 等
}
}
```
### 11.3 TF 跨机器漂移
**原因**: 不同机器墙钟时间不同步。
**解决**:
- 三机装 NTP,误差 < 10ms
- 用 `use_sim_time` 统一时钟源
- 用 ROS Time API 而非 `chrono::system_clock`
### 11.4 lookupTransform 报"context is not valid"
**原因**: rclpy 已经 shutdown,在 callback 里调用。
**解决**: callback 里用 try/catch,且确保 ROS 仍 alive。
### 11.5 大 TF 树性能
**原因**: 10万 frame 时 lookup 慢。
**解决**:
- 用静态 TF 减少动态节点数
- 拆分成多棵子树
- buffer cache 调大
---
## 12. 进阶:手眼标定 + 时间同步
### 12.1 手眼标定
相机装在机械臂末端(eye-in-hand)或固定在外部(eye-to-hand),需要求:
- camera_link ↔ gripper 的静态变换(eye-in-hand)
- camera_link ↔ base_link 的静态变换(eye-to-hand)
```bash
sudo apt install ros-humble-easy-handeye
# 流程:
# 1) 启动相机 + MoveIt2 + 标定板
# 2) MoveIt2 移动机械臂到多个 pose,采集数据
# 3) easy_handeye 离线计算 camera_link → gripper TF
# 4) 写进 URDF
# 5) 重启 robot_state_publisher,验证
```
### 12.2 时间同步
ROS2 默认 `use_sim_time: false`(wall clock)。多机时:
```bash
# 装 NTP
sudo apt install chrony
sudo systemctl enable chrony
# 验证
chronyc tracking
```
---
## 接下来读
| 主题 | 文档 |
|---|---|
| URDF 模型 | [`60-urdf.md`](60-urdf.md) |
| ros2_control | 在 [`99-embodied-ai.md`](99-embodied-ai.md) 阶段 2 |
| MoveIt2 | 在 [`99-embodied-ai.md`](99-embodied-ai.md) 阶段 2 |
| 三机部署 | [`100-embedded-deployment.md`](100-embedded-deployment.md) |
| VLA 应用 | [`99-embodied-ai.md`](99-embodied-ai.md) 阶段 5 |
doc/60-urdf.md
+711
View File
@@ -0,0 +1,711 @@
# 60 · URDF 机器人模型(完全指南)
> **目标**:能读懂 URDF,能从零写一个 6-DoF 机械臂 URDF,理解 link / joint / xacro / 物理参数,会校验 + 可视化。
---
## 目录
- [1. URDF 是什么](#1-urdf-是什么)
- [2. URDF 三大元素](#2-urdf-三大元素)
- [3. 完整 6-DoF 机械臂示例](#3-完整-6-dof-机械臂示例)
- [4. xacro:模板化 URDF](#4-xacro模板化-urdf)
- [5. 坐标约定 REP-103 / REP-105](#5-坐标约定-rep-103--rep-105)
- [6. ros2_control 集成(加 transmission)](#6-ros2_control-集成加-transmission)
- [7. Gazebo 集成(加 gazebo 标签)](#7-gazebo-集成加-gazebo-标签)
- [8. 校验 + 可视化](#8-校验--可视化)
- [9. 工具链总结](#9-工具链总结)
- [10. MoveIt2 SRDF(规划组)](#10-moveit2-srdf规划组)
- [11. 在本仓库里跑](#11-在本仓库里跑)
- [12. 进阶:6-DoF 真机械臂 URDF 实战](#12-进阶6-dof-真机械臂-urdf-实战)
---
## 1. URDF 是什么
**URDF** (Unified Robot Description Format) = 用 XML 描述机器人:
- 有哪些**刚体段**(`<link>`)
- 刚体之间怎么连(`<joint>`)
- 视觉 / 碰撞几何(`<visual>` / `<collision>`)
- 物理参数(质量、惯性,`<inertial>`)
URDF 通过 `robot_state_publisher` 翻译成 TF tree,被 MoveIt2 / Gazebo / RViz 读取。
---
## 2. URDF 三大元素
### 2.1 `<link>` — 刚体段
| 子标签 | 用途 | 必填 |
|---|---|---|
| `<visual>` | 视觉几何(RViz / Gazebo 显示) | 推荐 |
| `<collision>` | 碰撞几何(物理仿真) | 推荐 |
| `<inertial>` | 质量 + 转动惯量(动力学仿真必填) | 仿真必填 |
| `<material>` | 视觉颜色 | 否 |
```xml
<link name="base_link">
<visual>
<origin xyz="0 0 0.05" rpy="0 0 0"/>
<geometry><box size="0.1 0.1 0.1"/></geometry>
<material name="gray">
<color rgba="0.5 0.5 0.5 1.0"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0.05"/>
<geometry><box size="0.1 0.1 0.1"/></geometry>
</collision>
<inertial>
<mass value="1.0"/>
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
</inertial>
</link>
```
#### 几何类型
```xml
<geometry>
<box size="w h d"/> <!-- 长方体 -->
<sphere radius="r"/> <!-- 球 -->
<cylinder radius="r" length="L"/> <!-- 圆柱 -->
<mesh filename="package://my_pkg/meshes/arm.stl" scale="0.001 0.001 0.001"/>
</geometry>
```
#### 颜色 / 材质
```xml
<material name="gray">
<color rgba="0.5 0.5 0.5 1.0"/> <!-- RGBA,0~1 -->
<texture filename="..."/> <!-- 贴图(可选) -->
</material>
```
### 2.2 `<joint>` — 关节
| type | 含义 | DoF |
|---|---|---|
| `revolute` | 转动(有限位) | 1 |
| `continuous` | 转动(无限位) | 1 |
| `prismatic` | 滑动 | 1 |
| `fixed` | 固定连接 | 0 |
| `floating` | 6 DoF | 6 |
| `planar` | 平面 3 DoF | 3 |
```xml
<joint name="joint1" type="revolute">
<parent link="base_link"/>
<child link="link1"/>
<origin xyz="0 0 0.05" rpy="0 0 0"/> <!-- 子 link 相对父 link -->
<axis xyz="0 0 1"/> <!-- 在 parent 系下定义 -->
<limit lower="-3.14" upper="3.14" <!-- 关节限位 -->
effort="100" velocity="1.0"/> <!-- 力矩 / 速度上限 -->
<dynamics damping="0.1" friction="0.0"/> <!-- 仿真摩擦 -->
</joint>
```
**重要约定**:
- `axis` 在 **parent link 系**下定义
- `origin` 是 **child 相对 parent** 的偏移
- 关节运动绕 `axis` 旋转(或沿 axis 平移)
### 2.3 `<robot>` 根
- `name`:机器人名字,在 ROS 工具中显示
- 必须包含所有 `<link>` 和 `<joint>`
### 2.4 最小 URDF 示例
```xml
<?xml version="1.0"?>
<robot name="hello_arm">
<link name="base_link">
<visual><geometry><box size="0.1 0.1 0.1"/></geometry></visual>
</link>
<joint name="joint1" type="revolute">
<parent link="base_link"/>
<child link="link1"/>
<origin xyz="0 0 0.05"/>
<axis xyz="0 0 1"/>
<limit lower="-3.14" upper="3.14" effort="1.0" velocity="1.0"/>
</joint>
<link name="link1">
<visual><geometry><box size="0.05 0.05 0.1"/></geometry></visual>
</link>
</robot>
```
---
## 3. 完整 6-DoF 机械臂示例
模拟 6-DoF 工业机械臂(类似 UR5 / xArm 6):
```xml
<?xml version="1.0"?>
<robot name="my_6dof_arm">
<!-- 材料定义 -->
<material name="white"><color rgba="1 1 1 1"/></material>
<material name="red"><color rgba="0.8 0.2 0.2 1"/></material>
<!-- 1) base_link:固定基座 -->
<link name="base_link">
<visual>
<geometry><cylinder radius="0.05" length="0.05"/></geometry>
<material name="white"/>
</visual>
<collision>
<geometry><cylinder radius="0.05" length="0.05"/></geometry>
</collision>
<inertial>
<mass value="1.0"/>
<inertia ixx="0.005" ixy="0" ixz="0" iyy="0.005" iyz="0" izz="0.005"/>
</inertial>
</link>
<!-- 2) joint1:底盘转动(绕 z 轴) -->
<joint name="joint1" type="revolute">
<parent link="base_link"/>
<child link="shoulder_link"/>
<origin xyz="0 0 0.05"/>
<axis xyz="0 0 1"/>
<limit lower="-3.14" upper="3.14" effort="100" velocity="2.0"/>
</joint>
<link name="shoulder_link">
<visual>
<geometry><box size="0.05 0.05 0.05"/></geometry>
<material name="red"/>
</visual>
<collision>
<geometry><box size="0.05 0.05 0.05"/></geometry>
</collision>
<inertial>
<mass value="0.3"/>
<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
</inertial>
</link>
<!-- 3) joint2:肩部俯仰(绕 y 轴) -->
<joint name="joint2" type="revolute">
<parent link="shoulder_link"/>
<child link="upper_arm_link"/>
<origin xyz="0 0 0.05"/>
<axis xyz="0 1 0"/>
<limit lower="-1.57" upper="1.57" effort="100" velocity="2.0"/>
</joint>
<link name="upper_arm_link">
<visual>
<origin xyz="0 0 0.1"/>
<geometry><cylinder radius="0.025" length="0.2"/></geometry>
<material name="white"/>
</visual>
<collision>
<origin xyz="0 0 0.1"/>
<geometry><cylinder radius="0.025" length="0.2"/></geometry>
</collision>
<inertial>
<mass value="0.5"/>
<inertia ixx="0.002" ixy="0" ixz="0" iyy="0.002" iyz="0" izz="0.0005"/>
</inertial>
</link>
<!-- 4) joint3:肘部俯仰 -->
<joint name="joint3" type="revolute">
<parent link="upper_arm_link"/>
<child link="forearm_link"/>
<origin xyz="0 0 0.2"/>
<axis xyz="0 1 0"/>
<limit lower="-2.0" upper="2.0" effort="50" velocity="2.0"/>
</joint>
<link name="forearm_link">
<visual>
<origin xyz="0 0 0.075"/>
<geometry><cylinder radius="0.02" length="0.15"/></geometry>
<material name="white"/>
</visual>
<collision>
<origin xyz="0 0 0.075"/>
<geometry><cylinder radius="0.02" length="0.15"/></geometry>
</collision>
<inertial>
<mass value="0.3"/>
<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.0003"/>
</inertial>
</link>
<!-- 5) joint4:腕部旋转 -->
<joint name="joint4" type="revolute">
<parent link="forearm_link"/>
<child link="wrist_1_link"/>
<origin xyz="0 0 0.15"/>
<axis xyz="1 0 0"/>
<limit lower="-3.14" upper="3.14" effort="20" velocity="2.0"/>
</joint>
<link name="wrist_1_link">
<visual>
<geometry><box size="0.04 0.04 0.04"/></geometry>
<material name="red"/>
</visual>
<collision><geometry><box size="0.04 0.04 0.04"/></geometry></collision>
<inertial>
<mass value="0.1"/>
<inertia ixx="0.0003" ixy="0" ixz="0" iyy="0.0003" iyz="0" izz="0.0003"/>
</inertial>
</link>
<!-- 6) joint5:腕部俯仰 -->
<joint name="joint5" type="revolute">
<parent link="wrist_1_link"/>
<child link="wrist_2_link"/>
<origin xyz="0 0 0.04"/>
<axis xyz="0 0 1"/>
<limit lower="-3.14" upper="3.14" effort="20" velocity="2.0"/>
</joint>
<link name="wrist_2_link">
<visual><geometry><box size="0.03 0.03 0.04"/></geometry></visual>
<collision><geometry><box size="0.03 0.03 0.04"/></geometry></collision>
<inertial>
<mass value="0.1"/>
<inertia ixx="0.0002"/>
</inertial>
</link>
<!-- 7) joint6:末端旋转 -->
<joint name="joint6" type="revolute">
<parent link="wrist_2_link"/>
<child link="end_effector_link"/>
<origin xyz="0 0 0.04"/>
<axis xyz="1 0 0"/>
<limit lower="-3.14" upper="3.14" effort="10" velocity="2.0"/>
</joint>
<link name="end_effector_link">
<visual>
<geometry><cylinder radius="0.02" length="0.03"/></geometry>
<material name="red"/>
</visual>
<collision>
<geometry><cylinder radius="0.02" length="0.03"/></geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="0.0001"/>
</inertial>
</link>
<!-- 8) 末端夹爪(可选 gripper) -->
<joint name="gripper_joint" type="fixed">
<parent link="end_effector_link"/>
<child link="gripper_link"/>
<origin xyz="0 0 0.025"/>
</joint>
<link name="gripper_link">
<visual><geometry><box size="0.04 0.06 0.02"/></geometry></visual>
<collision><geometry><box size="0.04 0.06 0.02"/></geometry></collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="0.0001"/>
</inertial>
</link>
</robot>
```
**约定**:
- x = 前进
- y = 左
- z = 上
- joint1 绕 z 轴,旋转"底盘朝向"
- joint2/joint3 绕 y 轴,"肩膀/肘部俯仰"
- joint4/joint5/joint6 腕部 3 DoF
- 总长度约 0.4 m,可伸展约 0.5 m
---
## 4. xacro:模板化 URDF
URDF 重复内容多(左右轮、对称连杆)。**xacro** 提供宏、变量、数学。
### 4.1 安装
xacro 通常自带 ROS2:
```bash
sudo apt install ros-humble-xacro
```
### 4.2 xacro 基础语法
```xml
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="arm">
<!-- 变量 -->
<xacro:property name="link_len" value="0.1"/>
<xacro:property name="PI" value="3.14159"/>
<!-- 宏 -->
<xacro:macro name="make_link" params="name color *origin">
<link name="${name}">
<visual>
<xacro:insert_block name="origin"/>
<geometry><box size="0.05 0.05 ${link_len}"/></geometry>
</visual>
</link>
</xacro:macro>
<link name="base_link"/>
<xacro:make_link name="link1" color="red">
<origin xyz="0 0 0.05"/>
</xacro:make_link>
<xacro:make_link name="link2" color="green">
<origin xyz="0 0 0.1"/>
</xacro:make_link>
<!-- 数学 -->
<xacro:property name="mass" value="${1.0 * link_len}"/>
</robot>
```
### 4.3 编译 xacro → URDF
```bash
xacro arm.xacro > arm.urdf
# 或 ros2 launch 时直接传 xacro 输出:
ros2 run robot_state_publisher robot_state_publisher \
--ros-args -p robot_description:="$(xacro /path/to/arm.xacro)"
```
---
## 5. 坐标约定 REP-103 / REP-105
ROS2 的标准约定,**违反会导致 MoveIt2 / Gazebo 算错**:
### 5.1 REP-103(单位 + 坐标轴方向)
- 长度单位:**米**(m)
- 角度单位:**弧度**(rad)
- 右手系:
- **x** = 前进
- **y** = 左
- **z** = 上
### 5.2 REP-105(坐标系语义)
| frame | 含义 |
|---|---|
| `world` | 全局参考(通常不动) |
| `map` | SLAM 输出的地图 |
| `odom` | 里程计 / AMCL 定位 |
| `base_link` | 机器人底盘中心(刚体) |
| `base_footprint` | 底盘在地面的投影(2D 导航) |
关系:
- `world → map → odom → base_link` 是固定链
- `world → map` 由 SLAM 提供(变化)
- `map → odom` 由 AMCL / 里程计提供
- `odom → base_link` 由编码器 / IMU 提供
---
## 6. ros2_control 集成(加 transmission)
`ros2_control` 需要每个关节加 `<transmission>` 标签:
```xml
<joint name="joint1" type="revolute">
<parent link="base_link"/>
<child link="shoulder_link"/>
<axis xyz="0 0 1"/>
<limit effort="100" velocity="2.0" lower="-3.14" upper="3.14"/>
<!-- ros2_control 必填 -->
<transmission name="trans_joint1">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint1">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="motor_joint1">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</joint>
```
`<transmission>` 描述:
- 哪个 joint
- 用哪个 hardware interface(Position / Velocity / Effort)
- 减速比(`mechanicalReduction`)
---
## 7. Gazebo 集成(加 gazebo 标签)
Gazebo Sim / ros_gz 加载 URDF 时需要 `<gazebo>` 标签 + `<transmission>`(已经加)。
```xml
<gazebo>
<plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
<parameters>$(find my_arm)/config/controllers.yaml</parameters>
</plugin>
</gazebo>
<!-- 每个 link 加颜色(可选) -->
<gazebo reference="shoulder_link">
<material>Gazebo/Red</material>
</gazebo>
```
---
## 8. 校验 + 可视化
### 8.1 校验
```bash
sudo apt install liburdfdom-tools
# 检查 URDF 合法性
check_urdf my_arm.urdf
# 输出 OK 即合法,会打印 link / joint 树结构
```
### 8.2 生成 PDF/PNG 图
```bash
sudo apt install liburdfdom-tools python3-urdfdom-py
urdf_to_graphiz my_arm.urdf # 生成 .pdf
# 或
urdf_to_graphiz my_arm.urdf -o my_arm.pdf
```
### 8.3 RViz 可视化
```bash
ros2 launch urdf_tutorial display.launch.py
# 或自己写 launch:
ros2 run rviz2 rviz2
# 在 RViz:
# Fixed Frame → base_link
# Add → RobotModel ← 显示 URDF
# Add → TF ← 显示 TF tree
```
### 8.4 joint_state_publisher_gui(手动调关节)
```bash
ros2 run joint_state_publisher_gui joint_state_publisher_gui
# 弹出 GUI,拖滑块调关节角,看机械臂在 RViz 里动
```
---
## 9. 工具链总结
```
xacro .xacro ───> .urdf (用 xacro 编译)
│
├──> check_urdf (校验)
├──> urdf_to_graphiz (可视化)
│
├──> robot_state_publisher
│ │
│ │ 读 URDF + /joint_states → 算 TF → /tf
│ │
│ └──> tf2::Buffer (其他节点订阅)
│
├──> MoveIt2 (规划)
│ │
│ │ 读 URDF + SRDF → 算 IK / path
│ │
│ └──> trajectory_msgs/JointTrajectory
│
└──> Gazebo Sim (仿真)
│
│ 读 URDF + gazebo 标签 → 物理仿真
```
---
## 10. MoveIt2 SRDF(规划组)
URDF 描述"机器人长啥样",SRDF 加 MoveIt2 需要的规划信息:
- **虚拟关节**:base_link → world(让 MoveIt2 把臂放在世界系)
- **规划组**(Planning Group):哪些关节一起动
- **末端执行器**:夹爪
- **预设位姿**(Pose):home / ready / extended
### 10.1 用 MoveIt Setup Assistant 生成
```bash
ros2 launch moveit_setup_assistant setup_assistant.launch.py
```
GUI 里:
1. Load URDF
2. Add Virtual Joint(base_link → world)
3. Add Planning Group("arm",含 6 joint)
4. Add End Effector(gripper)
5. Add Poses(home / ready / up)
6. **Generate** SRDF + MoveIt config
### 10.2 SRDF 简例
```xml
<robot name="my_arm">
<!-- virtual joint: world → base_link -->
<virtual_joint name="world_to_base" type="fixed"
parent_frame="world" child_link="base_link"/>
<!-- planning group -->
<group name="arm">
<chain base_link="base_link" tip_link="gripper_link"/>
<joint name="joint1"/>
<joint name="joint2"/>
...
</group>
<!-- end effector -->
<end_effector name="gripper" parent_link="end_effector_link"
group="gripper"/>
<!-- pose -->
<group_state name="home" group="arm">
<joint name="joint1" value="0"/>
<joint name="joint2" value="0"/>
...
</group_state>
</robot>
```
---
## 11. 在本仓库里跑
### 11.1 启 3 关节机械臂 demo
```bash
docker exec ros2_dev bash -lc "source /root/ros2_ws/install/setup.bash && ros2 launch bringup robot_launch.py"
```
节点:
- `joint_state_publisher_cpp` (本包)
- `robot_state_publisher` (系统包)
- `tf2_listener_cpp` (本包)
### 11.2 校验本仓库 URDF
```bash
docker exec ros2_dev bash -lc "check_urdf /root/ros2_ws/install/cpp_robot_tf2/share/cpp_robot_tf2/urdf/simple_arm.urdf"
```
### 11.3 源码
- URDF: [`src/cpp_robot_tf2/urdf/simple_arm.urdf`](../src/cpp_robot_tf2/urdf/simple_arm.urdf)
- launch: [`src/cpp_robot_tf2/launch/robot_tf2_launch.py`](../src/cpp_robot_tf2/launch/robot_tf2_launch.py)
### 11.4 端到端日志
[`docker/robot_e2e.log`](../docker/robot_e2e.log)
---
## 12. 进阶:6-DoF 真机械臂 URDF 实战
### 12.1 找开源 URDF
| 机械臂 | URDF 来源 |
|---|---|
| Franka Panda | `ros-planning/moveit_resources/panda` |
| UR5 / UR10 | `ros-industrial/universal_robot` |
| Ufactory xArm 6 | `xArm-Robotics/xarm_ros2` |
| Aloha | `tonyzhaozh/aerial_manipulation` |
### 12.2 URDF 适配本仓库
把开源 URDF 拷到 `src/<your_arm>/urdf/`:
```bash
cp -r <upstream>/franka_description/urdf src/my_arm/urdf/
# 加 transmission(ros2_control 必填)
# 已经在开源版本里有,但要确认 hardwareInterface 是 PositionJointInterface
```
### 12.3 加 ros2_control 配置
`src/my_arm/config/controllers.yaml`:
```yaml
controller_manager:
ros__parameters:
update_rate: 100
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateController
arm_controller:
type: position_controllers/JointGroupPositionController
arm_controller:
ros__parameters:
joints:
- joint1
- joint2
- joint3
- joint4
- joint5
- joint6
```
### 12.4 加 launch
`src/my_arm/launch/arm_bringup.launch.py`:
```python
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os
def generate_launch_description():
pkg = get_package_share_directory('my_arm')
urdf = os.path.join(pkg, 'urdf', 'arm.urdf')
controllers = os.path.join(pkg, 'config', 'controllers.yaml')
with open(urdf, 'r') as f: urdf_content = f.read()
return LaunchDescription([
# 1) robot_state_publisher
Node(package='robot_state_publisher', executable='robot_state_publisher',
parameters=[{'robot_description': urdf_content}]),
# 2) ros2_control_node
Node(package='controller_manager', executable='ros2_control_node',
parameters=[{'robot_description': urdf_content,
'update_rate': 100}],
output='screen'),
# 3) spawn joint_state_broadcaster
Node(package='controller_manager', executable='spawner',
arguments=['joint_state_broadcaster', '-c', '/controller_manager']),
# 4) spawn arm_controller
Node(package='controller_manager', executable='spawner',
arguments=['arm_controller', '-c', '/controller_manager']),
])
```
### 12.5 加 MoveIt2
```bash
sudo apt install ros-humble-moveit
# 用 MoveIt Setup Assistant 加载你的 URDF 生成 SRDF
ros2 launch moveit_setup_assistant setup_assistant.launch.py
```
详见 [`99-embodied-ai.md`](99-embodied-ai.md) 阶段 2。
---
## 接下来读
| 主题 | 文档 |
|---|---|
| TF 坐标变换 | [`50-tf2.md`](50-tf2.md) |
| ros2_control + MoveIt2 | [`99-embodied-ai.md`](99-embodied-ai.md) 阶段 2 |
| launch 文件 | [`70-launch.md`](70-launch.md) |
| 三机部署 | [`100-embedded-deployment.md`](100-embedded-deployment.md) |
doc/70-launch.md
+468
View File
@@ -0,0 +1,468 @@
# 70 · Launch 文件系统(完全指南)
> **目标**:能写 ROS2 Python launch 文件,理解参数覆盖、嵌套、生命周期、条件启动。
---
## 目录
- [1. 什么是 launch](#1-什么是-launch)
- [2. 最小 launch 文件](#2-最小-launch-文件)
- [3. 启动方式](#3-启动方式)
- [4. 顶层 Actions](#4-顶层-actions)
- [5. 事件处理 / 生命周期](#5-事件处理--生命周期)
- [6. 条件启动](#6-条件启动)
- [7. 命名空间 / 多机器人](#7-命名空间--多机器人)
- [8. 常用模式(实战)](#8-常用模式实战)
- [9. ROS2 launch CLI](#9-ros2-launch-cli)
- [10. 常见坑](#10-常见坑)
- [11. 在本仓库里跑](#11-在本仓库里跑)
- [12. 进阶:复杂 launch + 自定义 Action](#12-进阶复杂-launch--自定义-action)
---
## 1. 什么是 launch
ROS1 用 XML 启动多节点繁琐。ROS2 的 launch = Python 脚本:
- 一键启多个节点 + 设参数
- 跨包嵌套复用
- 条件启动、按机器切换配置
- 生命周期管理(启动顺序、超时、失败重试)
---
## 2. 最小 launch 文件
```python
# src/<pkg>/launch/my_launch.py
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
"""ROS2 launch 框架会调这个函数生成 LaunchDescription 实例。"""
return LaunchDescription([
Node(
package='py_pubsub', # 包名
executable='talker', # 入口名(setup.py entry_points)
name='talker_py', # 节点名(可重命名避免冲突)
output='screen', # stdout/stderr 直打到终端
parameters=[ # 参数 dict
{'period_ms': 500, 'topic': 'chatter'},
],
remappings=[ # topic 重映射
('/chatter', '/my_chatter'),
],
),
])
```
**入口约定**:函数名必须是 `generate_launch_description()`,返回 `LaunchDescription`。
---
## 3. 启动方式
```bash
# 装包后,ROS 自动索引 launch 到 share/<pkg>/launch/
colcon build --packages-select py_pubsub
# 用包名 + launch 文件名(去 .py 后缀)
ros2 launch py_pubsub my_launch.py
# 加参数(覆盖 LaunchConfiguration)
ros2 launch py_pubsub my_launch.py topic:=hello period_ms:=200
```
ROS2 找 launch 文件路径:`<install>/share/<pkg>/launch/`。**launch 文件必须装到 share/<pkg>/launch/**,否则报 "file not found"。
---
## 4. 顶层 Actions
### 4.1 `Node`
```python
Node(
package=...,
executable=...,
name=...,
namespace='/', # 命名空间(可分多机器人)
output='screen', # 'screen' / 'log' / 'both'
parameters=[{...}], # 参数 dict
remappings=[(...)], # topic 重映射
arguments=[...], # 透传给 executable 的命令行参数
respawn=False, # 崩溃是否重启
respawn_delay=0,
)
```
### 4.2 `DeclareLaunchArgument` 声明命令行参数
```python
from launch.actions import DeclareLaunchArgument
DeclareLaunchArgument(
'topic', # 参数名
default_value='chatter', # 默认值
description='Topic name', # 说明(ros2 launch --help 显示)
choices=['chatter', 'hello'], # (可选)限定取值
)
```
### 4.3 `LaunchConfiguration` 取值
```python
from launch.substitutions import LaunchConfiguration
topic = LaunchConfiguration('topic')
Node(
...,
parameters=[{'topic': topic}],
)
```
### 4.4 `IncludeLaunchDescription` 嵌套
```python
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
IncludeLaunchDescription(
PythonLaunchDescriptionSource(<other_launch.py>),
launch_arguments={
'topic': 'chatter',
'period_ms': '500',
}.items(),
)
```
**用 FindPackageShare + PathJoinSubstitution**(跨包推荐):
```python
from launch_ros.substitutions import FindPackageShare
from launch.substitutions import PathJoinSubstitution
pkg_share = FindPackageShare('py_pubsub')
launch_path = PathJoinSubstitution([pkg_share, 'launch', 'pubsub_launch.py'])
IncludeLaunchDescription(PythonLaunchDescriptionSource(launch_path))
```
### 4.5 `ExecuteProcess` 跑 shell 命令
```python
from launch.actions import ExecuteProcess
ExecuteProcess(
cmd=['xacro', 'arm.xacro'],
output='screen',
)
```
> ⚠️ `Command(['cat', '/path'])` 偶尔会丢空格变成 `'cat/path'`,
> 建议**launch 启动前 `open().read()` 读文件**,不要 cat。
### 4.6 `OpaqueFunction` 任意函数
```python
from launch.actions import OpaqueFunction
def my_setup(context, *args, **kwargs):
# 自定义逻辑
return [...]
LaunchDescription([
OpaqueFunction(function=my_setup),
Node(...),
])
```
### 4.7 `TimerAction` 延迟启动
```python
from launch.actions import TimerAction
LaunchDescription([
Node(package='srv', executable='server'),
TimerAction(period=2.0, actions=[
Node(package='client', executable='client'),
]),
])
```
---
## 5. 事件处理 / 生命周期
### 5.1 启动顺序
ROS2 launch 按 LaunchDescription 里 action 的顺序**同步**启动(默认)。
### 5.2 事件 handler
```python
from launch import LaunchDescription
from launch.event_handlers import OnProcessExit
from launch.actions import RegisterEventHandler, LogInfo
def generate_launch_description():
server = Node(package='srv', executable='server')
return LaunchDescription([
server,
RegisterEventHandler(
OnProcessExit(
target_action=server,
on_exit=[LogInfo(msg='server exited, shutting down')],
)
),
])
```
支持的事件: `OnProcessStart` / `OnProcessExit` / `OnProcessIO` 等。
---
## 6. 条件启动
```python
from launch.conditions import IfCondition, UnlessCondition
Node(
...,
condition=IfCondition(LaunchConfiguration('use_camera')),
)
```
CLI 用 `:=true` / `:=false`:
```bash
ros2 launch my_pkg my.launch.py use_camera:=true
```
---
## 7. 命名空间 / 多机器人
### 7.1 namespace 隔离
```python
Node(
package='py_pubsub',
executable='talker',
namespace='robot1', # → topic /robot1/chatter
name='talker', # → 节点名 /robot1/talker
)
```
### 7.2 push_ros_namespace
```python
from launch.actions import PushRosNamespace
LaunchDescription([
PushRosNamespace('robot1'),
Node(...),
])
```
---
## 8. 常用模式(实战)
### 8.1 参数文件加载
```python
from ament_index_python.packages import get_package_share_directory
import os
params_file = os.path.join(
get_package_share_directory('my_pkg'),
'config', 'params.yaml'
)
Node(
package='my_pkg',
executable='node',
parameters=[params_file],
)
```
### 8.2 多节点同包
```python
nodes = [
Node(package='py_pubsub', executable='talker', name='talker_a', parameters=[{'topic': 'a'}]),
Node(package='py_pubsub', executable='listener', name='listener_a', parameters=[{'topic': 'a'}]),
]
```
### 8.3 跨包组合(`bringup` 模式)
本仓库 [`src/bringup/launch/full_demo_launch.py`](../src/bringup/launch/full_demo_launch.py):
```python
def _include(pkg_name, launch_file):
pkg = FindPackageShare(pkg_name)
path = PathJoinSubstitution([pkg, 'launch', launch_file])
return IncludeLaunchDescription(PythonLaunchDescriptionSource(path))
def generate_launch_description():
return LaunchDescription([
_include('py_pubsub', 'pubsub_launch.py'),
_include('cpp_pubsub', 'pubsub_launch.py'),
_include('py_srv', 'srv_launch.py'),
_include('py_action_demo', 'action_launch.py'),
_include('cpp_robot_tf2', 'robot_tf2_launch.py'),
_include('py_vision_demo', 'vision_launch.py'),
])
```
### 8.4 读 URDF 文件内容
```python
import os
from ament_index_python.packages import get_package_share_directory
pkg_share = get_package_share_directory('cpp_robot_tf2')
urdf_path = os.path.join(pkg_share, 'urdf', 'simple_arm.urdf')
with open(urdf_path, 'r', encoding='utf-8') as f:
robot_description = f.read()
Node(
package='robot_state_publisher',
executable='robot_state_publisher',
parameters=[{'robot_description': robot_description}],
)
```
---
## 9. ROS2 launch CLI
```bash
# 列出所有 launch 文件
ros2 launch --show-args <pkg> <file>
# 看可用参数
ros2 launch <pkg> <file> --show-args
# 调试(详细日志)
ros2 launch -d <pkg> <file>
# 传参
ros2 launch <pkg> <file> topic:=hello period_ms:=200
```
---
## 10. 常见坑
### 10.1 launch 文件找不到
- `setup.py` 的 `data_files` 必须包含 `'share/<pkg>/launch': ['launch/*.py']`
- colcon build 后 launch 文件没复制到 install/share → 检查 build log
### 10.2 节点名冲突
同一 ROS Domain 内**节点名必须唯一**。launch 里 `name=` 重命名,或用 `namespace=` 隔离。
### 10.3 参数失效
`parameters=[{'topic': 'chatter'}]` 里 topic 是字符串;如果想"取自 LaunchConfiguration":
```python
parameters=[{'topic': LaunchConfiguration('topic')}]
```
### 10.4 `Command(['cat', path])` 空格丢了
如前所述,**改用 launch 启动前 `open().read()`**。
### 10.5 launch 启动后节点秒退
- 节点 main 函数异常(看日志)
- 参数名拼错(节点没收到参数)
- launch `output='log'` 看不到错误 → 改 `output='screen'`
### 10.6 launch 嵌套找不到文件
`IncludeLaunchDescription` 引用的 launch 文件必须在被包含包的 `share/<pkg>/launch/` 下被 colcon 实际安装。
---
## 11. 在本仓库里跑
### 11.1 各包 launch 文件清单
```
src/
├── py_pubsub/launch/pubsub_launch.py (2 节点,纯 Python)
├── cpp_pubsub/launch/pubsub_launch.py (2 节点,C++ + topic 命令行参数)
├── py_srv/launch/srv_launch.py (1 service)
├── py_action_demo/launch/action_launch.py (1 action server)
├── cpp_robot_tf2/launch/robot_tf2_launch.py (3 节点,URDF + TF)
├── py_vision_demo/launch/vision_launch.py (2 节点,cv_bridge)
└── bringup/launch/
├── pubsub_launch.py (4 节点,Topic 跨包)
├── service_launch.py
├── action_launch.py
├── robot_launch.py (嵌套 cpp_robot_tf2)
├── vision_launch.py (嵌套 py_vision_demo)
└── full_demo_launch.py (11 节点,跨所有包)
```
### 11.2 看可用参数
```bash
docker exec ros2_dev bash -lc "source /root/ros2_ws/install/setup.bash && ros2 launch bringup full_demo_launch.py --show-args"
```
---
## 12. 进阶:复杂 launch + 自定义 Action
### 12.1 复杂 launch 模板
```python
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import (
DeclareLaunchArgument,
OpaqueFunction,
RegisterEventHandler,
LogInfo,
)
from launch.conditions import IfCondition
from launch.event_handlers import OnProcessExit
from launch.substitutions import (
LaunchConfiguration, Command, FindExecutable, PathJoinSubstitution,
)
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
def launch_setup(context, *args, **kwargs):
# 任何动态逻辑
config_file = LaunchConfiguration('config_file').perform(context)
if not os.path.exists(config_file):
return [LogInfo(msg=f'config not found: {config_file}')]
return [...]
def generate_launch_description():
return LaunchDescription([
DeclareLaunchArgument('config_file', default_value='config.yaml'),
DeclareLaunchArgument('enable_ai', default_value='true'),
Node(
package='my_pkg',
executable='main_node',
parameters=[LaunchConfiguration('config_file')],
condition=IfCondition(LaunchConfiguration('enable_ai')),
),
OpaqueFunction(function=launch_setup),
])
```
---
## 接下来读
| 主题 | 文档 |
|---|---|
| Topic | [`20-topics.md`](20-topics.md) |
| Service | [`30-services.md`](30-services.md) |
| Action | [`40-actions.md`](40-actions.md) |
| colcon / ament 包构建 | [`80-package-build.md`](80-package-build.md) |
| 测试策略 | [`90-testing.md`](90-testing.md) |
doc/80-package-build.md
+405
View File
@@ -0,0 +1,405 @@
# 80 · 包构建机制 colcon / ament(完全指南)
> **目标**:理解 ROS2 一个包从源码到 `ros2 run` 能找到的完整流程,能自己写 ament_python / ament_cmake 包。
---
## 目录
- [1. 工作空间结构](#1-工作空间结构)
- [2. 包类型](#2-包类型)
- [3. package.xml(包身份证)](#3-packagexml包身份证)
- [4. ament_python 包详解](#4-ament_python-包详解)
- [5. ament_cmake 包详解](#5-ament_cmake-包详解)
- [6. colcon build 内部流程](#6-colcon-build-内部流程)
- [7. ament_index 与 ros2 工具发现](#7-ament_index-与-ros2-工具发现)
- [8. 依赖解析与 rosdep](#8-依赖解析与-rosdep)
- [9. 自定义消息 / 服务 / Action(进阶)](#9-自定义消息--服务--action进阶)
- [10. 常见构建错误 + 解决](#10-常见构建错误--解决)
- [11. 在本仓库里跑](#11-在本仓库里跑)
- [12. 进阶:多包 workspace / release 流程](#12-进阶多包-workspace--release-流程)
---
## 1. 工作空间结构
```
ros2_ws/ ← 工作空间根
├── src/ ← 包源码(你写代码的地方)
│ ├── pkg_a/ ← 一个 ROS 包
│ ├── pkg_b/
│ └── ...
├── build/ ← colcon build 中间产物
├── install/ ← colcon install 产物(ros2 唯一识别)
├── log/ ← build/test 日志
└── ...
```
**`install/` 是唯一能被 `ros2` 命令识别的目录**。每次进新 shell 必须:
```bash
source install/setup.bash
```
---
## 2. 包类型
ROS2 有两种主流 build_type:
| build_type | 文件 | 用途 |
|---|---|---|
| **ament_python** | `setup.py` + `package.xml` | Python 包 |
| **ament_cmake** | `CMakeLists.txt` + `package.xml` | C++ 包 |
| ament_cmake_python | 两者混合 | 复杂项目 |
---
## 3. package.xml(包身份证)
```xml
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd"?>
<package format="3">
<name>my_pkg</name>
<version>0.1.0</version>
<description>...</description>
<maintainer email="x@y.com">Name</maintainer>
<license>Apache-2.0</license>
<depend>rclpy</depend> <!-- 构建 + 运行都需 -->
<build_depend>rosidl_default_generators</build_depend>
<exec_depend>rclpy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<test_depend>python3-pytest</test_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>
```
| 字段 | 含义 |
|---|---|
| `<depend>` | 构建 + 运行 |
| `<build_depend>` | 仅构建 |
| `<exec_depend>` | 仅运行 |
| `<test_depend>` | 仅测试 |
| `<build_type>` | `ament_python` / `ament_cmake` |
---
## 4. ament_python 包详解
### 4.1 目录结构
```
py_pubsub/
├── package.xml
├── setup.py ← 标准 setuptools
├── setup.cfg ← ament_python 脚本目录约定
├── resource/
│ └── py_pubsub ← 空文件,ament_index marker(必填!)
├── py_pubsub/ ← Python 模块
│ ├── __init__.py
│ └── *.py
├── launch/
│ └── *.py
└── test/
└── test_*.py
```
### 4.2 setup.py
```python
from setuptools import find_packages, setup
package_name = 'py_pubsub'
setup(
name=package_name,
version='0.1.0',
packages=find_packages(exclude=['test']),
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
('share/' + package_name + '/launch', ['launch/*.py']),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='xs',
maintainer_email='dev@example.com',
description='Python talker/listener demo for ROS2 Humble',
license='Apache-2.0',
tests_require=['pytest'],
entry_points={
'console_scripts': [
'talker = py_pubsub.publisher_member_function:main',
'listener = py_pubsub.subscriber_member_function:main',
],
},
)
```
**关键点**:
- `data_files` 必须把 `package.xml` + `launch/` 拷到 `share/<pkg>/`
- `resource/<pkg>` 空文件 → ament_index marker(必填!)
- `entry_points/console_scripts` 暴露 `ros2 run` 可执行名
### 4.3 setup.cfg
```ini
[develop]
script_dir=$base/lib/py_pubsub
[install]
install_scripts=$base/lib/py_pubsub
```
控制 `console_scripts` 装到 `$base/lib/<pkg>/`,ROS2 启动器能找到。
---
## 5. ament_cmake 包详解
### 5.1 目录结构
```
cpp_pubsub/
├── package.xml
├── CMakeLists.txt
├── src/
│ └── *.cpp
├── include/ ← 公开头文件(可选)
├── launch/
│ └── *.py
├── test/
│ └── test_*.cpp
└── config/ ← YAML 配置(可选)
```
### 5.2 CMakeLists.txt
```cmake
cmake_minimum_required(VERSION 3.16)
project(cpp_pubsub VERSION 0.1.0)
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17)
endif()
# GCC/Clang 编译警告
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
include_directories(include)
# 节点可执行
add_executable(talker src/publisher_member_function.cpp)
ament_target_dependencies(talker rclcpp std_msgs)
add_executable(listener src/subscriber_member_function.cpp)
ament_target_dependencies(listener rclcpp std_msgs)
# 装到 install/lib/<pkg>/
install(TARGETS talker listener DESTINATION lib/${PROJECT_NAME})
# launch 文件装到 share/<pkg>/launch/
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/)
# 测试
if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
ament_add_gtest(test_pub_sub test/test_pub_sub.cpp)
ament_target_dependencies(test_pub_sub rclcpp std_msgs)
endif()
ament_package() ← 必填!
```
**关键**:
- `ament_target_dependencies(<target> pkg1 pkg2)` 同时设置 include path 和 link
- `install(TARGETS ...)` 把可执行装到 install
- `install(DIRECTORY launch ...)` 把 launch 装到 share
- `ament_package()` 末尾必填
---
## 6. colcon build 内部流程
```bash
colcon build --symlink-install --packages-select <pkg>
```
按依赖顺序,每个包:
```
┌──────────────────────────────────────────────────────────┐
│ 1. 发现 src/<pkg>/ 下的 package.xml │
│ 2. 读 <export><build_type>,决定 build type │
│ 3. 调用对应 build_type 的 hook: │
│ ament_python → python setup.py build + install │
│ ament_cmake → cmake + make + ament_package │
│ 4. 写 ament_index 标记 (share/ament_index/...) │
│ 5. 把产物装到 install/<pkg>/ │
└──────────────────────────────────────────────────────────┘
```
加 `--symlink-install`:
- Python:源码**软链**到 install,改源码立即生效(不用重 build)
- C++:可执行仍硬编,但 launch / config 文件软链
---
## 7. ament_index 与 ros2 工具发现
`ros2 run / launch / pkg executables / topic info` 等命令都靠 **ament_index**:
- 启动时扫 `install/<pkg>/share/ament_index/resource_index/packages/<pkg>` 文件
- 找到包路径后,扫 `install/<pkg>/lib/<pkg>/` 找可执行
- 扫 `install/<pkg>/share/<pkg>/launch/` 找 launch 文件
**手动重建索引**(极少需要):
```bash
# 索引在 install/share/ament_index/ 里,正常情况下 colcon build 自动维护
ros2 doctor --report # 看索引健康
```
---
## 8. 依赖解析与 rosdep
`package.xml` 的 `<depend>` 让 colcon 自动排构建顺序:
- 你的 `py_pubsub` 依赖 `rclpy`,`rclpy` 是 apt 包,colcon 跳过
- `py_srv` 依赖 `example_interfaces`,同上
- colcon 只 build 我们 src/ 下的包
**`rosdep`** 是 apt 包管理:
```bash
sudo rosdep init && rosdep update
sudo rosdep install -i --from-paths src/ # 装齐所有 apt 依赖
```
本仓库基础镜像 `osrf/ros:humble-desktop` 已装齐所有 ROS2 客户端,**不需要额外 rosdep**。
---
## 9. 自定义消息 / 服务 / Action(进阶)
需要:
1. 在包内建 `msg/`、`srv/`、`action/` 目录
2. 写 `.msg` / `.srv` / `.action` 文件
3. `package.xml` 加:
- `<build_depend>rosidl_default_generators</build_depend>`
- `<exec_depend>rosidl_default_runtime</exec_depend>`
4. `CMakeLists.txt` 加 `rosidl_generate_interfaces(${PROJECT_NAME} ${MSG_FILES} ...)`
5. `setup.py` `data_files` 加 `('share/<pkg>/msg': ['msg/*.msg'], ...)`
6. **ament_python 需要单独 `rosidl` 包**(混合构建)
**本仓库只用 example_interfaces 自带的 Fibonacci / AddTwoInts**(由 ROS2 系统包提供),避开这个复杂度。
---
## 10. 常见构建错误 + 解决
### 10.1 `Could not find a package configuration file provided by "rclpy"`
少装 apt 包:
```bash
sudo apt install ros-humble-rclpy # 镜像外
```
### 10.2 `Package 'X' not found in colcon build`
ament_index 没刷新。重启容器 + source install/setup.bash。
### 10.3 `ament_python` install 0 files
`setup.py` 的 `packages=` 不对。检查 `py_pubsub/` 下有 `__init__.py`。
### 10.4 launch 文件未找到
检查 `setup.py`:
```python
('share/' + package_name + '/launch', ['launch/*.py']),
```
**`launch/*.py` 是 glob**,不是 `'launch/foo.py'`。
### 10.5 ROS Domain 冲突
`ROS_DOMAIN_ID=0` 默认。多项目用 `ROS_DOMAIN_ID=42` 隔离。
### 10.6 Cyclone DDS 缓存了 CMakeCache
`Could not find ROS middleware implementation 'rmw_cyclonedds_cpp'` 但又设了 `RMW_IMPLEMENTATION=rmw_cyclonedds_cpp` → CMake 缓存了。
**修法**:
```bash
rm -rf build install log
colcon build ...
```
---
## 11. 在本仓库里跑
### 11.1 全部 build
```bash
cd /root/ros2_ws
colcon build --symlink-install \
--packages-select py_pubsub cpp_pubsub py_srv py_action_demo cpp_robot_tf2 py_vision_demo bringup
```
### 11.2 增量 build(只编改的)
```bash
colcon build
```
### 11.3 清理后重 build
```bash
rm -rf build install log
colcon build --symlink-install
```
### 11.4 单包 build
```bash
colcon build --packages-select py_pubsub
```
---
## 12. 进阶:多包 workspace / release 流程
### 12.1 多 workspace 覆盖
```bash
# 用 setup.bash 叠加
source /opt/ros/humble/setup.bash
source /workspace1/install/setup.bash
source /workspace2/install/setup.bash
# 后 source 的覆盖前面
```
### 12.2 Release 流程
```bash
# 1) bloom-generate 准备 release
sudo apt install python3-bloom
bloom-generate rosdebian --os-only ubuntu:jammy
# 2) 构建 .deb 包
bloom-release --rosdistro humble --track humble --new 0.1.0 my_pkg
# 3) 推送到 ROS 仓库(rosindex / GitHub)
```
**本仓库未发布**,如要 release 走标准流程。
---
## 接下来读
| 主题 | 文档 |
|---|---|
| Launch 文件 | [`70-launch.md`](70-launch.md) |
| Docker 开发 | [`85-docker.md`](85-docker.md) |
| 测试策略 | [`90-testing.md`](90-testing.md) |
| 三机部署 | [`100-embedded-deployment.md`](100-embedded-deployment.md) |
doc/85-docker.md
+405
View File
@@ -0,0 +1,405 @@
# 85 · Docker 容器化开发(完全指南)
> **目标**:吃透本仓库 Docker 配置,知道每行在做什么,能改能扩,能在 Win/macOS/Linux 一致跑 ROS2。
---
## 目录
- [1. 为什么用 Docker](#1-为什么用-docker)
- [2. 镜像构建](#2-镜像构建)
- [3. 容器编排](#3-容器编排)
- [4. 容器生命周期命令](#4-容器生命周期命令)
- [5. 一键启动脚本](#5-一键启动脚本)
- [6. 调试技巧](#6-调试技巧)
- [7. 常见坑](#7-常见坑)
- [8. 多机 / 多机器人扩展](#8-多机--多机器人扩展)
- [9. 在本仓库里跑](#9-在本仓库里跑)
- [10. 进阶:多阶段构建 + 镜像瘦身](#10-进阶多阶段构建--镜像瘦身)
---
## 1. 为什么用 Docker
| 痛点 | Docker 怎么解 |
|---|---|
| Linux/Windows/macOS 行为差异 | 镜像固定 Linux,行为一致 |
| ROS2 apt 装一堆,污染系统 | 容器内隔离 |
| 团队协作"在我机器能跑" | 镜像统一 |
| 升级 ROS 版本代价大 | 换镜像就行 |
| 跨语言/多版本测试 | 同一镜像多容器 |
本仓库基于 **`osrf/ros:humble-desktop`**(OSRF 官方维护):
- ROS2 Humble 完整运行时
- 默认 RMW: `rmw_fastrtps_cpp`
- 自带 RViz2 / rqt / colcon / rosdep
---
## 2. 镜像构建
### 2.1 Dockerfile
[`docker/Dockerfile`](../docker/Dockerfile) 在 `osrf/ros:humble-desktop` 之上加:
```dockerfile
FROM osrf/ros:humble-desktop
ENV DEBIAN_FRONTEND=noninteractive
ENV ROS_DISTRO=humble
ENV WORKSPACE=/root/ros2_ws
RUN apt-get update && apt-get install -y \
python3-colcon-common-extensions \
python3-pip \
nano \
curl \
&& rm -rf /var/lib/apt/lists/*
RUN pip3 install -U colcon-argcomplete colcon-common-extensions
RUN echo 'source /opt/ros/humble/setup.bash' >> /root/.bashrc && \
echo 'source /usr/share/colcon_argcomplete/hook/colcon-argcomplete.bash' >> /root/.bashrc
WORKDIR ${WORKSPACE}
CMD ["bash"]
```
### 2.2 构建
```powershell
# Windows
docker compose -f D:\xs\ros2\docker\docker-compose.yml build
```
```bash
# Linux
docker compose -f docker/docker-compose.yml build
```
镜像名:`ros2-humble-dev:latest`,约 3GB。
### 2.3 镜像内已装
- ROS2 Humble Desktop
- `colcon-common-extensions` + `colcon-argcomplete`
- `cv_bridge` (openCV ↔ ROS Image)
- `ros-humble-tf2-tools` (`view_frames`)
- `ros-humble-robot-state-publisher` (URDF → TF)
**未装**(按需补):
- `ros-humble-rmw-cyclonedds-cpp`(默认 fastdds 够用)
- `ros-humble-ros2-control`
- `ros-humble-moveit`
- `ros-humble-navigation2`
### 2.4 修改镜像(自定义)
加包:在 `apt-get install` 加一行:
```dockerfile
RUN apt-get install -y ros-humble-ros2-control ros-humble-ros2-controllers
```
加 Python 包:
```dockerfile
RUN pip3 install ultralytics==8.0.0 numpy==1.26
```
---
## 3. 容器编排
### 3.1 docker-compose.yml
[`docker/docker-compose.yml`](../docker/docker-compose.yml):
```yaml
services:
ros2:
build: .
image: ros2-humble-dev:latest
container_name: ros2_dev
privileged: true # 调试用
stdin_open: true # docker exec -it
tty: true
network_mode: host # DDS multicast 必须
environment:
- ROS_DOMAIN_ID=0
# 不要设 RMW_IMPLEMENTATION=rmw_cyclonedds_cpp:
# 镜像没装 cyclone dds,CMake 配置会失败。
volumes:
- ..:/root/ros2_ws # bind mount 本机 D:\xs\ros2
working_dir: /root/ros2_ws
command: ["bash", "-lc", "tail -f /dev/null"] # 容器永不退
```
### 3.2 为什么 host network
ROS2 默认用 DDS multicast 在同一网段自动发现节点。多容器或跨主机时,**bridge 网络
会拦截 multicast**,导致节点看不见彼此。
`network_mode: host` 让容器用宿主机的网络栈,直接走 multicast。
### 3.3 为什么 bind mount 整个工程
`..` 是 docker-compose.yml 的上一级(`D:\xs\ros2`)。挂到容器内
`/root/ros2_ws`,**你在 Windows 改代码,容器内 colcon 立即看到**(`--symlink-install`)。
---
## 4. 容器生命周期命令
```powershell
# 构建镜像
docker compose -f D:\xs\ros2\docker\docker-compose.yml build
# 启动(后台)
docker compose -f D:\xs\ros2\docker\docker-compose.yml up -d
# 状态
docker compose -f D:\xs\ros2\docker\docker-compose.yml ps
# 进开发终端
docker exec -it ros2_dev bash -lc "source /root/ros2_ws/install/setup.bash && exec bash"
# 一次性跑命令
docker exec ros2_dev bash -lc "cd /root/ros2_ws && colcon build --packages-select py_pubsub"
# 关
docker compose -f D:\xs\ros2\docker\docker-compose.yml down
# 重启
docker compose -f D:\xs\ros2\docker\docker-compose.yml restart
# 删容器(保留镜像)
docker compose -f D:\xs\ros2\docker\docker-compose.yml down
# 删镜像
docker rmi ros2-humble-dev:latest
# 看日志
docker logs -f ros2_dev
```
---
## 5. 一键启动脚本
### 5.1 start.ps1 (Windows)
```powershell
powershell D:\xs\ros2\start.ps1
```
等价于:
```powershell
docker compose build # 首次 5-10min
docker compose up -d
docker exec ros2_dev bash -lc "cd /root/ros2_ws && bash build.sh"
docker exec -it ros2_dev bash -lc "source install/setup.bash && exec bash"
```
### 5.2 start.sh (Linux/macOS)
```bash
./start.sh
```
### 5.3 build.sh (容器内)
```bash
bash build.sh
# 1) source /opt/ros/humble/setup.bash
# 2) colcon build --symlink-install --packages-select <all>
# 3) ls install/
# 4) 打印运行命令速查
```
---
## 6. 调试技巧
### 6.1 看容器日志
```bash
docker logs ros2_dev
docker logs -f ros2_dev # 持续
```
### 6.2 进容器交互 shell
```bash
docker exec -it ros2_dev bash
# 进入后:
source /opt/ros/humble/setup.bash
cd /root/ros2_ws
source install/setup.bash
```
### 6.3 在容器内单次跑命令
```bash
docker exec ros2_dev bash -lc "source /opt/ros/humble/setup.bash && ros2 node list"
```
### 6.4 看 DDS 流量
```bash
docker exec ros2_dev bash -lc "source /opt/ros/humble/setup.bash && ros2 doctor --report"
```
### 6.5 看网络
```bash
docker exec ros2_dev bash -lc "ip addr; ip route"
```
### 6.6 容器与宿主机共享 GPU(可选)
```yaml
services:
ros2:
deploy:
resources:
reservations:
devices:
- driver: nvidia
count: 1
capabilities: [gpu]
```
---
## 7. 常见坑
### 7.1 容器启动后找不到 ros2 命令
容器里 `/opt/ros/humble/setup.bash` 没 source。每个 shell 都要 source,
或者写进 `~/.bashrc`(已加,新 bash 自动 source)。
### 7.2 改了源码但 colcon 看不到
确认 `--symlink-install`(默认配置里加上了)。否则需要重 build。
### 7.3 跨容器看不到节点
- 同一 ROS Domain(`ROS_DOMAIN_ID`)
- host network(本仓库用了)
- multicast 没被拦截
### 7.4 Windows bind mount 文件锁
有时 LSP / IDE 持文件锁,导致容器内 colcon 失败。关掉 IDE 或在容器内编辑。
### 7.5 容器启动很慢
- Docker Desktop 未运行
- WSL2 后端未启用
- 镜像太大(本仓库 ~3GB 正常)
### 7.6 容器 OOM
Docker Desktop → Settings → Resources → Memory 调到 ≥ 4GB。
---
## 8. 多机 / 多机器人扩展
```yaml
# docker-compose.yml 改成:
services:
robot1:
image: ros2-humble-dev:latest
container_name: ros2_robot1
network_mode: host
environment:
- ROS_DOMAIN_ID=42
- ROS_NAMESPACE=robot1
volumes:
- ./robot1_conf:/root/ros2_ws
robot2:
image: ros2-humble-dev:latest
container_name: ros2_robot2
network_mode: host
environment:
- ROS_DOMAIN_ID=42
- ROS_NAMESPACE=robot2
volumes:
- ./robot2_conf:/root/ros2_ws
central:
image: ros2-humble-dev:latest
container_name: ros2_central
network_mode: host
environment:
- ROS_DOMAIN_ID=42
command: ["bash", "-lc", "ros2 launch bringup multi_robot_launch.py"]
```
每个 container 各跑一组节点,`ROS_NAMESPACE` 隔离命名空间,
`ROS_DOMAIN_ID` 共享同一总线 → 跨容器通讯。
详见 [`doc/100-embedded-deployment.md`](100-embedded-deployment.md)。
---
## 9. 在本仓库里跑
### 9.1 一键启
```powershell
powershell D:\xs\ros2\start.ps1
```
### 9.2 进入后跑测试
```bash
docker exec ros2_dev bash -lc "source /opt/ros/humble/setup.bash && cd /root/ros2_ws && colcon test --packages-select py_pubsub cpp_pubsub py_srv py_action_demo cpp_robot_tf2 py_vision_demo bringup"
```
### 9.3 跑 demo
```bash
docker exec ros2_dev bash -lc "source /root/ros2_ws/install/setup.bash && ros2 launch bringup full_demo_launch.py"
```
---
## 10. 进阶:多阶段构建 + 镜像瘦身
### 10.1 当前镜像大小
```bash
docker images ros2-humble-dev
# SIZE: 3.4GB
```
来源:
- `osrf/ros:humble-desktop`: ~2.1GB(包含 RViz / rqt / Gazebo)
- 我们的 apt 包 + colcon: ~1.3GB
### 10.2 改用 `osrf/ros:humble-ros-base`
基础镜像改成 `humble-ros-base`(无 RViz / rqt):
```dockerfile
FROM osrf/ros:humble-ros-base
```
**SIZE 减少 ~1.5GB**。但少了 RViz / rqt / Gazebo,如果需要这些再加 apt。
### 10.3 多阶段构建(进一步瘦身)
```dockerfile
# Stage 1: build
FROM osrf/ros:humble-ros-base AS builder
# ... 装 build 工具,build 我们的包 ...
# Stage 2: runtime
FROM osrf/ros:humble-ros-base
COPY --from=builder /root/ros2_ws/install /root/ros2_ws/install
# 不带 build 工具,更小
```
适合 CI 流水线 + 生产环境。
---
## 接下来读
| 主题 | 文档 |
|---|---|
| venv 工作流 | [`02-virtualenv.md`](02-virtualenv.md) |
| 测试策略 | [`90-testing.md`](90-testing.md) |
| 三机部署 | [`100-embedded-deployment.md`](100-embedded-deployment.md) |
| 项目总览 | [`00-overview.md`](00-overview.md) |
doc/90-testing.md
+457
View File
@@ -0,0 +1,457 @@
# 90 · 测试策略(完全指南)
> **目标**:理解 ROS2 三层测试金字塔,会用 gtest / pytest / colcon test,能把本仓库的测试策略用到自己的项目。
---
## 目录
- [1. 测试金字塔](#1-测试金字塔)
- [2. C++ gtest 完整指南](#2-c-gtest-完整指南)
- [3. Python pytest 完整指南](#3-python-pytest-完整指南)
- [4. launch_testing(集成测试 launch 文件)](#4-launch_testing集成测试-launch-文件)
- [5. 本仓库测试策略详解](#5-本仓库测试策略详解)
- [6. 端到端自动化(进阶)](#6-端到端自动化进阶)
- [7. 标记 / 跳过 / 覆盖率](#7-标记--跳过--覆盖率)
- [8. CI 集成(GitHub Actions)](#8-ci-集成github-actions)
- [9. 在本仓库里跑](#9-在本仓库里跑)
- [10. 进阶:Mock DDS / 时间注入 / Fault Injection](#10-进阶mock-dds--时间注入--fault-injection)
---
## 1. 测试金字塔
```
┌────────────────┐
│ 端到端 E2E │ ← launch_testing + 真实 launch
│ (慢 / 1-2 个) │
├────────────────┤
│ 集成 in-process │ ← 同进程 spin + DDS 单跑
│ (中等 / 4-6) │
├────────────────┤
│ 单元 Unit │ ← gtest / pytest 单函数
│ (快 / 大量) │
└────────────────┘
```
| 层 | 跑在哪 | 速度 | 数量 |
|---|---|---|---|
| 单元 | 容器内 / 本机 venv | 秒级 | 几十 ~ 几百 |
| 集成 in-process | 容器内(colcon test) | 秒 ~ 分钟 | 几个 ~ 几十 |
| 端到端 E2E | launch + 真实 DDS | 分钟级 | 1-5 个核心场景 |
**本仓库**:
- 单元 + 集成:`colcon test` 一键跑(10 用例,秒级)
- 端到端:固化 5 个 `docker/*_e2e.log` 手动验证
---
## 2. C++ gtest 完整指南
### 2.1 测试文件
```cpp
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
class PubsubTest : public ::testing::Test {
protected:
static void SetUpTestSuite() {
rclcpp::init(0, nullptr); // 同进程共享一个 context
}
static void TearDownTestSuite() {
rclcpp::shutdown();
}
};
TEST_F(PubsubTest, MessageDataFieldIsNonEmpty) {
std_msgs::msg::String msg;
msg.data = "hi";
EXPECT_FALSE(msg.data.empty());
EXPECT_EQ(msg.data, "hi");
}
TEST_F(PubsubTest, PublisherOnce) {
auto node = std::make_shared<rclcpp::Node>("test_node");
auto pub = node->create_publisher<std_msgs::msg::String>("chatter", 10);
auto msg = std_msgs::msg::String();
msg.data = "unit-test";
pub->publish(msg);
auto exec = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
exec->add_node(node);
end = std::chrono::steady_clock::now() + std::chrono::seconds(1);
while (std::chrono::steady_clock::now() < end) {
exec->spin_some(50ms);
}
SUCCEED();
}
```
### 2.2 CMakeLists.txt 集成
```cmake
if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
ament_add_gtest(test_pub_sub test/test_pub_sub.cpp)
ament_target_dependencies(test_pub_sub rclcpp std_msgs)
endif()
```
### 2.3 跑测试
```bash
colcon build --packages-select cpp_pubsub
colcon test --packages-select cpp_pubsub
# 看详细
cat build/cpp_pubsub/test_results/cpp_pubsub/test_pub_sub.gtest.xml
```
### 2.4 关键 API
| API | 用途 |
|---|---|
| `TEST(suite, name)` | 测试用例 |
| `TEST_F(FixtureName, name)` | 用 fixture |
| `EXPECT_*`(非致命) / `ASSERT_*`(致命) | 断言 |
| `SetUp() / TearDown()` | 每个用例前后 |
| `SetUpTestSuite() / TearDownTestSuite()` | 所有用例前后 |
---
## 3. Python pytest 完整指南
### 3.1 测试文件
```python
import rclpy
import pytest
from py_pubsub.publisher_member_function import Talker
@pytest.fixture(scope='module')
def ros_context():
"""rclpy 是进程级单例,模块前 init,完成后 shutdown。"""
rclpy.init()
yield
rclpy.shutdown()
def test_talker_init(ros_context):
node = Talker()
assert node.get_name() == 'talker_py'
def test_inproc_roundtrip(ros_context):
"""同进程 spin Talker + 自建 subscriber,验证消息流。"""
import time
from sensor_msgs.msg import String
talker = Talker()
received = []
sub_node = rclpy.node.Node('test_subscriber')
sub_node.create_subscription(
String, 'chatter',
lambda msg: received.append(msg.data), 10)
exec_ = rclpy.executors.SingleThreadedExecutor()
exec_.add_node(talker)
exec_.add_node(sub_node)
end = time.time() + 1.5
while time.time() < end:
exec_.spin_once(timeout_sec=0.05)
assert any('Hello from PY' in s for s in received)
```
### 3.2 同进程 spin(关键模式)
```python
exec_ = rclpy.executors.SingleThreadedExecutor()
exec_.add_node(talker)
exec_.add_node(listener)
end = time.time() + timeout_sec
while time.time() < end:
exec_.spin_once(timeout_sec=0.05)
```
**`spin_once(timeout)`** 在主循环里手动驱动事件循环,**比 `rclpy.spin()` 易控制超时**。
### 3.3 pytest 标记(marks)
```python
@pytest.mark.ros # 需要 ROS2 环境
@pytest.mark.inproc # 同进程可跑
@pytest.mark.slow # 跑得慢
```
CLI:
```bash
pytest -m "not slow"
pytest -m ros and not inproc
```
### 3.4 pytest 配置(pyproject.toml)
本仓库已在 `pyproject.toml` 配:
```toml
[tool.pytest.ini_options]
markers = [
"ros: 需要 ROS2 运行环境",
"inproc: 同进程内 spin",
]
```
---
## 4. launch_testing(集成测试 launch 文件)
```python
import launch
import launch_ros
import launch_testing
import launch_testing.actions
import pytest
@pytest.mark.launch_test
def generate_test_description():
return launch.LaunchDescription([
launch_ros.actions.Node(
package='py_pubsub',
executable='talker',
parameters=[{'period_ms': 100, 'topic': 'chatter_test'}]),
launch_ros.actions.Node(
package='py_pubsub',
executable='listener',
parameters=[{'topic': 'chatter_test'}]),
launch_testing.actions.ReadyToTest(),
])
class TestPubSub(unittest.TestCase):
def test_message_received(self, proc_output):
assert proc_output.assertWaitFor(
'recv:', timeout=5, stream='stdout'
)
```
**注意**: launch_testing 在 colcon test 环境下与节点 rclpy.shutdown() 二次调用可能冲突,本仓库改用更稳的 in-process spin。
---
## 5. 本仓库测试策略详解
### 5.1 单元 + 集成 in-process(主测试)
**Python 包** 用 pytest + in-process spin:
```python
@pytest.fixture(scope='module')
def ros_context():
rclpy.init()
yield
rclpy.shutdown()
def test_xxx(ros_context):
server = SomeServer()
client = SomeClient()
exec_ = rclpy.executors.SingleThreadedExecutor()
exec_.add_node(server); exec_.add_node(client)
# ... 触发 + 验证
```
**C++ 包** 用 gtest + ament_add_gtest:
```cpp
TEST_F(MyFixture, SpinSomeWorks) {
auto node = std::make_shared<MyNode>();
auto exec = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
exec->add_node(node);
exec->spin_some(50ms);
}
```
### 5.2 端到端(固化日志,手工验证)
5 个核心场景,跑一遍固化到 `docker/`:
- `bringup_e2e.log`:Topic 4 节点跨包跨语言
- `srv_e2e.log`:Service 12+30=42
- `robot_e2e.log`:URDF + TF 实时打印
- `vision_e2e.log`:Image 流
- `full_demo_e2e.log`:11 节点一起
每次大改动后重跑 + 替换日志。
### 5.3 测试覆盖现状
| 包 | 测试 | 用例数 | 通过率 |
|---|---|---|---|
| py_pubsub | pytest + in-process | 4/4 | 100% |
| cpp_pubsub | gtest | 2/2 | 100% |
| py_srv | pytest + Service | 1/1 | 100% |
| py_action_demo | pytest + Action | 1/1 | 100% |
| cpp_robot_tf2 | gtest + TF2 | 2/2 | 100% |
| py_vision_demo | pytest + Image | 2/2 | 100% |
| bringup | launch 6 文件就绪 | OK | 100% |
**合计: 10/10 测试 100% PASSED**。
---
## 6. 端到端自动化(进阶)
```python
@pytest.mark.launch_test
def generate_test_description():
return launch.LaunchDescription([
IncludeLaunchDescription(
PythonLaunchDescriptionSource(<bringup>/launch/full_demo_launch.py)),
launch_testing.actions.ReadyToTest(),
])
class TestFullDemo(unittest.TestCase):
def test_all_nodes_running(self):
# 跑 ros2 node list 子进程,看 11 个节点
...
```
详见 [`20-topics.md`](20-topics.md) §11.3 录包回放。
---
## 7. 标记 / 跳过 / 覆盖率
### 7.1 标记
```python
@pytest.mark.skip(reason="硬件没到位")
def test_xxx():
...
@pytest.mark.skipif(sys.platform == 'win32', reason="Windows 跑不动")
def test_yyy():
...
@pytest.mark.xfail(reason="已知 bug") # 标记预期失败
def test_zzz():
...
```
### 7.2 覆盖率
```bash
# Python
pytest --cov=py_pubsub --cov-report=html src/py_pubsub/test
# 输出 build/htmlcov/index.html
```
```bash
# C++(用 lcov)
sudo apt install lcov
cd build/cpp_pubsub && lcov --capture --directory . --output-file coverage.info
genhtml coverage.info --output-directory coverage_html
```
---
## 8. CI 集成(GitHub Actions)
```yaml
# .github/workflows/test.yml
name: test
on: [push, pull_request]
jobs:
test:
runs-on: ubuntu-22.04
steps:
- uses: actions/checkout@v4
- name: Build image & test
run: |
docker compose -f docker/docker-compose.yml build
docker compose -f docker/docker-compose.yml up -d
docker exec ros2_dev bash -lc "cd /root/ros2_ws && colcon build --packages-select py_pubsub cpp_pubsub py_srv py_action_demo cpp_robot_tf2 py_vision_demo bringup"
docker exec ros2_dev bash -lc "cd /root/ros2_ws && colcon test --packages-select py_pubsub cpp_pubsub py_srv py_action_demo cpp_robot_tf2 py_vision_demo bringup"
```
---
## 9. 在本仓库里跑
### 9.1 跑全部测试
```bash
docker exec ros2_dev bash -lc "source /opt/ros/humble/setup.bash && cd /root/ros2_ws && colcon test --packages-select py_pubsub cpp_pubsub py_srv py_action_demo cpp_robot_tf2 py_vision_demo bringup"
```
### 9.2 单包测试
```bash
docker exec ros2_dev bash -lc "cd /root/ros2_ws && colcon test --packages-select cpp_pubsub"
```
### 9.3 看测试日志
```bash
# gtest
cat build/cpp_pubsub/test_results/cpp_pubsub/test_pub_sub.gtest.xml
# pytest
cat build/py_pubsub/pytest.xml
```
### 9.4 当前结果(固化)
```
✅ cpp_pubsub: 2/2 PASSED
✅ py_pubsub: 4/4 PASSED
✅ py_srv: 1/1 PASSED
✅ py_action_demo: 1/1 PASSED
✅ cpp_robot_tf2: 2/2 PASSED
✅ py_vision_demo: 2/2 PASSED
✅ bringup: OK (no tests)
合计: 10/10 PASSED
```
---
## 10. 进阶:Mock DDS / 时间注入 / Fault Injection
### 10.1 Mock DDS(测试纯逻辑)
```python
# 不起 ROS2 上下文,直接测算法
def test_message_parser():
raw = b'\x00\x01hello'
parsed = MyParser.parse(raw)
assert parsed == 'hello'
```
### 10.2 时间注入
```cpp
// C++: 用 Clock 抽象 + RosTime mock
node->set_parameter(rclcpp::Parameter("use_sim_time", true));
// 然后通过 /clock topic 推时间
```
### 10.3 Fault Injection
```python
# 杀掉 server 模拟断网
subprocess.run(['pkill', '-9', '-f', 'add_two_ints_server'])
# 看 client 怎么处理
# 慢响应模拟
# 修改 server callback sleep 时间
```
---
## 接下来读
| 主题 | 文档 |
|---|---|
| 三机部署 | [`100-embedded-deployment.md`](100-embedded-deployment.md) |
| 具身智能路径 | [`99-embodied-ai.md`](99-embodied-ai.md) |
| Docker 开发 | [`85-docker.md`](85-docker.md) |
doc/99-embodied-ai.md
+578
View File
@@ -0,0 +1,578 @@
# 99 · 入门具身智能:从 ROS2 到 VLA / 机器人(完全学习路线)
> **目标**:为"想用本仓库入门具身智能"的开发者,画一张**清晰、可执行、有时间估算**的学习路线图。
> 读完这篇,你就知道"接下来该装什么、写什么、跑什么"。
---
## 目录
- [0. 你现在的位置](#0-你现在的位置)
- [1. ROS2 全栈知识点地图](#1-ros2-全栈知识点地图)
- [2. 5 个学习阶段(完全路线)](#2-5-个学习阶段完全路线)
- [3. 阶段 1: 通信 + TF + 视觉(本仓库已完)](#3-阶段-1-通信--tf--视觉本仓库已完)
- [4. 阶段 2: 仿真机械臂(ros2_control + MoveIt2)](#4-阶段-2-仿真机械臂ros2_control--moveit2)
- [5. 阶段 3: 物理仿真(Gazebo / ros_gz)](#5-阶段-3-物理仿真gazebo--ros_gz)
- [6. 阶段 4: 视觉 + 抓取(GraspNet / 6-DoF)](#6-阶段-4-视觉--抓取graspnet--6-dof)
- [7. 阶段 5: 真机 + VLA(OpenVLA / Pi0)](#7-阶段-5-真机--vlaopenvla--pi0)
- [8. 3 个实战项目模板(从本仓库出发)](#8-3-个实战项目模板从本仓库出发)
- [9. 学习资源 + 时间表](#9-学习资源--时间表)
- [10. FAQ](#10-faq)
---
## 0. 你现在的位置
### 已掌握 ✅(本仓库覆盖)
- ROS2 通信:Topic / Service / Action / Parameter
- TF2 坐标变换 + URDF 描述
- sensor_msgs/Image + cv_bridge
- JointState 发布(模拟)
- launch 文件 + 测试金字塔
- Docker 容器化开发
- 跨包跨语言互通
### 还没掌握 ❌(缺口)
- ros2_control 硬件驱动抽象层
- MoveIt2 运动规划(IK / path / collision)
- Gazebo / ros_gz 物理仿真
- 真实机械臂 URDF(6-7 DoF)+ Gripper
- 手眼标定(eye-in-hand / eye-to-hand)
- Cartesion path planning
- 力控 / Force-Torque sensor
- 真实电机驱动(Bus Servo / EtherCAT)
- GraspNet / 6-DoF 抓取
- VLA 模型接入(OpenVLA / Pi0)
- 移动底盘(Nav2 / SLAM)
- 多机协调 / ROS2 多机器人
---
## 1. ROS2 全栈知识点地图
```
ROS2 全栈
│
┌───────────────────────────┼───────────────────────────┐
│ │ │
通信层(本仓库) 机器人层 应用层
│ │ │
┌─────┼─────┐ ┌───────┼───────┐ ┌───────┼───────┐
│ │ │ │ │ │ │ │ │
Topic Srv Action URDF ros2_ctrl MoveIt2 Nav2 VLA Gazebo
│ │ │ │ │ │ │ │ │
└─────┴─────┘ └───────┴───────┘ └───────┴───────┘
│ │ │
7 包已覆盖 需要继续补 需要仿真/真机
```
---
## 2. 5 个学习阶段(完全路线)
```
本仓库 ──── 阶段 1 (本仓库) ──── 阶段 2 (仿真机械臂) ──── 阶段 3 (Gazebo)
│ │
│ ▼
│ 阶段 4 (视觉抓取)
│ │
│ ▼
└─────────────────────────────────────────────────── 阶段 5 (真机+VLA)
```
| 阶段 | 主题 | 工作量 | 学习产出 |
|---|---|---|---|
| **1. 通信 + TF + 视觉** | 本仓库 | 已完成 | ROS2 全栈基础 |
| **2. 仿真机械臂** | ros2_control + MoveIt2 + 6-DoF URDF | 2-3 周 | 能用 MoveIt2 算关节轨迹 |
| **3. 物理仿真** | Gazebo + ros_gz | 1-2 周 | 离线测试机械臂 / 视觉 |
| **4. 视觉 + 抓取** | GraspNet / 6-DoF + 手眼标定 | 3-4 周 | "看到 → 抓" 端到端 |
| **5. 真机 + VLA** | OpenVLA / Pi0 + ros2_control + 真电机 | 4-8 周 | 具身智能 MVP |
**总计: 3-5 个月全职工作量**(取决于每周投入时间)。
---
## 3. 阶段 1: 通信 + TF + 视觉(本仓库已完)
### 验证清单
- [x] Topic pub/sub 跨语言互通(听 cpp 收 py / 听 py 收 cpp)
- [x] Service 同步 req/resp(12+30=42 验证)
- [x] Action 三件套(Fibonacci 验证)
- [x] TF 坐标变换(gripper 在 base_link 下位置)
- [x] Image + cv_bridge(图像流验证)
- [x] 10/10 测试 100% 通过
### 你应该已经能
- 写 Topic pub/sub 节点(单语言)
- 写 launch 文件
- 用 ros2 CLI 调试
- 理解 DDS 自动发现
- 跑 colcon build + test
---
## 4. 阶段 2: 仿真机械臂(ros2_control + MoveIt2)
### 4.1 安装清单
```bash
# PC 上(本机)
sudo apt install -y \
ros-humble-ros2-control \
ros-humble-ros2-controllers \
ros-humble-moveit \
ros-humble-moveit-resources \
ros-humble-moveit-servo \
ros-humble-moveit-ros-planners
```
预计下载 ~1GB。
### 4.2 写 6-DoF 机械臂 URDF
本仓库的 `cpp_robot_tf2/urdf/simple_arm.urdf` 是 3 关节玩具臂。
升级步骤:
1. 找一款**开源 6-DoF 机械臂 URDF**(推荐):
- [`panda_arm.urdf`](https://github.com/ros-planning/moveit_resources/tree/gh-pages/panda) - Franka Panda
- [`ur5.urdf`](https://github.com/ros-industrial/universal_robot/tree/ros2-devel) - UR5
- [`xarm6.urdf`](https://github.com/xArm-Robotics/xarm_ros2) - Ufactory xArm 6
2. 把它拷到 `src/<新包>/urdf/`
3. **加 `<transmission>` 标签**(ros2_control 需要)
4. **加 `<gazebo>` 标签**(后续 Gazebo 用,可选)
5. `check_urdf <urdf>` 校验
URDF 关键改造:
```xml
<!-- 关节加 transmission -->
<joint name="joint1" type="revolute">
<parent link="base_link"/>
<child link="link1"/>
<origin xyz="0 0 0.05" rpy="0 0 0"/>
<axis xyz="0 0 1"/>
<limit lower="-3.14" upper="3.14" effort="100" velocity="1.0"/>
<!-- ros2_control 需要 -->
<transmission name="trans_joint1">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint1">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="motor_joint1">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</joint>
```
### 4.3 ros2_control 配置
`src/my_arm_controller/config/my_controllers.yaml`:
```yaml
controller_manager:
ros__parameters:
update_rate: 100
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateController
arm_controller:
type: position_controllers/JointGroupPositionController
arm_controller:
ros__parameters:
joints:
- joint1
- joint2
- joint3
- joint4
- joint5
- joint6
```
### 4.4 ros2_control launch
`src/my_arm_controller/launch/arm_control.launch.py`:
```python
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os
def generate_launch_description():
pkg_share = get_package_share_directory('my_arm_controller')
config = os.path.join(pkg_share, 'config', 'my_controllers.yaml')
urdf = os.path.join(pkg_share, 'urdf', 'arm.urdf')
with open(urdf, 'r') as f: urdf_content = f.read()
return LaunchDescription([
Node(package='controller_manager', executable='ros2_control_node',
parameters=[{'robot_description': urdf_content, 'update_rate': 100}],
output='screen'),
Node(package='controller_manager', executable='spawner',
arguments=['joint_state_broadcaster', '-c', '/controller_manager'],
output='screen'),
Node(package='controller_manager', executable='spawner',
arguments=['arm_controller', '-c', '/controller_manager'],
output='screen'),
])
```
### 4.5 MoveIt2 SRDF(从 URDF 生成)
```bash
ros2 launch moveit_setup_assistant setup_assistant.launch.py
```
在 MoveIt Setup Assistant 里:
1. Load URDF
2. Add Virtual Joint(base_link → world)
3. Add Planning Group("arm" 包含 6 个 joint)
4. Add Poses(home / ready / extended)
5. Add End Effector(gripper)
6. Add Passive Joints
7. **Generate** SRDF + MoveIt 配置
### 4.6 MoveIt2 编程
```python
# PC 上: MoveIt2 客户端 → /joint_trajectory → ros2_control → 电机
from moveit.planning import MoveItPy
from moveit.core import PlanningComponent
moveit = MoveItPy(node=your_node)
arm = moveit.get_planning_component("arm")
arm.set_start_state_to_current_state()
arm.set_goal_state_from_pose_stamped(goal_pose)
plan_result = arm.plan()
if plan_result:
arm.execute(plan_result.trajectory, blocking=True)
```
**预期效果**: 在 RViz 点目标位姿 → MoveIt2 算关节轨迹 → ros2_control 执行 → 真实机械臂(或仿真臂)运动。
### 4.7 阶段 2 验证清单
- [ ] ros2_control 6 关节都能 set position
- [ ] MoveIt2 RViz 里 plan + execute 能看到机械臂动
- [ ] 写 Python 节点调用 MoveIt2 API,能指定目标 pose 让臂到达
- [ ] 加 UnitTest 验证 motion plan 正确性
**预计 2-3 周**。完成后你能:"告诉末端去哪个 pose,MoveIt2 算出关节轨迹,ros2_control 执行"。
---
## 5. 阶段 3: 物理仿真(Gazebo / ros_gz)
### 5.1 安装
```bash
sudo apt install -y ros-humble-ros-gz
```
### 5.2 把 URDF 改成 Gazebo 模型
加 `<gazebo>` 标签 + `<transmission>`(已在阶段 2 加)→ gazebo_ros2_control 自动识别。
### 5.3 Gazebo Sim 启动
```bash
ros2 launch ros_gz_sim gz_sim.launch.py gz_args:="-r empty.sdf"
# 加载机械臂
ros2 run ros_gz_sim create -world default \
-file ~/ros2_ws/src/my_arm/urdf/arm.urdf \
-name my_arm \
-x 0 -y 0 -z 1
```
### 5.4 ros_gz_bridge
把 Gazebo 的 Topic 桥到 ROS2:
- `/clock` ←→ `/clock`(仿真时间)
- `/world/default/model/my_arm/joint_state` ← `/joint_states`
- `/cmd_vel` → Gazebo twist controller
### 5.5 仿真 + MoveIt2
```bash
# 终端 1: Gazebo
ros2 launch my_arm gazebo.launch.py
# 终端 2: ros2_control spawner
ros2 launch my_arm ros2_control.launch.py
# 终端 3: MoveIt2
ros2 launch my_arm moveit.launch.py
# 终端 4: 写 Python 脚本,MoveIt2 算轨迹 → 发到 Gazebo → 看可视化
```
**预计 1-2 周**。完成后你能"在仿真里训练 / 验证 / 演示"。
---
## 6. 阶段 4: 视觉 + 抓取(GraspNet / 6-DoF)
### 6.1 硬件升级
- 加 RGB-D 相机(RealSense D435 / Azure Kinect)
- 加力矩传感器(可选)
### 6.2 相机驱动
```bash
sudo apt install -y ros-humble-realsense2-camera
# 或:
sudo apt install -y ros-humble-azure-kinect
```
发布 `/camera/color/image_raw` + `/camera/depth/image_rect_raw` + `/camera/depth/color/points`。
### 6.3 6-DoF GraspNet 集成
```python
# grasp_detector 节点
import rospy
from sensor_msgs.msg import Image, PointCloud2
from geometry_msgs.msg import PoseStamped
class GraspNetNode:
def __init__(self):
self.image_sub = rospy.Subscriber('/camera/color/image_raw', Image, self.image_cb)
self.cloud_sub = rospy.Subscriber('/camera/depth/color/points', PointCloud2, self.cloud_cb)
self.grasp_pub = rospy.Publisher('/grasp_pose', PoseStamped, queue_size=10)
def image_cb(self, msg):
# 1) 用 GraspNet 模型推理
# 2) 输出 6-DoF grasp pose
# 3) publish /grasp_pose
pass
```
### 6.4 手眼标定(eye-in-hand)
```bash
sudo apt install -y ros-humble-easy-handeye
# 标定流程:
# 1) 启动相机 + 标定板 + MoveIt2
# 2) 移动机械臂到多个 pose
# 3) easy_handeye 离线计算 camera_link → gripper 静态 TF
# 4) 把 TF 写进 URDF
```
### 6.5 端到端抓取
```
vision_node (YOLO/GraspNet)
↓ /grasp_pose
grasp_planner (MoveIt2 算轨迹)
↓ /joint_trajectory
ros2_control (PID 闭环)
↓
电机 → 真实 / 仿真机械臂 → 抓取
```
### 6.6 阶段 4 验证
- [ ] 摄像头能 publish 图像 + 点云
- [ ] GraspNet 能输出 6-DoF grasp pose
- [ ] MoveIt2 根据 grasp pose 算出无碰撞轨迹
- [ ] 仿真里端到端跑通"看到物体 → grasp → 抓取"
**预计 3-4 周**。完成后你能做"6-DoF 抓取 demo"。
---
## 7. 阶段 5: 真机 + VLA(OpenVLA / Pi0)
### 7.1 VLA 是什么
**VLA = Vision-Language-Action**,即"看图 + 听指令 → 输出动作"的端到端模型:
- OpenVLA(7B 参数)
- RT-2(Google)
- Pi0(Physical Intelligence)
- Octo
### 7.2 VLA 在 ROS2 里的角色
```
RGB Image + "Pick up the apple" → VLA Model
↓
7-DoF Joint Trajectory
↓
ros2_control
↓
真电机
```
### 7.3 集成示例
```python
# vla_inference_node
import torch
from vla_model import OpenVLA # 或 Pi0
from sensor_msgs.msg import Image
from trajectory_msgs.msg import JointTrajectory
class VLANode:
def __init__(self):
self.model = OpenVLA.from_pretrained("openvla-7b")
self.image_sub = rospy.Subscriber('/camera/color/image_raw', Image, self.cb)
self.cmd_pub = rospy.Publisher('/joint_trajectory', JointTrajectory, queue_size=10)
def cb(self, msg):
image = self.bridge.imgmsg_to_cv2(msg, 'rgb8')
instruction = rospy.get_param('~instruction', 'pick up the apple')
action = self.model.predict(image, instruction)
self.cmd_pub.publish(self.action_to_trajectory(action))
```
### 7.4 真机部署清单
| 项 | 推荐 |
|---|---|
| 6-DoF 机械臂 | Ufactory xArm 6 / Franka Panda(二手) / 国产 6-DoF |
| 相机 | Intel RealSense D435i(深度 + RGB) |
| 控制器 | ROS2 + ros2_control(已学) |
| 主机 | PC / RDK X5(NPU 加速 VLA) |
| 网络 | 千兆 LAN(DDS + 视觉流) |
### 7.5 阶段 5 验证
- [ ] VLA 模型加载到 PC / RDK X5
- [ ] 输入图像 + 指令 → 输出关节轨迹
- [ ] 真实机械臂(或者 Gazebo)执行轨迹
- [ ] 抓取成功率 > 50%(MVP)
**预计 4-8 周**。
---
## 8. 3 个实战项目模板(从本仓库出发)
### 项目 A:桌面级 6-DoF 机械臂抓取
**目标**: 给定桌面物体的 6-DoF 位姿,机械臂抓起来放到指定位置。
```
栈:
- 本仓库(通信 + TF + 视觉)
+ 6-DoF URDF + ros2_control
+ MoveIt2 规划
+ GraspNet(可选,先用固定位姿也行)
+ Gazebo 仿真
```
**改造点**:
- `src/cpp_robot_tf2/urdf/simple_arm.urdf` → 替换为 6-DoF URDF
- 新增 `src/<your_arm>/config/controllers.yaml`
- 新增 `src/<your_arm>/launch/<arm>_bringup.launch.py`
- 写 `pick_and_place` Python 节点
### 项目 B:差速底盘 + OpenVLA 抓桌面物体
**目标**: 移动底盘 + 机械臂 + VLA,自主导航到目标 + 抓取。
```
栈:
- 本仓库
+ ros2_control (底盘 diff_drive_controller + 机械臂 controller)
+ Nav2 (底盘导航)
+ MoveIt2 (机械臂)
+ OpenVLA / Pi0 (高层策略)
```
**改造点**: 在项目 A 基础上 + 底盘 URDF + Nav2 配置 + VLA 模型集成。
### 项目 C:ALOHA 双臂遥操 + Pi0 学习
**目标**: 双机械臂 ALOHA 配置 + 遥操 + Pi0 fine-tune + 部署。
```
栈:
- 本仓库
+ 双手臂 6+6 DoF
+ Gazebo + ALOHA 模型
+ Pi0 训练数据收集(bag 录制遥操)
+ Pi0 fine-tune + 部署
```
**改造点**: 在项目 A + 镜像配置 + 双臂 URDF + 数据收集脚本。
---
## 9. 学习资源 + 时间表
### 9.1 官方文档
- [ROS2 Humble Docs](https://docs.ros.org/en/humble/) - API + tutorials
- [REP 103](https://www.ros.org/reps/rep-0103.html) - 坐标约定
- [REP 105](https://www.ros.org/reps/rep-0105.html) - TF 语义
- [REP 2002](https://www.ros.org/reps/rep-2002.html) - ROS2 设计
### 9.2 中文社区
- [鱼香 ROS](https://fishros.org/) - 入门 + 工具链最友好
- [古月居](https://www.guyuehome.com/) - ROS2 实战
- [Autolabor](https://www.autolabor.com.cn/) - 仿真教程
### 9.3 开源项目
- [ros2_control_demos](https://github.com/ros-controls/ros2_control_demos) - 完整 demo
- [moveit2_tutorials](https://github.com/moveit/moveit2_tutorials)
- [nav2_tutorials](https://github.com/ros-navigation/navigation2_tutorials)
- [ros_gz](https://github.com/gazebosim/ros_gz) - Gazebo 集成
### 9.4 VLA / 具身智能
- [OpenVLA](https://openvla.github.io/)
- [Pi0](https://physicalintelligence.company/blog/pi0)
- [Octo](https://octo-models.github.io/)
### 9.5 总时间表
| 阶段 | 工作 | 时间(全职) |
|---|---|---|
| 1 | 本仓库 | 已完 |
| 2 | ros2_control + MoveIt2 + 6-DoF | 2-3 周 |
| 3 | Gazebo | 1-2 周 |
| 4 | 视觉 + GraspNet | 3-4 周 |
| 5 | 真机 + VLA | 4-8 周 |
| **总计** | | **3-5 月** |
---
## 10. FAQ
### Q1: 我可以跳阶段吗?
可以,但**不推荐**:
- 跳过阶段 2 直接搞 VLA: VLA 输出关节轨迹,你不理解轨迹,debug 不了
- 跳过阶段 3 直接真机: 真机调试成本高(电机可能撞机)
### Q2: 必须做 Gazebo 仿真吗?
不一定,但**强烈建议**:
- 没有仿真 = 直接真机,出错成本高
- Gazebo = 离线验证,出问题不伤人
### Q3: VLA 模型有多大?
OpenVLA 7B 约 15GB,需要 GPU(NVIDIA RTX 3090+)。
轻量方案: OpenVLA-OFT,Pi0-Lite,LLaRA 等。
### Q4: 我只有 1 个 RK3506,够吗?
可以做,但建议至少:
- 1 个 RK3506: 电机控制
- 1 个 RDK X5: VLA 推理(NPU)
- 1 个 PC: 决策 + 可视化
### Q5: 真机能直接用本仓库的代码吗?
**部分可以**:
- ✅ Topic / Service / Action / TF 通信: 直接用
- ❌ JointState 发布(`cpp_robot_tf2`): 是模拟的,要改读编码器
- ❌ MoveIt2 / ros2_control: 本仓库没,需要补
---
## 下一步
按你**当前状态**选:
| 你现在的状态 | 推荐 |
|---|---|
| 学完本仓库,想做机器人 | → 阶段 2(ros2_control + MoveIt2) |
| 想做 VLA | → 阶段 2(先把硬件层打通)→ 阶段 5 |
| 只想做软件/规划算法 | → 阶段 2 + 3(全程仿真,不上硬件) |
| 想做移动底盘 | → 阶段 2(底盘) + 阶段 5(Nav2) |
每阶段做完后,**回到本仓库做一次 colcon test 验证 ROS2 通信基础仍然稳**。
加油,具身智能之旅! 🚀
docker/Dockerfile
+50
View File
@@ -0,0 +1,50 @@
# =============================================================================
# Dockerfile —— ROS2 Humble 开发环境的镜像定义。
#
# 基础镜像:osrf/ros:humble-desktop
# 这是 OSRF(Robotis/Open Source Robotics Foundation)官方维护的 ROS2
# 桌面版镜像,内含:
# - ROS2 Humble 完整运行时(rclcpp/rclpy/ros2 CLI/DDS 默认 fastdds)
# - rviz2 / rqt / gazebo 等桌面工具
# - colcon / rosdep 等构建工具
#
# 在它之上我们再加:
# - python3-colcon-common-extensions:colcon 的 Python 插件
# - python3-pip + colcon-argcomplete :命令补全 (Ros2 CLI tab 补全)
# - nano / curl :调试常用工具
# =============================================================================
# FROM:基础镜像。也可选 osrf/ros:humble-desktop-full 或 ros:humble-ros-base。
FROM osrf/ros:humble-desktop
# DEBIAN_FRONTEND=noninteractive:
# 容器里 apt 不再弹出交互式提示,CI/Docker 必备。
ENV DEBIAN_FRONTEND=noninteractive
# ROS_DISTRO:环境里 source /opt/ros/<distro>/setup.bash 时用到的版本标记。
ENV ROS_DISTRO=humble
# WORKSPACE:容器内统一的工作空间根目录。
ENV WORKSPACE=/root/ros2_ws
# RUN apt-get update && install ...:
# 一次性安装 colcon / pip / nano / curl。
# --no-install-recommends 可减小体积,这里为了图片完整不解开。
# 最后清理 /var/lib/apt/lists/* 以减小镜像层大小。
RUN apt-get update && apt-get install -y \
python3-colcon-common-extensions \
python3-pip \
nano \
curl \
&& rm -rf /var/lib/apt/lists/*
# pip 升级 colcon-argcomplete,提供 ros2 / colcon 命令的 Bash 自动补全。
RUN pip3 install -U colcon-argcomplete colcon-common-extensions
# 写 ~/.bashrc:每次 bash 启动自动 source ROS2 + colcon 补全。
RUN echo 'source /opt/ros/humble/setup.bash' >> /root/.bashrc && \
echo 'source /usr/share/colcon_argcomplete/hook/colcon-argcomplete.bash' >> /root/.bashrc
# WORKDIR:之后所有 RUN / CMD 默认在此目录下执行;同时 docker exec 默认落在这里。
WORKDIR ${WORKSPACE}
# 默认命令:起 bash。docker exec 会覆盖,所以这里只用于 `docker run` 场景。
CMD ["bash"]
docker/bringup_e2e.log
+357
View File
@@ -0,0 +1,357 @@
[INFO] [launch]: All log files can be found below /root/.ros/log/2026-08-03-07-36-51-564894-docker-desktop-122
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [talker-1]: process started with pid [124]
[INFO] [listener-2]: process started with pid [126]
[INFO] [talker-3]: process started with pid [128]
[INFO] [listener-4]: process started with pid [130]
[talker-3] [INFO] [1785742613.034432226] [talker_cpp]: talker_cpp started -> topic=chatter, period=500ms
[listener-4] [INFO] [1785742613.246172624] [listener_cpp]: listener_cpp subscribed <- chatter
[talker-3] [INFO] [1785742613.534695653] [talker_cpp]: pub: "Hello from C++, seq=0"
[listener-4] [INFO] [1785742613.534881572] [listener_cpp]: recv: "Hello from C++, seq=0"
[talker-3] [INFO] [1785742614.034732100] [talker_cpp]: pub: "Hello from C++, seq=1"
[listener-4] [INFO] [1785742614.034995977] [listener_cpp]: recv: "Hello from C++, seq=1"
[talker-3] [INFO] [1785742614.534990040] [talker_cpp]: pub: "Hello from C++, seq=2"
[listener-4] [INFO] [1785742614.535315902] [listener_cpp]: recv: "Hello from C++, seq=2"
[talker-3] [INFO] [1785742615.034805507] [talker_cpp]: pub: "Hello from C++, seq=3"
[listener-4] [INFO] [1785742615.035001484] [listener_cpp]: recv: "Hello from C++, seq=3"
[talker-3] [INFO] [1785742615.534888516] [talker_cpp]: pub: "Hello from C++, seq=4"
[listener-4] [INFO] [1785742615.535129226] [listener_cpp]: recv: "Hello from C++, seq=4"
[talker-1] [INFO] [1785742615.579383621] [talker_py]: talker_py started -> topic=chatter, period=500ms
[listener-2] [INFO] [1785742615.630433398] [listener_py]: listener_py subscribed <- chatter
[listener-2] [INFO] [1785742615.695553673] [listener_py]: recv: "Hello from C++, seq=4"
[listener-4] [INFO] [1785742615.950913427] [listener_cpp]: recv: "Hello from PY, seq=0"
[listener-2] [INFO] [1785742616.029698379] [listener_py]: recv: "Hello from PY, seq=0"
[talker-1] [INFO] [1785742616.032624634] [talker_py]: pub: "Hello from PY, seq=0"
[talker-3] [INFO] [1785742616.035179655] [talker_cpp]: pub: "Hello from C++, seq=5"
[listener-4] [INFO] [1785742616.035407309] [listener_cpp]: recv: "Hello from C++, seq=5"
[listener-2] [INFO] [1785742616.333447910] [listener_py]: recv: "Hello from C++, seq=5"
[listener-4] [INFO] [1785742616.451293528] [listener_cpp]: recv: "Hello from PY, seq=1"
[listener-2] [INFO] [1785742616.532038517] [listener_py]: recv: "Hello from PY, seq=1"
[talker-1] [INFO] [1785742616.533425989] [talker_py]: pub: "Hello from PY, seq=1"
[talker-3] [INFO] [1785742616.535386941] [talker_cpp]: pub: "Hello from C++, seq=6"
[listener-4] [INFO] [1785742616.535524269] [listener_cpp]: recv: "Hello from C++, seq=6"
[listener-2] [INFO] [1785742616.766557349] [listener_py]: recv: "Hello from C++, seq=6"
[listener-4] [INFO] [1785742616.950952665] [listener_cpp]: recv: "Hello from PY, seq=2"
[talker-1] [INFO] [1785742616.999456619] [talker_py]: pub: "Hello from PY, seq=2"
[listener-2] [INFO] [1785742617.000932525] [listener_py]: recv: "Hello from PY, seq=2"
[talker-3] [INFO] [1785742617.035252139] [talker_cpp]: pub: "Hello from C++, seq=7"
[listener-4] [INFO] [1785742617.035627621] [listener_cpp]: recv: "Hello from C++, seq=7"
[listener-2] [INFO] [1785742617.149450857] [listener_py]: recv: "Hello from C++, seq=7"
[listener-4] [INFO] [1785742617.451232252] [listener_cpp]: recv: "Hello from PY, seq=3"
[talker-3] [INFO] [1785742617.535086579] [talker_cpp]: pub: "Hello from C++, seq=8"
[listener-4] [INFO] [1785742617.535329300] [listener_cpp]: recv: "Hello from C++, seq=8"
[talker-1] [INFO] [1785742617.563844397] [talker_py]: pub: "Hello from PY, seq=3"
[listener-2] [INFO] [1785742617.577273321] [listener_py]: recv: "Hello from PY, seq=3"
[listener-2] [INFO] [1785742617.915084727] [listener_py]: recv: "Hello from C++, seq=8"
[listener-4] [INFO] [1785742617.951357337] [listener_cpp]: recv: "Hello from PY, seq=4"
[talker-3] [INFO] [1785742618.035449631] [talker_cpp]: pub: "Hello from C++, seq=9"
[listener-4] [INFO] [1785742618.035760081] [listener_cpp]: recv: "Hello from C++, seq=9"
[talker-1] [INFO] [1785742618.157477837] [talker_py]: pub: "Hello from PY, seq=4"
[listener-2] [INFO] [1785742618.164144098] [listener_py]: recv: "Hello from PY, seq=4"
[listener-4] [INFO] [1785742618.450627317] [listener_cpp]: recv: "Hello from PY, seq=5"
[listener-2] [INFO] [1785742618.460405837] [listener_py]: recv: "Hello from C++, seq=9"
[talker-1] [INFO] [1785742618.482018164] [talker_py]: pub: "Hello from PY, seq=5"
[listener-2] [INFO] [1785742618.499725607] [listener_py]: recv: "Hello from PY, seq=5"
[talker-3] [INFO] [1785742618.535368459] [talker_cpp]: pub: "Hello from C++, seq=10"
[listener-4] [INFO] [1785742618.535672655] [listener_cpp]: recv: "Hello from C++, seq=10"
[listener-2] [INFO] [1785742618.634484824] [listener_py]: recv: "Hello from C++, seq=10"
[listener-4] [INFO] [1785742618.950651628] [listener_cpp]: recv: "Hello from PY, seq=6"
[talker-1] [INFO] [1785742619.001397475] [talker_py]: pub: "Hello from PY, seq=6"
[listener-2] [INFO] [1785742619.003147319] [listener_py]: recv: "Hello from PY, seq=6"
[talker-3] [INFO] [1785742619.035479969] [talker_cpp]: pub: "Hello from C++, seq=11"
[listener-4] [INFO] [1785742619.035795783] [listener_cpp]: recv: "Hello from C++, seq=11"
[listener-2] [INFO] [1785742619.149343454] [listener_py]: recv: "Hello from C++, seq=11"
[listener-4] [INFO] [1785742619.451122683] [listener_cpp]: recv: "Hello from PY, seq=7"
[talker-1] [INFO] [1785742619.499894268] [talker_py]: pub: "Hello from PY, seq=7"
[listener-2] [INFO] [1785742619.501156079] [listener_py]: recv: "Hello from PY, seq=7"
[talker-3] [INFO] [1785742619.535494134] [talker_cpp]: pub: "Hello from C++, seq=12"
[listener-4] [INFO] [1785742619.535756126] [listener_cpp]: recv: "Hello from C++, seq=12"
[listener-2] [INFO] [1785742619.645541458] [listener_py]: recv: "Hello from C++, seq=12"
[listener-4] [INFO] [1785742619.950810821] [listener_cpp]: recv: "Hello from PY, seq=8"
[listener-2] [INFO] [1785742619.983002547] [listener_py]: recv: "Hello from PY, seq=8"
[talker-1] [INFO] [1785742619.984033626] [talker_py]: pub: "Hello from PY, seq=8"
[talker-3] [INFO] [1785742620.035396781] [talker_cpp]: pub: "Hello from C++, seq=13"
[listener-4] [INFO] [1785742620.035853168] [listener_cpp]: recv: "Hello from C++, seq=13"
[listener-2] [INFO] [1785742620.162691124] [listener_py]: recv: "Hello from C++, seq=13"
[listener-4] [INFO] [1785742620.450646451] [listener_cpp]: recv: "Hello from PY, seq=9"
[listener-2] [INFO] [1785742620.490463265] [listener_py]: recv: "Hello from PY, seq=9"
[talker-1] [INFO] [1785742620.495925158] [talker_py]: pub: "Hello from PY, seq=9"
[talker-3] [INFO] [1785742620.535463372] [talker_cpp]: pub: "Hello from C++, seq=14"
[listener-4] [INFO] [1785742620.535817717] [listener_cpp]: recv: "Hello from C++, seq=14"
[listener-2] [INFO] [1785742620.624326531] [listener_py]: recv: "Hello from C++, seq=14"
[listener-4] [INFO] [1785742620.951380266] [listener_cpp]: recv: "Hello from PY, seq=10"
[listener-2] [INFO] [1785742621.011573731] [listener_py]: recv: "Hello from PY, seq=10"
[talker-1] [INFO] [1785742621.013342279] [talker_py]: pub: "Hello from PY, seq=10"
[talker-3] [INFO] [1785742621.035394377] [talker_cpp]: pub: "Hello from C++, seq=15"
[listener-4] [INFO] [1785742621.035654961] [listener_cpp]: recv: "Hello from C++, seq=15"
[listener-4] [INFO] [1785742621.450963583] [listener_cpp]: recv: "Hello from PY, seq=11"
[listener-2] [INFO] [1785742621.465170294] [listener_py]: recv: "Hello from C++, seq=15"
[talker-1] [INFO] [1785742621.497750761] [talker_py]: pub: "Hello from PY, seq=11"
[listener-2] [INFO] [1785742621.513665092] [listener_py]: recv: "Hello from PY, seq=11"
[talker-3] [INFO] [1785742621.535696857] [talker_cpp]: pub: "Hello from C++, seq=16"
[listener-4] [INFO] [1785742621.536095063] [listener_cpp]: recv: "Hello from C++, seq=16"
[listener-2] [INFO] [1785742621.651617042] [listener_py]: recv: "Hello from C++, seq=16"
[listener-4] [INFO] [1785742621.951342108] [listener_cpp]: recv: "Hello from PY, seq=12"
[listener-2] [INFO] [1785742621.990065192] [listener_py]: recv: "Hello from PY, seq=12"
[talker-1] [INFO] [1785742621.993467313] [talker_py]: pub: "Hello from PY, seq=12"
[talker-3] [INFO] [1785742622.035565494] [talker_cpp]: pub: "Hello from C++, seq=17"
[listener-4] [INFO] [1785742622.035936714] [listener_cpp]: recv: "Hello from C++, seq=17"
[listener-2] [INFO] [1785742622.114345699] [listener_py]: recv: "Hello from C++, seq=17"
[listener-4] [INFO] [1785742622.450639356] [listener_cpp]: recv: "Hello from PY, seq=13"
[listener-2] [INFO] [1785742622.479857579] [listener_py]: recv: "Hello from PY, seq=13"
[talker-1] [INFO] [1785742622.489282900] [talker_py]: pub: "Hello from PY, seq=13"
[talker-3] [INFO] [1785742622.535595919] [talker_cpp]: pub: "Hello from C++, seq=18"
[listener-4] [INFO] [1785742622.535975353] [listener_cpp]: recv: "Hello from C++, seq=18"
[listener-4] [INFO] [1785742622.950760498] [listener_cpp]: recv: "Hello from PY, seq=14"
[listener-2] [INFO] [1785742623.030105325] [listener_py]: recv: "Hello from C++, seq=18"
[talker-3] [INFO] [1785742623.035613577] [talker_cpp]: pub: "Hello from C++, seq=19"
[listener-4] [INFO] [1785742623.035868312] [listener_cpp]: recv: "Hello from C++, seq=19"
[talker-1] [INFO] [1785742623.045262387] [talker_py]: pub: "Hello from PY, seq=14"
[listener-2] [INFO] [1785742623.247180306] [listener_py]: recv: "Hello from PY, seq=14"
[listener-2] [INFO] [1785742623.382119241] [listener_py]: recv: "Hello from C++, seq=19"
[listener-4] [INFO] [1785742623.451299933] [listener_cpp]: recv: "Hello from PY, seq=15"
[listener-2] [INFO] [1785742623.501675244] [listener_py]: recv: "Hello from PY, seq=15"
[talker-1] [INFO] [1785742623.506280045] [talker_py]: pub: "Hello from PY, seq=15"
[talker-3] [INFO] [1785742623.535782872] [talker_cpp]: pub: "Hello from C++, seq=20"
[listener-4] [INFO] [1785742623.536066426] [listener_cpp]: recv: "Hello from C++, seq=20"
[listener-2] [INFO] [1785742623.669268946] [listener_py]: recv: "Hello from C++, seq=20"
[listener-4] [INFO] [1785742623.950641799] [listener_cpp]: recv: "Hello from PY, seq=16"
[talker-3] [INFO] [1785742624.035672426] [talker_cpp]: pub: "Hello from C++, seq=21"
[listener-4] [INFO] [1785742624.036014690] [listener_cpp]: recv: "Hello from C++, seq=21"
[talker-1] [INFO] [1785742624.136259601] [talker_py]: pub: "Hello from PY, seq=16"
[listener-2] [INFO] [1785742624.150957067] [listener_py]: recv: "Hello from PY, seq=16"
[listener-4] [INFO] [1785742624.450924980] [listener_cpp]: recv: "Hello from PY, seq=17"
[talker-3] [INFO] [1785742624.535743754] [talker_cpp]: pub: "Hello from C++, seq=22"
[listener-4] [INFO] [1785742624.536000376] [listener_cpp]: recv: "Hello from C++, seq=22"
[listener-2] [INFO] [1785742624.557253601] [listener_py]: recv: "Hello from C++, seq=21"
[talker-1] [INFO] [1785742624.594549092] [talker_py]: pub: "Hello from PY, seq=17"
[listener-2] [INFO] [1785742624.711834491] [listener_py]: recv: "Hello from PY, seq=17"
[listener-2] [INFO] [1785742624.863484234] [listener_py]: recv: "Hello from C++, seq=22"
[listener-4] [INFO] [1785742624.950640095] [listener_cpp]: recv: "Hello from PY, seq=18"
[talker-1] [INFO] [1785742624.987475631] [talker_py]: pub: "Hello from PY, seq=18"
[listener-2] [INFO] [1785742624.990515971] [listener_py]: recv: "Hello from PY, seq=18"
[talker-3] [INFO] [1785742625.035915916] [talker_cpp]: pub: "Hello from C++, seq=23"
[listener-4] [INFO] [1785742625.036100854] [listener_cpp]: recv: "Hello from C++, seq=23"
[listener-2] [INFO] [1785742625.117082097] [listener_py]: recv: "Hello from C++, seq=23"
[listener-4] [INFO] [1785742625.451566765] [listener_cpp]: recv: "Hello from PY, seq=19"
[listener-2] [INFO] [1785742625.507995963] [listener_py]: recv: "Hello from PY, seq=19"
[talker-1] [INFO] [1785742625.509793715] [talker_py]: pub: "Hello from PY, seq=19"
[talker-3] [INFO] [1785742625.535861491] [talker_cpp]: pub: "Hello from C++, seq=24"
[listener-4] [INFO] [1785742625.536160796] [listener_cpp]: recv: "Hello from C++, seq=24"
[listener-2] [INFO] [1785742625.662173708] [listener_py]: recv: "Hello from C++, seq=24"
[listener-4] [INFO] [1785742625.950570492] [listener_cpp]: recv: "Hello from PY, seq=20"
[talker-3] [INFO] [1785742626.035806750] [talker_cpp]: pub: "Hello from C++, seq=25"
[listener-4] [INFO] [1785742626.036052370] [listener_cpp]: recv: "Hello from C++, seq=25"
[talker-1] [INFO] [1785742626.134647077] [talker_py]: pub: "Hello from PY, seq=20"
[listener-2] [INFO] [1785742626.151766117] [listener_py]: recv: "Hello from PY, seq=20"
[listener-2] [INFO] [1785742626.333838661] [listener_py]: recv: "Hello from C++, seq=25"
[listener-4] [INFO] [1785742626.450841663] [listener_cpp]: recv: "Hello from PY, seq=21"
[listener-2] [INFO] [1785742626.505722119] [listener_py]: recv: "Hello from PY, seq=21"
[talker-1] [INFO] [1785742626.512135676] [talker_py]: pub: "Hello from PY, seq=21"
[talker-3] [INFO] [1785742626.535854390] [talker_cpp]: pub: "Hello from C++, seq=26"
[listener-4] [INFO] [1785742626.536257934] [listener_cpp]: recv: "Hello from C++, seq=26"
[listener-2] [INFO] [1785742626.647920694] [listener_py]: recv: "Hello from C++, seq=26"
[listener-4] [INFO] [1785742626.950527161] [listener_cpp]: recv: "Hello from PY, seq=22"
[talker-1] [INFO] [1785742626.983527443] [talker_py]: pub: "Hello from PY, seq=22"
[listener-2] [INFO] [1785742626.985468889] [listener_py]: recv: "Hello from PY, seq=22"
[talker-3] [INFO] [1785742627.035824420] [talker_cpp]: pub: "Hello from C++, seq=27"
[listener-4] [INFO] [1785742627.036111115] [listener_cpp]: recv: "Hello from C++, seq=27"
[listener-2] [INFO] [1785742627.200449747] [listener_py]: recv: "Hello from C++, seq=27"
[listener-4] [INFO] [1785742627.451616535] [listener_cpp]: recv: "Hello from PY, seq=23"
[listener-2] [INFO] [1785742627.514657741] [listener_py]: recv: "Hello from PY, seq=23"
[talker-1] [INFO] [1785742627.516802908] [talker_py]: pub: "Hello from PY, seq=23"
[talker-3] [INFO] [1785742627.536019087] [talker_cpp]: pub: "Hello from C++, seq=28"
[listener-4] [INFO] [1785742627.536297782] [listener_cpp]: recv: "Hello from C++, seq=28"
[listener-2] [INFO] [1785742627.630211897] [listener_py]: recv: "Hello from C++, seq=28"
[listener-4] [INFO] [1785742627.951064646] [listener_cpp]: recv: "Hello from PY, seq=24"
[listener-2] [INFO] [1785742627.994762997] [listener_py]: recv: "Hello from PY, seq=24"
[talker-1] [INFO] [1785742627.996488146] [talker_py]: pub: "Hello from PY, seq=24"
[talker-3] [INFO] [1785742628.036138918] [talker_cpp]: pub: "Hello from C++, seq=29"
[listener-4] [INFO] [1785742628.036380406] [listener_cpp]: recv: "Hello from C++, seq=29"
[listener-2] [INFO] [1785742628.114949033] [listener_py]: recv: "Hello from C++, seq=29"
[listener-4] [INFO] [1785742628.450964375] [listener_cpp]: recv: "Hello from PY, seq=25"
[talker-1] [INFO] [1785742628.486400650] [talker_py]: pub: "Hello from PY, seq=25"
[listener-2] [INFO] [1785742628.496240423] [listener_py]: recv: "Hello from PY, seq=25"
[talker-3] [INFO] [1785742628.535925206] [talker_cpp]: pub: "Hello from C++, seq=30"
[listener-4] [INFO] [1785742628.536213616] [listener_cpp]: recv: "Hello from C++, seq=30"
[listener-2] [INFO] [1785742628.618188729] [listener_py]: recv: "Hello from C++, seq=30"
[listener-4] [INFO] [1785742628.951032073] [listener_cpp]: recv: "Hello from PY, seq=26"
[listener-4] [INFO] [1785742629.036405639] [listener_cpp]: recv: "Hello from C++, seq=31"
[talker-3] [INFO] [1785742629.036316211] [talker_cpp]: pub: "Hello from C++, seq=31"
[listener-2] [INFO] [1785742629.071250078] [listener_py]: recv: "Hello from PY, seq=26"
[talker-1] [INFO] [1785742629.073999981] [talker_py]: pub: "Hello from PY, seq=26"
[listener-2] [INFO] [1785742629.266854516] [listener_py]: recv: "Hello from C++, seq=31"
[listener-4] [INFO] [1785742629.450989083] [listener_cpp]: recv: "Hello from PY, seq=27"
[talker-1] [INFO] [1785742629.485847962] [talker_py]: pub: "Hello from PY, seq=27"
[listener-2] [INFO] [1785742629.488092175] [listener_py]: recv: "Hello from PY, seq=27"
[talker-3] [INFO] [1785742629.535950466] [talker_cpp]: pub: "Hello from C++, seq=32"
[listener-4] [INFO] [1785742629.536131080] [listener_cpp]: recv: "Hello from C++, seq=32"
[listener-2] [INFO] [1785742629.640233471] [listener_py]: recv: "Hello from C++, seq=32"
[listener-4] [INFO] [1785742629.951321321] [listener_cpp]: recv: "Hello from PY, seq=28"
[talker-1] [INFO] [1785742629.978862491] [talker_py]: pub: "Hello from PY, seq=28"
[listener-2] [INFO] [1785742629.980607282] [listener_py]: recv: "Hello from PY, seq=28"
[talker-3] [INFO] [1785742630.036175423] [talker_cpp]: pub: "Hello from C++, seq=33"
[listener-4] [INFO] [1785742630.036473692] [listener_cpp]: recv: "Hello from C++, seq=33"
[listener-2] [INFO] [1785742630.132686313] [listener_py]: recv: "Hello from C++, seq=33"
[listener-4] [INFO] [1785742630.450990203] [listener_cpp]: recv: "Hello from PY, seq=29"
[talker-1] [INFO] [1785742630.485406886] [talker_py]: pub: "Hello from PY, seq=29"
[listener-2] [INFO] [1785742630.495128974] [listener_py]: recv: "Hello from PY, seq=29"
[talker-3] [INFO] [1785742630.536430152] [talker_cpp]: pub: "Hello from C++, seq=34"
[listener-4] [INFO] [1785742630.536578834] [listener_cpp]: recv: "Hello from C++, seq=34"
[listener-2] [INFO] [1785742630.667869741] [listener_py]: recv: "Hello from C++, seq=34"
[listener-4] [INFO] [1785742630.950695230] [listener_cpp]: recv: "Hello from PY, seq=30"
[talker-1] [INFO] [1785742630.999943347] [talker_py]: pub: "Hello from PY, seq=30"
[listener-2] [INFO] [1785742631.001222415] [listener_py]: recv: "Hello from PY, seq=30"
[talker-3] [INFO] [1785742631.036570335] [talker_cpp]: pub: "Hello from C++, seq=35"
[listener-4] [INFO] [1785742631.036816697] [listener_cpp]: recv: "Hello from C++, seq=35"
[listener-2] [INFO] [1785742631.126140343] [listener_py]: recv: "Hello from C++, seq=35"
[listener-4] [INFO] [1785742631.451137107] [listener_cpp]: recv: "Hello from PY, seq=31"
[listener-2] [INFO] [1785742631.484967173] [listener_py]: recv: "Hello from PY, seq=31"
[talker-1] [INFO] [1785742631.492239117] [talker_py]: pub: "Hello from PY, seq=31"
[talker-3] [INFO] [1785742631.536106961] [talker_cpp]: pub: "Hello from C++, seq=36"
[listener-4] [INFO] [1785742631.536405352] [listener_cpp]: recv: "Hello from C++, seq=36"
[listener-2] [INFO] [1785742631.662790382] [listener_py]: recv: "Hello from C++, seq=36"
[listener-4] [INFO] [1785742631.950793450] [listener_cpp]: recv: "Hello from PY, seq=32"
[listener-2] [INFO] [1785742631.983117446] [listener_py]: recv: "Hello from PY, seq=32"
[talker-1] [INFO] [1785742631.994455123] [talker_py]: pub: "Hello from PY, seq=32"
[talker-3] [INFO] [1785742632.036644081] [talker_cpp]: pub: "Hello from C++, seq=37"
[listener-4] [INFO] [1785742632.036937198] [listener_cpp]: recv: "Hello from C++, seq=37"
[listener-2] [INFO] [1785742632.117216071] [listener_py]: recv: "Hello from C++, seq=37"
[listener-4] [INFO] [1785742632.450750832] [listener_cpp]: recv: "Hello from PY, seq=33"
[talker-1] [INFO] [1785742632.492756783] [talker_py]: pub: "Hello from PY, seq=33"
[listener-2] [INFO] [1785742632.494198772] [listener_py]: recv: "Hello from PY, seq=33"
[talker-3] [INFO] [1785742632.536185999] [talker_cpp]: pub: "Hello from C++, seq=38"
[listener-4] [INFO] [1785742632.536446643] [listener_cpp]: recv: "Hello from C++, seq=38"
[listener-2] [INFO] [1785742632.628166547] [listener_py]: recv: "Hello from C++, seq=38"
[listener-4] [INFO] [1785742632.950703800] [listener_cpp]: recv: "Hello from PY, seq=34"
[listener-2] [INFO] [1785742632.982901656] [listener_py]: recv: "Hello from PY, seq=34"
[talker-1] [INFO] [1785742632.984206953] [talker_py]: pub: "Hello from PY, seq=34"
[talker-3] [INFO] [1785742633.033473582] [talker_cpp]: pub: "Hello from C++, seq=39"
[listener-4] [INFO] [1785742633.033685052] [listener_cpp]: recv: "Hello from C++, seq=39"
[listener-2] [INFO] [1785742633.196009825] [listener_py]: recv: "Hello from C++, seq=39"
[listener-4] [INFO] [1785742633.451080008] [listener_cpp]: recv: "Hello from PY, seq=35"
[talker-3] [INFO] [1785742633.533665321] [talker_cpp]: pub: "Hello from C++, seq=40"
[listener-4] [INFO] [1785742633.533922865] [listener_cpp]: recv: "Hello from C++, seq=40"
[listener-2] [INFO] [1785742633.564286446] [listener_py]: recv: "Hello from PY, seq=35"
[talker-1] [INFO] [1785742633.565960749] [talker_py]: pub: "Hello from PY, seq=35"
[listener-2] [INFO] [1785742633.814796006] [listener_py]: recv: "Hello from C++, seq=40"
[listener-4] [INFO] [1785742633.950684999] [listener_cpp]: recv: "Hello from PY, seq=36"
[talker-3] [INFO] [1785742634.033494848] [talker_cpp]: pub: "Hello from C++, seq=41"
[listener-4] [INFO] [1785742634.033752728] [listener_cpp]: recv: "Hello from C++, seq=41"
[talker-1] [INFO] [1785742634.043799410] [talker_py]: pub: "Hello from PY, seq=36"
[listener-2] [INFO] [1785742634.046578292] [listener_py]: recv: "Hello from PY, seq=36"
[listener-2] [INFO] [1785742634.139085463] [listener_py]: recv: "Hello from C++, seq=41"
[listener-4] [INFO] [1785742634.450715710] [listener_cpp]: recv: "Hello from PY, seq=37"
[talker-3] [INFO] [1785742634.533816379] [talker_cpp]: pub: "Hello from C++, seq=42"
[listener-4] [INFO] [1785742634.533989867] [listener_cpp]: recv: "Hello from C++, seq=42"
[listener-2] [INFO] [1785742634.549853548] [listener_py]: recv: "Hello from PY, seq=37"
[talker-1] [INFO] [1785742634.551553155] [talker_py]: pub: "Hello from PY, seq=37"
[listener-2] [INFO] [1785742634.672213592] [listener_py]: recv: "Hello from C++, seq=42"
[listener-4] [INFO] [1785742634.950759258] [listener_cpp]: recv: "Hello from PY, seq=38"
[talker-1] [INFO] [1785742634.998062187] [talker_py]: pub: "Hello from PY, seq=38"
[listener-2] [INFO] [1785742634.999689227] [listener_py]: recv: "Hello from PY, seq=38"
[talker-3] [INFO] [1785742635.033849085] [talker_cpp]: pub: "Hello from C++, seq=43"
[listener-4] [INFO] [1785742635.034201033] [listener_cpp]: recv: "Hello from C++, seq=43"
[listener-2] [INFO] [1785742635.167350785] [listener_py]: recv: "Hello from C++, seq=43"
[listener-4] [INFO] [1785742635.450945244] [listener_cpp]: recv: "Hello from PY, seq=39"
[listener-2] [INFO] [1785742635.497567292] [listener_py]: recv: "Hello from PY, seq=39"
[talker-1] [INFO] [1785742635.499589166] [talker_py]: pub: "Hello from PY, seq=39"
[talker-3] [INFO] [1785742635.533881519] [talker_cpp]: pub: "Hello from C++, seq=44"
[listener-4] [INFO] [1785742635.534207712] [listener_cpp]: recv: "Hello from C++, seq=44"
[listener-2] [INFO] [1785742635.641720064] [listener_py]: recv: "Hello from C++, seq=44"
[listener-4] [INFO] [1785742635.950556898] [listener_cpp]: recv: "Hello from PY, seq=40"
[listener-2] [INFO] [1785742636.008059821] [listener_py]: recv: "Hello from PY, seq=40"
[talker-1] [INFO] [1785742636.010569698] [talker_py]: pub: "Hello from PY, seq=40"
[talker-3] [INFO] [1785742636.034092187] [talker_cpp]: pub: "Hello from C++, seq=45"
[listener-4] [INFO] [1785742636.034348208] [listener_cpp]: recv: "Hello from C++, seq=45"
[listener-2] [INFO] [1785742636.212788027] [listener_py]: recv: "Hello from C++, seq=45"
[listener-4] [INFO] [1785742636.451142624] [listener_cpp]: recv: "Hello from PY, seq=41"
[listener-2] [INFO] [1785742636.528547965] [listener_py]: recv: "Hello from PY, seq=41"
[talker-1] [INFO] [1785742636.530125626] [talker_py]: pub: "Hello from PY, seq=41"
[talker-3] [INFO] [1785742636.533803519] [talker_cpp]: pub: "Hello from C++, seq=46"
[listener-4] [INFO] [1785742636.534101540] [listener_cpp]: recv: "Hello from C++, seq=46"
[listener-2] [INFO] [1785742636.678163669] [listener_py]: recv: "Hello from C++, seq=46"
[listener-4] [INFO] [1785742636.951076189] [listener_cpp]: recv: "Hello from PY, seq=42"
[listener-2] [INFO] [1785742637.006570626] [listener_py]: recv: "Hello from PY, seq=42"
[talker-1] [INFO] [1785742637.009861979] [talker_py]: pub: "Hello from PY, seq=42"
[talker-3] [INFO] [1785742637.033907929] [talker_cpp]: pub: "Hello from C++, seq=47"
[listener-4] [INFO] [1785742637.034334531] [listener_cpp]: recv: "Hello from C++, seq=47"
[listener-2] [INFO] [1785742637.163538947] [listener_py]: recv: "Hello from C++, seq=47"
[listener-4] [INFO] [1785742637.450830724] [listener_cpp]: recv: "Hello from PY, seq=43"
[talker-1] [INFO] [1785742637.493242987] [talker_py]: pub: "Hello from PY, seq=43"
[listener-2] [INFO] [1785742637.493987592] [listener_py]: recv: "Hello from PY, seq=43"
[talker-3] [INFO] [1785742637.533802589] [talker_cpp]: pub: "Hello from C++, seq=48"
[listener-4] [INFO] [1785742637.534023855] [listener_cpp]: recv: "Hello from C++, seq=48"
[listener-4] [INFO] [1785742637.950482640] [listener_cpp]: recv: "Hello from PY, seq=44"
[listener-2] [INFO] [1785742637.963618491] [listener_py]: recv: "Hello from C++, seq=48"
[talker-1] [INFO] [1785742637.979817625] [talker_py]: pub: "Hello from PY, seq=44"
[listener-2] [INFO] [1785742637.996642131] [listener_py]: recv: "Hello from PY, seq=44"
[talker-3] [INFO] [1785742638.033928086] [talker_cpp]: pub: "Hello from C++, seq=49"
[listener-4] [INFO] [1785742638.034206166] [listener_cpp]: recv: "Hello from C++, seq=49"
[listener-2] [INFO] [1785742638.164114071] [listener_py]: recv: "Hello from C++, seq=49"
[listener-4] [INFO] [1785742638.451172620] [listener_cpp]: recv: "Hello from PY, seq=45"
[talker-3] [INFO] [1785742638.534067444] [talker_cpp]: pub: "Hello from C++, seq=50"
[listener-4] [INFO] [1785742638.534346718] [listener_cpp]: recv: "Hello from C++, seq=50"
[talker-1] [INFO] [1785742638.575749393] [talker_py]: pub: "Hello from PY, seq=45"
[listener-2] [INFO] [1785742638.582129497] [listener_py]: recv: "Hello from PY, seq=45"
[listener-2] [INFO] [1785742638.792686564] [listener_py]: recv: "Hello from C++, seq=50"
[listener-4] [INFO] [1785742638.951092407] [listener_cpp]: recv: "Hello from PY, seq=46"
[listener-2] [INFO] [1785742638.996729532] [listener_py]: recv: "Hello from PY, seq=46"
[talker-1] [INFO] [1785742639.009495500] [talker_py]: pub: "Hello from PY, seq=46"
[talker-3] [INFO] [1785742639.033986608] [talker_cpp]: pub: "Hello from C++, seq=51"
[listener-4] [INFO] [1785742639.034233690] [listener_cpp]: recv: "Hello from C++, seq=51"
[listener-2] [INFO] [1785742639.131352510] [listener_py]: recv: "Hello from C++, seq=51"
[listener-4] [INFO] [1785742639.450893288] [listener_cpp]: recv: "Hello from PY, seq=47"
[listener-2] [INFO] [1785742639.492083311] [listener_py]: recv: "Hello from PY, seq=47"
[talker-1] [INFO] [1785742639.497075908] [talker_py]: pub: "Hello from PY, seq=47"
[talker-3] [INFO] [1785742639.534182728] [talker_cpp]: pub: "Hello from C++, seq=52"
[listener-4] [INFO] [1785742639.534497888] [listener_cpp]: recv: "Hello from C++, seq=52"
[listener-2] [INFO] [1785742639.811925095] [listener_py]: recv: "Hello from C++, seq=52"
[listener-4] [INFO] [1785742639.950555745] [listener_cpp]: recv: "Hello from PY, seq=48"
[listener-2] [INFO] [1785742639.999567182] [listener_py]: recv: "Hello from PY, seq=48"
[talker-1] [INFO] [1785742640.002576183] [talker_py]: pub: "Hello from PY, seq=48"
[talker-3] [INFO] [1785742640.034104788] [talker_cpp]: pub: "Hello from C++, seq=53"
[listener-4] [INFO] [1785742640.034543872] [listener_cpp]: recv: "Hello from C++, seq=53"
[listener-2] [INFO] [1785742640.151644893] [listener_py]: recv: "Hello from C++, seq=53"
[listener-4] [INFO] [1785742640.450538007] [listener_cpp]: recv: "Hello from PY, seq=49"
[listener-2] [INFO] [1785742640.478541170] [listener_py]: recv: "Hello from PY, seq=49"
[talker-1] [INFO] [1785742640.479419254] [talker_py]: pub: "Hello from PY, seq=49"
[talker-3] [INFO] [1785742640.534033901] [talker_cpp]: pub: "Hello from C++, seq=54"
[listener-4] [INFO] [1785742640.534327955] [listener_cpp]: recv: "Hello from C++, seq=54"
[listener-2] [INFO] [1785742640.660447738] [listener_py]: recv: "Hello from C++, seq=54"
[listener-4] [INFO] [1785742640.951031999] [listener_cpp]: recv: "Hello from PY, seq=50"
[listener-2] [INFO] [1785742641.010256117] [listener_py]: recv: "Hello from PY, seq=50"
[talker-1] [INFO] [1785742641.012173779] [talker_py]: pub: "Hello from PY, seq=50"
[talker-3] [INFO] [1785742641.034317205] [talker_cpp]: pub: "Hello from C++, seq=55"
[listener-4] [INFO] [1785742641.034537705] [listener_cpp]: recv: "Hello from C++, seq=55"
[listener-2] [INFO] [1785742641.146328971] [listener_py]: recv: "Hello from C++, seq=55"
[listener-4] [INFO] [1785742641.450628657] [listener_cpp]: recv: "Hello from PY, seq=51"
[listener-2] [INFO] [1785742641.486275045] [listener_py]: recv: "Hello from PY, seq=51"
[talker-1] [INFO] [1785742641.491827671] [talker_py]: pub: "Hello from PY, seq=51"
[talker-3] [INFO] [1785742641.534321165] [talker_cpp]: pub: "Hello from C++, seq=56"
[listener-4] [INFO] [1785742641.534663593] [listener_cpp]: recv: "Hello from C++, seq=56"
[listener-2] [INFO] [1785742641.625735591] [listener_py]: recv: "Hello from C++, seq=56"
[listener-4] [INFO] [1785742641.950677327] [listener_cpp]: recv: "Hello from PY, seq=52"
[listener-2] [INFO] [1785742642.028817983] [listener_py]: recv: "Hello from PY, seq=52"
[talker-1] [INFO] [1785742642.029580126] [talker_py]: pub: "Hello from PY, seq=52"
[talker-3] [INFO] [1785742642.034150605] [talker_cpp]: pub: "Hello from C++, seq=57"
[listener-4] [INFO] [1785742642.034379172] [listener_cpp]: recv: "Hello from C++, seq=57"
[listener-2] [INFO] [1785742642.113141790] [listener_py]: recv: "Hello from C++, seq=57"
[listener-4] [INFO] [1785742642.450960355] [listener_cpp]: recv: "Hello from PY, seq=53"
[listener-2] [INFO] [1785742642.506321364] [listener_py]: recv: "Hello from PY, seq=53"
[talker-1] [INFO] [1785742642.507935440] [talker_py]: pub: "Hello from PY, seq=53"
[talker-3] [INFO] [1785742642.534451649] [talker_cpp]: pub: "Hello from C++, seq=58"
[listener-4] [INFO] [1785742642.534677323] [listener_cpp]: recv: "Hello from C++, seq=58"
[listener-2] [INFO] [1785742642.632327267] [listener_py]: recv: "Hello from C++, seq=58"
[listener-4] [INFO] [1785742642.950635234] [listener_cpp]: recv: "Hello from PY, seq=54"
[listener-2] [INFO] [1785742643.001326755] [listener_py]: recv: "Hello from PY, seq=54"
[talker-1] [INFO] [1785742643.006901722] [talker_py]: pub: "Hello from PY, seq=54"
[talker-3] [INFO] [1785742643.034262533] [talker_cpp]: pub: "Hello from C++, seq=59"
[listener-4] [INFO] [1785742643.034612274] [listener_cpp]: recv: "Hello from C++, seq=59"
[listener-2] [INFO] [1785742643.208301916] [listener_py]: recv: "Hello from C++, seq=59"
[listener-4] [INFO] [1785742643.450705963] [listener_cpp]: recv: "Hello from PY, seq=55"
[listener-2] [INFO] [1785742643.494663477] [listener_py]: recv: "Hello from PY, seq=55"
[talker-1] [INFO] [1785742643.498326848] [talker_py]: pub: "Hello from PY, seq=55"
[talker-3] [INFO] [1785742643.534143954] [talker_cpp]: pub: "Hello from C++, seq=60"
[listener-4] [INFO] [1785742643.534381872] [listener_cpp]: recv: "Hello from C++, seq=60"
[listener-2] [INFO] [1785742643.610899945] [listener_py]: recv: "Hello from C++, seq=60"
docker/docker-compose.yml
+44
View File
@@ -0,0 +1,44 @@
# =============================================================================
# docker-compose.yml —— 用 docker-compose 编排 ROS2 开发容器。
#
# 与裸 docker run 相比,docker-compose 提供:
# - YAML 声明式配置(易读、易版本管理)
# - 一键 build / up / down / logs
# - 多服务依赖、网络、卷的统一管理
# =============================================================================
services:
ros2:
# build: 用同目录的 Dockerfile 构建镜像。
# image: 同时打标签,后续 docker-compose down 不会丢镜像。
build: .
image: ros2-humble-dev:latest
container_name: ros2_dev
# privileged: 特权模式,便于以后调系统工具(默认足够,不必开)。
privileged: true
# stdin_open + tty:docker exec -it 时能获得交互式 shell。
stdin_open: true
tty: true
# network_mode: host —— 让容器使用主机网络栈。
# 重要:DDS 默认走 UDP 多播,跨容器/跨主机时必须可见;
# host 网络让多容器间直接互见,适合单机多机的快速调试。
network_mode: host
environment:
# ROS_DOMAIN ID:0~232(默认 0);同一 ID 的节点互相可见,
# 同一机器多项目互不干扰时给每个项目不同 ID。
- ROS_DOMAIN_ID=0
# RMW 留空,使用 ROS2 Humble 默认的 rmw_fastrtps_cpp;
# 若要切换到 Cyclone DDS,需先在镜像内 apt install ros-humble-rmw-cyclonedds-cpp。
# 关键卷挂载:把宿主的整个 ros2 工程(上一级目录) 挂到容器 /root/ros2_ws,
# 改动代码即时同步,容器内 colcon build 即可生效。
volumes:
- ..:/root/ros2_ws
working_dir: /root/ros2_ws
# 容器内默认命令:tail -f /dev/null 让容器"挂起不死",
# 用户再 docker exec 进去手动开发。
command: ["bash", "-lc", "tail -f /dev/null"]
docker/full_demo_e2e.log
+464
View File
@@ -0,0 +1,464 @@
[INFO] [launch]: All log files can be found below /root/.ros/log/2026-08-03-08-42-57-065085-docker-desktop-46
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [talker-1]: process started with pid [56]
[INFO] [listener-2]: process started with pid [58]
[INFO] [talker-3]: process started with pid [60]
[INFO] [listener-4]: process started with pid [62]
[INFO] [add_two_ints_server-5]: process started with pid [64]
[INFO] [fibonacci_server-6]: process started with pid [66]
[INFO] [joint_state_publisher-7]: process started with pid [68]
[INFO] [robot_state_publisher-8]: process started with pid [70]
[INFO] [tf2_listener-9]: process started with pid [72]
[INFO] [fake_camera-10]: process started with pid [76]
[INFO] [image_processor-11]: process started with pid [78]
[listener-4] [INFO] [1785746579.885071317] [listener_cpp]: listener_cpp subscribed <- chatter
[tf2_listener-9] [INFO] [1785746580.101661615] [tf2_listener_cpp]: tf2_listener_cpp started
[joint_state_publisher-7] [INFO] [1785746580.414206301] [joint_state_publisher_cpp]: joint_state_publisher_cpp started, joints: 3
[tf2_listener-9] [WARN] [1785746580.641015818] [tf2_listener_cpp]: TF lookup failed: "base_link" passed to lookupTransform argument target_frame does not exist.
[talker-3] [INFO] [1785746580.793473150] [talker_cpp]: talker_cpp started -> topic=chatter, period=500ms
[robot_state_publisher-8] [INFO] [1785746580.962918847] [robot_state_publisher]: got segment base_link
[robot_state_publisher-8] [INFO] [1785746580.963109766] [robot_state_publisher]: got segment gripper
[robot_state_publisher-8] [INFO] [1785746580.963119425] [robot_state_publisher]: got segment link1
[robot_state_publisher-8] [INFO] [1785746580.963123370] [robot_state_publisher]: got segment link2
[tf2_listener-9] [INFO] [1785746581.101951180] [tf2_listener_cpp]: gripper in base_link: x=0.013 y=0.004 z=0.299
[talker-3] [INFO] [1785746581.293890094] [talker_cpp]: pub: "Hello from C++, seq=0"
[listener-4] [INFO] [1785746581.294106816] [listener_cpp]: recv: "Hello from C++, seq=0"
[tf2_listener-9] [INFO] [1785746581.602039670] [tf2_listener_cpp]: gripper in base_link: x=0.019 y=0.009 z=0.298
[talker-3] [INFO] [1785746581.793852317] [talker_cpp]: pub: "Hello from C++, seq=1"
[listener-4] [INFO] [1785746581.794030496] [listener_cpp]: recv: "Hello from C++, seq=1"
[tf2_listener-9] [INFO] [1785746582.102209413] [tf2_listener_cpp]: gripper in base_link: x=0.024 y=0.013 z=0.296
[talker-3] [INFO] [1785746582.293918538] [talker_cpp]: pub: "Hello from C++, seq=2"
[listener-4] [INFO] [1785746582.294181194] [listener_cpp]: recv: "Hello from C++, seq=2"
[tf2_listener-9] [INFO] [1785746582.602101273] [tf2_listener_cpp]: gripper in base_link: x=0.027 y=0.012 z=0.296
[talker-3] [INFO] [1785746582.793943887] [talker_cpp]: pub: "Hello from C++, seq=3"
[listener-4] [INFO] [1785746582.794123600] [listener_cpp]: recv: "Hello from C++, seq=3"
[tf2_listener-9] [INFO] [1785746583.102210755] [tf2_listener_cpp]: gripper in base_link: x=0.028 y=0.007 z=0.296
[talker-3] [INFO] [1785746583.294115229] [talker_cpp]: pub: "Hello from C++, seq=4"
[listener-4] [INFO] [1785746583.294353166] [listener_cpp]: recv: "Hello from C++, seq=4"
[tf2_listener-9] [INFO] [1785746583.602239234] [tf2_listener_cpp]: gripper in base_link: x=0.024 y=0.000 z=0.297
[talker-3] [INFO] [1785746583.794194342] [talker_cpp]: pub: "Hello from C++, seq=5"
[listener-4] [INFO] [1785746583.794472079] [listener_cpp]: recv: "Hello from C++, seq=5"
[tf2_listener-9] [INFO] [1785746584.102361796] [tf2_listener_cpp]: gripper in base_link: x=0.016 y=-0.004 z=0.299
[talker-3] [INFO] [1785746584.294166312] [talker_cpp]: pub: "Hello from C++, seq=6"
[listener-4] [INFO] [1785746584.294458278] [listener_cpp]: recv: "Hello from C++, seq=6"
[tf2_listener-9] [INFO] [1785746584.602351478] [tf2_listener_cpp]: gripper in base_link: x=0.006 y=-0.003 z=0.300
[talker-3] [INFO] [1785746584.794227996] [talker_cpp]: pub: "Hello from C++, seq=7"
[listener-4] [INFO] [1785746584.794455188] [listener_cpp]: recv: "Hello from C++, seq=7"
[tf2_listener-9] [INFO] [1785746585.102370562] [tf2_listener_cpp]: gripper in base_link: x=-0.003 y=0.002 z=0.300
[talker-3] [INFO] [1785746585.294206291] [talker_cpp]: pub: "Hello from C++, seq=8"
[listener-4] [INFO] [1785746585.294365618] [listener_cpp]: recv: "Hello from C++, seq=8"
[fake_camera-10] [INFO] [1785746585.443136834] [fake_camera_py]: fake_camera_py started: 320x240 @ 10fps -> /image_raw
[tf2_listener-9] [INFO] [1785746585.602480712] [tf2_listener_cpp]: gripper in base_link: x=-0.011 y=0.006 z=0.299
[fibonacci_server-6] [INFO] [1785746585.760864901] [fibonacci_action_server_py]: fibonacci_action_server_py ready
[talker-3] [INFO] [1785746585.794200042] [talker_cpp]: pub: "Hello from C++, seq=9"
[listener-4] [INFO] [1785746585.794392001] [listener_cpp]: recv: "Hello from C++, seq=9"
[tf2_listener-9] [INFO] [1785746586.102521754] [tf2_listener_cpp]: gripper in base_link: x=-0.021 y=0.006 z=0.298
[talker-3] [INFO] [1785746586.294315577] [talker_cpp]: pub: "Hello from C++, seq=10"
[listener-4] [INFO] [1785746586.294450138] [listener_cpp]: recv: "Hello from C++, seq=10"
[add_two_ints_server-5] [INFO] [1785746586.362786882] [add_two_ints_server_py]: add_two_ints_server_py ready, waiting for requests...
[tf2_listener-9] [INFO] [1785746586.602561974] [tf2_listener_cpp]: gripper in base_link: x=-0.027 y=0.002 z=0.296
[image_processor-11] [INFO] [1785746586.687038024] [image_processor_py]: image_processor_py subscribed <- /image_raw (cv_bridge=True)
[image_processor-11] [INFO] [1785746586.743383063] [image_processor_py]: rcv Image: 320x240 avg_brightness=103.0
[talker-3] [INFO] [1785746586.794341672] [talker_cpp]: pub: "Hello from C++, seq=11"
[listener-4] [INFO] [1785746586.794535536] [listener_cpp]: recv: "Hello from C++, seq=11"
[image_processor-11] [INFO] [1785746586.800462260] [image_processor_py]: rcv Image: 320x240 avg_brightness=104.7
[image_processor-11] [INFO] [1785746586.849583763] [image_processor_py]: rcv Image: 320x240 avg_brightness=106.3
[image_processor-11] [INFO] [1785746586.909618146] [image_processor_py]: rcv Image: 320x240 avg_brightness=108.0
[talker-1] [INFO] [1785746587.010030252] [talker_py]: talker_py started -> topic=chatter, period=500ms
[image_processor-11] [INFO] [1785746587.010312182] [image_processor_py]: rcv Image: 320x240 avg_brightness=109.7
[tf2_listener-9] [INFO] [1785746587.102638244] [tf2_listener_cpp]: gripper in base_link: x=-0.029 y=-0.005 z=0.296
[image_processor-11] [INFO] [1785746587.105459248] [image_processor_py]: rcv Image: 320x240 avg_brightness=111.3
[listener-2] [INFO] [1785746587.211079863] [listener_py]: listener_py subscribed <- chatter
[image_processor-11] [INFO] [1785746587.212640223] [image_processor_py]: rcv Image: 320x240 avg_brightness=113.0
[talker-3] [INFO] [1785746587.294398968] [talker_cpp]: pub: "Hello from C++, seq=12"
[listener-4] [INFO] [1785746587.294530997] [listener_cpp]: recv: "Hello from C++, seq=12"
[image_processor-11] [INFO] [1785746587.319556168] [image_processor_py]: rcv Image: 320x240 avg_brightness=114.7
[listener-4] [INFO] [1785746587.376392024] [listener_cpp]: recv: "Hello from PY, seq=0"
[talker-1] [INFO] [1785746587.426897875] [talker_py]: pub: "Hello from PY, seq=0"
[image_processor-11] [INFO] [1785746587.427527183] [image_processor_py]: rcv Image: 320x240 avg_brightness=116.3
[listener-2] [INFO] [1785746587.427933086] [listener_py]: recv: "Hello from PY, seq=0"
[listener-2] [INFO] [1785746587.473152343] [listener_py]: recv: "Hello from C++, seq=12"
[image_processor-11] [INFO] [1785746587.499039575] [image_processor_py]: rcv Image: 320x240 avg_brightness=118.0
[tf2_listener-9] [INFO] [1785746587.602678225] [tf2_listener_cpp]: gripper in base_link: x=-0.027 y=-0.010 z=0.296
[image_processor-11] [INFO] [1785746587.612136970] [image_processor_py]: rcv Image: 320x240 avg_brightness=119.7
[image_processor-11] [INFO] [1785746587.692713209] [image_processor_py]: rcv Image: 320x240 avg_brightness=121.3
[talker-3] [INFO] [1785746587.794476346] [talker_cpp]: pub: "Hello from C++, seq=13"
[listener-4] [INFO] [1785746587.794698773] [listener_cpp]: recv: "Hello from C++, seq=13"
[image_processor-11] [INFO] [1785746587.829587551] [image_processor_py]: rcv Image: 320x240 avg_brightness=123.0
[listener-2] [INFO] [1785746587.861772478] [listener_py]: recv: "Hello from C++, seq=13"
[listener-4] [INFO] [1785746587.876132374] [listener_cpp]: recv: "Hello from PY, seq=1"
[image_processor-11] [INFO] [1785746587.949572270] [image_processor_py]: rcv Image: 320x240 avg_brightness=124.7
[listener-2] [INFO] [1785746587.955608260] [listener_py]: recv: "Hello from PY, seq=1"
[talker-1] [INFO] [1785746587.956658082] [talker_py]: pub: "Hello from PY, seq=1"
[image_processor-11] [INFO] [1785746588.014516240] [image_processor_py]: rcv Image: 320x240 avg_brightness=126.3
[tf2_listener-9] [INFO] [1785746588.102654990] [tf2_listener_cpp]: gripper in base_link: x=-0.022 y=-0.011 z=0.297
[image_processor-11] [INFO] [1785746588.110125535] [image_processor_py]: rcv Image: 320x240 avg_brightness=128.0
[image_processor-11] [INFO] [1785746588.221055452] [image_processor_py]: rcv Image: 320x240 avg_brightness=129.7
[talker-3] [INFO] [1785746588.294566861] [talker_cpp]: pub: "Hello from C++, seq=14"
[listener-4] [INFO] [1785746588.294805199] [listener_cpp]: recv: "Hello from C++, seq=14"
[image_processor-11] [INFO] [1785746588.327357116] [image_processor_py]: rcv Image: 320x240 avg_brightness=131.3
[listener-2] [INFO] [1785746588.350983100] [listener_py]: recv: "Hello from C++, seq=14"
[listener-4] [INFO] [1785746588.376504316] [listener_cpp]: recv: "Hello from PY, seq=2"
[talker-1] [INFO] [1785746588.427953350] [talker_py]: pub: "Hello from PY, seq=2"
[listener-2] [INFO] [1785746588.428925886] [listener_py]: recv: "Hello from PY, seq=2"
[image_processor-11] [INFO] [1785746588.436518877] [image_processor_py]: rcv Image: 320x240 avg_brightness=133.0
[image_processor-11] [INFO] [1785746588.513283784] [image_processor_py]: rcv Image: 320x240 avg_brightness=134.7
[tf2_listener-9] [INFO] [1785746588.602836142] [tf2_listener_cpp]: gripper in base_link: x=-0.014 y=-0.007 z=0.299
[image_processor-11] [INFO] [1785746588.611830484] [image_processor_py]: rcv Image: 320x240 avg_brightness=136.3
[image_processor-11] [INFO] [1785746588.726370940] [image_processor_py]: rcv Image: 320x240 avg_brightness=138.0
[talker-3] [INFO] [1785746588.794590938] [talker_cpp]: pub: "Hello from C++, seq=15"
[listener-4] [INFO] [1785746588.794872698] [listener_cpp]: recv: "Hello from C++, seq=15"
[image_processor-11] [INFO] [1785746588.834391730] [image_processor_py]: rcv Image: 320x240 avg_brightness=139.7
[listener-4] [INFO] [1785746588.876014462] [listener_cpp]: recv: "Hello from PY, seq=3"
[listener-2] [INFO] [1785746588.903320695] [listener_py]: recv: "Hello from C++, seq=15"
[image_processor-11] [INFO] [1785746588.919806781] [image_processor_py]: rcv Image: 320x240 avg_brightness=141.3
[talker-1] [INFO] [1785746588.927188499] [talker_py]: pub: "Hello from PY, seq=3"
[listener-2] [INFO] [1785746588.971121964] [listener_py]: recv: "Hello from PY, seq=3"
[image_processor-11] [INFO] [1785746589.019310267] [image_processor_py]: rcv Image: 320x240 avg_brightness=143.0
[tf2_listener-9] [INFO] [1785746589.102803510] [tf2_listener_cpp]: gripper in base_link: x=-0.007 y=-0.003 z=0.300
[image_processor-11] [INFO] [1785746589.111864634] [image_processor_py]: rcv Image: 320x240 avg_brightness=144.7
[image_processor-11] [INFO] [1785746589.222420040] [image_processor_py]: rcv Image: 320x240 avg_brightness=146.3
[talker-3] [INFO] [1785746589.294607086] [talker_cpp]: pub: "Hello from C++, seq=16"
[listener-4] [INFO] [1785746589.294819606] [listener_cpp]: recv: "Hello from C++, seq=16"
[image_processor-11] [INFO] [1785746589.309709569] [image_processor_py]: rcv Image: 320x240 avg_brightness=148.0
[listener-2] [INFO] [1785746589.344319856] [listener_py]: recv: "Hello from C++, seq=16"
[listener-4] [INFO] [1785746589.376068857] [listener_cpp]: recv: "Hello from PY, seq=4"
[talker-1] [INFO] [1785746589.415533229] [talker_py]: pub: "Hello from PY, seq=4"
[image_processor-11] [INFO] [1785746589.416221948] [image_processor_py]: rcv Image: 320x240 avg_brightness=149.7
[listener-2] [INFO] [1785746589.416475494] [listener_py]: recv: "Hello from PY, seq=4"
[image_processor-11] [INFO] [1785746589.509650883] [image_processor_py]: rcv Image: 320x240 avg_brightness=151.3
[tf2_listener-9] [INFO] [1785746589.602863606] [tf2_listener_cpp]: gripper in base_link: x=0.004 y=0.000 z=0.300
[image_processor-11] [INFO] [1785746589.629149673] [image_processor_py]: rcv Image: 320x240 avg_brightness=153.0
[image_processor-11] [INFO] [1785746589.730733017] [image_processor_py]: rcv Image: 320x240 avg_brightness=154.7
[talker-3] [INFO] [1785746589.794745903] [talker_cpp]: pub: "Hello from C++, seq=17"
[listener-4] [INFO] [1785746589.794937293] [listener_cpp]: recv: "Hello from C++, seq=17"
[image_processor-11] [INFO] [1785746589.826186951] [image_processor_py]: rcv Image: 320x240 avg_brightness=156.3
[listener-2] [INFO] [1785746589.844816530] [listener_py]: recv: "Hello from C++, seq=17"
[listener-4] [INFO] [1785746589.876223526] [listener_cpp]: recv: "Hello from PY, seq=5"
[image_processor-11] [INFO] [1785746589.915497600] [image_processor_py]: rcv Image: 320x240 avg_brightness=158.0
[talker-1] [INFO] [1785746589.926768881] [talker_py]: pub: "Hello from PY, seq=5"
[listener-2] [INFO] [1785746589.927519052] [listener_py]: recv: "Hello from PY, seq=5"
[image_processor-11] [INFO] [1785746590.011979645] [image_processor_py]: rcv Image: 320x240 avg_brightness=159.7
[tf2_listener-9] [INFO] [1785746590.102948466] [tf2_listener_cpp]: gripper in base_link: x=0.013 y=-0.001 z=0.299
[image_processor-11] [INFO] [1785746590.133308671] [image_processor_py]: rcv Image: 320x240 avg_brightness=161.3
[image_processor-11] [INFO] [1785746590.227876207] [image_processor_py]: rcv Image: 320x240 avg_brightness=163.0
[talker-3] [INFO] [1785746590.294943994] [talker_cpp]: pub: "Hello from C++, seq=18"
[listener-4] [INFO] [1785746590.295201351] [listener_cpp]: recv: "Hello from C++, seq=18"
[image_processor-11] [INFO] [1785746590.315453459] [image_processor_py]: rcv Image: 320x240 avg_brightness=164.7
[listener-2] [INFO] [1785746590.347946371] [listener_py]: recv: "Hello from C++, seq=18"
[listener-4] [INFO] [1785746590.376130205] [listener_cpp]: recv: "Hello from PY, seq=6"
[image_processor-11] [INFO] [1785746590.429730648] [image_processor_py]: rcv Image: 320x240 avg_brightness=166.3
[talker-1] [INFO] [1785746590.440717874] [talker_py]: pub: "Hello from PY, seq=6"
[listener-2] [INFO] [1785746590.442703106] [listener_py]: recv: "Hello from PY, seq=6"
[image_processor-11] [INFO] [1785746590.512629712] [image_processor_py]: rcv Image: 320x240 avg_brightness=168.0
[tf2_listener-9] [INFO] [1785746590.602925763] [tf2_listener_cpp]: gripper in base_link: x=0.020 y=-0.007 z=0.298
[image_processor-11] [INFO] [1785746590.625537321] [image_processor_py]: rcv Image: 320x240 avg_brightness=169.7
[image_processor-11] [INFO] [1785746590.709380963] [image_processor_py]: rcv Image: 320x240 avg_brightness=86.0
[talker-3] [INFO] [1785746590.794799089] [talker_cpp]: pub: "Hello from C++, seq=19"
[listener-4] [INFO] [1785746590.794974900] [listener_cpp]: recv: "Hello from C++, seq=19"
[image_processor-11] [INFO] [1785746590.808390329] [image_processor_py]: rcv Image: 320x240 avg_brightness=87.7
[listener-2] [INFO] [1785746590.841705298] [listener_py]: recv: "Hello from C++, seq=19"
[listener-4] [INFO] [1785746590.876028243] [listener_cpp]: recv: "Hello from PY, seq=7"
[image_processor-11] [INFO] [1785746590.912439495] [image_processor_py]: rcv Image: 320x240 avg_brightness=89.3
[listener-2] [INFO] [1785746590.921167230] [listener_py]: recv: "Hello from PY, seq=7"
[talker-1] [INFO] [1785746590.922331917] [talker_py]: pub: "Hello from PY, seq=7"
[image_processor-11] [INFO] [1785746591.024211735] [image_processor_py]: rcv Image: 320x240 avg_brightness=91.0
[tf2_listener-9] [INFO] [1785746591.103137850] [tf2_listener_cpp]: gripper in base_link: x=0.024 y=-0.012 z=0.296
[image_processor-11] [INFO] [1785746591.113413665] [image_processor_py]: rcv Image: 320x240 avg_brightness=92.7
[image_processor-11] [INFO] [1785746591.210239027] [image_processor_py]: rcv Image: 320x240 avg_brightness=94.3
[talker-3] [INFO] [1785746591.295085979] [talker_cpp]: pub: "Hello from C++, seq=20"
[listener-4] [INFO] [1785746591.295302611] [listener_cpp]: recv: "Hello from C++, seq=20"
[image_processor-11] [INFO] [1785746591.303226972] [image_processor_py]: rcv Image: 320x240 avg_brightness=96.0
[listener-2] [INFO] [1785746591.347474550] [listener_py]: recv: "Hello from C++, seq=20"
[listener-4] [INFO] [1785746591.376338063] [listener_cpp]: recv: "Hello from PY, seq=8"
[talker-1] [INFO] [1785746591.428823235] [talker_py]: pub: "Hello from PY, seq=8"
[listener-2] [INFO] [1785746591.430845470] [listener_py]: recv: "Hello from PY, seq=8"
[image_processor-11] [INFO] [1785746591.434535420] [image_processor_py]: rcv Image: 320x240 avg_brightness=97.7
[image_processor-11] [INFO] [1785746591.504712631] [image_processor_py]: rcv Image: 320x240 avg_brightness=99.3
[tf2_listener-9] [INFO] [1785746591.603148776] [tf2_listener_cpp]: gripper in base_link: x=0.026 y=-0.014 z=0.296
[image_processor-11] [INFO] [1785746591.611231455] [image_processor_py]: rcv Image: 320x240 avg_brightness=101.0
[image_processor-11] [INFO] [1785746591.716768226] [image_processor_py]: rcv Image: 320x240 avg_brightness=102.7
[talker-3] [INFO] [1785746591.794894501] [talker_cpp]: pub: "Hello from C++, seq=21"
[listener-4] [INFO] [1785746591.795034412] [listener_cpp]: recv: "Hello from C++, seq=21"
[image_processor-11] [INFO] [1785746591.815933868] [image_processor_py]: rcv Image: 320x240 avg_brightness=104.3
[listener-2] [INFO] [1785746591.855508927] [listener_py]: recv: "Hello from C++, seq=21"
[listener-4] [INFO] [1785746591.876085384] [listener_cpp]: recv: "Hello from PY, seq=9"
[talker-1] [INFO] [1785746591.921156791] [talker_py]: pub: "Hello from PY, seq=9"
[listener-2] [INFO] [1785746591.921935455] [listener_py]: recv: "Hello from PY, seq=9"
[image_processor-11] [INFO] [1785746591.922858776] [image_processor_py]: rcv Image: 320x240 avg_brightness=106.0
[image_processor-11] [INFO] [1785746592.041447348] [image_processor_py]: rcv Image: 320x240 avg_brightness=107.7
[tf2_listener-9] [INFO] [1785746592.103101056] [tf2_listener_cpp]: gripper in base_link: x=0.026 y=-0.011 z=0.296
[image_processor-11] [INFO] [1785746592.116499496] [image_processor_py]: rcv Image: 320x240 avg_brightness=109.3
[image_processor-11] [INFO] [1785746592.215911058] [image_processor_py]: rcv Image: 320x240 avg_brightness=111.0
[talker-3] [INFO] [1785746592.294923949] [talker_cpp]: pub: "Hello from C++, seq=22"
[listener-4] [INFO] [1785746592.295123826] [listener_cpp]: recv: "Hello from C++, seq=22"
[image_processor-11] [INFO] [1785746592.329595365] [image_processor_py]: rcv Image: 320x240 avg_brightness=112.7
[listener-2] [INFO] [1785746592.342941168] [listener_py]: recv: "Hello from C++, seq=22"
[listener-4] [INFO] [1785746592.376139312] [listener_cpp]: recv: "Hello from PY, seq=10"
[listener-2] [INFO] [1785746592.424051743] [listener_py]: recv: "Hello from PY, seq=10"
[talker-1] [INFO] [1785746592.424779662] [talker_py]: pub: "Hello from PY, seq=10"
[image_processor-11] [INFO] [1785746592.427126038] [image_processor_py]: rcv Image: 320x240 avg_brightness=114.3
[image_processor-11] [INFO] [1785746592.513174653] [image_processor_py]: rcv Image: 320x240 avg_brightness=116.0
[tf2_listener-9] [INFO] [1785746592.603103018] [tf2_listener_cpp]: gripper in base_link: x=0.023 y=-0.005 z=0.297
[image_processor-11] [INFO] [1785746592.624490666] [image_processor_py]: rcv Image: 320x240 avg_brightness=117.7
[image_processor-11] [INFO] [1785746592.702527584] [image_processor_py]: rcv Image: 320x240 avg_brightness=119.3
[talker-3] [INFO] [1785746592.795136040] [talker_cpp]: pub: "Hello from C++, seq=23"
[listener-4] [INFO] [1785746592.795429431] [listener_cpp]: recv: "Hello from C++, seq=23"
[image_processor-11] [INFO] [1785746592.837392947] [image_processor_py]: rcv Image: 320x240 avg_brightness=121.0
[listener-2] [INFO] [1785746592.858002380] [listener_py]: recv: "Hello from C++, seq=23"
[listener-4] [INFO] [1785746592.876304198] [listener_cpp]: recv: "Hello from PY, seq=11"
[image_processor-11] [INFO] [1785746592.909286762] [image_processor_py]: rcv Image: 320x240 avg_brightness=122.7
[talker-1] [INFO] [1785746592.916209030] [talker_py]: pub: "Hello from PY, seq=11"
[listener-2] [INFO] [1785746592.916952034] [listener_py]: recv: "Hello from PY, seq=11"
[image_processor-11] [INFO] [1785746593.011340908] [image_processor_py]: rcv Image: 320x240 avg_brightness=124.3
[image_processor-11] [INFO] [1785746593.098361136] [image_processor_py]: rcv Image: 320x240 avg_brightness=126.0
[tf2_listener-9] [INFO] [1785746593.099932098] [tf2_listener_cpp]: gripper in base_link: x=0.017 y=0.000 z=0.299
[image_processor-11] [INFO] [1785746593.237599493] [image_processor_py]: rcv Image: 320x240 avg_brightness=127.7
[talker-3] [INFO] [1785746593.292009111] [talker_cpp]: pub: "Hello from C++, seq=24"
[listener-4] [INFO] [1785746593.292352443] [listener_cpp]: recv: "Hello from C++, seq=24"
[image_processor-11] [INFO] [1785746593.316459020] [image_processor_py]: rcv Image: 320x240 avg_brightness=129.3
[listener-2] [INFO] [1785746593.359237915] [listener_py]: recv: "Hello from C++, seq=24"
[listener-4] [INFO] [1785746593.376428894] [listener_cpp]: recv: "Hello from PY, seq=12"
[image_processor-11] [INFO] [1785746593.420117632] [image_processor_py]: rcv Image: 320x240 avg_brightness=131.0
[listener-2] [INFO] [1785746593.423524483] [listener_py]: recv: "Hello from PY, seq=12"
[talker-1] [INFO] [1785746593.424158075] [talker_py]: pub: "Hello from PY, seq=12"
[image_processor-11] [INFO] [1785746593.534782287] [image_processor_py]: rcv Image: 320x240 avg_brightness=132.7
[tf2_listener-9] [INFO] [1785746593.600002608] [tf2_listener_cpp]: gripper in base_link: x=0.006 y=0.002 z=0.300
[image_processor-11] [INFO] [1785746593.644448755] [image_processor_py]: rcv Image: 320x240 avg_brightness=134.3
[image_processor-11] [INFO] [1785746593.728971985] [image_processor_py]: rcv Image: 320x240 avg_brightness=136.0
[talker-3] [INFO] [1785746593.791791771] [talker_cpp]: pub: "Hello from C++, seq=25"
[listener-4] [INFO] [1785746593.792068381] [listener_cpp]: recv: "Hello from C++, seq=25"
[image_processor-11] [INFO] [1785746593.823714794] [image_processor_py]: rcv Image: 320x240 avg_brightness=137.7
[listener-2] [INFO] [1785746593.850826017] [listener_py]: recv: "Hello from C++, seq=25"
[listener-4] [INFO] [1785746593.876675417] [listener_cpp]: recv: "Hello from PY, seq=13"
[listener-2] [INFO] [1785746593.927148229] [listener_py]: recv: "Hello from PY, seq=13"
[talker-1] [INFO] [1785746593.927934141] [talker_py]: pub: "Hello from PY, seq=13"
[image_processor-11] [INFO] [1785746593.928798739] [image_processor_py]: rcv Image: 320x240 avg_brightness=139.3
[image_processor-11] [INFO] [1785746594.061709697] [image_processor_py]: rcv Image: 320x240 avg_brightness=141.0
[tf2_listener-9] [INFO] [1785746594.100011800] [tf2_listener_cpp]: gripper in base_link: x=-0.004 y=-0.002 z=0.300
[image_processor-11] [INFO] [1785746594.126776026] [image_processor_py]: rcv Image: 320x240 avg_brightness=142.7
[image_processor-11] [INFO] [1785746594.220181038] [image_processor_py]: rcv Image: 320x240 avg_brightness=144.3
[talker-3] [INFO] [1785746594.291823638] [talker_cpp]: pub: "Hello from C++, seq=26"
[listener-4] [INFO] [1785746594.292047412] [listener_cpp]: recv: "Hello from C++, seq=26"
[image_processor-11] [INFO] [1785746594.337745165] [image_processor_py]: rcv Image: 320x240 avg_brightness=146.0
[listener-2] [INFO] [1785746594.362521343] [listener_py]: recv: "Hello from C++, seq=26"
[listener-4] [INFO] [1785746594.376731167] [listener_cpp]: recv: "Hello from PY, seq=14"
[listener-2] [INFO] [1785746594.442478016] [listener_py]: recv: "Hello from PY, seq=14"
[image_processor-11] [INFO] [1785746594.443983992] [image_processor_py]: rcv Image: 320x240 avg_brightness=147.7
[talker-1] [INFO] [1785746594.445489148] [talker_py]: pub: "Hello from PY, seq=14"
[image_processor-11] [INFO] [1785746594.532745068] [image_processor_py]: rcv Image: 320x240 avg_brightness=149.3
[tf2_listener-9] [INFO] [1785746594.600014581] [tf2_listener_cpp]: gripper in base_link: x=-0.012 y=-0.007 z=0.299
[image_processor-11] [INFO] [1785746594.656442977] [image_processor_py]: rcv Image: 320x240 avg_brightness=151.0
[image_processor-11] [INFO] [1785746594.714596728] [image_processor_py]: rcv Image: 320x240 avg_brightness=152.7
[talker-3] [INFO] [1785746594.791843616] [talker_cpp]: pub: "Hello from C++, seq=27"
[listener-4] [INFO] [1785746594.792076984] [listener_cpp]: recv: "Hello from C++, seq=27"
[image_processor-11] [INFO] [1785746594.832464580] [image_processor_py]: rcv Image: 320x240 avg_brightness=154.3
[listener-2] [INFO] [1785746594.855533606] [listener_py]: recv: "Hello from C++, seq=27"
[listener-4] [INFO] [1785746594.876106387] [listener_cpp]: recv: "Hello from PY, seq=15"
[image_processor-11] [INFO] [1785746594.904548194] [image_processor_py]: rcv Image: 320x240 avg_brightness=156.0
[listener-2] [INFO] [1785746594.909111952] [listener_py]: recv: "Hello from PY, seq=15"
[talker-1] [INFO] [1785746594.910056983] [talker_py]: pub: "Hello from PY, seq=15"
[image_processor-11] [INFO] [1785746595.023500429] [image_processor_py]: rcv Image: 320x240 avg_brightness=157.7
[tf2_listener-9] [INFO] [1785746595.100014373] [tf2_listener_cpp]: gripper in base_link: x=-0.020 y=-0.009 z=0.298
[image_processor-11] [INFO] [1785746595.106570377] [image_processor_py]: rcv Image: 320x240 avg_brightness=159.3
[image_processor-11] [INFO] [1785746595.196057719] [image_processor_py]: rcv Image: 320x240 avg_brightness=161.0
[talker-3] [INFO] [1785746595.291880675] [talker_cpp]: pub: "Hello from C++, seq=28"
[listener-4] [INFO] [1785746595.292088637] [listener_cpp]: recv: "Hello from C++, seq=28"
[image_processor-11] [INFO] [1785746595.311602734] [image_processor_py]: rcv Image: 320x240 avg_brightness=162.7
[listener-2] [INFO] [1785746595.348453491] [listener_py]: recv: "Hello from C++, seq=28"
[listener-4] [INFO] [1785746595.376102921] [listener_cpp]: recv: "Hello from PY, seq=16"
[talker-1] [INFO] [1785746595.430139125] [talker_py]: pub: "Hello from PY, seq=16"
[image_processor-11] [INFO] [1785746595.430833222] [image_processor_py]: rcv Image: 320x240 avg_brightness=164.3
[listener-2] [INFO] [1785746595.431131994] [listener_py]: recv: "Hello from PY, seq=16"
[image_processor-11] [INFO] [1785746595.515316394] [image_processor_py]: rcv Image: 320x240 avg_brightness=166.0
[tf2_listener-9] [INFO] [1785746595.600136499] [tf2_listener_cpp]: gripper in base_link: x=-0.026 y=-0.007 z=0.296
[image_processor-11] [INFO] [1785746595.614206061] [image_processor_py]: rcv Image: 320x240 avg_brightness=167.7
[image_processor-11] [INFO] [1785746595.707783282] [image_processor_py]: rcv Image: 320x240 avg_brightness=169.3
[talker-3] [INFO] [1785746595.791941407] [talker_cpp]: pub: "Hello from C++, seq=29"
[listener-4] [INFO] [1785746595.792181744] [listener_cpp]: recv: "Hello from C++, seq=29"
[image_processor-11] [INFO] [1785746595.812530334] [image_processor_py]: rcv Image: 320x240 avg_brightness=85.7
[listener-2] [INFO] [1785746595.852863041] [listener_py]: recv: "Hello from C++, seq=29"
[listener-4] [INFO] [1785746595.876133793] [listener_cpp]: recv: "Hello from PY, seq=17"
[image_processor-11] [INFO] [1785746595.915390781] [image_processor_py]: rcv Image: 320x240 avg_brightness=87.3
[listener-2] [INFO] [1785746595.924290162] [listener_py]: recv: "Hello from PY, seq=17"
[talker-1] [INFO] [1785746595.925751580] [talker_py]: pub: "Hello from PY, seq=17"
[image_processor-11] [INFO] [1785746596.038349615] [image_processor_py]: rcv Image: 320x240 avg_brightness=89.0
[tf2_listener-9] [INFO] [1785746596.100123102] [tf2_listener_cpp]: gripper in base_link: x=-0.030 y=-0.001 z=0.296
[image_processor-11] [INFO] [1785746596.107160505] [image_processor_py]: rcv Image: 320x240 avg_brightness=90.7
[image_processor-11] [INFO] [1785746596.223948645] [image_processor_py]: rcv Image: 320x240 avg_brightness=92.3
[talker-3] [INFO] [1785746596.292148054] [talker_cpp]: pub: "Hello from C++, seq=30"
[listener-4] [INFO] [1785746596.292440968] [listener_cpp]: recv: "Hello from C++, seq=30"
[image_processor-11] [INFO] [1785746596.323870182] [image_processor_py]: rcv Image: 320x240 avg_brightness=94.0
[listener-2] [INFO] [1785746596.347701077] [listener_py]: recv: "Hello from C++, seq=30"
[listener-4] [INFO] [1785746596.376083570] [listener_cpp]: recv: "Hello from PY, seq=18"
[image_processor-11] [INFO] [1785746596.421964446] [image_processor_py]: rcv Image: 320x240 avg_brightness=95.7
[talker-1] [INFO] [1785746596.426007381] [talker_py]: pub: "Hello from PY, seq=18"
[listener-2] [INFO] [1785746596.426726219] [listener_py]: recv: "Hello from PY, seq=18"
[image_processor-11] [INFO] [1785746596.499429736] [image_processor_py]: rcv Image: 320x240 avg_brightness=97.3
[tf2_listener-9] [INFO] [1785746596.600181318] [tf2_listener_cpp]: gripper in base_link: x=-0.028 y=0.005 z=0.296
[image_processor-11] [INFO] [1785746596.628068495] [image_processor_py]: rcv Image: 320x240 avg_brightness=99.0
[image_processor-11] [INFO] [1785746596.729927484] [image_processor_py]: rcv Image: 320x240 avg_brightness=100.7
[talker-3] [INFO] [1785746596.791944325] [talker_cpp]: pub: "Hello from C++, seq=31"
[listener-4] [INFO] [1785746596.792233336] [listener_cpp]: recv: "Hello from C++, seq=31"
[image_processor-11] [INFO] [1785746596.811784316] [image_processor_py]: rcv Image: 320x240 avg_brightness=102.3
[listener-2] [INFO] [1785746596.843708671] [listener_py]: recv: "Hello from C++, seq=31"
[listener-4] [INFO] [1785746596.876102842] [listener_cpp]: recv: "Hello from PY, seq=19"
[image_processor-11] [INFO] [1785746596.916141236] [image_processor_py]: rcv Image: 320x240 avg_brightness=104.0
[listener-2] [INFO] [1785746596.920785481] [listener_py]: recv: "Hello from PY, seq=19"
[talker-1] [INFO] [1785746596.921408857] [talker_py]: pub: "Hello from PY, seq=19"
[image_processor-11] [INFO] [1785746597.011376741] [image_processor_py]: rcv Image: 320x240 avg_brightness=105.7
[tf2_listener-9] [INFO] [1785746597.100166049] [tf2_listener_cpp]: gripper in base_link: x=-0.022 y=0.009 z=0.297
[image_processor-11] [INFO] [1785746597.106599249] [image_processor_py]: rcv Image: 320x240 avg_brightness=107.3
[image_processor-11] [INFO] [1785746597.242028990] [image_processor_py]: rcv Image: 320x240 avg_brightness=109.0
[talker-3] [INFO] [1785746597.292269091] [talker_cpp]: pub: "Hello from C++, seq=32"
[listener-4] [INFO] [1785746597.292501373] [listener_cpp]: recv: "Hello from C++, seq=32"
[image_processor-11] [INFO] [1785746597.330439538] [image_processor_py]: rcv Image: 320x240 avg_brightness=110.7
[listener-2] [INFO] [1785746597.357719564] [listener_py]: recv: "Hello from C++, seq=32"
[listener-4] [INFO] [1785746597.376353140] [listener_cpp]: recv: "Hello from PY, seq=20"
[talker-1] [INFO] [1785746597.447030534] [talker_py]: pub: "Hello from PY, seq=20"
[listener-2] [INFO] [1785746597.448569108] [listener_py]: recv: "Hello from PY, seq=20"
[image_processor-11] [INFO] [1785746597.457556313] [image_processor_py]: rcv Image: 320x240 avg_brightness=112.3
[image_processor-11] [INFO] [1785746597.509248729] [image_processor_py]: rcv Image: 320x240 avg_brightness=114.0
[tf2_listener-9] [INFO] [1785746597.600213042] [tf2_listener_cpp]: gripper in base_link: x=-0.014 y=0.008 z=0.299
[image_processor-11] [INFO] [1785746597.606268000] [image_processor_py]: rcv Image: 320x240 avg_brightness=115.7
[image_processor-11] [INFO] [1785746597.706511923] [image_processor_py]: rcv Image: 320x240 avg_brightness=117.3
[talker-3] [INFO] [1785746597.792052882] [talker_cpp]: pub: "Hello from C++, seq=33"
[listener-4] [INFO] [1785746597.792287605] [listener_cpp]: recv: "Hello from C++, seq=33"
[image_processor-11] [INFO] [1785746597.823782376] [image_processor_py]: rcv Image: 320x240 avg_brightness=119.0
[listener-2] [INFO] [1785746597.848619387] [listener_py]: recv: "Hello from C++, seq=33"
[listener-4] [INFO] [1785746597.876140824] [listener_cpp]: recv: "Hello from PY, seq=21"
[image_processor-11] [INFO] [1785746597.915272207] [image_processor_py]: rcv Image: 320x240 avg_brightness=120.7
[talker-1] [INFO] [1785746597.916990254] [talker_py]: pub: "Hello from PY, seq=21"
[listener-2] [INFO] [1785746597.918878693] [listener_py]: recv: "Hello from PY, seq=21"
[image_processor-11] [INFO] [1785746598.009179775] [image_processor_py]: rcv Image: 320x240 avg_brightness=122.3
[tf2_listener-9] [INFO] [1785746598.100256386] [tf2_listener_cpp]: gripper in base_link: x=-0.006 y=0.003 z=0.300
[image_processor-11] [INFO] [1785746598.109292986] [image_processor_py]: rcv Image: 320x240 avg_brightness=124.0
[image_processor-11] [INFO] [1785746598.222507613] [image_processor_py]: rcv Image: 320x240 avg_brightness=125.7
[talker-3] [INFO] [1785746598.292121401] [talker_cpp]: pub: "Hello from C++, seq=34"
[listener-4] [INFO] [1785746598.292281308] [listener_cpp]: recv: "Hello from C++, seq=34"
[image_processor-11] [INFO] [1785746598.346407766] [image_processor_py]: rcv Image: 320x240 avg_brightness=127.3
[listener-2] [INFO] [1785746598.364286095] [listener_py]: recv: "Hello from C++, seq=34"
[listener-4] [INFO] [1785746598.376104733] [listener_cpp]: recv: "Hello from PY, seq=22"
[talker-1] [INFO] [1785746598.441237467] [talker_py]: pub: "Hello from PY, seq=22"
[listener-2] [INFO] [1785746598.442407191] [listener_py]: recv: "Hello from PY, seq=22"
[image_processor-11] [INFO] [1785746598.453883781] [image_processor_py]: rcv Image: 320x240 avg_brightness=129.0
[image_processor-11] [INFO] [1785746598.534351194] [image_processor_py]: rcv Image: 320x240 avg_brightness=130.7
[tf2_listener-9] [INFO] [1785746598.600307485] [tf2_listener_cpp]: gripper in base_link: x=0.004 y=-0.001 z=0.300
[image_processor-11] [INFO] [1785746598.613172743] [image_processor_py]: rcv Image: 320x240 avg_brightness=132.3
[image_processor-11] [INFO] [1785746598.726494453] [image_processor_py]: rcv Image: 320x240 avg_brightness=134.0
[talker-3] [INFO] [1785746598.792367046] [talker_cpp]: pub: "Hello from C++, seq=35"
[listener-4] [INFO] [1785746598.792638829] [listener_cpp]: recv: "Hello from C++, seq=35"
[image_processor-11] [INFO] [1785746598.861432361] [image_processor_py]: rcv Image: 320x240 avg_brightness=135.7
[listener-2] [INFO] [1785746598.872863351] [listener_py]: recv: "Hello from C++, seq=35"
[listener-4] [INFO] [1785746598.876022058] [listener_cpp]: recv: "Hello from PY, seq=23"
[image_processor-11] [INFO] [1785746598.919783995] [image_processor_py]: rcv Image: 320x240 avg_brightness=137.3
[listener-2] [INFO] [1785746598.923068859] [listener_py]: recv: "Hello from PY, seq=23"
[talker-1] [INFO] [1785746598.923920775] [talker_py]: pub: "Hello from PY, seq=23"
[image_processor-11] [INFO] [1785746599.016638328] [image_processor_py]: rcv Image: 320x240 avg_brightness=139.0
[tf2_listener-9] [INFO] [1785746599.100364452] [tf2_listener_cpp]: gripper in base_link: x=0.014 y=-0.001 z=0.299
[image_processor-11] [INFO] [1785746599.123615664] [image_processor_py]: rcv Image: 320x240 avg_brightness=140.7
[image_processor-11] [INFO] [1785746599.204230491] [image_processor_py]: rcv Image: 320x240 avg_brightness=142.3
[talker-3] [INFO] [1785746599.292489761] [talker_cpp]: pub: "Hello from C++, seq=36"
[listener-4] [INFO] [1785746599.292805459] [listener_cpp]: recv: "Hello from C++, seq=36"
[image_processor-11] [INFO] [1785746599.319189769] [image_processor_py]: rcv Image: 320x240 avg_brightness=144.0
[listener-2] [INFO] [1785746599.357059207] [listener_py]: recv: "Hello from C++, seq=36"
[listener-4] [INFO] [1785746599.376147402] [listener_cpp]: recv: "Hello from PY, seq=24"
[image_processor-11] [INFO] [1785746599.407007445] [image_processor_py]: rcv Image: 320x240 avg_brightness=145.7
[talker-1] [INFO] [1785746599.412449920] [talker_py]: pub: "Hello from PY, seq=24"
[listener-2] [INFO] [1785746599.413139704] [listener_py]: recv: "Hello from PY, seq=24"
[image_processor-11] [INFO] [1785746599.519711657] [image_processor_py]: rcv Image: 320x240 avg_brightness=147.3
[image_processor-11] [INFO] [1785746599.599748517] [image_processor_py]: rcv Image: 320x240 avg_brightness=149.0
[tf2_listener-9] [INFO] [1785746599.600337307] [tf2_listener_cpp]: gripper in base_link: x=0.022 y=0.003 z=0.298
[image_processor-11] [INFO] [1785746599.721064243] [image_processor_py]: rcv Image: 320x240 avg_brightness=150.7
[talker-3] [INFO] [1785746599.792283993] [talker_cpp]: pub: "Hello from C++, seq=37"
[listener-4] [INFO] [1785746599.792565657] [listener_cpp]: recv: "Hello from C++, seq=37"
[image_processor-11] [INFO] [1785746599.815274490] [image_processor_py]: rcv Image: 320x240 avg_brightness=152.3
[listener-2] [INFO] [1785746599.839899135] [listener_py]: recv: "Hello from C++, seq=37"
[listener-4] [INFO] [1785746599.876230094] [listener_cpp]: recv: "Hello from PY, seq=25"
[listener-2] [INFO] [1785746599.918288292] [listener_py]: recv: "Hello from PY, seq=25"
[talker-1] [INFO] [1785746599.919344436] [talker_py]: pub: "Hello from PY, seq=25"
[image_processor-11] [INFO] [1785746599.919855032] [image_processor_py]: rcv Image: 320x240 avg_brightness=154.0
[image_processor-11] [INFO] [1785746600.015856504] [image_processor_py]: rcv Image: 320x240 avg_brightness=155.7
[tf2_listener-9] [INFO] [1785746600.100401041] [tf2_listener_cpp]: gripper in base_link: x=0.026 y=0.009 z=0.296
[image_processor-11] [INFO] [1785746600.137932539] [image_processor_py]: rcv Image: 320x240 avg_brightness=157.3
[image_processor-11] [INFO] [1785746600.220319027] [image_processor_py]: rcv Image: 320x240 avg_brightness=159.0
[talker-3] [INFO] [1785746600.292531961] [talker_cpp]: pub: "Hello from C++, seq=38"
[listener-4] [INFO] [1785746600.292811866] [listener_cpp]: recv: "Hello from C++, seq=38"
[image_processor-11] [INFO] [1785746600.331811937] [image_processor_py]: rcv Image: 320x240 avg_brightness=160.7
[listener-2] [INFO] [1785746600.360358545] [listener_py]: recv: "Hello from C++, seq=38"
[listener-4] [INFO] [1785746600.376139249] [listener_cpp]: recv: "Hello from PY, seq=26"
[listener-2] [INFO] [1785746600.444202216] [listener_py]: recv: "Hello from PY, seq=26"
[talker-1] [INFO] [1785746600.444878022] [talker_py]: pub: "Hello from PY, seq=26"
[image_processor-11] [INFO] [1785746600.445151582] [image_processor_py]: rcv Image: 320x240 avg_brightness=162.3
[image_processor-11] [INFO] [1785746600.523351298] [image_processor_py]: rcv Image: 320x240 avg_brightness=164.0
[tf2_listener-9] [INFO] [1785746600.600478905] [tf2_listener_cpp]: gripper in base_link: x=0.026 y=0.014 z=0.296
[image_processor-11] [INFO] [1785746600.621160322] [image_processor_py]: rcv Image: 320x240 avg_brightness=165.7
[image_processor-11] [INFO] [1785746600.716218475] [image_processor_py]: rcv Image: 320x240 avg_brightness=167.3
[talker-3] [INFO] [1785746600.792384083] [talker_cpp]: pub: "Hello from C++, seq=39"
[listener-4] [INFO] [1785746600.792604233] [listener_cpp]: recv: "Hello from C++, seq=39"
[image_processor-11] [INFO] [1785746600.830545931] [image_processor_py]: rcv Image: 320x240 avg_brightness=169.0
[listener-2] [INFO] [1785746600.857937052] [listener_py]: recv: "Hello from C++, seq=39"
[listener-4] [INFO] [1785746600.876288220] [listener_cpp]: recv: "Hello from PY, seq=27"
[image_processor-11] [INFO] [1785746600.921301662] [image_processor_py]: rcv Image: 320x240 avg_brightness=85.3
[listener-2] [INFO] [1785746600.933199580] [listener_py]: recv: "Hello from PY, seq=27"
[talker-1] [INFO] [1785746600.937206158] [talker_py]: pub: "Hello from PY, seq=27"
[image_processor-11] [INFO] [1785746601.050267021] [image_processor_py]: rcv Image: 320x240 avg_brightness=87.0
[tf2_listener-9] [INFO] [1785746601.100569465] [tf2_listener_cpp]: gripper in base_link: x=0.025 y=0.013 z=0.296
[image_processor-11] [INFO] [1785746601.124160939] [image_processor_py]: rcv Image: 320x240 avg_brightness=88.7
[image_processor-11] [INFO] [1785746601.223360247] [image_processor_py]: rcv Image: 320x240 avg_brightness=90.3
[talker-3] [INFO] [1785746601.292594443] [talker_cpp]: pub: "Hello from C++, seq=40"
[listener-4] [INFO] [1785746601.292906163] [listener_cpp]: recv: "Hello from C++, seq=40"
[image_processor-11] [INFO] [1785746601.315825427] [image_processor_py]: rcv Image: 320x240 avg_brightness=92.0
[listener-2] [INFO] [1785746601.353807004] [listener_py]: recv: "Hello from C++, seq=40"
[listener-4] [INFO] [1785746601.376465623] [listener_cpp]: recv: "Hello from PY, seq=28"
[listener-2] [INFO] [1785746601.425101665] [listener_py]: recv: "Hello from PY, seq=28"
[talker-1] [INFO] [1785746601.426009327] [talker_py]: pub: "Hello from PY, seq=28"
[image_processor-11] [INFO] [1785746601.426741315] [image_processor_py]: rcv Image: 320x240 avg_brightness=93.7
[image_processor-11] [INFO] [1785746601.508967229] [image_processor_py]: rcv Image: 320x240 avg_brightness=95.3
[tf2_listener-9] [INFO] [1785746601.600565378] [tf2_listener_cpp]: gripper in base_link: x=0.022 y=0.008 z=0.297
[image_processor-11] [INFO] [1785746601.620613766] [image_processor_py]: rcv Image: 320x240 avg_brightness=97.0
[image_processor-11] [INFO] [1785746601.721728195] [image_processor_py]: rcv Image: 320x240 avg_brightness=98.7
[talker-3] [INFO] [1785746601.792483733] [talker_cpp]: pub: "Hello from C++, seq=41"
[listener-4] [INFO] [1785746601.792666073] [listener_cpp]: recv: "Hello from C++, seq=41"
[image_processor-11] [INFO] [1785746601.813295138] [image_processor_py]: rcv Image: 320x240 avg_brightness=100.3
[listener-2] [INFO] [1785746601.843244409] [listener_py]: recv: "Hello from C++, seq=41"
[listener-4] [INFO] [1785746601.876143315] [listener_cpp]: recv: "Hello from PY, seq=29"
[image_processor-11] [INFO] [1785746601.921441367] [image_processor_py]: rcv Image: 320x240 avg_brightness=102.0
[listener-2] [INFO] [1785746601.925338885] [listener_py]: recv: "Hello from PY, seq=29"
[talker-1] [INFO] [1785746601.926637741] [talker_py]: pub: "Hello from PY, seq=29"
[image_processor-11] [INFO] [1785746602.009932226] [image_processor_py]: rcv Image: 320x240 avg_brightness=103.7
[tf2_listener-9] [INFO] [1785746602.100618126] [tf2_listener_cpp]: gripper in base_link: x=0.016 y=0.003 z=0.299
[image_processor-11] [INFO] [1785746602.119545741] [image_processor_py]: rcv Image: 320x240 avg_brightness=105.3
[image_processor-11] [INFO] [1785746602.215247752] [image_processor_py]: rcv Image: 320x240 avg_brightness=107.0
[talker-3] [INFO] [1785746602.292505620] [talker_cpp]: pub: "Hello from C++, seq=42"
[listener-4] [INFO] [1785746602.292690324] [listener_cpp]: recv: "Hello from C++, seq=42"
[image_processor-11] [INFO] [1785746602.315017926] [image_processor_py]: rcv Image: 320x240 avg_brightness=108.7
[listener-2] [INFO] [1785746602.363199157] [listener_py]: recv: "Hello from C++, seq=42"
[listener-4] [INFO] [1785746602.376253694] [listener_cpp]: recv: "Hello from PY, seq=30"
[image_processor-11] [INFO] [1785746602.420936074] [image_processor_py]: rcv Image: 320x240 avg_brightness=110.3
[talker-1] [INFO] [1785746602.424527417] [talker_py]: pub: "Hello from PY, seq=30"
[listener-2] [INFO] [1785746602.426245442] [listener_py]: recv: "Hello from PY, seq=30"
[image_processor-11] [INFO] [1785746602.523759412] [image_processor_py]: rcv Image: 320x240 avg_brightness=112.0
[tf2_listener-9] [INFO] [1785746602.600650195] [tf2_listener_cpp]: gripper in base_link: x=0.006 y=-0.000 z=0.300
[image_processor-11] [INFO] [1785746602.617129226] [image_processor_py]: rcv Image: 320x240 avg_brightness=113.7
[image_processor-11] [INFO] [1785746602.717403199] [image_processor_py]: rcv Image: 320x240 avg_brightness=115.3
[talker-3] [INFO] [1785746602.792538414] [talker_cpp]: pub: "Hello from C++, seq=43"
[listener-4] [INFO] [1785746602.792768766] [listener_cpp]: recv: "Hello from C++, seq=43"
[image_processor-11] [INFO] [1785746602.816249625] [image_processor_py]: rcv Image: 320x240 avg_brightness=117.0
[listener-2] [INFO] [1785746602.860430749] [listener_py]: recv: "Hello from C++, seq=43"
[listener-4] [INFO] [1785746602.876318828] [listener_cpp]: recv: "Hello from PY, seq=31"
[listener-2] [INFO] [1785746602.919364569] [listener_py]: recv: "Hello from PY, seq=31"
[talker-1] [INFO] [1785746602.921552241] [talker_py]: pub: "Hello from PY, seq=31"
[image_processor-11] [INFO] [1785746602.923372244] [image_processor_py]: rcv Image: 320x240 avg_brightness=118.7
[image_processor-11] [INFO] [1785746603.040670610] [image_processor_py]: rcv Image: 320x240 avg_brightness=120.3
[tf2_listener-9] [INFO] [1785746603.100739820] [tf2_listener_cpp]: gripper in base_link: x=-0.004 y=0.001 z=0.300
[image_processor-11] [INFO] [1785746603.129210908] [image_processor_py]: rcv Image: 320x240 avg_brightness=122.0
[image_processor-11] [INFO] [1785746603.240955465] [image_processor_py]: rcv Image: 320x240 avg_brightness=123.7
[talker-3] [INFO] [1785746603.292634036] [talker_cpp]: pub: "Hello from C++, seq=44"
[listener-4] [INFO] [1785746603.292854928] [listener_cpp]: recv: "Hello from C++, seq=44"
[image_processor-11] [INFO] [1785746603.331464135] [image_processor_py]: rcv Image: 320x240 avg_brightness=125.3
[listener-2] [INFO] [1785746603.359020572] [listener_py]: recv: "Hello from C++, seq=44"
[listener-4] [INFO] [1785746603.376583454] [listener_cpp]: recv: "Hello from PY, seq=32"
[listener-2] [INFO] [1785746603.445724599] [listener_py]: recv: "Hello from PY, seq=32"
[talker-1] [INFO] [1785746603.447176631] [talker_py]: pub: "Hello from PY, seq=32"
[image_processor-11] [INFO] [1785746603.448342955] [image_processor_py]: rcv Image: 320x240 avg_brightness=127.0
docker/robot_e2e.log
+103
View File
@@ -0,0 +1,103 @@
[INFO] [launch]: All log files can be found below /root/.ros/log/2026-08-03-08-39-30-381003-docker-desktop-45
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [joint_state_publisher-1]: process started with pid [46]
[INFO] [robot_state_publisher-2]: process started with pid [48]
[INFO] [tf2_listener-3]: process started with pid [50]
[joint_state_publisher-1] [INFO] [1785746371.317076490] [joint_state_publisher_cpp]: joint_state_publisher_cpp started, joints: 3
[tf2_listener-3] [INFO] [1785746371.496087617] [tf2_listener_cpp]: tf2_listener_cpp started
[robot_state_publisher-2] [INFO] [1785746371.881604147] [robot_state_publisher]: got segment base_link
[robot_state_publisher-2] [INFO] [1785746371.881920376] [robot_state_publisher]: got segment gripper
[robot_state_publisher-2] [INFO] [1785746371.881940674] [robot_state_publisher]: got segment link1
[robot_state_publisher-2] [INFO] [1785746371.881948723] [robot_state_publisher]: got segment link2
[tf2_listener-3] [INFO] [1785746371.996437265] [tf2_listener_cpp]: gripper in base_link: x=0.013 y=0.004 z=0.299
[tf2_listener-3] [INFO] [1785746372.496379332] [tf2_listener_cpp]: gripper in base_link: x=0.019 y=0.009 z=0.298
[tf2_listener-3] [INFO] [1785746372.996514629] [tf2_listener_cpp]: gripper in base_link: x=0.024 y=0.013 z=0.296
[tf2_listener-3] [INFO] [1785746373.496537884] [tf2_listener_cpp]: gripper in base_link: x=0.027 y=0.012 z=0.296
[tf2_listener-3] [INFO] [1785746373.996671137] [tf2_listener_cpp]: gripper in base_link: x=0.028 y=0.007 z=0.296
[tf2_listener-3] [INFO] [1785746374.496810765] [tf2_listener_cpp]: gripper in base_link: x=0.024 y=-0.000 z=0.297
[tf2_listener-3] [INFO] [1785746374.996864552] [tf2_listener_cpp]: gripper in base_link: x=0.017 y=-0.004 z=0.298
[tf2_listener-3] [INFO] [1785746375.496864454] [tf2_listener_cpp]: gripper in base_link: x=0.006 y=-0.003 z=0.300
[tf2_listener-3] [INFO] [1785746375.996864732] [tf2_listener_cpp]: gripper in base_link: x=-0.002 y=0.001 z=0.300
[tf2_listener-3] [INFO] [1785746376.496761854] [tf2_listener_cpp]: gripper in base_link: x=-0.012 y=0.006 z=0.299
[tf2_listener-3] [INFO] [1785746376.997136114] [tf2_listener_cpp]: gripper in base_link: x=-0.021 y=0.006 z=0.298
[tf2_listener-3] [INFO] [1785746377.496974435] [tf2_listener_cpp]: gripper in base_link: x=-0.027 y=0.002 z=0.296
[tf2_listener-3] [INFO] [1785746377.997189104] [tf2_listener_cpp]: gripper in base_link: x=-0.029 y=-0.005 z=0.296
[tf2_listener-3] [INFO] [1785746378.497026589] [tf2_listener_cpp]: gripper in base_link: x=-0.026 y=-0.011 z=0.296
[tf2_listener-3] [INFO] [1785746378.997122056] [tf2_listener_cpp]: gripper in base_link: x=-0.021 y=-0.011 z=0.297
[tf2_listener-3] [INFO] [1785746379.497295054] [tf2_listener_cpp]: gripper in base_link: x=-0.014 y=-0.007 z=0.299
[tf2_listener-3] [INFO] [1785746379.997304065] [tf2_listener_cpp]: gripper in base_link: x=-0.006 y=-0.002 z=0.300
[tf2_listener-3] [INFO] [1785746380.497290417] [tf2_listener_cpp]: gripper in base_link: x=0.004 y=0.000 z=0.300
[tf2_listener-3] [INFO] [1785746380.997262459] [tf2_listener_cpp]: gripper in base_link: x=0.014 y=-0.002 z=0.299
[tf2_listener-3] [INFO] [1785746381.497306715] [tf2_listener_cpp]: gripper in base_link: x=0.021 y=-0.007 z=0.298
[tf2_listener-3] [INFO] [1785746381.997472857] [tf2_listener_cpp]: gripper in base_link: x=0.024 y=-0.012 z=0.296
[tf2_listener-3] [INFO] [1785746382.497517542] [tf2_listener_cpp]: gripper in base_link: x=0.026 y=-0.014 z=0.296
[tf2_listener-3] [INFO] [1785746382.997489577] [tf2_listener_cpp]: gripper in base_link: x=0.026 y=-0.011 z=0.296
[tf2_listener-3] [INFO] [1785746383.493353822] [tf2_listener_cpp]: gripper in base_link: x=0.024 y=-0.005 z=0.297
[tf2_listener-3] [INFO] [1785746383.993345141] [tf2_listener_cpp]: gripper in base_link: x=0.016 y=0.001 z=0.299
[tf2_listener-3] [INFO] [1785746384.493508096] [tf2_listener_cpp]: gripper in base_link: x=0.007 y=0.002 z=0.300
[tf2_listener-3] [INFO] [1785746384.993422845] [tf2_listener_cpp]: gripper in base_link: x=-0.004 y=-0.002 z=0.300
[tf2_listener-3] [INFO] [1785746385.493589851] [tf2_listener_cpp]: gripper in base_link: x=-0.011 y=-0.006 z=0.299
[tf2_listener-3] [INFO] [1785746385.993580865] [tf2_listener_cpp]: gripper in base_link: x=-0.020 y=-0.009 z=0.298
[tf2_listener-3] [INFO] [1785746386.493741973] [tf2_listener_cpp]: gripper in base_link: x=-0.026 y=-0.007 z=0.296
[tf2_listener-3] [INFO] [1785746386.993760576] [tf2_listener_cpp]: gripper in base_link: x=-0.030 y=-0.001 z=0.296
[tf2_listener-3] [INFO] [1785746387.493818888] [tf2_listener_cpp]: gripper in base_link: x=-0.028 y=0.006 z=0.296
[tf2_listener-3] [INFO] [1785746387.993897792] [tf2_listener_cpp]: gripper in base_link: x=-0.022 y=0.009 z=0.297
[tf2_listener-3] [INFO] [1785746388.493876517] [tf2_listener_cpp]: gripper in base_link: x=-0.014 y=0.008 z=0.299
[tf2_listener-3] [INFO] [1785746388.993929006] [tf2_listener_cpp]: gripper in base_link: x=-0.006 y=0.003 z=0.300
[tf2_listener-3] [INFO] [1785746389.494117544] [tf2_listener_cpp]: gripper in base_link: x=0.004 y=-0.001 z=0.300
[tf2_listener-3] [INFO] [1785746389.994044812] [tf2_listener_cpp]: gripper in base_link: x=0.013 y=-0.002 z=0.299
[tf2_listener-3] [INFO] [1785746390.494123474] [tf2_listener_cpp]: gripper in base_link: x=0.022 y=0.003 z=0.298
[tf2_listener-3] [INFO] [1785746390.994017428] [tf2_listener_cpp]: gripper in base_link: x=0.026 y=0.010 z=0.296
[tf2_listener-3] [INFO] [1785746391.494201169] [tf2_listener_cpp]: gripper in base_link: x=0.026 y=0.014 z=0.296
[tf2_listener-3] [INFO] [1785746391.994225935] [tf2_listener_cpp]: gripper in base_link: x=0.025 y=0.013 z=0.296
[tf2_listener-3] [INFO] [1785746392.494365400] [tf2_listener_cpp]: gripper in base_link: x=0.022 y=0.008 z=0.297
[tf2_listener-3] [INFO] [1785746392.994299594] [tf2_listener_cpp]: gripper in base_link: x=0.016 y=0.003 z=0.299
[tf2_listener-3] [INFO] [1785746393.494336899] [tf2_listener_cpp]: gripper in base_link: x=0.006 y=-0.000 z=0.300
[tf2_listener-3] [INFO] [1785746393.994478649] [tf2_listener_cpp]: gripper in base_link: x=-0.004 y=0.001 z=0.300
[tf2_listener-3] [INFO] [1785746394.494587177] [tf2_listener_cpp]: gripper in base_link: x=-0.012 y=0.006 z=0.299
[tf2_listener-3] [INFO] [1785746394.994674211] [tf2_listener_cpp]: gripper in base_link: x=-0.019 y=0.010 z=0.298
[tf2_listener-3] [INFO] [1785746395.494691448] [tf2_listener_cpp]: gripper in base_link: x=-0.025 y=0.011 z=0.296
[tf2_listener-3] [INFO] [1785746395.994634551] [tf2_listener_cpp]: gripper in base_link: x=-0.029 y=0.007 z=0.296
[tf2_listener-3] [INFO] [1785746396.494795424] [tf2_listener_cpp]: gripper in base_link: x=-0.028 y=-0.000 z=0.296
[tf2_listener-3] [INFO] [1785746396.994828501] [tf2_listener_cpp]: gripper in base_link: x=-0.023 y=-0.005 z=0.297
[tf2_listener-3] [INFO] [1785746397.494801750] [tf2_listener_cpp]: gripper in base_link: x=-0.014 y=-0.006 z=0.299
[tf2_listener-3] [INFO] [1785746397.994993450] [tf2_listener_cpp]: gripper in base_link: x=-0.005 y=-0.003 z=0.300
[tf2_listener-3] [INFO] [1785746398.495096046] [tf2_listener_cpp]: gripper in base_link: x=0.004 y=0.002 z=0.300
[tf2_listener-3] [INFO] [1785746398.994936454] [tf2_listener_cpp]: gripper in base_link: x=0.013 y=0.004 z=0.299
[tf2_listener-3] [INFO] [1785746399.495189976] [tf2_listener_cpp]: gripper in base_link: x=0.022 y=0.001 z=0.297
[tf2_listener-3] [INFO] [1785746399.995182111] [tf2_listener_cpp]: gripper in base_link: x=0.027 y=-0.005 z=0.296
[tf2_listener-3] [INFO] [1785746400.495324050] [tf2_listener_cpp]: gripper in base_link: x=0.027 y=-0.011 z=0.296
[tf2_listener-3] [INFO] [1785746400.995201645] [tf2_listener_cpp]: gripper in base_link: x=0.025 y=-0.013 z=0.296
[tf2_listener-3] [INFO] [1785746401.495322185] [tf2_listener_cpp]: gripper in base_link: x=0.020 y=-0.011 z=0.297
[tf2_listener-3] [INFO] [1785746401.995312174] [tf2_listener_cpp]: gripper in base_link: x=0.014 y=-0.005 z=0.299
[tf2_listener-3] [INFO] [1785746402.495516455] [tf2_listener_cpp]: gripper in base_link: x=0.005 y=-0.001 z=0.300
[tf2_listener-3] [INFO] [1785746402.995357182] [tf2_listener_cpp]: gripper in base_link: x=-0.005 y=-0.001 z=0.300
[tf2_listener-3] [INFO] [1785746403.495721348] [tf2_listener_cpp]: gripper in base_link: x=-0.013 y=-0.004 z=0.299
[tf2_listener-3] [INFO] [1785746403.995673080] [tf2_listener_cpp]: gripper in base_link: x=-0.020 y=-0.010 z=0.297
[tf2_listener-3] [INFO] [1785746404.495639517] [tf2_listener_cpp]: gripper in base_link: x=-0.024 y=-0.013 z=0.296
[tf2_listener-3] [INFO] [1785746404.995571059] [tf2_listener_cpp]: gripper in base_link: x=-0.027 y=-0.012 z=0.296
[tf2_listener-3] [INFO] [1785746405.495699621] [tf2_listener_cpp]: gripper in base_link: x=-0.028 y=-0.006 z=0.296
[tf2_listener-3] [INFO] [1785746405.995755115] [tf2_listener_cpp]: gripper in base_link: x=-0.023 y=0.001 z=0.297
[tf2_listener-3] [INFO] [1785746406.495935458] [tf2_listener_cpp]: gripper in base_link: x=-0.015 y=0.004 z=0.299
[tf2_listener-3] [INFO] [1785746406.995915855] [tf2_listener_cpp]: gripper in base_link: x=-0.006 y=0.003 z=0.300
[tf2_listener-3] [INFO] [1785746407.496177567] [tf2_listener_cpp]: gripper in base_link: x=0.004 y=-0.002 z=0.300
[tf2_listener-3] [INFO] [1785746407.996289984] [tf2_listener_cpp]: gripper in base_link: x=0.014 y=-0.006 z=0.299
[tf2_listener-3] [INFO] [1785746408.496079448] [tf2_listener_cpp]: gripper in base_link: x=0.022 y=-0.006 z=0.297
[tf2_listener-3] [INFO] [1785746408.996244465] [tf2_listener_cpp]: gripper in base_link: x=0.027 y=-0.001 z=0.296
[tf2_listener-3] [INFO] [1785746409.496147848] [tf2_listener_cpp]: gripper in base_link: x=0.029 y=0.006 z=0.296
[tf2_listener-3] [INFO] [1785746409.996263188] [tf2_listener_cpp]: gripper in base_link: x=0.025 y=0.011 z=0.296
[tf2_listener-3] [INFO] [1785746410.496388565] [tf2_listener_cpp]: gripper in base_link: x=0.021 y=0.011 z=0.297
[tf2_listener-3] [INFO] [1785746410.996257817] [tf2_listener_cpp]: gripper in base_link: x=0.014 y=0.007 z=0.299
[tf2_listener-3] [INFO] [1785746411.496573337] [tf2_listener_cpp]: gripper in base_link: x=0.006 y=0.002 z=0.300
[tf2_listener-3] [INFO] [1785746411.996493063] [tf2_listener_cpp]: gripper in base_link: x=-0.005 y=-0.001 z=0.300
[tf2_listener-3] [INFO] [1785746412.496630532] [tf2_listener_cpp]: gripper in base_link: x=-0.015 y=0.002 z=0.299
[tf2_listener-3] [INFO] [1785746412.996505296] [tf2_listener_cpp]: gripper in base_link: x=-0.021 y=0.008 z=0.297
[tf2_listener-3] [INFO] [1785746413.493986425] [tf2_listener_cpp]: gripper in base_link: x=-0.025 y=0.013 z=0.296
[tf2_listener-3] [INFO] [1785746413.994008183] [tf2_listener_cpp]: gripper in base_link: x=-0.026 y=0.014 z=0.296
[tf2_listener-3] [INFO] [1785746414.494203819] [tf2_listener_cpp]: gripper in base_link: x=-0.026 y=0.010 z=0.296
[tf2_listener-3] [INFO] [1785746414.994030874] [tf2_listener_cpp]: gripper in base_link: x=-0.023 y=0.004 z=0.297
[tf2_listener-3] [INFO] [1785746415.494112809] [tf2_listener_cpp]: gripper in base_link: x=-0.015 y=-0.001 z=0.299
[tf2_listener-3] [INFO] [1785746415.994161575] [tf2_listener_cpp]: gripper in base_link: x=-0.006 y=-0.002 z=0.300
[tf2_listener-3] [INFO] [1785746416.494364698] [tf2_listener_cpp]: gripper in base_link: x=0.005 y=0.003 z=0.300
[tf2_listener-3] [INFO] [1785746416.994450475] [tf2_listener_cpp]: gripper in base_link: x=0.013 y=0.007 z=0.299
[tf2_listener-3] [INFO] [1785746417.494504962] [tf2_listener_cpp]: gripper in base_link: x=0.021 y=0.009 z=0.297
docker/srv_e2e.log
+6
View File
@@ -0,0 +1,6 @@
[INFO] [launch]: All log files can be found below /root/.ros/log/2026-08-03-07-34-16-425307-docker-desktop-236
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [add_two_ints_server-1]: process started with pid [239]
[add_two_ints_server-1] [INFO] [1785742461.842140774] [add_two_ints_server_py]: add_two_ints_server_py ready, waiting for requests...
[add_two_ints_server-1] [INFO] [1785742461.964626865] [add_two_ints_server_py]: incoming: a=12, b=30 -> sum=42
[ERROR] [add_two_ints_server-1]: process has died [pid 239, exit code -9, cmd '/root/ros2_ws/install/py_srv/lib/py_srv/add_two_ints_server --ros-args -r __node:=add_two_ints_server_py'].
docker/vision_e2e.log
+448
View File
@@ -0,0 +1,448 @@
[INFO] [launch]: All log files can be found below /root/.ros/log/2026-08-03-08-35-41-686134-docker-desktop-133
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [fake_camera-1]: process started with pid [143]
[INFO] [image_processor-2]: process started with pid [145]
[fake_camera-1] [INFO] [1785746146.828625294] [fake_camera_py]: fake_camera_py started: 320x240 @ 10fps -> /image_raw
[image_processor-2] [INFO] [1785746147.719355071] [image_processor_py]: image_processor_py subscribed <- /image_raw (cv_bridge=True)
[image_processor-2] [INFO] [1785746147.805634979] [image_processor_py]: rcv Image: 320x240 avg_brightness=98.0
[image_processor-2] [INFO] [1785746147.872298331] [image_processor_py]: rcv Image: 320x240 avg_brightness=99.7
[image_processor-2] [INFO] [1785746147.947698778] [image_processor_py]: rcv Image: 320x240 avg_brightness=101.3
[image_processor-2] [INFO] [1785746148.008680953] [image_processor_py]: rcv Image: 320x240 avg_brightness=103.0
[image_processor-2] [INFO] [1785746148.070560447] [image_processor_py]: rcv Image: 320x240 avg_brightness=104.7
[image_processor-2] [INFO] [1785746148.137776786] [image_processor_py]: rcv Image: 320x240 avg_brightness=106.3
[image_processor-2] [INFO] [1785746148.214646941] [image_processor_py]: rcv Image: 320x240 avg_brightness=108.0
[image_processor-2] [INFO] [1785746148.346313137] [image_processor_py]: rcv Image: 320x240 avg_brightness=109.7
[image_processor-2] [INFO] [1785746148.437212263] [image_processor_py]: rcv Image: 320x240 avg_brightness=111.3
[image_processor-2] [INFO] [1785746148.603979392] [image_processor_py]: rcv Image: 320x240 avg_brightness=113.0
[image_processor-2] [INFO] [1785746148.659502646] [image_processor_py]: rcv Image: 320x240 avg_brightness=114.7
[image_processor-2] [INFO] [1785746148.736855456] [image_processor_py]: rcv Image: 320x240 avg_brightness=116.3
[image_processor-2] [INFO] [1785746148.847967223] [image_processor_py]: rcv Image: 320x240 avg_brightness=118.0
[image_processor-2] [INFO] [1785746148.925718305] [image_processor_py]: rcv Image: 320x240 avg_brightness=119.7
[image_processor-2] [INFO] [1785746149.064389261] [image_processor_py]: rcv Image: 320x240 avg_brightness=121.3
[image_processor-2] [INFO] [1785746149.145747650] [image_processor_py]: rcv Image: 320x240 avg_brightness=123.0
[image_processor-2] [INFO] [1785746149.256369154] [image_processor_py]: rcv Image: 320x240 avg_brightness=124.7
[image_processor-2] [INFO] [1785746149.343186627] [image_processor_py]: rcv Image: 320x240 avg_brightness=126.3
[image_processor-2] [INFO] [1785746149.448057770] [image_processor_py]: rcv Image: 320x240 avg_brightness=128.0
[image_processor-2] [INFO] [1785746149.562558566] [image_processor_py]: rcv Image: 320x240 avg_brightness=129.7
[image_processor-2] [INFO] [1785746149.674121112] [image_processor_py]: rcv Image: 320x240 avg_brightness=131.3
[image_processor-2] [INFO] [1785746149.879303679] [image_processor_py]: rcv Image: 320x240 avg_brightness=133.0
[image_processor-2] [INFO] [1785746149.962212352] [image_processor_py]: rcv Image: 320x240 avg_brightness=134.7
[image_processor-2] [INFO] [1785746150.074138301] [image_processor_py]: rcv Image: 320x240 avg_brightness=136.3
[image_processor-2] [INFO] [1785746150.430030809] [image_processor_py]: rcv Image: 320x240 avg_brightness=138.0
[image_processor-2] [INFO] [1785746150.695592486] [image_processor_py]: rcv Image: 320x240 avg_brightness=139.7
[image_processor-2] [INFO] [1785746150.791395501] [image_processor_py]: rcv Image: 320x240 avg_brightness=141.3
[image_processor-2] [INFO] [1785746150.854832580] [image_processor_py]: rcv Image: 320x240 avg_brightness=143.0
[image_processor-2] [INFO] [1785746150.915132659] [image_processor_py]: rcv Image: 320x240 avg_brightness=144.7
[image_processor-2] [INFO] [1785746150.990448904] [image_processor_py]: rcv Image: 320x240 avg_brightness=146.3
[image_processor-2] [INFO] [1785746151.071316967] [image_processor_py]: rcv Image: 320x240 avg_brightness=148.0
[image_processor-2] [INFO] [1785746151.133631548] [image_processor_py]: rcv Image: 320x240 avg_brightness=149.7
[image_processor-2] [INFO] [1785746151.210814389] [image_processor_py]: rcv Image: 320x240 avg_brightness=151.3
[image_processor-2] [INFO] [1785746151.273188137] [image_processor_py]: rcv Image: 320x240 avg_brightness=153.0
[image_processor-2] [INFO] [1785746151.349802462] [image_processor_py]: rcv Image: 320x240 avg_brightness=154.7
[image_processor-2] [INFO] [1785746151.410932722] [image_processor_py]: rcv Image: 320x240 avg_brightness=156.3
[image_processor-2] [INFO] [1785746151.465386119] [image_processor_py]: rcv Image: 320x240 avg_brightness=158.0
[image_processor-2] [INFO] [1785746151.524077652] [image_processor_py]: rcv Image: 320x240 avg_brightness=159.7
[image_processor-2] [INFO] [1785746151.594388033] [image_processor_py]: rcv Image: 320x240 avg_brightness=161.3
[image_processor-2] [INFO] [1785746151.664085803] [image_processor_py]: rcv Image: 320x240 avg_brightness=163.0
[image_processor-2] [INFO] [1785746151.722597794] [image_processor_py]: rcv Image: 320x240 avg_brightness=164.7
[image_processor-2] [INFO] [1785746151.788138072] [image_processor_py]: rcv Image: 320x240 avg_brightness=166.3
[image_processor-2] [INFO] [1785746151.847435022] [image_processor_py]: rcv Image: 320x240 avg_brightness=168.0
[image_processor-2] [INFO] [1785746151.927900842] [image_processor_py]: rcv Image: 320x240 avg_brightness=169.7
[image_processor-2] [INFO] [1785746152.041036790] [image_processor_py]: rcv Image: 320x240 avg_brightness=86.0
[image_processor-2] [INFO] [1785746152.142994206] [image_processor_py]: rcv Image: 320x240 avg_brightness=87.7
[image_processor-2] [INFO] [1785746152.231253409] [image_processor_py]: rcv Image: 320x240 avg_brightness=89.3
[image_processor-2] [INFO] [1785746152.332765264] [image_processor_py]: rcv Image: 320x240 avg_brightness=91.0
[image_processor-2] [INFO] [1785746152.424617775] [image_processor_py]: rcv Image: 320x240 avg_brightness=92.7
[image_processor-2] [INFO] [1785746152.527732699] [image_processor_py]: rcv Image: 320x240 avg_brightness=94.3
[image_processor-2] [INFO] [1785746152.627293830] [image_processor_py]: rcv Image: 320x240 avg_brightness=96.0
[image_processor-2] [INFO] [1785746152.725079046] [image_processor_py]: rcv Image: 320x240 avg_brightness=97.7
[image_processor-2] [INFO] [1785746152.836152731] [image_processor_py]: rcv Image: 320x240 avg_brightness=99.3
[image_processor-2] [INFO] [1785746152.930695969] [image_processor_py]: rcv Image: 320x240 avg_brightness=101.0
[image_processor-2] [INFO] [1785746153.026978690] [image_processor_py]: rcv Image: 320x240 avg_brightness=102.7
[image_processor-2] [INFO] [1785746153.129892060] [image_processor_py]: rcv Image: 320x240 avg_brightness=104.3
[image_processor-2] [INFO] [1785746153.225589675] [image_processor_py]: rcv Image: 320x240 avg_brightness=106.0
[image_processor-2] [INFO] [1785746153.326981838] [image_processor_py]: rcv Image: 320x240 avg_brightness=107.7
[image_processor-2] [INFO] [1785746153.424495811] [image_processor_py]: rcv Image: 320x240 avg_brightness=109.3
[image_processor-2] [INFO] [1785746153.525693569] [image_processor_py]: rcv Image: 320x240 avg_brightness=111.0
[image_processor-2] [INFO] [1785746153.623269203] [image_processor_py]: rcv Image: 320x240 avg_brightness=112.7
[image_processor-2] [INFO] [1785746153.737475291] [image_processor_py]: rcv Image: 320x240 avg_brightness=114.3
[image_processor-2] [INFO] [1785746153.823699631] [image_processor_py]: rcv Image: 320x240 avg_brightness=116.0
[image_processor-2] [INFO] [1785746153.927499210] [image_processor_py]: rcv Image: 320x240 avg_brightness=117.7
[image_processor-2] [INFO] [1785746154.051819653] [image_processor_py]: rcv Image: 320x240 avg_brightness=119.3
[image_processor-2] [INFO] [1785746154.132154734] [image_processor_py]: rcv Image: 320x240 avg_brightness=121.0
[image_processor-2] [INFO] [1785746154.238393302] [image_processor_py]: rcv Image: 320x240 avg_brightness=122.7
[image_processor-2] [INFO] [1785746154.541705794] [image_processor_py]: rcv Image: 320x240 avg_brightness=124.3
[image_processor-2] [INFO] [1785746154.600629644] [image_processor_py]: rcv Image: 320x240 avg_brightness=126.0
[image_processor-2] [INFO] [1785746155.632814497] [image_processor_py]: rcv Image: 320x240 avg_brightness=127.7
[image_processor-2] [INFO] [1785746155.904528151] [image_processor_py]: rcv Image: 320x240 avg_brightness=131.0
[image_processor-2] [INFO] [1785746156.015731659] [image_processor_py]: rcv Image: 320x240 avg_brightness=136.0
[image_processor-2] [INFO] [1785746156.108443262] [image_processor_py]: rcv Image: 320x240 avg_brightness=137.7
[image_processor-2] [INFO] [1785746156.215784831] [image_processor_py]: rcv Image: 320x240 avg_brightness=139.3
[image_processor-2] [INFO] [1785746156.284515306] [image_processor_py]: rcv Image: 320x240 avg_brightness=141.0
[image_processor-2] [INFO] [1785746156.336784253] [image_processor_py]: rcv Image: 320x240 avg_brightness=142.7
[image_processor-2] [INFO] [1785746156.396219172] [image_processor_py]: rcv Image: 320x240 avg_brightness=144.3
[image_processor-2] [INFO] [1785746156.451076602] [image_processor_py]: rcv Image: 320x240 avg_brightness=146.0
[image_processor-2] [INFO] [1785746156.507495273] [image_processor_py]: rcv Image: 320x240 avg_brightness=147.7
[image_processor-2] [INFO] [1785746156.566547662] [image_processor_py]: rcv Image: 320x240 avg_brightness=149.3
[image_processor-2] [INFO] [1785746156.638450023] [image_processor_py]: rcv Image: 320x240 avg_brightness=151.0
[image_processor-2] [INFO] [1785746156.708641530] [image_processor_py]: rcv Image: 320x240 avg_brightness=152.7
[image_processor-2] [INFO] [1785746156.788774373] [image_processor_py]: rcv Image: 320x240 avg_brightness=154.3
[image_processor-2] [INFO] [1785746156.853303194] [image_processor_py]: rcv Image: 320x240 avg_brightness=156.0
[image_processor-2] [INFO] [1785746156.931216753] [image_processor_py]: rcv Image: 320x240 avg_brightness=157.7
[image_processor-2] [INFO] [1785746156.994285548] [image_processor_py]: rcv Image: 320x240 avg_brightness=159.3
[image_processor-2] [INFO] [1785746157.047209101] [image_processor_py]: rcv Image: 320x240 avg_brightness=161.0
[image_processor-2] [INFO] [1785746157.099666730] [image_processor_py]: rcv Image: 320x240 avg_brightness=162.7
[image_processor-2] [INFO] [1785746157.173228754] [image_processor_py]: rcv Image: 320x240 avg_brightness=164.3
[image_processor-2] [INFO] [1785746157.241272412] [image_processor_py]: rcv Image: 320x240 avg_brightness=166.0
[image_processor-2] [INFO] [1785746157.297852294] [image_processor_py]: rcv Image: 320x240 avg_brightness=167.7
[image_processor-2] [INFO] [1785746157.355489904] [image_processor_py]: rcv Image: 320x240 avg_brightness=169.3
[image_processor-2] [INFO] [1785746157.437639327] [image_processor_py]: rcv Image: 320x240 avg_brightness=85.7
[image_processor-2] [INFO] [1785746157.514449770] [image_processor_py]: rcv Image: 320x240 avg_brightness=87.3
[image_processor-2] [INFO] [1785746157.576905972] [image_processor_py]: rcv Image: 320x240 avg_brightness=89.0
[image_processor-2] [INFO] [1785746157.648035361] [image_processor_py]: rcv Image: 320x240 avg_brightness=90.7
[image_processor-2] [INFO] [1785746157.747799312] [image_processor_py]: rcv Image: 320x240 avg_brightness=92.3
[image_processor-2] [INFO] [1785746157.829504016] [image_processor_py]: rcv Image: 320x240 avg_brightness=94.0
[image_processor-2] [INFO] [1785746157.897387643] [image_processor_py]: rcv Image: 320x240 avg_brightness=95.7
[image_processor-2] [INFO] [1785746157.973743746] [image_processor_py]: rcv Image: 320x240 avg_brightness=97.3
[image_processor-2] [INFO] [1785746158.032526668] [image_processor_py]: rcv Image: 320x240 avg_brightness=99.0
[image_processor-2] [INFO] [1785746158.116171101] [image_processor_py]: rcv Image: 320x240 avg_brightness=100.7
[image_processor-2] [INFO] [1785746158.198663748] [image_processor_py]: rcv Image: 320x240 avg_brightness=102.3
[image_processor-2] [INFO] [1785746158.264959370] [image_processor_py]: rcv Image: 320x240 avg_brightness=104.0
[image_processor-2] [INFO] [1785746158.346038233] [image_processor_py]: rcv Image: 320x240 avg_brightness=105.7
[image_processor-2] [INFO] [1785746158.448730176] [image_processor_py]: rcv Image: 320x240 avg_brightness=107.3
[image_processor-2] [INFO] [1785746158.574379170] [image_processor_py]: rcv Image: 320x240 avg_brightness=109.0
[image_processor-2] [INFO] [1785746158.654415141] [image_processor_py]: rcv Image: 320x240 avg_brightness=110.7
[image_processor-2] [INFO] [1785746158.745856301] [image_processor_py]: rcv Image: 320x240 avg_brightness=112.3
[image_processor-2] [INFO] [1785746158.843573366] [image_processor_py]: rcv Image: 320x240 avg_brightness=114.0
[image_processor-2] [INFO] [1785746158.939544708] [image_processor_py]: rcv Image: 320x240 avg_brightness=115.7
[image_processor-2] [INFO] [1785746159.357516926] [image_processor_py]: rcv Image: 320x240 avg_brightness=117.3
[image_processor-2] [INFO] [1785746159.791683464] [image_processor_py]: rcv Image: 320x240 avg_brightness=119.0
[image_processor-2] [INFO] [1785746160.099164620] [image_processor_py]: rcv Image: 320x240 avg_brightness=120.7
[image_processor-2] [INFO] [1785746160.202359131] [image_processor_py]: rcv Image: 320x240 avg_brightness=122.3
[image_processor-2] [INFO] [1785746160.289421080] [image_processor_py]: rcv Image: 320x240 avg_brightness=124.0
[image_processor-2] [INFO] [1785746160.384664176] [image_processor_py]: rcv Image: 320x240 avg_brightness=125.7
[image_processor-2] [INFO] [1785746160.494976141] [image_processor_py]: rcv Image: 320x240 avg_brightness=127.3
[image_processor-2] [INFO] [1785746160.590052039] [image_processor_py]: rcv Image: 320x240 avg_brightness=129.0
[image_processor-2] [INFO] [1785746160.680716747] [image_processor_py]: rcv Image: 320x240 avg_brightness=130.7
[image_processor-2] [INFO] [1785746160.777256677] [image_processor_py]: rcv Image: 320x240 avg_brightness=132.3
[image_processor-2] [INFO] [1785746160.845598106] [image_processor_py]: rcv Image: 320x240 avg_brightness=134.0
[image_processor-2] [INFO] [1785746160.964340972] [image_processor_py]: rcv Image: 320x240 avg_brightness=135.7
[image_processor-2] [INFO] [1785746161.043873659] [image_processor_py]: rcv Image: 320x240 avg_brightness=137.3
[image_processor-2] [INFO] [1785746161.112255084] [image_processor_py]: rcv Image: 320x240 avg_brightness=139.0
[image_processor-2] [INFO] [1785746161.200500269] [image_processor_py]: rcv Image: 320x240 avg_brightness=140.7
[image_processor-2] [INFO] [1785746161.277043710] [image_processor_py]: rcv Image: 320x240 avg_brightness=142.3
[image_processor-2] [INFO] [1785746161.356783589] [image_processor_py]: rcv Image: 320x240 avg_brightness=144.0
[image_processor-2] [INFO] [1785746161.426687291] [image_processor_py]: rcv Image: 320x240 avg_brightness=145.7
[image_processor-2] [INFO] [1785746161.514283627] [image_processor_py]: rcv Image: 320x240 avg_brightness=147.3
[image_processor-2] [INFO] [1785746161.592304545] [image_processor_py]: rcv Image: 320x240 avg_brightness=149.0
[image_processor-2] [INFO] [1785746161.671860414] [image_processor_py]: rcv Image: 320x240 avg_brightness=150.7
[image_processor-2] [INFO] [1785746161.731973278] [image_processor_py]: rcv Image: 320x240 avg_brightness=152.3
[image_processor-2] [INFO] [1785746161.797512734] [image_processor_py]: rcv Image: 320x240 avg_brightness=154.0
[image_processor-2] [INFO] [1785746161.869047912] [image_processor_py]: rcv Image: 320x240 avg_brightness=155.7
[image_processor-2] [INFO] [1785746161.941657921] [image_processor_py]: rcv Image: 320x240 avg_brightness=157.3
[image_processor-2] [INFO] [1785746162.022954321] [image_processor_py]: rcv Image: 320x240 avg_brightness=159.0
[image_processor-2] [INFO] [1785746162.097274057] [image_processor_py]: rcv Image: 320x240 avg_brightness=160.7
[image_processor-2] [INFO] [1785746162.165201000] [image_processor_py]: rcv Image: 320x240 avg_brightness=162.3
[image_processor-2] [INFO] [1785746162.225858436] [image_processor_py]: rcv Image: 320x240 avg_brightness=164.0
[image_processor-2] [INFO] [1785746162.286814309] [image_processor_py]: rcv Image: 320x240 avg_brightness=165.7
[image_processor-2] [INFO] [1785746162.358763898] [image_processor_py]: rcv Image: 320x240 avg_brightness=167.3
[image_processor-2] [INFO] [1785746162.424852726] [image_processor_py]: rcv Image: 320x240 avg_brightness=169.0
[image_processor-2] [INFO] [1785746162.490747259] [image_processor_py]: rcv Image: 320x240 avg_brightness=85.3
[image_processor-2] [INFO] [1785746162.557828752] [image_processor_py]: rcv Image: 320x240 avg_brightness=87.0
[image_processor-2] [INFO] [1785746162.626145580] [image_processor_py]: rcv Image: 320x240 avg_brightness=88.7
[image_processor-2] [INFO] [1785746162.704895118] [image_processor_py]: rcv Image: 320x240 avg_brightness=90.3
[image_processor-2] [INFO] [1785746162.778885654] [image_processor_py]: rcv Image: 320x240 avg_brightness=92.0
[image_processor-2] [INFO] [1785746162.860376000] [image_processor_py]: rcv Image: 320x240 avg_brightness=93.7
[image_processor-2] [INFO] [1785746162.950500147] [image_processor_py]: rcv Image: 320x240 avg_brightness=95.3
[image_processor-2] [INFO] [1785746163.017818742] [image_processor_py]: rcv Image: 320x240 avg_brightness=97.0
[image_processor-2] [INFO] [1785746163.075402578] [image_processor_py]: rcv Image: 320x240 avg_brightness=98.7
[image_processor-2] [INFO] [1785746163.139753964] [image_processor_py]: rcv Image: 320x240 avg_brightness=100.3
[image_processor-2] [INFO] [1785746163.253543474] [image_processor_py]: rcv Image: 320x240 avg_brightness=102.0
[image_processor-2] [INFO] [1785746163.338704518] [image_processor_py]: rcv Image: 320x240 avg_brightness=103.7
[image_processor-2] [INFO] [1785746163.450154047] [image_processor_py]: rcv Image: 320x240 avg_brightness=105.3
[image_processor-2] [INFO] [1785746163.534436530] [image_processor_py]: rcv Image: 320x240 avg_brightness=107.0
[image_processor-2] [INFO] [1785746163.666670457] [image_processor_py]: rcv Image: 320x240 avg_brightness=108.7
[image_processor-2] [INFO] [1785746163.749183256] [image_processor_py]: rcv Image: 320x240 avg_brightness=110.3
[image_processor-2] [INFO] [1785746163.843574497] [image_processor_py]: rcv Image: 320x240 avg_brightness=112.0
[image_processor-2] [INFO] [1785746163.954023758] [image_processor_py]: rcv Image: 320x240 avg_brightness=113.7
[image_processor-2] [INFO] [1785746164.028429718] [image_processor_py]: rcv Image: 320x240 avg_brightness=115.3
[image_processor-2] [INFO] [1785746164.141250799] [image_processor_py]: rcv Image: 320x240 avg_brightness=117.0
[image_processor-2] [INFO] [1785746164.266268135] [image_processor_py]: rcv Image: 320x240 avg_brightness=118.7
[image_processor-2] [INFO] [1785746164.372126721] [image_processor_py]: rcv Image: 320x240 avg_brightness=120.3
[image_processor-2] [INFO] [1785746164.445790649] [image_processor_py]: rcv Image: 320x240 avg_brightness=122.0
[image_processor-2] [INFO] [1785746164.560991666] [image_processor_py]: rcv Image: 320x240 avg_brightness=123.7
[image_processor-2] [INFO] [1785746164.668864819] [image_processor_py]: rcv Image: 320x240 avg_brightness=125.3
[image_processor-2] [INFO] [1785746164.745176401] [image_processor_py]: rcv Image: 320x240 avg_brightness=127.0
[image_processor-2] [INFO] [1785746164.943674381] [image_processor_py]: rcv Image: 320x240 avg_brightness=128.7
[image_processor-2] [INFO] [1785746165.054370108] [image_processor_py]: rcv Image: 320x240 avg_brightness=130.3
[image_processor-2] [INFO] [1785746165.181510169] [image_processor_py]: rcv Image: 320x240 avg_brightness=132.0
[image_processor-2] [INFO] [1785746165.287287403] [image_processor_py]: rcv Image: 320x240 avg_brightness=133.7
[image_processor-2] [INFO] [1785746165.380390403] [image_processor_py]: rcv Image: 320x240 avg_brightness=135.3
[image_processor-2] [INFO] [1785746165.448561856] [image_processor_py]: rcv Image: 320x240 avg_brightness=137.0
[image_processor-2] [INFO] [1785746165.538510218] [image_processor_py]: rcv Image: 320x240 avg_brightness=138.7
[image_processor-2] [INFO] [1785746165.594009260] [image_processor_py]: rcv Image: 320x240 avg_brightness=140.3
[image_processor-2] [INFO] [1785746165.660665504] [image_processor_py]: rcv Image: 320x240 avg_brightness=142.0
[image_processor-2] [INFO] [1785746165.731362207] [image_processor_py]: rcv Image: 320x240 avg_brightness=143.7
[image_processor-2] [INFO] [1785746165.841379075] [image_processor_py]: rcv Image: 320x240 avg_brightness=145.3
[image_processor-2] [INFO] [1785746165.941578116] [image_processor_py]: rcv Image: 320x240 avg_brightness=147.0
[image_processor-2] [INFO] [1785746166.071969385] [image_processor_py]: rcv Image: 320x240 avg_brightness=148.7
[image_processor-2] [INFO] [1785746166.145231757] [image_processor_py]: rcv Image: 320x240 avg_brightness=150.3
[image_processor-2] [INFO] [1785746166.247006051] [image_processor_py]: rcv Image: 320x240 avg_brightness=152.0
[image_processor-2] [INFO] [1785746166.343052449] [image_processor_py]: rcv Image: 320x240 avg_brightness=153.7
[image_processor-2] [INFO] [1785746166.456863301] [image_processor_py]: rcv Image: 320x240 avg_brightness=155.3
[image_processor-2] [INFO] [1785746166.521222673] [image_processor_py]: rcv Image: 320x240 avg_brightness=157.0
[image_processor-2] [INFO] [1785746166.654425965] [image_processor_py]: rcv Image: 320x240 avg_brightness=158.7
[image_processor-2] [INFO] [1785746166.776010970] [image_processor_py]: rcv Image: 320x240 avg_brightness=160.3
[image_processor-2] [INFO] [1785746166.838594096] [image_processor_py]: rcv Image: 320x240 avg_brightness=162.0
[image_processor-2] [INFO] [1785746166.937283789] [image_processor_py]: rcv Image: 320x240 avg_brightness=163.7
[image_processor-2] [INFO] [1785746167.034848243] [image_processor_py]: rcv Image: 320x240 avg_brightness=165.3
[image_processor-2] [INFO] [1785746167.162210925] [image_processor_py]: rcv Image: 320x240 avg_brightness=167.0
[image_processor-2] [INFO] [1785746167.261338152] [image_processor_py]: rcv Image: 320x240 avg_brightness=168.7
[image_processor-2] [INFO] [1785746167.355380447] [image_processor_py]: rcv Image: 320x240 avg_brightness=85.0
[image_processor-2] [INFO] [1785746167.448469436] [image_processor_py]: rcv Image: 320x240 avg_brightness=86.7
[image_processor-2] [INFO] [1785746167.545815364] [image_processor_py]: rcv Image: 320x240 avg_brightness=88.3
[image_processor-2] [INFO] [1785746167.655853411] [image_processor_py]: rcv Image: 320x240 avg_brightness=90.0
[image_processor-2] [INFO] [1785746167.721410648] [image_processor_py]: rcv Image: 320x240 avg_brightness=91.7
[image_processor-2] [INFO] [1785746167.864786557] [image_processor_py]: rcv Image: 320x240 avg_brightness=93.3
[image_processor-2] [INFO] [1785746167.952231208] [image_processor_py]: rcv Image: 320x240 avg_brightness=95.0
[image_processor-2] [INFO] [1785746168.034651397] [image_processor_py]: rcv Image: 320x240 avg_brightness=96.7
[image_processor-2] [INFO] [1785746168.122235271] [image_processor_py]: rcv Image: 320x240 avg_brightness=98.3
[image_processor-2] [INFO] [1785746168.260858672] [image_processor_py]: rcv Image: 320x240 avg_brightness=100.0
[image_processor-2] [INFO] [1785746168.350130220] [image_processor_py]: rcv Image: 320x240 avg_brightness=101.7
[image_processor-2] [INFO] [1785746168.443120715] [image_processor_py]: rcv Image: 320x240 avg_brightness=103.3
[image_processor-2] [INFO] [1785746168.541614104] [image_processor_py]: rcv Image: 320x240 avg_brightness=105.0
[image_processor-2] [INFO] [1785746168.649803413] [image_processor_py]: rcv Image: 320x240 avg_brightness=106.7
[image_processor-2] [INFO] [1785746168.746410927] [image_processor_py]: rcv Image: 320x240 avg_brightness=108.3
[image_processor-2] [INFO] [1785746168.852468864] [image_processor_py]: rcv Image: 320x240 avg_brightness=110.0
[image_processor-2] [INFO] [1785746168.931438849] [image_processor_py]: rcv Image: 320x240 avg_brightness=111.7
[image_processor-2] [INFO] [1785746169.241911380] [image_processor_py]: rcv Image: 320x240 avg_brightness=113.3
[image_processor-2] [INFO] [1785746169.442466601] [image_processor_py]: rcv Image: 320x240 avg_brightness=115.0
[image_processor-2] [INFO] [1785746169.594830770] [image_processor_py]: rcv Image: 320x240 avg_brightness=116.7
[image_processor-2] [INFO] [1785746169.751760490] [image_processor_py]: rcv Image: 320x240 avg_brightness=118.3
[image_processor-2] [INFO] [1785746170.022300043] [image_processor_py]: rcv Image: 320x240 avg_brightness=120.0
[image_processor-2] [INFO] [1785746170.162675553] [image_processor_py]: rcv Image: 320x240 avg_brightness=121.7
[image_processor-2] [INFO] [1785746170.276975711] [image_processor_py]: rcv Image: 320x240 avg_brightness=123.3
[image_processor-2] [INFO] [1785746170.367641538] [image_processor_py]: rcv Image: 320x240 avg_brightness=125.0
[image_processor-2] [INFO] [1785746170.434082079] [image_processor_py]: rcv Image: 320x240 avg_brightness=126.7
[image_processor-2] [INFO] [1785746170.542188690] [image_processor_py]: rcv Image: 320x240 avg_brightness=128.3
[image_processor-2] [INFO] [1785746170.634837856] [image_processor_py]: rcv Image: 320x240 avg_brightness=130.0
[image_processor-2] [INFO] [1785746170.714709549] [image_processor_py]: rcv Image: 320x240 avg_brightness=131.7
[image_processor-2] [INFO] [1785746170.800150849] [image_processor_py]: rcv Image: 320x240 avg_brightness=133.3
[image_processor-2] [INFO] [1785746170.861378700] [image_processor_py]: rcv Image: 320x240 avg_brightness=135.0
[image_processor-2] [INFO] [1785746170.926718927] [image_processor_py]: rcv Image: 320x240 avg_brightness=136.7
[image_processor-2] [INFO] [1785746170.998252640] [image_processor_py]: rcv Image: 320x240 avg_brightness=138.3
[image_processor-2] [INFO] [1785746171.060996696] [image_processor_py]: rcv Image: 320x240 avg_brightness=140.0
[image_processor-2] [INFO] [1785746171.124024801] [image_processor_py]: rcv Image: 320x240 avg_brightness=141.7
[image_processor-2] [INFO] [1785746171.180678384] [image_processor_py]: rcv Image: 320x240 avg_brightness=143.3
[image_processor-2] [INFO] [1785746171.241411627] [image_processor_py]: rcv Image: 320x240 avg_brightness=145.0
[image_processor-2] [INFO] [1785746171.295563468] [image_processor_py]: rcv Image: 320x240 avg_brightness=146.7
[image_processor-2] [INFO] [1785746171.368949308] [image_processor_py]: rcv Image: 320x240 avg_brightness=148.3
[image_processor-2] [INFO] [1785746171.434259813] [image_processor_py]: rcv Image: 320x240 avg_brightness=150.0
[image_processor-2] [INFO] [1785746171.494279685] [image_processor_py]: rcv Image: 320x240 avg_brightness=151.7
[image_processor-2] [INFO] [1785746171.557306115] [image_processor_py]: rcv Image: 320x240 avg_brightness=153.3
[image_processor-2] [INFO] [1785746171.619478924] [image_processor_py]: rcv Image: 320x240 avg_brightness=155.0
[image_processor-2] [INFO] [1785746171.683756135] [image_processor_py]: rcv Image: 320x240 avg_brightness=156.7
[image_processor-2] [INFO] [1785746171.761613965] [image_processor_py]: rcv Image: 320x240 avg_brightness=158.3
[image_processor-2] [INFO] [1785746171.836576455] [image_processor_py]: rcv Image: 320x240 avg_brightness=160.0
[image_processor-2] [INFO] [1785746171.945067565] [image_processor_py]: rcv Image: 320x240 avg_brightness=161.7
[image_processor-2] [INFO] [1785746172.075749643] [image_processor_py]: rcv Image: 320x240 avg_brightness=163.3
[image_processor-2] [INFO] [1785746172.145853932] [image_processor_py]: rcv Image: 320x240 avg_brightness=165.0
[image_processor-2] [INFO] [1785746172.232431475] [image_processor_py]: rcv Image: 320x240 avg_brightness=166.7
[image_processor-2] [INFO] [1785746172.361604706] [image_processor_py]: rcv Image: 320x240 avg_brightness=168.3
[image_processor-2] [INFO] [1785746172.438758134] [image_processor_py]: rcv Image: 320x240 avg_brightness=84.7
[image_processor-2] [INFO] [1785746172.531150914] [image_processor_py]: rcv Image: 320x240 avg_brightness=86.3
[image_processor-2] [INFO] [1785746172.630903198] [image_processor_py]: rcv Image: 320x240 avg_brightness=88.0
[image_processor-2] [INFO] [1785746172.764094349] [image_processor_py]: rcv Image: 320x240 avg_brightness=89.7
[image_processor-2] [INFO] [1785746172.834346914] [image_processor_py]: rcv Image: 320x240 avg_brightness=91.3
[image_processor-2] [INFO] [1785746172.944940659] [image_processor_py]: rcv Image: 320x240 avg_brightness=93.0
[image_processor-2] [INFO] [1785746173.042434392] [image_processor_py]: rcv Image: 320x240 avg_brightness=94.7
[image_processor-2] [INFO] [1785746173.192455917] [image_processor_py]: rcv Image: 320x240 avg_brightness=96.3
[image_processor-2] [INFO] [1785746173.296070013] [image_processor_py]: rcv Image: 320x240 avg_brightness=98.0
[image_processor-2] [INFO] [1785746173.506766702] [image_processor_py]: rcv Image: 320x240 avg_brightness=99.7
[image_processor-2] [INFO] [1785746173.563914581] [image_processor_py]: rcv Image: 320x240 avg_brightness=101.3
[image_processor-2] [INFO] [1785746173.651196015] [image_processor_py]: rcv Image: 320x240 avg_brightness=103.0
[image_processor-2] [INFO] [1785746173.709562210] [image_processor_py]: rcv Image: 320x240 avg_brightness=104.7
[image_processor-2] [INFO] [1785746173.761589024] [image_processor_py]: rcv Image: 320x240 avg_brightness=106.3
[image_processor-2] [INFO] [1785746173.825998380] [image_processor_py]: rcv Image: 320x240 avg_brightness=108.0
[image_processor-2] [INFO] [1785746173.958412970] [image_processor_py]: rcv Image: 320x240 avg_brightness=109.7
[image_processor-2] [INFO] [1785746174.059034838] [image_processor_py]: rcv Image: 320x240 avg_brightness=111.3
[image_processor-2] [INFO] [1785746174.135232225] [image_processor_py]: rcv Image: 320x240 avg_brightness=113.0
[image_processor-2] [INFO] [1785746174.264480087] [image_processor_py]: rcv Image: 320x240 avg_brightness=114.7
[image_processor-2] [INFO] [1785746174.380180211] [image_processor_py]: rcv Image: 320x240 avg_brightness=116.3
[image_processor-2] [INFO] [1785746174.524222603] [image_processor_py]: rcv Image: 320x240 avg_brightness=118.0
[image_processor-2] [INFO] [1785746174.623054453] [image_processor_py]: rcv Image: 320x240 avg_brightness=119.7
[image_processor-2] [INFO] [1785746174.694851000] [image_processor_py]: rcv Image: 320x240 avg_brightness=121.3
[image_processor-2] [INFO] [1785746174.802207106] [image_processor_py]: rcv Image: 320x240 avg_brightness=123.0
[image_processor-2] [INFO] [1785746174.914574927] [image_processor_py]: rcv Image: 320x240 avg_brightness=124.7
[image_processor-2] [INFO] [1785746174.993435029] [image_processor_py]: rcv Image: 320x240 avg_brightness=126.3
[image_processor-2] [INFO] [1785746175.073356618] [image_processor_py]: rcv Image: 320x240 avg_brightness=128.0
[image_processor-2] [INFO] [1785746175.136016533] [image_processor_py]: rcv Image: 320x240 avg_brightness=129.7
[image_processor-2] [INFO] [1785746175.223998099] [image_processor_py]: rcv Image: 320x240 avg_brightness=131.3
[image_processor-2] [INFO] [1785746175.355232516] [image_processor_py]: rcv Image: 320x240 avg_brightness=133.0
[image_processor-2] [INFO] [1785746175.441914340] [image_processor_py]: rcv Image: 320x240 avg_brightness=134.7
[image_processor-2] [INFO] [1785746175.531039273] [image_processor_py]: rcv Image: 320x240 avg_brightness=136.3
[image_processor-2] [INFO] [1785746175.620077584] [image_processor_py]: rcv Image: 320x240 avg_brightness=138.0
[image_processor-2] [INFO] [1785746175.772887653] [image_processor_py]: rcv Image: 320x240 avg_brightness=139.7
[image_processor-2] [INFO] [1785746175.827548267] [image_processor_py]: rcv Image: 320x240 avg_brightness=141.3
[image_processor-2] [INFO] [1785746175.930516876] [image_processor_py]: rcv Image: 320x240 avg_brightness=143.0
[image_processor-2] [INFO] [1785746176.033795526] [image_processor_py]: rcv Image: 320x240 avg_brightness=144.7
[image_processor-2] [INFO] [1785746176.127900629] [image_processor_py]: rcv Image: 320x240 avg_brightness=146.3
[image_processor-2] [INFO] [1785746176.264153314] [image_processor_py]: rcv Image: 320x240 avg_brightness=148.0
[image_processor-2] [INFO] [1785746176.339676947] [image_processor_py]: rcv Image: 320x240 avg_brightness=149.7
[image_processor-2] [INFO] [1785746176.442639353] [image_processor_py]: rcv Image: 320x240 avg_brightness=151.3
[image_processor-2] [INFO] [1785746176.543624434] [image_processor_py]: rcv Image: 320x240 avg_brightness=153.0
[image_processor-2] [INFO] [1785746176.659435985] [image_processor_py]: rcv Image: 320x240 avg_brightness=154.7
[image_processor-2] [INFO] [1785746176.795303448] [image_processor_py]: rcv Image: 320x240 avg_brightness=156.3
[image_processor-2] [INFO] [1785746176.857886164] [image_processor_py]: rcv Image: 320x240 avg_brightness=158.0
[image_processor-2] [INFO] [1785746176.940154135] [image_processor_py]: rcv Image: 320x240 avg_brightness=159.7
[image_processor-2] [INFO] [1785746177.036964424] [image_processor_py]: rcv Image: 320x240 avg_brightness=161.3
[image_processor-2] [INFO] [1785746177.131802488] [image_processor_py]: rcv Image: 320x240 avg_brightness=163.0
[image_processor-2] [INFO] [1785746177.237706915] [image_processor_py]: rcv Image: 320x240 avg_brightness=164.7
[image_processor-2] [INFO] [1785746177.340650847] [image_processor_py]: rcv Image: 320x240 avg_brightness=166.3
[image_processor-2] [INFO] [1785746177.429316600] [image_processor_py]: rcv Image: 320x240 avg_brightness=168.0
[image_processor-2] [INFO] [1785746177.544368031] [image_processor_py]: rcv Image: 320x240 avg_brightness=169.7
[image_processor-2] [INFO] [1785746177.663883775] [image_processor_py]: rcv Image: 320x240 avg_brightness=86.0
[image_processor-2] [INFO] [1785746177.752275169] [image_processor_py]: rcv Image: 320x240 avg_brightness=87.7
[image_processor-2] [INFO] [1785746177.839994275] [image_processor_py]: rcv Image: 320x240 avg_brightness=89.3
[image_processor-2] [INFO] [1785746177.976526325] [image_processor_py]: rcv Image: 320x240 avg_brightness=91.0
[image_processor-2] [INFO] [1785746178.051073308] [image_processor_py]: rcv Image: 320x240 avg_brightness=92.7
[image_processor-2] [INFO] [1785746178.125507726] [image_processor_py]: rcv Image: 320x240 avg_brightness=94.3
[image_processor-2] [INFO] [1785746178.237627889] [image_processor_py]: rcv Image: 320x240 avg_brightness=96.0
[image_processor-2] [INFO] [1785746178.324772460] [image_processor_py]: rcv Image: 320x240 avg_brightness=97.7
[image_processor-2] [INFO] [1785746178.437507807] [image_processor_py]: rcv Image: 320x240 avg_brightness=99.3
[image_processor-2] [INFO] [1785746178.535221927] [image_processor_py]: rcv Image: 320x240 avg_brightness=101.0
[image_processor-2] [INFO] [1785746178.640033805] [image_processor_py]: rcv Image: 320x240 avg_brightness=102.7
[image_processor-2] [INFO] [1785746178.751813901] [image_processor_py]: rcv Image: 320x240 avg_brightness=104.3
[image_processor-2] [INFO] [1785746178.840937650] [image_processor_py]: rcv Image: 320x240 avg_brightness=106.0
[image_processor-2] [INFO] [1785746178.946742034] [image_processor_py]: rcv Image: 320x240 avg_brightness=107.7
[image_processor-2] [INFO] [1785746179.057980670] [image_processor_py]: rcv Image: 320x240 avg_brightness=109.3
[image_processor-2] [INFO] [1785746179.344096596] [image_processor_py]: rcv Image: 320x240 avg_brightness=111.0
[image_processor-2] [INFO] [1785746179.524724330] [image_processor_py]: rcv Image: 320x240 avg_brightness=112.7
[image_processor-2] [INFO] [1785746179.661425928] [image_processor_py]: rcv Image: 320x240 avg_brightness=114.3
[image_processor-2] [INFO] [1785746179.763399490] [image_processor_py]: rcv Image: 320x240 avg_brightness=116.0
[image_processor-2] [INFO] [1785746179.855243825] [image_processor_py]: rcv Image: 320x240 avg_brightness=117.7
[image_processor-2] [INFO] [1785746179.932865913] [image_processor_py]: rcv Image: 320x240 avg_brightness=119.3
[image_processor-2] [INFO] [1785746180.014756211] [image_processor_py]: rcv Image: 320x240 avg_brightness=121.0
[image_processor-2] [INFO] [1785746180.078000400] [image_processor_py]: rcv Image: 320x240 avg_brightness=122.7
[image_processor-2] [INFO] [1785746180.148805441] [image_processor_py]: rcv Image: 320x240 avg_brightness=124.3
[image_processor-2] [INFO] [1785746180.214137150] [image_processor_py]: rcv Image: 320x240 avg_brightness=126.0
[image_processor-2] [INFO] [1785746180.291401969] [image_processor_py]: rcv Image: 320x240 avg_brightness=127.7
[image_processor-2] [INFO] [1785746180.361953593] [image_processor_py]: rcv Image: 320x240 avg_brightness=129.3
[image_processor-2] [INFO] [1785746180.426251966] [image_processor_py]: rcv Image: 320x240 avg_brightness=131.0
[image_processor-2] [INFO] [1785746180.495534736] [image_processor_py]: rcv Image: 320x240 avg_brightness=132.7
[image_processor-2] [INFO] [1785746180.599603620] [image_processor_py]: rcv Image: 320x240 avg_brightness=134.3
[image_processor-2] [INFO] [1785746180.675787943] [image_processor_py]: rcv Image: 320x240 avg_brightness=136.0
[image_processor-2] [INFO] [1785746180.766066703] [image_processor_py]: rcv Image: 320x240 avg_brightness=137.7
[image_processor-2] [INFO] [1785746180.862912002] [image_processor_py]: rcv Image: 320x240 avg_brightness=139.3
[image_processor-2] [INFO] [1785746180.940711709] [image_processor_py]: rcv Image: 320x240 avg_brightness=141.0
[image_processor-2] [INFO] [1785746181.038824560] [image_processor_py]: rcv Image: 320x240 avg_brightness=142.7
[image_processor-2] [INFO] [1785746181.123518679] [image_processor_py]: rcv Image: 320x240 avg_brightness=144.3
[image_processor-2] [INFO] [1785746181.254064069] [image_processor_py]: rcv Image: 320x240 avg_brightness=146.0
[image_processor-2] [INFO] [1785746181.341771638] [image_processor_py]: rcv Image: 320x240 avg_brightness=147.7
[image_processor-2] [INFO] [1785746181.445122235] [image_processor_py]: rcv Image: 320x240 avg_brightness=149.3
[image_processor-2] [INFO] [1785746181.530666168] [image_processor_py]: rcv Image: 320x240 avg_brightness=151.0
[image_processor-2] [INFO] [1785746181.639839468] [image_processor_py]: rcv Image: 320x240 avg_brightness=152.7
[image_processor-2] [INFO] [1785746181.770089972] [image_processor_py]: rcv Image: 320x240 avg_brightness=154.3
[image_processor-2] [INFO] [1785746181.843478951] [image_processor_py]: rcv Image: 320x240 avg_brightness=156.0
[image_processor-2] [INFO] [1785746181.960071006] [image_processor_py]: rcv Image: 320x240 avg_brightness=157.7
[image_processor-2] [INFO] [1785746182.062986445] [image_processor_py]: rcv Image: 320x240 avg_brightness=159.3
[image_processor-2] [INFO] [1785746182.138624795] [image_processor_py]: rcv Image: 320x240 avg_brightness=161.0
[image_processor-2] [INFO] [1785746182.258879156] [image_processor_py]: rcv Image: 320x240 avg_brightness=162.7
[image_processor-2] [INFO] [1785746182.352887423] [image_processor_py]: rcv Image: 320x240 avg_brightness=164.3
[image_processor-2] [INFO] [1785746182.439759766] [image_processor_py]: rcv Image: 320x240 avg_brightness=166.0
[image_processor-2] [INFO] [1785746182.542572853] [image_processor_py]: rcv Image: 320x240 avg_brightness=167.7
[image_processor-2] [INFO] [1785746182.667889562] [image_processor_py]: rcv Image: 320x240 avg_brightness=169.3
[image_processor-2] [INFO] [1785746182.735493313] [image_processor_py]: rcv Image: 320x240 avg_brightness=85.7
[image_processor-2] [INFO] [1785746182.828613604] [image_processor_py]: rcv Image: 320x240 avg_brightness=87.3
[image_processor-2] [INFO] [1785746182.952949732] [image_processor_py]: rcv Image: 320x240 avg_brightness=89.0
[image_processor-2] [INFO] [1785746183.041343773] [image_processor_py]: rcv Image: 320x240 avg_brightness=90.7
[image_processor-2] [INFO] [1785746183.161858347] [image_processor_py]: rcv Image: 320x240 avg_brightness=92.3
[image_processor-2] [INFO] [1785746183.257778859] [image_processor_py]: rcv Image: 320x240 avg_brightness=94.0
[image_processor-2] [INFO] [1785746183.348466849] [image_processor_py]: rcv Image: 320x240 avg_brightness=95.7
[image_processor-2] [INFO] [1785746183.432493081] [image_processor_py]: rcv Image: 320x240 avg_brightness=97.3
[image_processor-2] [INFO] [1785746183.537498307] [image_processor_py]: rcv Image: 320x240 avg_brightness=99.0
[image_processor-2] [INFO] [1785746183.633903985] [image_processor_py]: rcv Image: 320x240 avg_brightness=100.7
[image_processor-2] [INFO] [1785746183.739841647] [image_processor_py]: rcv Image: 320x240 avg_brightness=102.3
[image_processor-2] [INFO] [1785746183.853898813] [image_processor_py]: rcv Image: 320x240 avg_brightness=104.0
[image_processor-2] [INFO] [1785746183.982298735] [image_processor_py]: rcv Image: 320x240 avg_brightness=105.7
[image_processor-2] [INFO] [1785746184.155849881] [image_processor_py]: rcv Image: 320x240 avg_brightness=107.3
[image_processor-2] [INFO] [1785746184.260583192] [image_processor_py]: rcv Image: 320x240 avg_brightness=109.0
[image_processor-2] [INFO] [1785746184.361685626] [image_processor_py]: rcv Image: 320x240 avg_brightness=110.7
[image_processor-2] [INFO] [1785746184.441802094] [image_processor_py]: rcv Image: 320x240 avg_brightness=112.3
[image_processor-2] [INFO] [1785746184.547094494] [image_processor_py]: rcv Image: 320x240 avg_brightness=114.0
[image_processor-2] [INFO] [1785746184.630434403] [image_processor_py]: rcv Image: 320x240 avg_brightness=115.7
[image_processor-2] [INFO] [1785746184.711841501] [image_processor_py]: rcv Image: 320x240 avg_brightness=117.3
[image_processor-2] [INFO] [1785746184.792458265] [image_processor_py]: rcv Image: 320x240 avg_brightness=119.0
[image_processor-2] [INFO] [1785746184.849712158] [image_processor_py]: rcv Image: 320x240 avg_brightness=120.7
[image_processor-2] [INFO] [1785746184.928970519] [image_processor_py]: rcv Image: 320x240 avg_brightness=122.3
[image_processor-2] [INFO] [1785746185.067966655] [image_processor_py]: rcv Image: 320x240 avg_brightness=124.0
[image_processor-2] [INFO] [1785746185.147475299] [image_processor_py]: rcv Image: 320x240 avg_brightness=125.7
[image_processor-2] [INFO] [1785746185.277355183] [image_processor_py]: rcv Image: 320x240 avg_brightness=127.3
[image_processor-2] [INFO] [1785746185.359391935] [image_processor_py]: rcv Image: 320x240 avg_brightness=129.0
[image_processor-2] [INFO] [1785746185.472738444] [image_processor_py]: rcv Image: 320x240 avg_brightness=130.7
[image_processor-2] [INFO] [1785746185.563474990] [image_processor_py]: rcv Image: 320x240 avg_brightness=132.3
[image_processor-2] [INFO] [1785746185.643084788] [image_processor_py]: rcv Image: 320x240 avg_brightness=134.0
[image_processor-2] [INFO] [1785746185.741524561] [image_processor_py]: rcv Image: 320x240 avg_brightness=135.7
[image_processor-2] [INFO] [1785746185.839411121] [image_processor_py]: rcv Image: 320x240 avg_brightness=137.3
[image_processor-2] [INFO] [1785746185.955768251] [image_processor_py]: rcv Image: 320x240 avg_brightness=139.0
[image_processor-2] [INFO] [1785746186.076370978] [image_processor_py]: rcv Image: 320x240 avg_brightness=140.7
[image_processor-2] [INFO] [1785746186.152817713] [image_processor_py]: rcv Image: 320x240 avg_brightness=142.3
[image_processor-2] [INFO] [1785746186.232545747] [image_processor_py]: rcv Image: 320x240 avg_brightness=144.0
[image_processor-2] [INFO] [1785746186.343450992] [image_processor_py]: rcv Image: 320x240 avg_brightness=145.7
[image_processor-2] [INFO] [1785746186.418098491] [image_processor_py]: rcv Image: 320x240 avg_brightness=147.3
[image_processor-2] [INFO] [1785746186.527992804] [image_processor_py]: rcv Image: 320x240 avg_brightness=149.0
[image_processor-2] [INFO] [1785746186.658276623] [image_processor_py]: rcv Image: 320x240 avg_brightness=150.7
[image_processor-2] [INFO] [1785746186.742369818] [image_processor_py]: rcv Image: 320x240 avg_brightness=152.3
[image_processor-2] [INFO] [1785746186.869539618] [image_processor_py]: rcv Image: 320x240 avg_brightness=154.0
[image_processor-2] [INFO] [1785746186.994675142] [image_processor_py]: rcv Image: 320x240 avg_brightness=155.7
[image_processor-2] [INFO] [1785746187.210775412] [image_processor_py]: rcv Image: 320x240 avg_brightness=157.3
[image_processor-2] [INFO] [1785746187.293337808] [image_processor_py]: rcv Image: 320x240 avg_brightness=159.0
[image_processor-2] [INFO] [1785746187.390652703] [image_processor_py]: rcv Image: 320x240 avg_brightness=160.7
[image_processor-2] [INFO] [1785746187.556529076] [image_processor_py]: rcv Image: 320x240 avg_brightness=162.3
[image_processor-2] [INFO] [1785746187.728169526] [image_processor_py]: rcv Image: 320x240 avg_brightness=164.0
[image_processor-2] [INFO] [1785746188.383607374] [image_processor_py]: rcv Image: 320x240 avg_brightness=165.7
[image_processor-2] [INFO] [1785746188.472993671] [image_processor_py]: rcv Image: 320x240 avg_brightness=167.3
[image_processor-2] [INFO] [1785746188.548308513] [image_processor_py]: rcv Image: 320x240 avg_brightness=169.0
[image_processor-2] [INFO] [1785746188.714229686] [image_processor_py]: rcv Image: 320x240 avg_brightness=85.3
[image_processor-2] [INFO] [1785746188.926396972] [image_processor_py]: rcv Image: 320x240 avg_brightness=87.0
[image_processor-2] [INFO] [1785746189.232378348] [image_processor_py]: rcv Image: 320x240 avg_brightness=88.7
[image_processor-2] [INFO] [1785746189.388305708] [image_processor_py]: rcv Image: 320x240 avg_brightness=93.7
[image_processor-2] [INFO] [1785746189.544417510] [image_processor_py]: rcv Image: 320x240 avg_brightness=97.0
[image_processor-2] [INFO] [1785746189.629474554] [image_processor_py]: rcv Image: 320x240 avg_brightness=98.7
[image_processor-2] [INFO] [1785746189.699982525] [image_processor_py]: rcv Image: 320x240 avg_brightness=100.3
[image_processor-2] [INFO] [1785746189.827674287] [image_processor_py]: rcv Image: 320x240 avg_brightness=102.0
[fake_camera-1] Traceback (most recent call last):
[fake_camera-1] File "/root/ros2_ws/install/py_vision_demo/lib/py_vision_demo/fake_camera", line 33, in <module>
[fake_camera-1] sys.exit(load_entry_point('py-vision-demo', 'console_scripts', 'fake_camera')())
[fake_camera-1] File "/root/ros2_ws/build/py_vision_demo/py_vision_demo/fake_camera.py", line 86, in main
[fake_camera-1] rclpy.spin(node)
[fake_camera-1] File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/__init__.py", line 229, in spin
[fake_camera-1] executor.spin_once()
[fake_camera-1] File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 808, in spin_once
[fake_camera-1] self._spin_once_impl(timeout_sec)
[fake_camera-1] File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 805, in _spin_once_impl
[fake_camera-1] raise handler.exception()
[fake_camera-1] File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/task.py", line 272, in _execute_coroutine_step
[fake_camera-1] result = coro.send(None)
[fake_camera-1] File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 488, in handler
[fake_camera-1] await call_coroutine(entity, arg)
[fake_camera-1] File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 396, in _execute_timer
[fake_camera-1] await await_or_execute(tmr.callback)
[fake_camera-1] File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 110, in await_or_execute
[fake_camera-1] return callback(*args)
[fake_camera-1] File "/root/ros2_ws/build/py_vision_demo/py_vision_demo/fake_camera.py", line 78, in tick
[fake_camera-1] self.publisher_.publish(msg)
[fake_camera-1] File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/publisher.py", line 70, in publish
[fake_camera-1] self.__publisher.publish(msg)
[fake_camera-1] rclpy._rclpy_pybind11.RCLError: Failed to publish: publisher's context is invalid, at ./src/rcl/publisher.c:389
[image_processor-2] [INFO] [1785746189.915325523] [image_processor_py]: rcv Image: 320x240 avg_brightness=103.7
[image_processor-2] [INFO] [1785746190.011160071] [image_processor_py]: rcv Image: 320x240 avg_brightness=105.3
[ERROR] [fake_camera-1]: process has died [pid 143, exit code 1, cmd '/root/ros2_ws/install/py_vision_demo/lib/py_vision_demo/fake_camera --ros-args -r __node:=fake_camera_py --params-file /tmp/launch_params_yy83q6nm'].
[image_processor-2] [INFO] [1785746190.111495157] [image_processor_py]: rcv Image: 320x240 avg_brightness=107.0
[image_processor-2] [INFO] [1785746190.276766359] [image_processor_py]: rcv Image: 320x240 avg_brightness=108.7
[image_processor-2] [INFO] [1785746190.421619850] [image_processor_py]: rcv Image: 320x240 avg_brightness=110.3
[image_processor-2] [INFO] [1785746190.530816444] [image_processor_py]: rcv Image: 320x240 avg_brightness=112.0
[image_processor-2] [INFO] [1785746190.644356186] [image_processor_py]: rcv Image: 320x240 avg_brightness=113.7
[image_processor-2] [INFO] [1785746190.749298421] [image_processor_py]: rcv Image: 320x240 avg_brightness=115.3
[image_processor-2] [INFO] [1785746191.060880109] [image_processor_py]: rcv Image: 320x240 avg_brightness=117.0
[image_processor-2] [INFO] [1785746191.217882294] [image_processor_py]: rcv Image: 320x240 avg_brightness=118.7
pyproject.toml
+107
View File
@@ -0,0 +1,107 @@
# =============================================================================
# pyproject.toml —— 标准 PEP 621 项目元数据(workspace 模式)。
#
# 设计取舍:
# - ROS2 实际构建/运行仍走 colcon + ament_python(ROS2 官方工具链);
# - 本文件提供 *额外* 的标准 Python 入口,让本机 IDE / 类型检查 /
# pytest / ruff 能识别为 Python 项目,且不污染系统 Python。
# - 真正运行 ROS2 节点时,需要在容器内 / WSL 内 source /opt/ros/humble/setup.bash,
# 因为 rclpy / sensor_msgs 等只有 apt 上的发行版,Windows 无 wheels。
#
# venv 工作流:
# 本机: ./tools/setup_venv.sh (Linux/WSL) 或 ./tools/setup_venv.ps1 (Windows)
# 进入开发: source .venv/bin/activate (Linux) 或 .venv\Scripts\Activate.ps1 (Win)
# pytest: pytest src/py_pubsub/test # 纯逻辑测试
# 全套测试: colcon test (在 Docker 容器内) # 真 ROS2 测试
# =============================================================================
[build-system]
# 不用 setuptools 实际构建,只是给 IDE 一个工具入口。
# 各 ROS2 ament_python 包各自的 setup.py 由 colcon 调用。
requires = ["setuptools>=61", "wheel"]
build-backend = "setuptools.build_meta"
[project]
name = "ros2-learning-suite"
version = "0.1.0"
description = "ROS2 Humble 全栈实战:Topics/Services/Actions/TF2/URDF/Vision(具身智能入门)"
readme = "README.md"
requires-python = ">=3.10"
license = { text = "Apache-2.0" }
authors = [{ name = "xs", email = "dev@example.com" }]
keywords = ["ros2", "humble", "robotics", "embodied-ai", "vla", "tf2", "urdf"]
# 标准 Python 依赖(运行时不强制,运行时 ROS2 客户端库来自 apt)。
dependencies = [
# 测试/工具依赖见 requirements-dev.txt;这里只列运行相关的纯 Python 库。
"numpy>=1.23",
]
[project.optional-dependencies]
# pip install -e .[dev] 装开发工具
dev = [
"pytest>=7",
"pytest-cov>=4",
"pytest-timeout>=2",
"mypy>=1.5",
"ruff>=0.1",
"black>=23",
"flake8>=6",
"types-setuptools",
]
# pip install -e .[vision] 装视觉相关
vision = [
"opencv-python-headless>=4.7",
]
[project.urls]
Documentation = "https://docs.ros.org/en/humble/"
Source = "https://github.com/ros2"
[tool.setuptools]
# 不让 setuptools 试图打包本根目录——各子包由 ament_python 单独构建。
py-modules = []
# ---------------- pytest 配置 ----------------
[tool.pytest.ini_options]
# 让 pytest 自动找到 src/ 下所有 test_*.py / *_test.py
testpaths = ["src"]
python_files = ["test_*.py", "*_test.py"]
python_classes = ["Test*"]
python_functions = ["test_*"]
addopts = [
"-ra",
"--strict-markers",
"--tb=short",
]
# 标记:需要 ROS2 环境(容器内跑),用 pytest -m "not ros" 在本机只跑纯逻辑测试
markers = [
"ros: 需要 ROS2 运行环境(rclpy / sensor_msgs / tf2)",
"inproc: 同进程内 spin 跑得通,不依赖外部 ROS daemon",
]
# ---------------- ruff 配置(快速 lint) ----------------
[tool.ruff]
line-length = 100
target-version = "py310"
extend-exclude = [".venv", "build", "install", "log", "src/*/build"]
[tool.ruff.lint]
select = ["E", "F", "W", "I", "UP", "B", "C4"]
ignore = ["E501", "B008"] # 容忍长行 / 函数调用默认值
[tool.ruff.lint.per-file-ignores]
"src/**/test_*.py" = ["B011"] # 测试允许 assert False
# ---------------- mypy 配置(静态类型检查) ----------------
[tool.mypy]
python_version = "3.10"
ignore_missing_imports = true # rclpy / sensor_msgs 在 venv 里看不到
warn_unused_ignores = true
no_implicit_optional = true
# ---------------- black 配置 ----------------
[tool.black]
line-length = 100
target-version = ["py310"]
extend-exclude = '((\\.venv|build|install|log)/)'
pyrightconfig.json
+32
View File
@@ -0,0 +1,32 @@
{
"_comment": "pyright (Pylance) 配置。让 IDE 知道 rclpy / sensor_msgs 等在哪里,且排除构建/venv 目录。",
"include": [
"src/**/*.py",
"tools/**/*.py"
],
"exclude": [
"**/__pycache__",
".venv",
"build",
"install",
"log",
"src/**/build",
"src/**/install"
],
"_extraPaths_comment": "Linux/WSL/Docker 容器内 ROS2 的 site-packages;Windows 上不存在,被 ignore_missing_imports 兜底。",
"extraPaths": [
"/opt/ros/humble/lib/python3.10/site-packages",
"/opt/ros/humble/local/lib/python3.10/dist-packages"
],
"reportMissingImports": "warning",
"reportMissingTypeStubs": "none",
"pythonVersion": "3.10",
"typeCheckingMode": "basic",
"venvPath": ".venv"
}
requirements-dev.txt
+28
View File
@@ -0,0 +1,28 @@
# =============================================================================
# requirements-dev.txt —— 开发工具集合(lint / format / type-check)。
#
# 安装: pip install -r requirements-dev.txt
# 用途:
# ruff —— Rust 写的超快 linter,替代 flake8 + isort
# black —— 不可配置的代码格式化器
# mypy —— 静态类型检查
# pytest —— 测试框架(已在 requirements.txt 同步一份)
# pre-commit —— git hook 框架(可选)
# =============================================================================
# 代码风格 / lint
ruff>=0.1.0
black>=23.0
flake8>=6.0
isort>=5.12
# 静态类型
mypy>=1.5
# 测试
pytest>=7.4
pytest-cov>=4.1
pytest-timeout>=2.1
# Git hooks(可选: pre-commit install)
pre-commit>=3.4
requirements.txt
+22
View File
@@ -0,0 +1,22 @@
# =============================================================================
# requirements.txt —— 本机 venv 安装运行时纯 Python 依赖。
#
# 注意:rclpy / sensor_msgs / tf2_ros / cv_bridge 等 ROS2 客户端库
# **不在这里**——它们通过 apt 装(ros-humble-desktop / ros-humble-cv-bridge),
# 在 Docker 容器内或 WSL Ubuntu 上有效;Windows 没有官方 wheels。
#
# 测试运行:
# Docker/WSL 内:rclpy 等来自 apt,Python venv 加 PYTHONPATH 可 import。
# Windows 本机:venv 仅装这里的依赖,跑"非 ROS"测试(如 cv2 解码的纯逻辑)。
# =============================================================================
# 数组/图像处理
numpy>=1.23,<2.0
# pytest 测试框架本体(开发工具在 requirements-dev.txt)
pytest>=7.4
pytest-cov>=4.1
pytest-timeout>=2.1
# typing 辅助
typing-extensions>=4.5
src/bringup/launch/action_launch.py
+20
View File
@@ -0,0 +1,20 @@
"""
action_launch.py —— 启动 Fibonacci Action 服务端。
client 不在此 launch,按需手动调用:
ros2 action send_goal /fibonacci example_interfaces/action/Fibonacci "{order: 8}" --feedback
"""
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='py_action_demo',
executable='fibonacci_server',
name='fibonacci_action_server_py',
output='screen',
),
])
src/bringup/launch/all_launch.py
+52
View File
@@ -0,0 +1,52 @@
"""
all_launch.py —— 顶层 bringup 启动文件:同时把 py 和 cpp 的 pub/sub 启起来。
ROS2 进阶:
IncludeLaunchDescription 把其他包的 .launch.py 嵌进本 launch,
并通过 launch_arguments 把参数"穿透"传过去。模板渲染阶段会
把 LaunchConfiguration('topic') 当文本替换,生成最终子 launch。
关键 helper:
FindPackageShare('pkg') —— 解析某 ROS 包的 share/ 目录。
PathJoinSubstitution(...) —— 用 "/" 把多个 part 拼成路径。
PythonLaunchDescriptionSource(path) —— 指明来源是另一个 launch.py。
运行:
ros2 launch bringup all_launch.py
ros2 launch bringup all_launch.py topic:=demo
"""
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
def generate_launch_description():
# 声明顶层可覆盖的形参 topic。
topic_arg = DeclareLaunchArgument('topic', default_value='chatter')
# 解析两个子包 share 目录(不依赖绝对路径,部署后也能跑)。
pp_pkg = FindPackageShare('py_pubsub') # → <install>/share/py_pubsub
cp_pkg = FindPackageShare('cpp_pubsub') # → <install>/share/cpp_pubsub
# 子 launch 文件的绝对路径。
py_launch = PathJoinSubstitution([pp_pkg, 'launch', 'pubsub_launch.py'])
cpp_launch = PathJoinSubstitution([cp_pkg, 'launch', 'pubsub_launch.py'])
return LaunchDescription([
topic_arg,
# 嵌入 Python 版 pubsub,顺便把顶层 'topic' 透传给子 launch 的形参。
IncludeLaunchDescription(
PythonLaunchDescriptionSource(py_launch),
launch_arguments={'topic': LaunchConfiguration('topic')}.items(),
),
# 嵌入 C++ 版 pubsub,同上。
IncludeLaunchDescription(
PythonLaunchDescriptionSource(cpp_launch),
launch_arguments={'topic': LaunchConfiguration('topic')}.items(),
),
])
src/bringup/launch/full_demo_launch.py
+35
View File
@@ -0,0 +1,35 @@
"""
full_demo_launch.py —— 整合所有 demo(全套启动)。
启动的节点:
- 4 个 pub/sub 节点(pubsub)
- add_two_ints server(srv)
- fibonacci action server(action)
- 3 节点 URDF TF2(robot)
- fake_camera + image_processor(vision)
总计 ~10 个节点同时运行,展示整个 ROS2 通信栈。
"""
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.substitutions import FindPackageShare
from launch.substitutions import PathJoinSubstitution
def _include(pkg_name, launch_file):
pkg = FindPackageShare(pkg_name)
path = PathJoinSubstitution([pkg, 'launch', launch_file])
return IncludeLaunchDescription(PythonLaunchDescriptionSource(path))
def generate_launch_description():
return LaunchDescription([
_include('py_pubsub', 'pubsub_launch.py'),
_include('cpp_pubsub', 'pubsub_launch.py'),
_include('py_srv', 'srv_launch.py'),
_include('py_action_demo', 'action_launch.py'),
_include('cpp_robot_tf2', 'robot_tf2_launch.py'),
_include('py_vision_demo', 'vision_launch.py'),
])
src/bringup/launch/pubsub_launch.py
+47
View File
@@ -0,0 +1,47 @@
"""
pubsub_launch.py —— 同时启动 4 个 pub/sub 节点(2 Python + 2 C++)。
节点:
- talker_py (py_pubsub)
- listener_py (py_pubsub)
- talker_cpp (cpp_pubsub)
- listener_cpp (cpp_pubsub)
全部订阅同一个 topic 'chatter',验证跨语言互通。
"""
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='py_pubsub',
executable='talker',
name='talker_py',
output='screen',
parameters=[{'period_ms': 500, 'topic': 'chatter'}],
),
Node(
package='py_pubsub',
executable='listener',
name='listener_py',
output='screen',
parameters=[{'topic': 'chatter'}],
),
Node(
package='cpp_pubsub',
executable='talker',
name='talker_cpp',
output='screen',
parameters=[{'period_ms': 500, 'topic': 'chatter'}],
),
Node(
package='cpp_pubsub',
executable='listener',
name='listener_cpp',
output='screen',
parameters=[{'topic': 'chatter'}],
),
])
src/bringup/launch/robot_launch.py
+18
View File
@@ -0,0 +1,18 @@
"""
robot_launch.py —— 启动 URDF + TF2 + JointState 演示。
通过 IncludeLaunchDescription 复用 cpp_robot_tf2 的 launch。
"""
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.substitutions import FindPackageShare
from launch.substitutions import PathJoinSubstitution
def generate_launch_description():
pkg = FindPackageShare('cpp_robot_tf2')
path = PathJoinSubstitution([pkg, 'launch', 'robot_tf2_launch.py'])
return LaunchDescription([
IncludeLaunchDescription(PythonLaunchDescriptionSource(path)),
])
src/bringup/launch/service_launch.py
+21
View File
@@ -0,0 +1,21 @@
"""
service_launch.py —— 启动 add_two_ints 服务端。
client 不在此 launch,按需手动调用:
source install/setup.bash
ros2 service call /add_two_ints example_interfaces/srv/AddTwoInts "{a: 12, b: 30}"
"""
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='py_srv',
executable='add_two_ints_server',
name='add_two_ints_server_py',
output='screen',
),
])
src/bringup/launch/vision_launch.py
+17
View File
@@ -0,0 +1,17 @@
"""
vision_launch.py —— 启动 fake_camera + image_processor。
"""
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.substitutions import FindPackageShare
from launch.substitutions import PathJoinSubstitution
def generate_launch_description():
pkg = FindPackageShare('py_vision_demo')
path = PathJoinSubstitution([pkg, 'launch', 'vision_launch.py'])
return LaunchDescription([
IncludeLaunchDescription(PythonLaunchDescriptionSource(path)),
])
src/bringup/package.xml
+29
View File
@@ -0,0 +1,29 @@
<?xml version="1.0"?>
<!--
package.xml —— bringup(顶层 bringup)包,作用是聚合另外两个演示包。
它本身不提供节点,只提供 launch 文件,通过 exec_depend 让 colcon
在 build bringup 之前先 build 它的依赖。
-->
<?xml-model
href="http://download.ros.org/schema/package_format3.xsd"
schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<!-- 包名是 'bringup'(与目录名一致,避免和系统自带的 'launch' 包重名)。 -->
<name>bringup</name>
<version>0.1.0</version>
<description>Top-level bringup launching both py and cpp pubsub</description>
<maintainer email="dev@example.com">xs</maintainer>
<license>Apache-2.0</license>
<!-- exec_depend:仅在运行(ros2 launch)时需要,不需要在编译时。 -->
<exec_depend>py_pubsub</exec_depend> <!-- 嵌入子 launch 时需要。 -->
<exec_depend>cpp_pubsub</exec_depend> <!-- 同上。 -->
<!-- build_depend / exec_depend 的 launch / launch_ros:本包 launch 文件 import 它们。 -->
<exec_depend>launch</exec_depend>
<exec_depend>launch_ros</exec_depend>
<!-- ament_python 是为了让 launch 文件(纯 .py)能被 colcon 找见。 -->
<export>
<build_type>ament_python</build_type>
</export>
</package>
src/bringup/resource/bringup
View File
src/bringup/setup.cfg
+6
View File
@@ -0,0 +1,6 @@
[develop]
# ament_python 期望 console scripts 安装到 install/lib/<pkg>/;
# 本包没有 entry_points,只留默认即可。
script_dir=$base/lib/bringup
[install]
install_scripts=$base/lib/bringup
src/bringup/setup.py
+48
View File
@@ -0,0 +1,48 @@
# =============================================================================
# setup.py —— bringup 包的安装清单。
#
# 与 py_pubsub 不同,bringup 不需要 entry_points(没有节点可执行)。
# 只用 data_files 把资源拷到 ROS 标准位置,让
# ros2 launch bringup all_launch.py
# 能找到 launch 文件。
#
# 注意:package_name 必须与 package.xml <name> 一致("bringup",避免和
# ROS 自带的 'launch' 包同名产生 ament 索引冲突)。
# =============================================================================
# glob:用于展开当前目录下的 launch/*.py,自动收集所有 launch 文件。
from glob import glob
# setuptools.setup:标准 Python 安装入口。
from setuptools import setup
# 必须与 package.xml <name> 一致。
package_name = 'bringup'
setup(
name=package_name,
version='0.1.0',
# 本包没有业务 Python 模块,告诉 setuptools 不要安装任何 .py。
# (launch/all_launch.py 是 launch 文件,不属于 pip 安装,放 data_files。)
packages=[],
data_files=[
# ament 索引必备,标记本包存在,ros2 工具才认这个 ROS 包。
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
# 让 ros2 工具读到 package.xml 元数据。
('share/' + package_name, ['package.xml']),
# launch 文件目录必须叫 share/<pkg>/launch/,
# glob('launch/*.py') 自动收录该目录下所有 .py launch。
('share/' + package_name + '/launch', glob('launch/*.py')),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='xs',
maintainer_email='dev@example.com',
description='Top-level bringup launching both py and cpp pubsub',
license='Apache-2.0',
)
src/cpp_pubsub/CMakeLists.txt
+44
View File
@@ -0,0 +1,44 @@
cmake_minimum_required(VERSION 3.16)
project(cpp_pubsub VERSION 0.1.0)
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
include_directories(include)
add_executable(talker src/publisher_member_function.cpp)
ament_target_dependencies(talker rclcpp std_msgs)
add_executable(listener src/subscriber_member_function.cpp)
ament_target_dependencies(listener rclcpp std_msgs)
install(TARGETS
talker
listener
DESTINATION lib/${PROJECT_NAME}
)
install(DIRECTORY launch
DESTINATION share/${PROJECT_NAME}/
)
# ------------------------------------------------------------------
# 测试:ament_cmake_gtest 集成 gtest,
# colcon test --packages-select cpp_pubsub 时自动跑 test_pub_sub。
# ------------------------------------------------------------------
if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
ament_add_gtest(test_pub_sub test/test_pub_sub.cpp)
ament_target_dependencies(test_pub_sub rclcpp std_msgs)
endif()
ament_package()
src/cpp_pubsub/launch/pubsub_launch.py
+68
View File
@@ -0,0 +1,68 @@
"""
pubsub_launch.py —— 启动 talker_cpp + listener_cpp,演示 launch 命令行参数。
与 py_pubsub 的同名 launch 文件对照学习:
1) 这里引入了 DeclareLaunchArgument:把 'topic' 声明为命令行可改的形参;
2) 用 LaunchConfiguration('topic') 在生成时取其值,
让 Node 的 topic 参数跟随命令行传入变化(launch_arguments 是
IncludeLaunchDescription 内部传参的标准做法)。
3) 这样从外部:
ros2 launch cpp_pubsub pubsub_launch.py topic:=my_topic
会让两个 C++ 节点都订阅/发布到 my_topic。
运行:
source install/setup.bash
ros2 launch cpp_pubsub pubsub_launch.py
ros2 launch cpp_pubsub pubsub_launch.py topic:=hello
"""
# LaunchDescription:容器类型。
from launch import LaunchDescription
# DeclareLaunchArgument:声明一个可被命令行 / 父 launch 覆盖的形参。
from launch.actions import DeclareLaunchArgument
# LaunchConfiguration:在生成 LaunchDescription 期间"懒求值"某个参数的值。
from launch.substitutions import LaunchConfiguration
# Node:声明运行一个 ROS2 节点的动作。
from launch_ros.actions import Node
def generate_launch_description():
"""导出函数,返回 LaunchDescription 实例。"""
# 声明 topic 形参,默认值 'chatter',在终端可用 topic:=xxx 覆盖。
topic_arg = DeclareLaunchArgument(
'topic',
default_value='chatter',
description='Topic name for both pubs/subs',
)
# 取出 LaunchConfiguration('topic') 的当前值,后续当参数用。
topic = LaunchConfiguration('topic')
return LaunchDescription([
topic_arg, # 把声明本身也放进 LaunchDescription 里(声明是动作之一)。
# C++ 发布节点
Node(
package='cpp_pubsub',
executable='talker', # CMakeLists 里 add_executable(talker ...)
name='talker_cpp',
output='screen',
parameters=[{
'period_ms': 500,
'topic': topic, # 用上面 LaunchConfiguration 取代硬编码字符串,
# 让命令行参数动态注入。
}],
),
# C++ 订阅节点
Node(
package='cpp_pubsub',
executable='listener',
name='listener_cpp',
output='screen',
parameters=[{
'topic': topic,
}],
),
])
src/cpp_pubsub/package.xml
+31
View File
@@ -0,0 +1,31 @@
<?xml version="1.0"?>
<!--
package.xml —— cpp_pubsub 包的元数据。
对 C++ 包而言,<exec_depend> 一般可缺,因为 CMakeLists.txt 里 find_package
已经显式声明依赖;但仍建议写 <depend> 让 colcon 能正确排序构建顺序。
-->
<?xml-model
href="http://download.ros.org/schema/package_format3.xsd"
schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>cpp_pubsub</name>
<version>0.1.0</version>
<description>C++ talker/listener demo for ROS2 Humble</description>
<maintainer email="dev@example.com">xs</maintainer>
<license>Apache-2.0</license>
<!-- depend:这是 C++ 编译 + 链接 + 运行都需要的 ROS 库。 -->
<depend>rclcpp</depend> <!-- C++ 客户端库。 -->
<depend>std_msgs</depend> <!-- 内置基础消息。 -->
<!-- test_depend:仅在 colcon test 时被安装/使用。 -->
<test_depend>ament_lint_auto</test_depend> <!-- 自动调度几种 lint -->
<test_depend>ament_lint_common</test_depend> <!-- cpplint / uncrustify -->
<test_depend>ament_cmake_gtest</test_depend> <!-- gtest 集成 -->
<test_depend>launch_testing_ros</test_depend><!-- launch integration -->
<!-- export/build_type = ament_cmake:让 colcon 调用 CMake 构建。 -->
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
src/cpp_pubsub/src/publisher_member_function.cpp
+113
View File
@@ -0,0 +1,113 @@
// =============================================================================
// talker_cpp —— ROS2 C++ 发布者节点(Publisher)
//
// 用途:与 Python 版本的 talker_py 形成"跨语言通信"演示,二者通过同一 Topic
// 'chatter' 都用 std_msgs/String,体现 ROS2 的多语言互操作性
// (因为底层都是 DDS,语言只是同一套消息定义的视图)。
//
// ROS2 概念速览:
// Node —— 节点,所有发布/订阅/服务/参数都挂在节点上。
// Publisher<T> —— 发布者,T 是消息类型,publish(msg) 异步投递到 DDS。
// Timer —— 周期性回调,WallTimer / SteadyTimer 等类型。
// Parameter —— 节点配置,declare_parameter<T>() + get_parameter()。
// std_msgs/String —— 自动生成的消息结构体,只含 std::string data。
//
// 构建:本文件由 ament_cmake 编译,见 src/cpp_pubsub/CMakeLists.txt。
// 运行:ros2 run cpp_pubsub talker
// ros2 run cpp_pubsub talker --ros-args -p period_ms:=200 -p topic:=hello
// =============================================================================
#include <chrono> // std::chrono::milliseconds,用来描述 timer 的周期。
#include <memory> // std::shared_ptr / std::make_shared,ROS2 句柄大量用到。
#include <string> // std::string + std::to_string。
// rclcpp:ROS Client Library for C++,所有 ROS2 C++ 节点的入口库,提供
// rclcpp::Node / Publisher / Subscription / Timer 等高级封装。
#include "rclcpp/rclcpp.hpp"
// std_msgs/msg/string.hpp:由 .msg 文件在编译期生成的 C++ 结构体,
// 内含 std::string data;(RCLCPP_LOGGER 自动序列化/反序列化它到 DDS)。
#include "std_msgs/msg/string.hpp"
// using namespace std::chrono_literals; // 允许 500ms 这种字面量。
using namespace std::chrono_literals;
/// @brief 自定义发布者节点:TalkerCPP,继承自 rclcpp::Node 基类。
class TalkerCPP : public rclcpp::Node
{
public:
/// 构造函数:在这里完成 publisher/timer/parameter 三大初始化。
TalkerCPP()
: rclcpp::Node("talker_cpp"), count_(0)
{
// declare_parameter<T>(name, default):模板版的参数声明方式,比通用
// 指针版更类型安全。这里声明两个参数:
// period_ms —— 发布周期(整数,毫秒)
// topic —— 目标 topic 名(字符串)
this->declare_parameter<int>("period_ms", 500);
this->declare_parameter<std::string>("topic", "chatter");
// 取回参数实际值(as_int() / as_string()),用于初始化 publisher / timer。
int period_ms = this->get_parameter("period_ms").as_int();
std::string topic = this->get_parameter("topic").as_string();
// create_publisher<T>(topic, qos_depth):创建发布者。
// T = std_msgs::msg::String —— 模板参数告诉编译器消息类型。
// qos_depth = 10,队列最多缓存 10 条未送达样本(慢消费者场景)。
publisher_ = this->create_publisher<std_msgs::msg::String>(topic, 10);
// create_wall_timer(period, callback):挂一个 wall-clock 定时器。
// 当 period = std::chrono::milliseconds(period_ms) 时,
// 每次"节点事件循环"经过 period_ms 就回调 timer_callback 一次。
// 注意:std::bind 把 this + 成员函数绑定成可调用对象。
timer_ = this->create_wall_timer(
std::chrono::milliseconds(period_ms),
std::bind(&TalkerCPP::timer_callback, this));
// RCLCPP_INFO:节点级日志,前缀自动带 [INFO] [时间] [节点名]。
RCLCPP_INFO(this->get_logger(),
"talker_cpp started -> topic=%s, period=%dms",
topic.c_str(), period_ms);
}
private:
/// @brief 定时器回调,每次触发就构造并发布一条消息。
void timer_callback()
{
// 1) 构造一条 std_msgs/String 消息,填写 data。
auto msg = std_msgs::msg::String();
msg.data = "Hello from C++, seq=" + std::to_string(count_++);
// 2) publisher_->publish(msg) — 异步把消息放入 DDS 队列,
// 等订阅方 QoS 匹配即被接收;此调用不阻塞。
publisher_->publish(msg);
// 3) 顺手打印日志,便于观察节奏。
RCLCPP_INFO(this->get_logger(), "pub: \"%s\"", msg.data.c_str());
}
// SharedPtr:节点内所有"句柄"都用智能指针管理,可被 Node 持有,
// 并在节点析构时一起释放,避免生命周期问题。
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
rclcpp::TimerBase::SharedPtr timer_;
size_t count_; // 消息序号,用于调试观测。
};
/// @brief ROS2 C++ 节点的标准 main 模板:
/// init -> 实例化节点 -> spin -> shutdown。
/// 注意:C++ 里没有 KeyboardInterrupt 捕获,因为 Ctrl+C 在终端层
/// 直接发给 rclcpp::shutdown(),spin() 自动返回。
int main(int argc, char * argv[])
{
// rclcpp::init:解析 ROS 专属命令行参数(--ros-args 之后的部分),
// 初始化底层 DDS 上下文。
rclcpp::init(argc, argv);
// std::make_shared<TalkerCPP>():把节点对象交给 shared_ptr,
// 这是 rclcpp::spin 的强制要求,Node 内部要拿到 shared_ptr 计数。
rclcpp::spin(std::make_shared<TalkerCPP>());
// spin() 返回后做清理。
rclcpp::shutdown();
return 0;
}
src/cpp_pubsub/src/subscriber_member_function.cpp
+68
View File
@@ -0,0 +1,68 @@
// =============================================================================
// listener_cpp —— ROS2 C++ 订阅者节点(Subscriber)
//
// 与 talker_cpp / talker_py 配对使用,演示 C++ 侧订阅 + 跨语言互联互通。
//
// 关键概念:
// Subscription<T>:由 node.create_subscription<T>(...) 创建,
// 每收到一条 T 类型消息,ROS2 自动调用回调函数,回调签名:
// void cb(const T::SharedPtr msg)
// SharedPtr 指 std::shared_ptr<T>,可 0 拷贝访问 msg 字段。
//
// 工具:
// ros2 topic info chatter -v 看 publisher/subscriber 列表 + 消息类型。
// ros2 topic echo chatter 命令行实时打印消息内容。
// =============================================================================
#include <memory> // std::placeholders,std::bind 会用到。
#include <string> // std::string。
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
/// @brief 订阅者节点:ListenerCPP,继承自 rclcpp::Node。
class ListenerCPP : public rclcpp::Node
{
public:
ListenerCPP()
: rclcpp::Node("listener_cpp")
{
// 声明 + 读取 topic 参数,与 talker 配对。
this->declare_parameter<std::string>("topic", "chatter");
std::string topic = this->get_parameter("topic").as_string();
// create_subscription<T>(topic, qos_depth, callback):
// - T = std_msgs::msg::String —— 与 publisher 一致才能匹配。
// - topic —— 与 publisher 一致才能通信。
// - qos_depth = 10 —— 缓冲队列深度。
// - callback 用 std::bind 绑定 this + 成员函数,
// 而 `_1` (std::placeholders::_1) 表示回调的第一个参数 = msg。
subscription_ = this->create_subscription<std_msgs::msg::String>(
topic, 10,
std::bind(&ListenerCPP::topic_callback, this, std::placeholders::_1));
RCLCPP_INFO(this->get_logger(), "listener_cpp subscribed <- %s", topic.c_str());
}
private:
/// @brief 收到消息回调。const 修饰保证不修改成员,SharedPtr 安全。
void topic_callback(const std_msgs::msg::String::SharedPtr msg) const
{
// 这里只是示例,真实节点通常:
// 1) 解析 msg->data(若是 JSON,反序列化成业务对象);
// 2) 推入线程安全队列给控制线程消费;
// 3) 触发 TF 变换、可视化、决策等下游动作。
RCLCPP_INFO(this->get_logger(), "recv: \"%s\"", msg->data.c_str());
}
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
};
/// @brief C++ 节点标准 main。
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<ListenerCPP>());
rclcpp::shutdown();
return 0;
}
src/cpp_pubsub/test/test_pub_sub.cpp
+95
View File
@@ -0,0 +1,95 @@
// =============================================================================
// test_pub_sub.cpp —— C++ 节点端到端集成测试 (gtest)。
//
// 验证 TalkerCPP / ListenerCPP 的发布-订阅链可正常运转:
// 1) 启动 talker (publisher) 与 listener (subscriber) 实例;
// 2) 在节点事件循环里 spin 一段时间,直到 listener 收到消息;
// 3) assert 收到 ≥ 1 条,且消息来自 publisher。
//
// 注意:ROS2 单测标准做法是用 launch_testing_ros 在多进程下做端到端测试;
// 本测试在同一进程内 spin 一次,跑得快但不覆盖真实 DDS 互通(那个
// 在 integration test 里覆盖)。此处更偏"单元"性质。
//
// 运行:本测试由 colcon test --packages-select cpp_pubsub 自动调用,
// 其依赖在 CMakeLists.txt 的 if(BUILD_TESTING) ... 块内拉起。
// =============================================================================
#include <chrono>
#include <memory>
#include <string>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
using namespace std::chrono_literals;
// 测试夹具:继承 rclcpp::Node + 携带一个 listener 订阅,做"收到 N 条"断言。
class PubsubFixture : public rclcpp::Node
{
public:
PubsubFixture()
: rclcpp::Node("pubsub_test_node"), received_(0)
{
subscription_ = this->create_subscription<std_msgs::msg::String>(
"chatter_unit_test", 10,
[this](const std_msgs::msg::String::SharedPtr msg) {
(void)msg;
received_++;
});
}
int received_count() const { return received_; }
private:
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
int received_;
};
// Test 1:验证 publisher 在 1s 内能发出至少 1 条消息,
// 通过 ros2 topic hz 等价手段(同进程订阅计数)做侧面验证。
TEST(PubsubTest, TalkerPublishesAtLeastOnce)
{
auto node = std::make_shared<PubsubFixture>();
auto publisher = node->create_publisher<std_msgs::msg::String>("chatter_unit_test", 10);
// 用 wall timer 触发一次 publish,然后 spin 200ms 让回调跑完。
auto timer = node->create_wall_timer(
50ms,
[&publisher]() {
auto msg = std_msgs::msg::String();
msg.data = "unit-test-msg";
publisher->publish(msg);
});
// Humble 的 rclcpp::spin_some 只接 1 个参数,
// 用 SingleThreadedExecutor 显式给每次 spin 一段时间。
auto exec = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
exec->add_node(node);
auto end = std::chrono::steady_clock::now() + 500ms;
while (std::chrono::steady_clock::now() < end) {
exec->spin_some(50ms);
}
ASSERT_GE(node->received_count(), 1) <<
"subscriber should have received at least one message within 500ms";
}
// Test 2:验证 std_msgs/String 字段正确填充(数据类型契约)。
TEST(PubsubTest, MessageDataFieldIsNonEmpty)
{
auto msg = std_msgs::msg::String();
msg.data = "non-empty";
EXPECT_FALSE(msg.data.empty());
EXPECT_EQ(msg.data, "non-empty");
}
int main(int argc, char ** argv)
{
::testing::InitGoogleTest(&argc, argv);
rclcpp::init(argc, argv);
int rc = RUN_ALL_TESTS();
rclcpp::shutdown();
return rc;
}
src/cpp_robot_tf2/CMakeLists.txt
+64
View File
@@ -0,0 +1,64 @@
cmake_minimum_required(VERSION 3.16)
project(cpp_robot_tf2 VERSION 0.1.0)
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
# --- 节点 1:joint_state_publisher ---
# 发布 sensor_msgs/JointState,周期 ~100ms,内容为每个关节的当前角度。
# robot_state_publisher 节点(系统包)会消费 /joint_states + robot_description,
# 自动算出 base_link -> link1 -> link2 -> gripper 的 TF 链。
add_executable(joint_state_publisher src/joint_state_publisher.cpp)
ament_target_dependencies(joint_state_publisher
rclcpp sensor_msgs geometry_msgs
tf2 tf2_ros tf2_geometry_msgs
)
# --- 节点 2:tf2_listener ---
# 用 tf2::Buffer + TransformListener 监听 base_link -> gripper 的变换,
# 周期性打印抓取点在 base_link 系下的 (x, y, z)。
add_executable(tf2_listener src/tf2_listener.cpp)
ament_target_dependencies(tf2_listener
rclcpp sensor_msgs geometry_msgs
tf2 tf2_ros tf2_geometry_msgs
)
install(TARGETS
joint_state_publisher
tf2_listener
DESTINATION lib/${PROJECT_NAME}
)
# 把 urdf/ 整个目录安装到 share/<pkg>/urdf/,让 launch 可以读到 URDF。
install(DIRECTORY urdf
DESTINATION share/${PROJECT_NAME}/
)
install(DIRECTORY launch
DESTINATION share/${PROJECT_NAME}/
)
# --- 测试 ---
if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
ament_add_gtest(test_tf2_lookup test/test_tf2_lookup.cpp)
ament_target_dependencies(test_tf2_lookup
rclcpp sensor_msgs geometry_msgs
tf2 tf2_ros tf2_geometry_msgs
)
endif()
ament_package()
src/cpp_robot_tf2/launch/robot_tf2_launch.py
+72
View File
@@ -0,0 +1,72 @@
"""
robot_tf2_launch.py —— 启动 3 关节机械臂的 TF2 演示。
启动顺序:
joint_state_publisher (本包) → /joint_states
↓
robot_state_publisher (系统包 ros-humble-robot-state-publisher)
读取 URDF + JointState → 算 base_link -> link1 -> link2 -> gripper 的 TF → /tf
↓
tf2_listener (本包) 查 base_link -> gripper,周期打印末端位姿。
URDF 通过 CommandLineAction 用 xacro/直接读 XML,
这里用 'command' 直接 cat 文件,launch 时读出来给 robot_state_publisher。
"""
import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
def generate_launch_description():
pkg_share = get_package_share_directory('cpp_robot_tf2')
urdf_path = os.path.join(pkg_share, 'urdf', 'simple_arm.urdf')
# robot_description 参数:把 URDF 文件内容作为字符串直接传入。
# 注意:不在 launch 时执行 shell 命令(launch 的 Command 列表拼接有时会
# 丢空格,产生 'cat/root/...' 之类错误);改为 launch 启动前直接读文件。
with open(urdf_path, 'r', encoding='utf-8') as f:
robot_description = f.read()
use_sim_time = LaunchConfiguration('use_sim_time')
return LaunchDescription([
DeclareLaunchArgument(
'use_sim_time', default_value='false',
description='Use simulation (Gazebo) clock if true'),
# 1) JointState 发布者:本包 C++ 节点,模拟关节运动。
Node(
package='cpp_robot_tf2',
executable='joint_state_publisher',
name='joint_state_publisher_cpp',
output='screen',
parameters=[{'use_sim_time': use_sim_time}],
),
# 2) robot_state_publisher:ROS2 系统包,URDF + JointState -> /tf。
Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
output='screen',
parameters=[{
'robot_description': robot_description,
'use_sim_time': use_sim_time,
}],
),
# 3) tf2_listener:本包 C++ 节点,周期打印 gripper 位姿。
Node(
package='cpp_robot_tf2',
executable='tf2_listener',
name='tf2_listener_cpp',
output='screen',
parameters=[{'use_sim_time': use_sim_time}],
),
])
src/cpp_robot_tf2/package.xml
+25
View File
@@ -0,0 +1,25 @@
<?xml version="1.0"?>
<?xml-model
href="http://download.ros.org/schema/package_format3.xsd"
schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>cpp_robot_tf2</name>
<version>0.1.0</version>
<description>C++ robot URDF + TF2 + JointState demo (3-link arm)</description>
<maintainer email="dev@example.com">xs</maintainer>
<license>Apache-2.0</license>
<depend>rclcpp</depend>
<depend>geometry_msgs</depend>
<depend>sensor_msgs</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>tf2_geometry_msgs</depend>
<test_depend>ament_cmake_gtest</test_depend>
<test_depend>launch_testing_ros</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
src/cpp_robot_tf2/src/joint_state_publisher.cpp
+93
View File
@@ -0,0 +1,93 @@
// =============================================================================
// joint_state_publisher.cpp —— ROS2 C++ JointState 发布者(机器人关节驱动)。
//
// 用途:
// 周期性发布 sensor_msgs/JointState 到 /joint_states,
// 让 robot_state_publisher(系统包)能算出 base_link -> link1 -> link2
// -> gripper 的 TF 链;下游 tf2_listener 可以 lookup_transform
// "base_link" -> "gripper" 得到夹爪在世界系下的位姿。
//
// 关节运动(模拟):
// joint1 = 0.5 * sin(t) (水平面摆动)
// joint2 = 0.3 * sin(t * 0.7) (俯仰)
// joint3 = 0.4 * cos(t * 1.2) (腕部)
//
// 真实机器人这一节点会订阅电机的编码器反馈,把实际关节角填进 msg.position。
//
// 运行:
// ros2 run cpp_robot_tf2 joint_state_publisher
// ros2 topic echo /joint_states # 看消息
// =============================================================================
#include <chrono>
#include <cmath>
#include <memory>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/joint_state.hpp"
using namespace std::chrono_literals;
class JointStatePublisher : public rclcpp::Node
{
public:
JointStatePublisher()
: rclcpp::Node("joint_state_publisher_cpp"), start_time_(this->now())
{
// 关节名顺序必须与 URDF 中 <joint name="..."> 一一对应。
joint_names_ = {"joint1", "joint2", "joint3"};
publisher_ = this->create_publisher<sensor_msgs::msg::JointState>("/joint_states", 10);
timer_ = this->create_wall_timer(50ms, std::bind(&JointStatePublisher::tick, this));
RCLCPP_INFO(this->get_logger(),
"joint_state_publisher_cpp started, joints: %zu", joint_names_.size());
}
private:
void tick()
{
auto msg = sensor_msgs::msg::JointState();
// header.stamp 必须设,robot_state_publisher 据此选最新的 JointState。
msg.header.stamp = this->now();
msg.header.frame_id = ""; // JointState 不需要 frame_id
msg.name = joint_names_;
// 计算从节点启动以来的 elapsed 时间(秒),用作三角函数相位。
const double t = (this->now() - start_time_).seconds();
// 三个关节位置 (rad),用 sin / cos 模拟连续运动。
msg.position = {
0.5 * std::sin(t * 1.0),
0.3 * std::sin(t * 0.7),
0.4 * std::cos(t * 1.2),
};
// 速度 = 解析求导 (关节角对 t 求导),仅供下游可视化用。
msg.velocity = {
0.5 * std::cos(t * 1.0),
0.21 * std::cos(t * 0.7),
-0.48 * std::sin(t * 1.2),
};
// 假设无外力,effort = 0(简化为匀速运动)。
msg.effort = {0.0, 0.0, 0.0};
publisher_->publish(msg);
}
std::vector<std::string> joint_names_;
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr publisher_;
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Time start_time_;
};
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<JointStatePublisher>());
rclcpp::shutdown();
return 0;
}
src/cpp_robot_tf2/src/tf2_listener.cpp
+80
View File
@@ -0,0 +1,80 @@
// =============================================================================
// tf2_listener.cpp —— ROS2 C++ TF2 监听器,周期查询"base_link -> gripper"。
//
// 用途:
// 把机械臂的"夹爪在哪"实时打出来。这是 VLA / 机器人感知的基础:
// 知道当前末端位姿,才能做目标点对齐、碰撞检测、视觉伺服等。
//
// TF2 关键概念:
// tf2::Buffer —— 时间缓冲,存储最近若干秒的 TF 树快照;
// tf2_ros::TransformListener(buffer):
// 订阅 /tf 与 /tf_static,在后台把变换塞进 buffer;
// buffer.lookup_transform(target_frame, source_frame, time):
// 查"在 time 时刻,source_frame 在 target_frame 下的位姿";
// time=rclcpp::Time(0) 表示"最新可用"。
//
// 运行:
// ros2 run cpp_robot_tf2 tf2_listener
// (前提:robot_state_publisher 已把 URDF + JointState 算成 TF 发布出来)
// =============================================================================
#include <chrono>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
using namespace std::chrono_literals;
class Tf2ListenerNode : public rclcpp::Node
{
public:
Tf2ListenerNode()
: rclcpp::Node("tf2_listener_cpp")
{
// tf2_ros::Buffer 默认保留 10 秒的历史变换,够 lookup 用了。
buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
// TransformListener 在节点构造函数里会订阅 /tf 与 /tf_static。
listener_ = std::make_shared<tf2_ros::TransformListener>(*buffer_, this);
timer_ = this->create_wall_timer(
500ms, std::bind(&Tf2ListenerNode::tick, this));
RCLCPP_INFO(this->get_logger(), "tf2_listener_cpp started");
}
private:
void tick()
{
try {
// rclcpp::Time(0) = latest available。try/catch 包住是因为
// 当 buffer 里还没收到该变换时,lookupTransform 抛 tf2::LookupException。
const auto tf = buffer_->lookupTransform(
"base_link", "gripper", tf2::TimePointZero);
RCLCPP_INFO(this->get_logger(),
"gripper in base_link: x=%.3f y=%.3f z=%.3f",
tf.transform.translation.x,
tf.transform.translation.y,
tf.transform.translation.z);
} catch (const tf2::TransformException & ex) {
// 启动初期 / TF 还没发布时这里会抛,只是 warning,不致命。
RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000,
"TF lookup failed: %s", ex.what());
}
}
std::shared_ptr<tf2_ros::Buffer> buffer_;
std::shared_ptr<tf2_ros::TransformListener> listener_;
rclcpp::TimerBase::SharedPtr timer_;
};
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<Tf2ListenerNode>());
rclcpp::shutdown();
return 0;
}
src/cpp_robot_tf2/test/test_tf2_lookup.cpp
+126
View File
@@ -0,0 +1,126 @@
// =============================================================================
// test_tf2_lookup.cpp —— cpp_robot_tf2 单元测试,验证 TF2 buffer + lookup 工具。
//
// 这里只测试 tf2 缓冲的"读写"语义,不依赖外部 JointState/URDF;
// 集成端到端验证留给 launch 文件 + docker e2e。
//
// 测试 1:广播一个静态变换 source_frame -> target_frame,
// 用 buffer.lookup_transform 取回,断言 translation 一致。
// 测试 2:几何变换应用:tf2::doTransform 把点 (1, 0, 0) 经该变换映射到目标系。
// =============================================================================
#include <chrono>
#include <thread>
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
class Tf2TestFixture : public ::testing::Test
{
protected:
static void SetUpTestSuite()
{
rclcpp::init(0, nullptr);
}
static void TearDownTestSuite()
{
rclcpp::shutdown();
}
};
// Test 1:广播静态变换,listener 应能 lookup 到。
TEST_F(Tf2TestFixture, StaticTransformRoundtrip)
{
auto node = rclcpp::Node::make_shared("tf2_test_node");
tf2_ros::Buffer buffer(node->get_clock());
tf2_ros::TransformListener listener(buffer, node);
tf2_ros::TransformBroadcaster broadcaster(node);
// 构造 source_frame = "world", target_frame = "robot",
// 平移 (1, 2, 3),无旋转。
geometry_msgs::msg::TransformStamped tf;
tf.header.stamp = node->now();
tf.header.frame_id = "world";
tf.child_frame_id = "robot";
tf.transform.translation.x = 1.0;
tf.transform.translation.y = 2.0;
tf.transform.translation.z = 3.0;
tf.transform.rotation.w = 1.0;
tf.transform.rotation.x = 0.0;
tf.transform.rotation.y = 0.0;
tf.transform.rotation.z = 0.0;
broadcaster.sendTransform(tf);
// spin 一小会儿让 listener 把变换塞进 buffer。
auto exec = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
exec->add_node(node);
auto end = std::chrono::steady_clock::now() + std::chrono::seconds(2);
while (std::chrono::steady_clock::now() < end) {
exec->spin_some(std::chrono::milliseconds(50));
}
// 查"robot 在 world 系下的位姿",期望 (1, 2, 3)。
try {
auto out = buffer.lookupTransform(
"world", "robot", tf2::TimePointZero);
EXPECT_NEAR(out.transform.translation.x, 1.0, 1e-3);
EXPECT_NEAR(out.transform.translation.y, 2.0, 1e-3);
EXPECT_NEAR(out.transform.translation.z, 3.0, 1e-3);
} catch (const tf2::TransformException & ex) {
FAIL() << "lookup failed: " << ex.what();
}
}
// Test 2:几何变换:点 (1, 0, 0) 经 world->robot 变换(平移 (1, 2, 3))
// 在 robot 坐标系下应是 (0, -2, -3)。
TEST_F(Tf2TestFixture, PointTransform)
{
auto node = rclcpp::Node::make_shared("tf2_test_node_2");
tf2_ros::Buffer buffer(node->get_clock());
tf2_ros::TransformListener listener(buffer, node);
tf2_ros::TransformBroadcaster broadcaster(node);
geometry_msgs::msg::TransformStamped tf;
tf.header.stamp = node->now();
tf.header.frame_id = "world";
tf.child_frame_id = "robot";
tf.transform.translation.x = 1.0;
tf.transform.translation.y = 2.0;
tf.transform.translation.z = 3.0;
tf.transform.rotation.w = 1.0;
broadcaster.sendTransform(tf);
auto exec = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
exec->add_node(node);
auto end = std::chrono::steady_clock::now() + std::chrono::seconds(2);
while (std::chrono::steady_clock::now() < end) {
exec->spin_some(std::chrono::milliseconds(50));
}
geometry_msgs::msg::PointStamped p_in, p_out;
p_in.header.frame_id = "world";
p_in.point.x = 1.0;
p_in.point.y = 0.0;
p_in.point.z = 0.0;
try {
p_out = buffer.transform(p_in, "robot");
EXPECT_NEAR(p_out.point.x, 0.0, 1e-3); // 1 - 1 = 0
EXPECT_NEAR(p_out.point.y, -2.0, 1e-3); // 0 - 2 = -2
EXPECT_NEAR(p_out.point.z, -3.0, 1e-3); // 0 - 3 = -3
} catch (const tf2::TransformException & ex) {
FAIL() << "transform failed: " << ex.what();
}
}
int main(int argc, char ** argv)
{
::testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
src/cpp_robot_tf2/urdf/simple_arm.urdf
+95
View File
@@ -0,0 +1,95 @@
<?xml version="1.0"?>
<!--
simple_arm.urdf —— 3 关节机械臂的简化 URDF 模型。
URDF (Unified Robot Description Format):
XML 格式的机器人描述,定义:
<link> —— 刚体段(质量、惯性、视觉/碰撞几何);
<joint> —— 关节(类型 + parent/child + 轴 + 限位);
<origin> —— 坐标系偏移(xyz 位置 + rpy 姿态);
<axis> —— 旋转关节的旋转轴。
我们的"3 关节臂":
base_link ──joint1(绕 z 旋转)──> link1
──joint2(绕 y 旋转)──> link2
──joint3(绕 y 旋转)──> gripper
关键尺寸:
- 每节连杆长度 0.1 m,叠在 z 轴上;
- joint1 转角控制 link1 在水平面内绕 z 旋转;
- joint2 / joint3 控制 link2 / gripper 在俯仰面内绕 y 旋转。
-->
<robot name="simple_arm">
<!-- ========== base_link ========== -->
<link name="base_link">
<visual>
<origin xyz="0 0 0.05" rpy="0 0 0"/>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
<material name="gray">
<color rgba="0.5 0.5 0.5 1.0"/>
</material>
</visual>
</link>
<!-- ========== joint1 + link1 ========== -->
<joint name="joint1" type="revolute">
<parent link="base_link"/>
<child link="link1"/>
<origin xyz="0 0 0.1" rpy="0 0 0"/>
<axis xyz="0 0 1"/>
<limit lower="-3.14159" upper="3.14159" effort="1.0" velocity="1.0"/>
</joint>
<link name="link1">
<visual>
<origin xyz="0 0 0.05" rpy="0 0 0"/>
<geometry>
<box size="0.05 0.05 0.1"/>
</geometry>
<material name="red">
<color rgba="0.9 0.2 0.2 1.0"/>
</material>
</visual>
</link>
<!-- ========== joint2 + link2 ========== -->
<joint name="joint2" type="revolute">
<parent link="link1"/>
<child link="link2"/>
<origin xyz="0 0 0.1" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<limit lower="-1.5708" upper="1.5708" effort="1.0" velocity="1.0"/>
</joint>
<link name="link2">
<visual>
<origin xyz="0 0 0.05" rpy="0 0 0"/>
<geometry>
<box size="0.05 0.05 0.1"/>
</geometry>
<material name="green">
<color rgba="0.2 0.8 0.2 1.0"/>
</material>
</visual>
</link>
<!-- ========== joint3 + gripper ========== -->
<joint name="joint3" type="revolute">
<parent link="link2"/>
<child link="gripper"/>
<origin xyz="0 0 0.1" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<limit lower="-1.5708" upper="1.5708" effort="1.0" velocity="1.0"/>
</joint>
<link name="gripper">
<visual>
<origin xyz="0 0 0.02" rpy="0 0 0"/>
<geometry>
<box size="0.04 0.04 0.04"/>
</geometry>
<material name="blue">
<color rgba="0.2 0.2 0.9 1.0"/>
</material>
</visual>
</link>
</robot>
src/py_action_demo/launch/action_launch.py
+23
View File
@@ -0,0 +1,23 @@
"""
action_launch.py —— 启动 fibonacci_action_server 的 launch 文件。
client 通常不在 launch 里启动,而是按需执行:
source install/setup.bash
ros2 run py_action_demo fibonacci_server # 先启
ros2 run py_action_demo fibonacci_client -- -o 8 # 再 client
ros2 action send_goal /fibonacci example_interfaces/action/Fibonacci "{order: 5}"
"""
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='py_action_demo',
executable='fibonacci_server',
name='fibonacci_action_server_py',
output='screen',
),
])
src/py_action_demo/package.xml
+24
View File
@@ -0,0 +1,24 @@
<?xml version="1.0"?>
<?xml-model
href="http://download.ros.org/schema/package_format3.xsd"
schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>py_action_demo</name>
<version>0.1.0</version>
<description>Python Action demo: Fibonacci server/client + launch</description>
<maintainer email="dev@example.com">xs</maintainer>
<license>Apache-2.0</license>
<depend>rclpy</depend>
<depend>action_msgs</depend>
<depend>example_interfaces</depend>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>
src/py_action_demo/py_action_demo/__init__.py
View File
src/py_action_demo/py_action_demo/fibonacci_client.py
+92
View File
@@ -0,0 +1,92 @@
"""
fibonacci_client.py —— ROS2 Python Action Client 演示。
Client API:
ActionClient(node, ActionType, action_name):
构造一个 client;需要在 spin 后 wait_for server 上线。
client.wait_for_server(timeout_sec=...):
阻塞等服务端上线;ROS2 发现约 1-2s。
goal = ActionType.Goal(); goal.field = ...
构造 Goal 消息。
future = client.send_goal_async(goal, feedback_callback=...)
异步发送 Goal,返回 Future;future.done() 后调用 result_callback。
feedback_callback(feedback_msg):
每收到一次服务端 Feedback 都会被调用,可用于实时显示进度。
"""
import sys
import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
from example_interfaces.action import Fibonacci
class FibonacciActionClient(Node):
def __init__(self):
super().__init__('fibonacci_action_client_py')
self._action_client = ActionClient(self, Fibonacci, 'fibonacci')
# 注意:不在 __init__ 阻塞 wait_for_server()!
# wait_for_server 阻塞会让事件循环停下来,server 的 ActionServer
# 没法 spin 注册 DDS 节点,死锁。改为 server_is_ready() + 主循环 spin。
self._server_ready = False
self.get_logger().info('Fibonacci action client ready (waiting for server)')
def wait_for_server(self, timeout_sec: float = 5.0) -> bool:
"""非阻塞:每次调用 server_is_ready() 后让出,直到 timeout 或就绪。"""
import time as _t
end = _t.time() + timeout_sec
while _t.time() < end:
if self._action_client.server_is_ready():
self._server_ready = True
return True
_t.sleep(0.05)
return False
def send_goal(self, order):
goal_msg = Fibonacci.Goal()
goal_msg.order = order
self._send_goal_future = self._action_client.send_goal_async(
goal_msg,
feedback_callback=self.feedback_callback,
)
self._send_goal_future.add_done_callback(self.goal_response_callback)
def goal_response_callback(self, future):
# 服务端 accept/reject 后被回调;future.result() 是 GoalHandle。
goal_handle = future.result()
if not goal_handle.accepted:
self.get_logger().warn('Goal rejected by server')
return
self.get_logger().info('Goal accepted, waiting for result...')
# get_result_async:等服务端 succeed/abort/cancel 后被回调。
self._get_result_future = goal_handle.get_result_async()
self._get_result_future.add_done_callback(self.get_result_callback)
def feedback_callback(self, feedback_msg):
# feedback_msg 是 Fibonacci_FeedbackMessage(带 feedback 子字段);
# 真正的序列字段在 msg.feedback.sequence(Feedback.message 字段名是 'sequence')。
seq = list(feedback_msg.feedback.sequence)
self.get_logger().info(f'feedback: {seq}')
def get_result_callback(self, future):
# result 是 Fibonacci.Result,字段 sequence 是最终完整序列。
result = future.result().result
seq = list(result.sequence)
self.get_logger().info(f'FINAL RESULT: sequence={seq}')
def main(args=None):
rclpy.init(args=args)
node = FibonacciActionClient()
order = int(sys.argv[1]) if len(sys.argv) > 1 else 5
node.send_goal(order)
rclpy.spin(node)
if __name__ == '__main__':
main()
src/py_action_demo/py_action_demo/fibonacci_server.py
+113
View File
@@ -0,0 +1,113 @@
"""
fibonacci_server.py —— ROS2 Python Action Server 演示(Fibonacci 序列)。
ROS2 Action 与 Service 的区别:
Service —— 一次性 req/resp,适合"快速问答",几毫秒级;
Action —— 长任务,client 发 Goal,server 边做边周期性反馈 Feedback,
最终返回 Result;client 可中途 cancel。
适合场景:机械臂抓取、SLAM 建图、路径规划等"几分钟到几小时"的任务。
Action 三件套 (Goal / Feedback / Result):
Goal —— 客户端发起任务时携带的输入;
Feedback —— 服务端周期性回报的中间进度(0..N 次);
Result —— 任务最终完成时的产出(一次)。
本 demo 用 ROS2 内置 example_interfaces/action/Fibonacci:
Goal: int32 order # 要生成第几项 Fibonacci
Feedback: int32[] partial_sequence # 当前已算出的部分序列
Result: int32[] sequence # 完整序列
运行:
ros2 run py_action_demo fibonacci_server
ros2 run py_action_demo fibonacci_client -- -o 8
"""
import time
import rclpy
from rclpy.action import ActionServer
from rclpy.node import Node
from rclpy.executors import MultiThreadedExecutor
from example_interfaces.action import Fibonacci
class FibonacciActionServer(Node):
def __init__(self):
super().__init__('fibonacci_action_server_py')
# ActionServer(node, ActionType, action_name, execute_callback):
# - ActionType:由 .action 自动生成的 Python 类(含 Goal/Feedback/Result 子类);
# - action_name:client 用 'ros2 action send_goal <action_name> ...' 调用;
# - execute_callback:ROS2 接收到 Goal 时调用,签名:
# callback(goal_handle) -> result
# goal_handle 提供:
# - goal_handle.accept() / reject():同意/拒绝 Goal;
# - goal_handle.publish_feedback(msg):周期性反馈;
# - goal_handle.is_cancel_requested:client 是否请求取消;
# - goal_handle.succeed() / abort() / canceled():终态回报。
self._action_server = ActionServer(
self,
Fibonacci,
'fibonacci',
self.execute_callback,
)
self.get_logger().info('fibonacci_action_server_py ready')
def execute_callback(self, goal_handle):
# 1) 读取 Goal 字段
order = goal_handle.request.order
self.get_logger().info(f'Received goal: order={order}')
# 2) 构造 Feedback 与 Result 消息
feedback_msg = Fibonacci.Feedback()
result_msg = Fibonacci.Result()
# 3) 边做边反馈:Fibonacci 序列生成。
# 每生成一项 sleep 0.5s 模拟"耗时任务",期间把当前序列作为
# Feedback 推回给 client,client 端就能实时看到进度。
#
# 字段名说明:ROS2 example_interfaces/action/Fibonacci 中,
# Feedback 字段名为 'sequence'(不是 partial_sequence),
# Result 字段名也是 'sequence'(最终完整序列)。
sequence = [0, 1]
for i in range(1, order):
# 检查 client 是否请求取消(随时可中断)
if goal_handle.is_cancel_requested:
goal_handle.canceled()
self.get_logger().info('Goal canceled by client')
return Fibonacci.Result()
sequence.append(sequence[i] + sequence[i - 1])
feedback_msg.sequence = sequence # ← 注意字段名
goal_handle.publish_feedback(feedback_msg)
time.sleep(0.5)
# 4) 任务成功完成,告诉 client 最终 Result
goal_handle.succeed()
result_msg.sequence = sequence
self.get_logger().info(f'Goal succeeded, sequence={sequence}')
return result_msg
def main(args=None):
rclpy.init(args=args)
node = FibonacciActionServer()
# Action server 内部用了多个回调线程(MultiThreadedExecutor 是 ROS2
# 官方推荐配合 ActionServer 的 executor,避免反馈与执行互锁)。
executor = MultiThreadedExecutor()
executor.add_node(node)
try:
executor.spin()
except KeyboardInterrupt:
pass
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
src/py_action_demo/resource/py_action_demo
View File
src/py_action_demo/setup.cfg
+4
View File
@@ -0,0 +1,4 @@
[develop]
script_dir=$base/lib/py_action_demo
[install]
install_scripts=$base/lib/py_action_demo
src/py_action_demo/setup.py
+28
View File
@@ -0,0 +1,28 @@
from setuptools import find_packages, setup
package_name = 'py_action_demo'
setup(
name=package_name,
version='0.1.0',
packages=find_packages(exclude=['test']),
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
('share/' + package_name + '/launch', ['launch/action_launch.py']),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='xs',
maintainer_email='dev@example.com',
description='Python Action demo: Fibonacci server/client + launch',
license='Apache-2.0',
tests_require=['pytest'],
entry_points={
'console_scripts': [
'fibonacci_server = py_action_demo.fibonacci_server:main',
'fibonacci_client = py_action_demo.fibonacci_client:main',
],
},
)
src/py_action_demo/test/test_action.py
+63
View File
@@ -0,0 +1,63 @@
"""py_action_demo 单元测试:同进程内 Action server + client 跑通 Fibonacci。
测试流程:
1) FibonacciActionServer + FibonacciActionClient 起来;
2) Client 发 Goal order=5;
3) 在 MultiThreadedExecutor 里 spin,直到 result ready;
4) 断言收到的最终 sequence == [0, 1, 1, 2, 3, 5]。
注意:不再让 client 在 callback 里调 rclpy.shutdown(),
否则 fixture 末尾的 rclpy.shutdown() 会抛
"Context must be initialized before it can be shutdown"。
"""
import time
import rclpy
import pytest
from py_action_demo.fibonacci_server import FibonacciActionServer
from py_action_demo.fibonacci_client import FibonacciActionClient
@pytest.fixture(scope='module')
def ros_context():
rclpy.init()
yield
rclpy.shutdown()
def test_fibonacci_inproc_roundtrip(ros_context):
"""order=5 → 序列应为 [0, 1, 1, 2, 3, 5]。"""
server = FibonacciActionServer()
client = FibonacciActionClient()
exec_ = rclpy.executors.MultiThreadedExecutor()
exec_.add_node(server)
exec_.add_node(client)
# 让 server 先 spin 几秒注册到 DDS,再让 client 等 server 就绪。
end = time.time() + 3.0
while time.time() < end and not client.wait_for_server(timeout_sec=0.1):
exec_.spin_once(timeout_sec=0.05)
assert client._server_ready, 'action server did not become ready'
client.send_goal(5)
# spin 直到 _get_result_future 完成,最多 8s。
end = time.time() + 8.0
while rclpy.ok() and time.time() < end:
exec_.spin_once(timeout_sec=0.1)
if getattr(client, '_get_result_future', None) and client._get_result_future.done():
for _ in range(5):
exec_.spin_once(timeout_sec=0.05)
break
assert getattr(client, '_get_result_future', None), \
'goal was rejected (no _get_result_future)'
assert client._get_result_future.done(), \
'client did not receive result within 8s'
result = client._get_result_future.result().result
seq = list(result.sequence)
# Fibonacci(5) = 0, 1, 1, 2, 3, 5
assert seq == [0, 1, 1, 2, 3, 5], f'unexpected sequence: {seq}'
src/py_pubsub/launch/pubsub_launch.py
+54
View File
@@ -0,0 +1,54 @@
"""
pubsub_launch.py —— 启动一个 talker_py + listener_py 的最小 launch 文件。
什么是 launch?
ROS2 里,launch = 一种"启动多个节点 + 配置参数 + 跨机器调度"的
Python 描述文件(取代了 ROS1 的 XML launch)。
由 `ros2 launch <pkg> <launch_file.py>` 调用。
关键概念:
LaunchDescription —— 顶层容器,里面装一组"动作(Action)"。
Node —— launch_ros.actions.Node,声明一个要运行的节点。
DeclareLaunchArgument —— 声明命令行可覆盖的参数(--xxx)。
LaunchConfiguration —— 运行时再取该参数的当前值。
IncludeLaunchDescription —— 把另一个 .launch.py 嵌套进来。
运行:
source install/setup.bash
ros2 launch py_pubsub pubsub_launch.py
ros2 launch py_pubsub pubsub_launch.py topic:=hello # 改 topic
"""
# launch:ROS2 官方 launch 框架,提供 LaunchDescription/Action 等基础类。
from launch import LaunchDescription
# launch_ros.actions.Node:声明"运行一个 ROS2 节点"的动作。
from launch_ros.actions import Node
def generate_launch_description():
"""ROS2 要求 launch 文件导出 generate_launch_description() 函数。
launch 系统加载本文件时,会调用它返回 LaunchDescription 实例。
"""
return LaunchDescription([
# 启动 talker_py (Python 发布者,周期 500ms)
Node(
package='py_pubsub', # 节点所在的 ROS 包名。
executable='talker', # 节点的可执行文件名(setup.py entry_points 里)。
name='talker_py', # 在 ROS Domain 中重命名(避免和 cpp 的 talker 重名)。
output='screen', # 标准输出/错误直接显示到终端(默认会重定向到日志)。
parameters=[{ # 透传 ROS 参数给节点,等价于 --ros-args -p k:=v ...
'period_ms': 500, # 话题发布间隔。
'topic': 'chatter', # 目标 topic。
}],
),
# 同时启动 listener_py (Python 订阅者)订阅同一 topic。
Node(
package='py_pubsub',
executable='listener',
name='listener_py',
output='screen',
parameters=[{
'topic': 'chatter',
}],
),
])
src/py_pubsub/package.xml
+35
View File
@@ -0,0 +1,35 @@
<?xml version="1.0"?>
<!--
package.xml —— ROS 包的元数据清单,定义:
包名 / 版本 / 维护人 / 许可证 / 构建依赖 / 运行依赖 / 测试依赖。
format="3" 对应 ament 时代的最新格式;
字段取自 http://download.ros.org/schema/package_format3.xsd。
-->
<?xml-model
href="http://download.ros.org/schema/package_format3.xsd"
schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<!-- ROS 包的唯一标识,必须与目录名 / setup.py / CMakeLists 一致。 -->
<name>py_pubsub</name>
<version>0.1.0</version>
<description>Python talker/listener demo for ROS2 Humble</description>
<maintainer email="dev@example.com">xs</maintainer>
<license>Apache-2.0</license>
<!-- depend:构建 + 运行 + 导出时都需要(对应 ament_python 的 setup.py 隐式依赖)。 -->
<depend>rclpy</depend> <!-- Python 客户端库。 -->
<depend>std_msgs</depend> <!-- 内置基础消息,如 std_msgs/String。 -->
<!-- test_depend:仅在 colcon test 时用到,不写不会影响 build / run。 -->
<test_depend>ament_copyright</test_depend> <!-- Apache-2.0 license header 检查 -->
<test_depend>ament_flake8</test_depend> <!-- Python 代码风格 -->
<test_depend>ament_pep257</test_depend> <!-- docstring 风格 -->
<test_depend>python3-pytest</test_depend> <!-- Python 单元测试框架 -->
<test_depend>launch_testing</test_depend> <!-- ROS2 launch 集成测试 -->
<test_depend>launch_testing_ros</test_depend>
<!-- export/build_type = ament_python:声明 colcon 该用 Python 构建链。 -->
<export>
<build_type>ament_python</build_type>
</export>
</package>
src/py_pubsub/py_pubsub/__init__.py
View File
src/py_pubsub/py_pubsub/publisher_member_function.py
+128
View File
@@ -0,0 +1,128 @@
"""
talker_py — ROS2 Python 发布者节点(Publisher)
ROS2 核心概念速览:
Node (节点): 一个独立运行的可执行单元,负责业务逻辑;本文件就是
一个叫 talker_py 的节点。
Topic (话题): 节点间基于发布/订阅(pub/sub)的异步通信通道,
是一种多对多的"消息总线",通过话题名(如 chatter)匹配。
Publisher (发布者): 往 Topic 发送数据的对象。
Message (消息): 在 Topic 上传递的数据载体,通常是 .msg 自动生成的
类型,这里用 std_msgs/String。
Timer (定时器): ROS2 提供的周期性回调,在节点的事件循环里触发,
用于定时发布传感器数据、状态等。
Parameter (参数): 节点可声明、可动态改写的配置项,方便不重编
代码就调参(经 launch 文件、命令行、ros2 param set 修改)。
运行方式:
ros2 run py_pubsub talker # 用默认参数
ros2 run py_pubsub talker --ros-args -p period_ms:=200 -p topic:=hello
ros2 launch py_pubsub pubsub_launch.py # 同时启动 talker + listener
可见工具:
ros2 node list # 查看运行中的节点
ros2 topic list # 查看 Topic 列表
ros2 topic echo chatter # 实时打印 chatter 消息
ros2 topic hz chatter # 统计 Topic 频率
"""
# rclpy: ROS Client Library for Python —— Python 写 ROS2 节点的标准入口库,
# 屏蔽 DDS 中间件的细节,把 C++ 的 rclcpp 能力以面向对象方式映射到 Python。
import rclpy
# Node 是 rclpy 里所有 ROS2 Python 节点的基类,提供 publisher/subscription/
# timer/parameter/log 接口。
from rclpy.node import Node
# std_msgs 是 ROS2 自带的基础消息包;String 是其中最简单的一种,只含 data 字段。
from std_msgs.msg import String
class Talker(Node):
"""Talker 是一个发布节点,定时向 topic 'chatter' 发布 String 消息。"""
def __init__(self):
# super().__init__('talker_py'): 给节点起名为 'talker_py'。
# 节点名在同一 ROS Domain 内必须唯一,重复会冲突报错。
super().__init__('talker_py')
# declare_parameter: 在节点上声明一个 ROS 参数(含默认值),
# 让节点行为可在运行时通过命令行 / launch / ros2 param set 改写,
# 不必改代码、不必重编译。
self.declare_parameter('period_ms', 500) # 发布周期,毫秒
self.declare_parameter('topic', 'chatter') # 目标话题名
# 读取已声明的参数值(get_parameter 的返回值需要 .get_parameter_value()
# 才能拿到原始类型,再按 .integer_value / .string_value 取值)。
period = self.get_parameter('period_ms').get_parameter_value().integer_value
topic = self.get_parameter('topic').get_parameter_value().string_value
# create_publisher(类型, 话题名, QoS深度):
# - 类型: 该 Publisher 发出的消息类 (std_msgs/String)。
# - 话题名: 与订阅端的 topic 一致才能匹配通信。
# - QoS深度 10: 队列最多缓存 10 条未送达的样本 (用于慢消费者)。
# 返回 Publisher 对象,通过 .publish(msg) 投递消息。
self.publisher_ = self.create_publisher(String, topic, 10)
# create_timer(周期秒, 回调): 把回调挂到节点事件循环,以固定
# 周期触发。注意 period 单位是秒,所以这里 /1000 转换。
self.timer = self.create_timer(period / 1000.0, self.timer_callback)
self.count = 0 # 递增序号,用于在消息里看出是否连续、是否丢消息。
# get_logger() 拿节点专属日志;按严重级别 (info/warn/error) 输出。
# ROS2 默认会把日志同时打到终端和 ros2 topic echo /rosout。
self.get_logger().info(f'talker_py started -> topic={topic}, period={period}ms')
def timer_callback(self):
# 这是定时器回调,每次触发就构造一条新消息并发布。
# 1) 实例化消息对象,赋值 data 字段 (std_msgs/String 只有这一个字段)。
msg = String()
msg.data = f'Hello from PY, seq={self.count}'
# 2) publisher_.publish: 把消息投到 DDS 总线,所有订阅了 chatter 的
# Subscriber 都会收到(异步,非阻塞)。
self.publisher_.publish(msg)
# 3) 顺手打一条日志,便于调试观察节奏。
self.get_logger().info(f'pub: "{msg.data}"')
# 4) 计数加一,让下次消息能看到序号。
self.count += 1
def main(args=None):
"""ROS2 Python 节点的标准入口模板,任何节点 main() 都大致长这样。
步骤:
1) rclpy.init() —— 初始化 rclpy 与底层 DDS。
2) Node 实例化 —— 启动节点,内部跑事件循环线程。
3) rclpy.spin(node) —— 进入"阻塞 + 事件循环",由 ROS2 调度
所有回调(timer / subscription / service) ,
直到收到 KeyboardInterrupt(Ctrl+C)才返回。
4) destroy_node() —— 释放节点资源(publisher/sub/timer 句柄)。
5) rclpy.shutdown() —— 关闭 rclpy 全局状态。
"""
# init 接收 args,允许外部程序在 ROS2 之上再传 --ros-args 给 ROS。
rclpy.init(args=args)
# 构造节点对象,完成 publisher/timer/logger 等初始化。
node = Talker()
try:
# spin() 会阻塞,直到 Ctrl+C 或者 shutdown。
rclpy.spin(node)
except KeyboardInterrupt:
# Ctrl+C 时安静退出,避免栈信息被打到日志里。
pass
# 善后:如果不调用,节点对象在进程退出时会被 GC,一般无副作用,
# 但显式释放是 ROS2 推荐写法。
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
# 当本文件被 `python3 xxx.py` 直接运行时,走 main()。
# `ros2 run` 时通过 setup.py 的 entry_points(console_scripts)
# 也是调用 main();二者本质相同。
main()
src/py_pubsub/py_pubsub/subscriber_member_function.py
+72
View File
@@ -0,0 +1,72 @@
"""
listener_py — ROS2 Python 订阅者节点(Subscriber)
配合 talker_py 演示发布/订阅(pub/sub)通信模型:
一个或多个 Publisher 发消息 -> Topic 总线 -> 一个或多个 Subscriber 接收,
双方解耦,不需要知道对方是否存在。
ROS2 关键概念:
Subscription (订阅): node.create_subscription(...) 创建的对象,
负责在指定 Topic 上监听消息;每收到一条消息自动触发回调函数。
回调接收到的 msg 是 std_msgs/msg/String 的实例, msg.data 即真实数据。
运行:
ros2 run py_pubsub listener
ros2 run py_pubsub listener --ros-args -p topic:=chatter
可见工具:
ros2 topic info chatter -v # 看 pub/sub 端和它们的消息类型
rqt_plot # 把数值字段绘成折线图(需选 String 字段不支持,改用 Echo)
"""
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
class Listener(Node):
"""Listener 是一个订阅节点,接收 topic 'chatter' 上的 String 消息并打印。"""
def __init__(self):
# 起名 'listener_py',与 talker_py 同 ROS Domain 下也不能重名。
super().__init__('listener_py')
# 同样声明一个 topic 参数,便于和 talker 配对切换(默认都是 'chatter')。
self.declare_parameter('topic', 'chatter')
topic = self.get_parameter('topic').get_parameter_value().string_value
# create_subscription(类型, 话题名, 回调, QoS深度):
# - 类型: 必须与 Publisher 发出的消息类一致,否则 rclpy 会校验失败。
# - 话题名: 一致才能收到对应 Publisher 的消息。
# - 回调: 收到消息时由节点事件循环自动调用,签名 callback(msg)。
# - QoS 深度: 队列容量,与 publisher 协商,通常 10 够用。
#
# 这里把 self.listener_callback 当回调传入(注意不要带括号),
# 它每收到一条消息都会被 ROS2 调用一次。
self.subscription = self.create_subscription(
String, topic, self.listener_callback, 10)
self.get_logger().info(f'listener_py subscribed <- {topic}')
def listener_callback(self, msg):
# 收到消息后的处理函数。msg 类型是 std_msgs/msg/String, msg.data 是字符串。
# 这里只演示"打印日志";真实节点常做解析、坐标变换、入队缓冲、下发指令等。
self.get_logger().info(f'recv: "{msg.data}"')
def main(args=None):
"""节点入口,模板与 talker_py 完全一致。"""
rclpy.init(args=args)
node = Listener()
try:
# spin() 跑事件循环,timer/subscription/service 都会在该循环里被触发。
rclpy.spin(node)
except KeyboardInterrupt:
pass
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
src/py_pubsub/resource/py_pubsub
View File
src/py_pubsub/setup.cfg
+4
View File
@@ -0,0 +1,4 @@
[develop]
script_dir=$base/lib/py_pubsub
[install]
install_scripts=$base/lib/py_pubsub
src/py_pubsub/setup.py
+73
View File
@@ -0,0 +1,73 @@
# =============================================================================
# setup.py —— ament_python 包的安装清单,等同于"Python 项目的 setup.py"。
#
# 与 ROS1 不同,ROS2 采用 setuptools + ament_python 协同:
# - 标准 Python setup() 描述包元数据;
# - data_files 把"ROS 标签"文件(package.xml、ament 索引、launch 等)
# 装到 install/<pkg>/share/<pkg>/,被 colcon 和 ros2 工具识别。
#
# colcon build 调用链: colcon → ament_python build_type → python setup.py build
# install → install/bin + install/lib/...
# + install/<pkg>/share/<pkg>/
# =============================================================================
# find_packages:setuptools 工具,自动扫描当前目录下含 __init__.py 的目录,
# 收录为要安装的 Python 包。
# setup:setuptools 的入口函数,描述包元数据 / 文件 / 入口点。
from setuptools import find_packages, setup
# 这个 ROS 包的"名字"——必须与 package.xml 中的 <name> 一致,这是 ament
# 索引文件 resource/<package_name> 用到的标识。
package_name = 'py_pubsub'
setup(
# 包名,暴露给 pip / colcon 用以识别。
name=package_name,
version='0.1.0',
# find_packages(exclude=['test']):
# 自动发现 "py_pubsub/" 这个 Python 子包,
# "test/" 目录被排除,以免污染运行时。
packages=find_packages(exclude=['test']),
# data_files:(target_dir, [src_file, ...]) 元组列表。
# 描述"额外要装的非 .py 文件",ament 强制要求三类:
# 1) ament 索引标记 ← resource/<pkg>
# 2) 元数据 package.xml ← share/<pkg>/
# 3) launch 文件 ← share/<pkg>/launch/
data_files=[
# 让 `ros2 pkg prefix py_pubsub` 能找到本包路径(ament_index 必备)。
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
# 把 package.xml 拷到 share/<pkg>/,ros2 工具靠它识别包元数据。
('share/' + package_name, ['package.xml']),
# launch 文件必须装到 share/<pkg>/launch/ 下,
# `ros2 launch py_pubsub pubsub_launch.py` 才能找到。
('share/' + package_name + '/launch', ['launch/pubsub_launch.py']),
],
# 仅依赖 setuptools(ROS 基础能力由 apt 安装的 rclpy 提供,不需列在 install_requires)。
install_requires=['setuptools'],
# 安装时把所有 .py 编到 egg,zip-safe 让 colcon 能直接用源码/egg。
zip_safe=True,
maintainer='xs',
maintainer_email='dev@example.com',
description='Python talker/listener demo for ROS2 Humble',
license='Apache-2.0',
# 标记这是 Python 测试依赖(本项目未配测试,可保留以备扩展)。
tests_require=['pytest'],
# entry_points/console_scripts:
# 列出本包提供的"命令行可执行"。`ros2 run py_pubsub talker` 会
# 在该列表里找 'talker',实际指向 py_pubsub.publisher_member_function.main
# (注意 main() 的位置必须是模块级函数,被 import 调用)。
entry_points={
'console_scripts': [
# 左:ros2 run 后跟的可执行名;右:模块路径:main() 函数
'talker = py_pubsub.publisher_member_function:main',
'listener = py_pubsub.subscriber_member_function:main',
],
},
)
src/py_pubsub/test/test_pubsub_launch.py
+88
View File
@@ -0,0 +1,88 @@
"""py_pubsub 单元测试:在测试进程内直接验证 Talker / Listener 节点。
测试策略:
测试 1-2: 验证节点对象创建、参数声明、节点名等"硬契约"。
测试 3: 同进程 spin(单线程 executor)Talker + Listener,验证
Listener 在 ~1s 内能收到至少 1 条 chatter 消息。
"""
import time
import rclpy
import pytest
from py_pubsub.publisher_member_function import Talker
from py_pubsub.subscriber_member_function import Listener
from std_msgs.msg import String
@pytest.fixture(scope='module')
def ros_context():
"""rclpy 是进程级单例,在模块所有测试前 init,完成后 shutdown。"""
rclpy.init()
yield
rclpy.shutdown()
def test_talker_init_default_params(ros_context):
"""Talker 默认参数应为 period_ms=500, topic='chatter', 节点名 'talker_py'。"""
node = Talker()
assert node.get_name() == 'talker_py'
period = node.get_parameter('period_ms').get_parameter_value().integer_value
topic = node.get_parameter('topic').get_parameter_value().string_value
assert period == 500
assert topic == 'chatter'
# publisher 已注册(topic='chatter', type=String)。
assert node.publisher_ is not None
def test_listener_init_default_params(ros_context):
"""Listener 默认参数 topic='chatter',节点名 'listener_py'。"""
node = Listener()
assert node.get_name() == 'listener_py'
topic = node.get_parameter('topic').get_parameter_value().string_value
assert topic == 'chatter'
assert node.subscription is not None
def test_talker_publishes_one_message(ros_context):
"""Talker.timer_callback 调用一次后,内部 count 自增,逻辑不抛异常。"""
node = Talker()
before = node.count
node.timer_callback()
assert node.count == before + 1
def test_inproc_roundtrip(ros_context):
"""同进程 spin:Talker 发布 + Listener 订阅,1s 内 listener 至少收 1 条。
走 SingleThreadedExecutor 而非 launch_testing:避免 launch 系统
shutdown 二次调用的兼容问题,且 colcon test 环境下更稳。
"""
talker = Talker()
listener = Listener()
# 让 talker timer 更密,以便 1s 内能产生足够消息。
talker.destroy_timer(talker.timer)
talker.timer = talker.create_timer(0.05, talker.timer_callback)
received = []
listener.subscription = listener.create_subscription(
String,
'chatter',
lambda msg: received.append(msg.data),
10,
)
exec_ = rclpy.executors.SingleThreadedExecutor()
exec_.add_node(talker)
exec_.add_node(listener)
end = time.time() + 1.0
while time.time() < end:
exec_.spin_once(timeout_sec=0.05)
# 期望至少收到若干消息(seq 0..N,周期 50ms,1s 内约 20 条)。
assert len(received) >= 1, f'expected >=1 received, got 0'
# 收到的内容必须是 Talker 发送的 'Hello from PY'。
assert any('Hello from PY' in s for s in received), \
f'received messages do not contain Talker marker: {received[:3]}'
src/py_srv/launch/srv_launch.py
+23
View File
@@ -0,0 +1,23 @@
"""
srv_launch.py —— 启动 add_two_ints 服务端的 launch 文件。
注意:client 不在这里启动,因它是"一次性调用",通常在终端或脚本里跑。
client 用法:
source install/setup.bash
ros2 run py_srv add_two_ints_client 1 2 # 默认参数
ros2 service call /add_two_ints example_interfaces/srv/AddTwoInts "{a: 3, b: 5}"
"""
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='py_srv',
executable='add_two_ints_server',
name='add_two_ints_server_py',
output='screen',
),
])
src/py_srv/package.xml
+23
View File
@@ -0,0 +1,23 @@
<?xml version="1.0"?>
<?xml-model
href="http://download.ros.org/schema/package_format3.xsd"
schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>py_srv</name>
<version>0.1.0</version>
<description>Python service demo: AddTwoInts server + client</description>
<maintainer email="dev@example.com">xs</maintainer>
<license>Apache-2.0</license>
<depend>rclpy</depend>
<depend>example_interfaces</depend>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>
src/py_srv/py_srv/__init__.py
View File
src/py_srv/py_srv/add_two_ints_client.py
+64
View File
@@ -0,0 +1,64 @@
"""
add_two_ints_client.py —— ROS2 Python 服务端调用方(Service Client)。
Client API:
Client = node.create_client(srv_type, srv_name):
创建一个"客户端",绑定到指定 srv_name 的服务端。
client.wait_for_service(timeout_sec=...) ->
阻塞等服务端上线;ROS2 网络发现需要 1-2s。
request = srv_type.Request():
构造请求对象,填写字段。
future = client.call_async(request) -> Future:
异步调用,返回 Future;不阻塞主线程。
rclpy.spin_until_future_complete(node, future, timeout_sec=...):
在节点事件循环里等 future 完成(或超时),取回 response。
"""
import sys
import rclpy
from rclpy.node import Node
from example_interfaces.srv import AddTwoInts
class AddTwoIntsClient(Node):
def __init__(self):
super().__init__('add_two_ints_client_py')
self.client = self.create_client(AddTwoInts, 'add_two_ints')
# wait_for_service:阻塞到服务端上线或超时(秒)。
# 这里给 5 秒,通常 1-2s 内 ROS2 完成 DDS discovery。
while not self.client.wait_for_service(timeout_sec=1.0):
self.get_logger().info('waiting for add_two_ints service...')
def call(self, a, b):
req = AddTwoInts.Request()
req.a = a
req.b = b
# call_async 返回 Future,由 spin_until_future_complete 触发回调。
future = self.client.call_async(req)
# spin_until_future_complete:在节点事件循环里"短轮询"等到 future 完成。
rclpy.spin_until_future_complete(self, future, timeout_sec=5.0)
if future.result() is not None:
return future.result().sum
self.get_logger().error('service call failed')
return None
def main(args=None):
rclpy.init(args=args)
node = AddTwoIntsClient()
# 从命令行参数读取两个整数;默认 1 + 2。
a = int(sys.argv[1]) if len(sys.argv) > 1 else 1
b = int(sys.argv[2]) if len(sys.argv) > 2 else 2
result = node.call(a, b)
if result is not None:
node.get_logger().info(f'result: {a} + {b} = {result}')
print(f'RESULT: {a} + {b} = {result}')
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
src/py_srv/py_srv/add_two_ints_server.py
+57
View File
@@ -0,0 +1,57 @@
"""
add_two_ints_server.py —— ROS2 Python 服务端(Service Server)。
ROS2 Service 是另一种通信模式,与 Topic 的区别:
Topic — 多对多、无连接、异步、单向(pub/sub)。
Service — 一对一、有连接、同步(可异步)、双向(request/response)。
适合"一次性调用 + 等结果"的场景,如拍照、开关机械臂、计算查询等。
关键 API:
Service[T_Request, T_Response]:
节点上的"服务句柄",由 create_service(...) 构造。
T_Request / T_Response:由 .srv 文件自动生成的 Python 类型;
本 demo 用 example_interfaces/srv/AddTwoInts,
含 a, b(int64) 与 sum(int64) 三个字段。
回调签名:
handler(req, response) -> response
handler 内部填 response 字段,ROS2 把它发回给 client。
"""
import rclpy
from rclpy.node import Node
from example_interfaces.srv import AddTwoInts
class AddTwoIntsServer(Node):
def __init__(self):
super().__init__('add_two_ints_server_py')
# create_service(srv_type, srv_name, callback):
# - srv_type:服务接口类(本例 AddTwoInts);
# - srv_name:客户端调用时使用的服务名;
# - callback:收到请求时由 ROS2 事件循环调用,
# 返回值就是发回 client 的 response。
self.srv = self.create_service(
AddTwoInts, 'add_two_ints', self.add_two_ints_callback)
self.get_logger().info('add_two_ints_server_py ready, waiting for requests...')
def add_two_ints_callback(self, request, response):
# request.a / request.b 是 AddTwoInts.Request 的 int64 字段。
response.sum = request.a + request.b
self.get_logger().info(f'incoming: a={request.a}, b={request.b} -> sum={response.sum}')
return response
def main(args=None):
rclpy.init(args=args)
node = AddTwoIntsServer()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
src/py_srv/resource/py_srv
View File
src/py_srv/setup.cfg
+4
View File
@@ -0,0 +1,4 @@
[develop]
script_dir=$base/lib/py_srv
[install]
install_scripts=$base/lib/py_srv
src/py_srv/setup.py
+28
View File
@@ -0,0 +1,28 @@
from setuptools import find_packages, setup
package_name = 'py_srv'
setup(
name=package_name,
version='0.1.0',
packages=find_packages(exclude=['test']),
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
('share/' + package_name + '/launch', ['launch/srv_launch.py']),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='xs',
maintainer_email='dev@example.com',
description='Python service demo: AddTwoInts server + client',
license='Apache-2.0',
tests_require=['pytest'],
entry_points={
'console_scripts': [
'add_two_ints_server = py_srv.add_two_ints_server:main',
'add_two_ints_client = py_srv.add_two_ints_client:main',
],
},
)
src/py_srv/test/test_srv.py
+43
View File
@@ -0,0 +1,43 @@
"""py_srv 单元测试:同进程 spin + 验证 Service 通信。"""
import time
import rclpy
import pytest
from py_srv.add_two_ints_server import AddTwoIntsServer
from py_srv.add_two_ints_client import AddTwoIntsClient
from example_interfaces.srv import AddTwoInts
@pytest.fixture(scope='module')
def ros_context():
rclpy.init()
yield
rclpy.shutdown()
def test_service_inproc_roundtrip(ros_context):
"""同进程内 server + client 互调,验证 a + b == sum。"""
server = AddTwoIntsServer()
client = AddTwoIntsClient()
exec_ = rclpy.executors.SingleThreadedExecutor()
exec_.add_node(server)
exec_.add_node(client)
req = AddTwoInts.Request()
req.a = 7
req.b = 35
future = client.client.call_async(req)
end = time.time() + 3.0
while not future.done() and time.time() < end:
exec_.spin_once(timeout_sec=0.05)
assert future.done(), 'service call did not complete in 3s'
assert future.result() is not None
assert future.result().sum == 42
exec_.remove_node(server)
exec_.remove_node(client)
src/py_vision_demo/launch/vision_launch.py
+29
View File
@@ -0,0 +1,29 @@
"""
vision_launch.py —— 启动 fake_camera + image_processor 的 launch 文件。
"""
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='py_vision_demo',
executable='fake_camera',
name='fake_camera_py',
output='screen',
parameters=[{
'width': 320,
'height': 240,
'fps': 10,
'frame_id': 'camera_optical_frame',
}],
),
Node(
package='py_vision_demo',
executable='image_processor',
name='image_processor_py',
output='screen',
),
])
src/py_vision_demo/package.xml
+25
View File
@@ -0,0 +1,25 @@
<?xml version="1.0"?>
<?xml-model
href="http://download.ros.org/schema/package_format3.xsd"
schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>py_vision_demo</name>
<version>0.1.0</version>
<description>Python vision demo: simulated camera publishes Image, subscriber processes</description>
<maintainer email="dev@example.com">xs</maintainer>
<license>Apache-2.0</license>
<depend>rclpy</depend>
<depend>sensor_msgs</depend>
<depend>cv_bridge</depend>
<depend>python3-numpy</depend>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>
src/py_vision_demo/py_vision_demo/__init__.py
View File
src/py_vision_demo/py_vision_demo/fake_camera.py
+94
View File
@@ -0,0 +1,94 @@
"""
fake_camera.py —— ROS2 Python 模拟相机节点(发布 sensor_msgs/Image)。
sensor_msgs/Image 关键字段:
header —— 标准 ROS header (含 frame_id);
height/width —— 像素尺寸;
encoding —— 像素编码,如 'rgb8' / 'bgr8' / 'mono8' / '32fc1';
is_bigendian —— 默认 0 (小端);
step —— 每一行字节数 (= width * bytes_per_pixel);
data —— 紧凑像素数组,顺序按 encoding 解释。
为什么不用真实相机?
本 demo 不依赖硬件,用 numpy 生成一张"渐变 + 时间戳水印"图像,
让下游 image_processor 能验证 Image 消息流的端到端通路。
真实部署时把图像源换成 realsense_ros2 / usb_cam 等 ROS 驱动即可。
运行:
ros2 run py_vision_demo fake_camera --ros-args -p width:=320 -p height:=240 -p fps:=10
"""
import numpy as np
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
class FakeCamera(Node):
"""周期性生成随机彩色渐变图像并发布到 /image_raw。"""
def __init__(self):
super().__init__('fake_camera_py')
# Parameters:可被 launch / 命令行覆盖。
self.declare_parameter('width', 320)
self.declare_parameter('height', 240)
self.declare_parameter('fps', 10)
self.declare_parameter('frame_id', 'camera_optical_frame')
w = self.get_parameter('width').value
h = self.get_parameter('height').value
fps = self.get_parameter('fps').value
self.frame_id_ = self.get_parameter('frame_id').value
# 创建发布者;bgr8 与 OpenCV 默认一致,下游用 cv_bridge 转换最方便。
self.publisher_ = self.create_publisher(Image, '/image_raw', 10)
period = 1.0 / max(fps, 1)
self.timer_ = self.create_timer(period, self.tick)
self.count_ = 0
self.get_logger().info(
f'fake_camera_py started: {w}x{h} @ {fps}fps -> /image_raw')
def tick(self):
w = self.get_parameter('width').value
h = self.get_parameter('height').value
# 生成一张渐变图:R / G 通道分别是 x / y 方向线性递增,B 通道随时间变化。
x = np.linspace(0, 255, w, dtype=np.uint8)
y = np.linspace(0, 255, h, dtype=np.uint8)
r = np.tile(x, (h, 1)) # (h, w)
g = np.tile(y[:, None], (1, w)) # (h, w)
b = np.full((h, w), (self.count_ * 5) % 256, dtype=np.uint8)
bgr = np.stack([b, g, r], axis=-1) # bgr8
# 构造 ROS Image 消息。
msg = Image()
msg.header.stamp = self.get_clock().now().to_msg()
msg.header.frame_id = self.frame_id_
msg.height = h
msg.width = w
msg.encoding = 'bgr8'
msg.is_bigendian = 0
msg.step = w * 3 # 3 bytes per pixel (BGR)
msg.data = bgr.tobytes() # numpy -> bytes 紧凑存储
self.publisher_.publish(msg)
self.count_ += 1
def main(args=None):
rclpy.init(args=args)
node = FakeCamera()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
src/py_vision_demo/py_vision_demo/image_processor.py
+79
View File
@@ -0,0 +1,79 @@
"""
image_processor.py —— ROS2 Python 图像处理订阅节点(cv_bridge 演示)。
ROS2 图像处理标准链:
sensor_msgs/Image (raw bytes)
↓ cv_ros/cv_bridge
OpenCV cv::Mat / numpy.ndarray
↓ OpenCV / numpy / torchvision 处理
处理结果(标注框 / 跟踪 / 分类)
↓ 再回 sensor_msgs/Image 或自定义消息
下游节点(决策、控制、可视化)
cv_bridge.CvBridge:
bridge.imgmsg_to_cv2(image_msg, desired_encoding='bgr8')
把 ROS Image 转成 numpy ndarray (H, W, C)。
bridge.cv2_to_imgmsg(ndarray, encoding='bgr8')
把 ndarray 转回 ROS Image。
运行:
ros2 run py_vision_demo image_processor
# 同时另开终端跑 fake_camera,会看到 'received image: ... avg=NNN' 日志。
"""
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
try:
# 镜像里默认有 python3-opencv / ros-humble-cv-bridge。
from cv_bridge import CvBridge
import numpy as np
HAS_CV_BRIDGE = True
except ImportError:
HAS_CV_BRIDGE = False
class ImageProcessor(Node):
def __init__(self):
super().__init__('image_processor_py')
self.bridge_ = CvBridge() if HAS_CV_BRIDGE else None
self.subscription_ = self.create_subscription(
Image, '/image_raw', self.listener_callback, 10)
self.get_logger().info(
f'image_processor_py subscribed <- /image_raw '
f'(cv_bridge={HAS_CV_BRIDGE})')
def listener_callback(self, msg):
if not HAS_CV_BRIDGE:
# cv_bridge 缺失时不做事,只打 log 验证链路通。
self.get_logger().info(
f'rcv Image: {msg.width}x{msg.height} {msg.encoding} '
f'bytes={len(msg.data)}')
return
# ROS Image -> numpy ndarray (bgr8)
cv_img = self.bridge_.imgmsg_to_cv2(msg, desired_encoding='bgr8')
# 简单计算:全图平均亮度 = (B+G+R)/3 的均值。
avg = float(np.mean(cv_img))
self.get_logger().info(
f'rcv Image: {msg.width}x{msg.height} '
f'avg_brightness={avg:.1f}')
def main(args=None):
rclpy.init(args=args)
node = ImageProcessor()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
src/py_vision_demo/resource/py_vision_demo
View File
src/py_vision_demo/setup.cfg
+4
View File
@@ -0,0 +1,4 @@
[develop]
script_dir=$base/lib/py_vision_demo
[install]
install_scripts=$base/lib/py_vision_demo
src/py_vision_demo/setup.py
+28
View File
@@ -0,0 +1,28 @@
from setuptools import find_packages, setup
package_name = 'py_vision_demo'
setup(
name=package_name,
version='0.1.0',
packages=find_packages(exclude=['test']),
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
('share/' + package_name + '/launch', ['launch/vision_launch.py']),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='xs',
maintainer_email='dev@example.com',
description='Python vision demo: simulated camera + image processor',
license='Apache-2.0',
tests_require=['pytest'],
entry_points={
'console_scripts': [
'fake_camera = py_vision_demo.fake_camera:main',
'image_processor = py_vision_demo.image_processor:main',
],
},
)
src/py_vision_demo/test/test_vision.py
+93
View File
@@ -0,0 +1,93 @@
"""py_vision_demo 单元测试:同进程内 fake_camera -> image_processor 链路。
注意:image_processor.listener_callback 在订阅时就被 bound 到
rclpy 的 subscription 对象,monkey-patch 不会影响已存的引用。
我们用 ImageProcessor 子类或独立 subscription 来收取消息。
"""
import time
import numpy as np
import rclpy
import pytest
from sensor_msgs.msg import Image
from py_vision_demo.fake_camera import FakeCamera
@pytest.fixture(scope='module')
def ros_context():
rclpy.init()
yield
rclpy.shutdown()
def test_image_pipeline_inproc(ros_context):
"""fake_camera 发布 -> 自建 subscriber 接收,1.5s 内应收到 ≥1 条带正确尺寸的 Image。"""
cam = FakeCamera()
received = []
# 不直接复用 ImageProcessor,因为它的 callback 已经被 bound。
# 自建一个 subscriber 来采集。
sub_node = rclpy.node.Node('test_subscriber')
sub_node.create_subscription(
Image, '/image_raw',
lambda msg: received.append(msg), 10)
exec_ = rclpy.executors.SingleThreadedExecutor()
exec_.add_node(cam)
exec_.add_node(sub_node)
end = time.time() + 1.5
while time.time() < end:
exec_.spin_once(timeout_sec=0.05)
# 至少 1 条消息,尺寸与 cam 默认 320x240 一致。
assert len(received) >= 1, f'expected ≥1 image, got {len(received)}'
img = received[0]
assert img.width == 320
assert img.height == 240
assert img.encoding == 'bgr8'
assert len(img.data) == 320 * 240 * 3
# 优雅清理
exec_.remove_node(cam)
exec_.remove_node(sub_node)
cam.destroy_node()
sub_node.destroy_node()
def test_image_bytes_roundtrip(ros_context):
"""手工构造一条 Image 直接 publish,验证数据完整性。"""
cam = FakeCamera()
received = []
sub_node = rclpy.node.Node('test_subscriber_2')
sub_node.create_subscription(
Image, '/image_raw',
lambda msg: received.append(msg), 10)
msg = Image()
msg.height = 4
msg.width = 4
msg.encoding = 'rgb8'
msg.step = 4 * 3
msg.data = bytes(range(4 * 4 * 3))
exec_ = rclpy.executors.SingleThreadedExecutor()
exec_.add_node(cam)
exec_.add_node(sub_node)
end = time.time() + 1.0
while not received and time.time() < end:
cam.publisher_.publish(msg)
exec_.spin_once(timeout_sec=0.05)
assert len(received) == 1
assert received[0].data == msg.data
exec_.remove_node(cam)
exec_.remove_node(sub_node)
cam.destroy_node()
sub_node.destroy_node()
start.ps1
+30
View File
@@ -0,0 +1,30 @@
# =============================================================================
# start.ps1 —— Windows 一键启动脚本。
#
# 步骤:
# 1) build —— 用 docker-compose 构建本地镜像(首次会拉基础层,5–10 分钟)。
# 2) up -d —— 后台起容器。
# 3) exec build.sh —— 进容器内部执行 colcon build。
# 4) exec bash —— 进入开发交互 shell。
#
# 注意:用 `docker compose`(空格,plugin 版) 而非 `docker-compose`(旧版 CLI);
# Docker Desktop 4.x 起默认带 compose v2。
# =============================================================================
# 临时开启 DOCKER_BUILDKIT:获得更快的并行构建 + 缓存。
param($Env:DOCKER_BUILDKIT = "1")
Write-Host "[1/4] 构建镜像 (首次约 5-10 分钟)..." -ForegroundColor Cyan
docker compose -f D:\xs\ros2\docker\docker-compose.yml build
Write-Host "[2/4] 启动容器..." -ForegroundColor Cyan
docker compose -f D:\xs\ros2\docker\docker-compose.yml up -d
Write-Host "[3/4] 进入容器并编译..." -ForegroundColor Cyan
# docker exec -it:交互式 shell;-c "cmd" 让 bash -lc 跑指定命令再退出。
docker exec -it ros2_dev bash -lc "cd /root/ros2_ws && bash build.sh"
Write-Host "[4/4] 进入开发终端..." -ForegroundColor Cyan
# source install/setup.bash 让 ros2 命令能识别 build 出的包;
# 最外层 exec bash 把该 shell 留给你后续手动操作。
docker exec -it ros2_dev bash -lc "source install/setup.bash && exec bash"
start.sh
+25
View File
@@ -0,0 +1,25 @@
#!/usr/bin/env bash
# =============================================================================
# start.sh —— Linux / WSL / macOS 一键启动脚本。
#
# 与 start.ps1 同样四步,只是 shell 改 bash:
# 1) docker compose build
# 2) docker compose up -d
# 3) docker exec ... bash build.sh
# 4) docker exec ... source install/setup.bash && bash
# =============================================================================
set -euo pipefail # 出错即停;严格变量未定义检查;管道失败传播。
cd "$(dirname "$0")" # 切到本脚本所在目录(确保 docker-compose.yml 路径对)。
echo "[1/4] 构建镜像 (首次约 5-10 分钟)..." >&2
docker compose -f docker/docker-compose.yml build
echo "[2/4] 启动容器..." >&2
docker compose -f docker/docker-compose.yml up -d
echo "[3/4] 进入容器并编译..." >&2
docker exec -it ros2_dev bash -lc "cd /root/ros2_ws && bash build.sh"
echo "[4/4] 进入开发终端..." >&2
docker exec -it ros2_dev bash -lc "source install/setup.bash && exec bash"
tools/setup_venv.ps1
+71
View File
@@ -0,0 +1,71 @@
# =============================================================================
# setup_venv.ps1 —— 在 Windows 上创建标准 Python venv,不污染系统 Python。
#
# 重要前提:
# - Windows 上 rclpy / sensor_msgs / cv_bridge 没有官方 pip wheels,
# 所以 venv 里 *只能装纯 Python 工具*(ruff / black / mypy / pytest / numpy)。
# - 真正的 ROS2 节点必须在 Docker Desktop 容器内 / WSL Ubuntu 内运行。
# - 本机用 VSCode / PyCharm 等 IDE 时,把 interpreter 指向 .venv\Scripts\python.exe
# 即可享受自动补全 / 类型检查,即使 rclpy 解析不了也没关系
# (pyrightconfig.json 已配置 ignore_missing_imports)。
#
# 使用:
# PS> .\tools\setup_venv.ps1
# PS> .\.venv\Scripts\Activate.ps1
# =============================================================================
$ErrorActionPreference = 'Stop'
# 切到脚本所在目录的上一级(项目根)
Set-Location -LiteralPath (Join-Path $PSScriptRoot '..')
# 优先 python,找不到再 python3
$PYTHON = $null
foreach ($cand in @('python', 'python3', 'py')) {
$cmd = Get-Command $cand -ErrorAction SilentlyContinue
if ($cmd) { $PYTHON = $cand; break }
}
if (-not $PYTHON) {
throw 'Python not found in PATH. Install Python 3.10+ from python.org first.'
}
Write-Host "==> Using: $PYTHON" -ForegroundColor Cyan
& $PYTHON --version
$VENV_DIR = '.venv'
# 1) 创建 venv(若已存在则提示并复用)
if (-not (Test-Path -LiteralPath $VENV_DIR)) {
Write-Host "==> Creating venv at $VENV_DIR" -ForegroundColor Cyan
& $PYTHON -m venv $VENV_DIR
} else {
Write-Host "==> Reusing existing venv at $VENV_DIR" -ForegroundColor Yellow
}
$venvPython = Join-Path $VENV_DIR 'Scripts\python.exe'
if (-not (Test-Path -LiteralPath $venvPython)) {
throw "venv python.exe not found at $venvPython"
}
# 2) 升级 pip + 装开发依赖
Write-Host "==> Upgrading pip + installing requirements-dev.txt" -ForegroundColor Cyan
& $venvPython -m pip install --upgrade pip wheel setuptools
& $venvPython -m pip install -r requirements-dev.txt
# 3) pip install -e 把本项目 ROS2 Python 包装到 venv(纯 Python 部分,不动 ROS 客户端)
Write-Host "==> Installing ROS2 Python packages (editable)" -ForegroundColor Cyan
foreach ($pkg in @('py_pubsub','py_srv','py_action_demo','py_vision_demo')) {
$pkgDir = Join-Path 'src' $pkg
if (Test-Path -LiteralPath $pkgDir) {
& $venvPython -m pip install -e $pkgDir --no-deps
Write-Host " OK $pkg" -ForegroundColor Green
}
}
Write-Host ''
Write-Host 'venv ready. Activate with:' -ForegroundColor Green
Write-Host ' .\.venv\Scripts\Activate.ps1' -ForegroundColor Green
Write-Host ''
Write-Host 'Next steps (Windows):' -ForegroundColor Yellow
Write-Host ' pytest src/py_pubsub/test -m "not ros" # 本机纯逻辑测试'
Write-Host ' docker exec ros2_dev bash -lc "cd /root/ros2_ws && colcon test" # 容器内 ROS2 测试'
tools/setup_venv.sh
+68
View File
@@ -0,0 +1,68 @@
#!/usr/bin/env bash
# =============================================================================
# setup_venv.sh —— 在 Linux / macOS / WSL / Docker 容器内创建标准 Python venv,
# 不污染系统 Python。
#
# 流程:
# 1) python3 -m venv .venv —— 在项目根建一个 .venv 目录;
# 2) source .venv/bin/activate —— 激活 venv,后续 pip/python 都走 venv;
# 3) pip install -r requirements-dev.txt —— 装开发工具(ruff/black/mypy/pytest);
# 4) pip install -e <每个 ROS2 Python 包> —— 把本项目 Python 包装到 venv (editable);
# 5) 若 ROS2 客户端库已通过 apt 装(ros-humble-desktop),把 /opt/ros/humble/lib/python3.10/site-packages
# 加进 PYTHONPATH,使 venv 里也能 import rclpy / sensor_msgs 等。
#
# 注意:Windows 没有 rclpy wheels,所以在 Windows 上跑这脚本只能装"开发工具 + numpy",
# ROS2 节点必须在 Docker / WSL 内执行。
# =============================================================================
set -euo pipefail
cd "$(dirname "$0")/.." # 切到项目根(脚本在 tools/)
PYTHON="${PYTHON:-python3}"
VENV_DIR=".venv"
echo "==> Python: $($PYTHON --version) at $($PYTHON -c 'import sys; print(sys.executable)')"
# 1) venv
if [ ! -d "$VENV_DIR" ]; then
echo "==> Creating venv at $VENV_DIR"
$PYTHON -m venv "$VENV_DIR"
else
echo "==> Reusing existing venv at $VENV_DIR"
fi
# 2) 激活 venv(本 shell 子进程里)
# shellcheck disable=SC1091
source "$VENV_DIR/bin/activate"
echo "==> venv Python: $(python --version) at $(which python)"
# 3) 升级 pip + 装开发依赖
echo "==> Upgrading pip + installing requirements-dev.txt"
python -m pip install --upgrade pip wheel setuptools
python -m pip install -r requirements-dev.txt
# 4) pip install -e 把本项目 ROS2 Python 包装到 venv(可编辑模式)
echo "==> Installing ROS2 Python packages (editable)"
for pkg in py_pubsub py_srv py_action_demo py_vision_demo; do
if [ -d "src/$pkg" ]; then
python -m pip install -e "src/$pkg" --no-deps
echo " OK $pkg"
fi
done
# 5) 若 /opt/ros/humble 存在,自动把 site-packages 注入 venv
ROS_PYTHON_SITE="/opt/ros/humble/lib/python3.10/site-packages"
if [ -d "$ROS_PYTHON_SITE" ]; then
echo "==> ROS2 detected at /opt/ros/humble. To import rclpy inside venv, run:"
echo " export PYTHONPATH=\"$ROS_PYTHON_SITE:\${PYTHONPATH:-}\""
echo " (this script does NOT mutate the system; do it manually when you need it)"
fi
echo
echo "✅ venv ready. Activate with:"
echo " source $VENV_DIR/bin/activate"
echo
echo "Next steps:"
echo " pytest src/py_pubsub/test -m 'not ros' # 本机纯逻辑测试"
echo " docker exec ros2_dev bash -lc 'cd /root/ros2_ws && colcon test' # 容器内 ROS2 测试"