commit 5ef38ab5086f9fd08cabdd20eb92f6cec643e3c7 Author: xs Date: Mon Aug 3 18:09:35 2026 +0800 init: ROS2 learning suite diff --git a/.flake8 b/.flake8 new file mode 100644 index 0000000..10cc3fa --- /dev/null +++ b/.flake8 @@ -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 \ No newline at end of file diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..bc43f5c --- /dev/null +++ b/.gitignore @@ -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 \ No newline at end of file diff --git a/AGENTS.md b/AGENTS.md new file mode 100644 index 0000000..b8d893a --- /dev/null +++ b/AGENTS.md @@ -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//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,在容器里跑 | \ No newline at end of file diff --git a/README.md b/README.md new file mode 100644 index 0000000..723eb39 --- /dev/null +++ b/README.md @@ -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` 部署。 \ No newline at end of file diff --git a/build.sh b/build.sh new file mode 100644 index 0000000..5e94655 --- /dev/null +++ b/build.sh @@ -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 +# --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 \ No newline at end of file diff --git a/doc/00-overview.md b/doc/00-overview.md new file mode 100644 index 0000000..43df206 --- /dev/null +++ b/doc/00-overview.md @@ -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//share//launch/*.py │ │ +│ │ install//lib// │ │ +│ └───────────────────────────────────────────────────────┘ │ +│ ┌───────────────────────────────────────────────────────┐ │ +│ │ 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) | \ No newline at end of file diff --git a/doc/01-quickstart.md b/doc/01-quickstart.md new file mode 100644 index 0000000..1e8c77e --- /dev/null +++ b/doc/01-quickstart.md @@ -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- +``` + +### 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 之旅开始! 🚀 \ No newline at end of file diff --git a/doc/02-virtualenv.md b/doc/02-virtualenv.md new file mode 100644 index 0000000..965c938 --- /dev/null +++ b/doc/02-virtualenv.md @@ -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//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 " +docker exec ros2_dev bash -lc "cd /root/ros2_ws && colcon test --packages-select " + +# 3. 跑 demo +docker exec ros2_dev bash -lc "source install/setup.bash && ros2 launch bringup " +``` + +--- + +## 接下来读 + +| 主题 | 文档 | +|---|---| +| 项目总览 | [`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) | \ No newline at end of file diff --git a/doc/10-concepts.md b/doc/10-concepts.md new file mode 100644 index 0000000..eb505fb --- /dev/null +++ b/doc/10-concepts.md @@ -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()); + 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("chatter", 10); + +auto msg = std_msgs::msg::String(); +msg.data = "hello"; +pub->publish(msg); + +// Subscriber +auto sub = node->create_subscription( + "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 -v # 类型 + pub/sub 列表 +ros2 topic echo # 实时打印 +ros2 topic hz # 频率 Hz +ros2 topic bw # 带宽 bytes/s +ros2 topic pub "" --once # 发一条测试 +ros2 bag record # 录包 +ros2 bag play # 回放 +``` + +### 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 # 类型 +ros2 service call "" +# 例: +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 + + + + + + + + + + + + + + + + + + + + + + +``` + +### 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 +ros2 launch 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 ` | [`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) | \ No newline at end of file diff --git a/doc/100-embedded-deployment.md b/doc/100-embedded-deployment.md new file mode 100644 index 0000000..634bf57 --- /dev/null +++ b/doc/100-embedded-deployment.md @@ -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' + + + + + udp_transport + UDPv4 + + + + + + + + +
192.168.1.10
+ 7400 + + +
192.168.1.20
+ 7400 + + + + + SIMPLE + STATIC + 30 + + + + + +
0.0.0.0
+ 7400 + + + + + + + +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 +#include +#include +#include +#include + +#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("/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( + "/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((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::SharedPtr pub_; + rclcpp::Subscription::SharedPtr sub_; + rclcpp::TimerBase::SharedPtr timer_; + int fd_ = -1; +}; + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + 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 --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 +nc -zv 7400 + +# DDS 配置 +cat ~/.ros/fastdds.xml +echo $ROS_STATIC_PEERS + +# colcon build +cd ~/ros2_ws && colcon build --packages-select +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 即可。 \ No newline at end of file diff --git a/doc/20-topics.md b/doc/20-topics.md new file mode 100644 index 0000000..6bc3dc4 --- /dev/null +++ b/doc/20-topics.md @@ -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` 加 `rosidl_default_generators` + `rosidl_default_runtime` +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 +#include +#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("period_ms", 500); + this->declare_parameter("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(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::SharedPtr publisher_; + rclcpp::TimerBase::SharedPtr timer_; + size_t count_; +}; + +int main(int argc, char * argv[]) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} +``` + +### 3.3 关键 API 速查 + +| Python | C++ | 用途 | +|---|---|---| +| `create_publisher(MsgType, name, depth)` | `create_publisher(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("topic", "chatter"); + std::string topic = this->get_parameter("topic").as_string(); + + // create_subscription(topic, depth, callback) + // callback 签名: [](const T::SharedPtr msg) { ... } + subscription_ = this->create_subscription( + topic, 10, + [this](const std_msgs::msg::String::SharedPtr msg) { + RCLCPP_INFO(this->get_logger(), "recv: \"%s\"", msg->data.c_str()); + }); + } +private: + rclcpp::Subscription::SharedPtr subscription_; +}; +``` + +### 4.3 关键 API 速查 + +| Python | C++ | 用途 | +|---|---|---| +| `create_subscription(MsgType, name, cb, depth)` | `create_subscription(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) | \ No newline at end of file diff --git a/doc/30-services.md b/doc/30-services.md new file mode 100644 index 0000000..6de1335 --- /dev/null +++ b/doc/30-services.md @@ -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( + "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::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("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(); + 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(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 | `_server` (如 `add_two_ints_server`) | +| Service Client | `_client` (如 `add_two_ints_client`) | +| Service 名 | `` (如 `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) | \ No newline at end of file diff --git a/doc/40-actions.md b/doc/40-actions.md new file mode 100644 index 0000000..056851d --- /dev/null +++ b/doc/40-actions.md @@ -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; + + Server() : rclcpp::Node("server") { + server_ = rclcpp_action::create_server( + this, "fibonacci", + [this](auto, auto) { return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }, + [this](auto) { return rclcpp_action::CancelResponse::ACCEPT; }, + [this](std::shared_ptr gh) { + // 主逻辑 + auto feedback = std::make_shared(); + auto result = std::make_shared(); + std::vector 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::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( + 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) | \ No newline at end of file diff --git a/doc/50-tf2.md b/doc/50-tf2.md new file mode 100644 index 0000000..a33df9a --- /dev/null +++ b/doc/50-tf2.md @@ -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(this->get_clock()); + listener_ = std::make_shared(*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 buffer_; + std::shared_ptr 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_.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 | \ No newline at end of file diff --git a/doc/60-urdf.md b/doc/60-urdf.md new file mode 100644 index 0000000..19e6d7b --- /dev/null +++ b/doc/60-urdf.md @@ -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 描述机器人: +- 有哪些**刚体段**(``) +- 刚体之间怎么连(``) +- 视觉 / 碰撞几何(`` / ``) +- 物理参数(质量、惯性,``) + +URDF 通过 `robot_state_publisher` 翻译成 TF tree,被 MoveIt2 / Gazebo / RViz 读取。 + +--- + +## 2. URDF 三大元素 + +### 2.1 `` — 刚体段 + +| 子标签 | 用途 | 必填 | +|---|---|---| +| `` | 视觉几何(RViz / Gazebo 显示) | 推荐 | +| `` | 碰撞几何(物理仿真) | 推荐 | +| `` | 质量 + 转动惯量(动力学仿真必填) | 仿真必填 | +| `` | 视觉颜色 | 否 | + +```xml + + + + + + + + + + + + + + + + + +``` + +#### 几何类型 + +```xml + + + + + + +``` + +#### 颜色 / 材质 + +```xml + + + + +``` + +### 2.2 `` — 关节 + +| type | 含义 | DoF | +|---|---|---| +| `revolute` | 转动(有限位) | 1 | +| `continuous` | 转动(无限位) | 1 | +| `prismatic` | 滑动 | 1 | +| `fixed` | 固定连接 | 0 | +| `floating` | 6 DoF | 6 | +| `planar` | 平面 3 DoF | 3 | + +```xml + + + + + + + effort="100" velocity="1.0"/> + + +``` + +**重要约定**: +- `axis` 在 **parent link 系**下定义 +- `origin` 是 **child 相对 parent** 的偏移 +- 关节运动绕 `axis` 旋转(或沿 axis 平移) + +### 2.3 `` 根 +- `name`:机器人名字,在 ROS 工具中显示 +- 必须包含所有 `` 和 `` + +### 2.4 最小 URDF 示例 + +```xml + + + + + + + + + + + + + + + + +``` + +--- + +## 3. 完整 6-DoF 机械臂示例 + +模拟 6-DoF 工业机械臂(类似 UR5 / xArm 6): + +```xml + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +``` + +**约定**: +- 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 + + + + + + + + + + + + + + + + + + + + + + + + + + + +``` + +### 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` 需要每个关节加 `` 标签: + +```xml + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + +``` + +`` 描述: +- 哪个 joint +- 用哪个 hardware interface(Position / Velocity / Effort) +- 减速比(`mechanicalReduction`) + +--- + +## 7. Gazebo 集成(加 gazebo 标签) + +Gazebo Sim / ros_gz 加载 URDF 时需要 `` 标签 + ``(已经加)。 + +```xml + + + $(find my_arm)/config/controllers.yaml + + + + + + Gazebo/Red + +``` + +--- + +## 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 + + + + + + + + + + ... + + + + + + + + + + ... + + +``` + +--- + +## 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//urdf/`: +```bash +cp -r /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) | \ No newline at end of file diff --git a/doc/70-launch.md b/doc/70-launch.md new file mode 100644 index 0000000..6938a20 --- /dev/null +++ b/doc/70-launch.md @@ -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//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//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 文件路径:`/share//launch/`。**launch 文件必须装到 share//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(), + 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 + +# 看可用参数 +ros2 launch --show-args + +# 调试(详细日志) +ros2 launch -d + +# 传参 +ros2 launch topic:=hello period_ms:=200 +``` + +--- + +## 10. 常见坑 + +### 10.1 launch 文件找不到 +- `setup.py` 的 `data_files` 必须包含 `'share//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//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) | \ No newline at end of file diff --git a/doc/80-package-build.md b/doc/80-package-build.md new file mode 100644 index 0000000..4d379b3 --- /dev/null +++ b/doc/80-package-build.md @@ -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 + + + + my_pkg + 0.1.0 + ... + Name + Apache-2.0 + + rclpy + rosidl_default_generators + rclpy + std_msgs + + python3-pytest + + + ament_python + + +``` + +| 字段 | 含义 | +|---|---| +| `` | 构建 + 运行 | +| `` | 仅构建 | +| `` | 仅运行 | +| `` | 仅测试 | +| `` | `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//` +- `resource/` 空文件 → 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//`,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// +install(TARGETS talker listener DESTINATION lib/${PROJECT_NAME}) + +# launch 文件装到 share//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( 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 +``` + +按依赖顺序,每个包: + +``` +┌──────────────────────────────────────────────────────────┐ +│ 1. 发现 src// 下的 package.xml │ +│ 2. 读 ,决定 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// │ +└──────────────────────────────────────────────────────────┘ +``` + +加 `--symlink-install`: +- Python:源码**软链**到 install,改源码立即生效(不用重 build) +- C++:可执行仍硬编,但 launch / config 文件软链 + +--- + +## 7. ament_index 与 ros2 工具发现 + +`ros2 run / launch / pkg executables / topic info` 等命令都靠 **ament_index**: +- 启动时扫 `install//share/ament_index/resource_index/packages/` 文件 +- 找到包路径后,扫 `install//lib//` 找可执行 +- 扫 `install//share//launch/` 找 launch 文件 + +**手动重建索引**(极少需要): +```bash +# 索引在 install/share/ament_index/ 里,正常情况下 colcon build 自动维护 +ros2 doctor --report # 看索引健康 +``` + +--- + +## 8. 依赖解析与 rosdep + +`package.xml` 的 `` 让 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` 加: + - `rosidl_default_generators` + - `rosidl_default_runtime` +4. `CMakeLists.txt` 加 `rosidl_generate_interfaces(${PROJECT_NAME} ${MSG_FILES} ...)` +5. `setup.py` `data_files` 加 `('share//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) | \ No newline at end of file diff --git a/doc/85-docker.md b/doc/85-docker.md new file mode 100644 index 0000000..16a78b3 --- /dev/null +++ b/doc/85-docker.md @@ -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 +# 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) | \ No newline at end of file diff --git a/doc/90-testing.md b/doc/90-testing.md new file mode 100644 index 0000000..77cfeef --- /dev/null +++ b/doc/90-testing.md @@ -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("test_node"); + auto pub = node->create_publisher("chatter", 10); + auto msg = std_msgs::msg::String(); + msg.data = "unit-test"; + pub->publish(msg); + + auto exec = std::make_shared(); + 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(); + auto exec = std::make_shared(); + 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(/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) | \ No newline at end of file diff --git a/doc/99-embodied-ai.md b/doc/99-embodied-ai.md new file mode 100644 index 0000000..a3d3183 --- /dev/null +++ b/doc/99-embodied-ai.md @@ -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. **加 `` 标签**(ros2_control 需要) +4. **加 `` 标签**(后续 Gazebo 用,可选) +5. `check_urdf ` 校验 + +URDF 关键改造: +```xml + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + +``` + +### 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 模型 + +加 `` 标签 + ``(已在阶段 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//config/controllers.yaml` +- 新增 `src//launch/_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 通信基础仍然稳**。 + +加油,具身智能之旅! 🚀 \ No newline at end of file diff --git a/docker/Dockerfile b/docker/Dockerfile new file mode 100644 index 0000000..110dabd --- /dev/null +++ b/docker/Dockerfile @@ -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//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"] diff --git a/docker/bringup_e2e.log b/docker/bringup_e2e.log new file mode 100644 index 0000000..0f5f8cd --- /dev/null +++ b/docker/bringup_e2e.log @@ -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" diff --git a/docker/docker-compose.yml b/docker/docker-compose.yml new file mode 100644 index 0000000..f7cc3f7 --- /dev/null +++ b/docker/docker-compose.yml @@ -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"] diff --git a/docker/full_demo_e2e.log b/docker/full_demo_e2e.log new file mode 100644 index 0000000..63983ef --- /dev/null +++ b/docker/full_demo_e2e.log @@ -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 diff --git a/docker/robot_e2e.log b/docker/robot_e2e.log new file mode 100644 index 0000000..4613ab8 --- /dev/null +++ b/docker/robot_e2e.log @@ -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 diff --git a/docker/srv_e2e.log b/docker/srv_e2e.log new file mode 100644 index 0000000..e8d67ac --- /dev/null +++ b/docker/srv_e2e.log @@ -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']. diff --git a/docker/vision_e2e.log b/docker/vision_e2e.log new file mode 100644 index 0000000..46b6bfe --- /dev/null +++ b/docker/vision_e2e.log @@ -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 +[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 diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 0000000..60650b7 --- /dev/null +++ b/pyproject.toml @@ -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)/)' \ No newline at end of file diff --git a/pyrightconfig.json b/pyrightconfig.json new file mode 100644 index 0000000..e0606be --- /dev/null +++ b/pyrightconfig.json @@ -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" +} \ No newline at end of file diff --git a/requirements-dev.txt b/requirements-dev.txt new file mode 100644 index 0000000..354319c --- /dev/null +++ b/requirements-dev.txt @@ -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 \ No newline at end of file diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..cd9b272 --- /dev/null +++ b/requirements.txt @@ -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 \ No newline at end of file diff --git a/src/bringup/launch/action_launch.py b/src/bringup/launch/action_launch.py new file mode 100644 index 0000000..cfd117d --- /dev/null +++ b/src/bringup/launch/action_launch.py @@ -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', + ), + ]) \ No newline at end of file diff --git a/src/bringup/launch/all_launch.py b/src/bringup/launch/all_launch.py new file mode 100644 index 0000000..9eee939 --- /dev/null +++ b/src/bringup/launch/all_launch.py @@ -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') # → /share/py_pubsub + cp_pkg = FindPackageShare('cpp_pubsub') # → /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(), + ), + ]) diff --git a/src/bringup/launch/full_demo_launch.py b/src/bringup/launch/full_demo_launch.py new file mode 100644 index 0000000..d49ab70 --- /dev/null +++ b/src/bringup/launch/full_demo_launch.py @@ -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'), + ]) \ No newline at end of file diff --git a/src/bringup/launch/pubsub_launch.py b/src/bringup/launch/pubsub_launch.py new file mode 100644 index 0000000..0dc2f9b --- /dev/null +++ b/src/bringup/launch/pubsub_launch.py @@ -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'}], + ), + ]) \ No newline at end of file diff --git a/src/bringup/launch/robot_launch.py b/src/bringup/launch/robot_launch.py new file mode 100644 index 0000000..9b6c81b --- /dev/null +++ b/src/bringup/launch/robot_launch.py @@ -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)), + ]) \ No newline at end of file diff --git a/src/bringup/launch/service_launch.py b/src/bringup/launch/service_launch.py new file mode 100644 index 0000000..ad74f4f --- /dev/null +++ b/src/bringup/launch/service_launch.py @@ -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', + ), + ]) \ No newline at end of file diff --git a/src/bringup/launch/vision_launch.py b/src/bringup/launch/vision_launch.py new file mode 100644 index 0000000..8a7ca57 --- /dev/null +++ b/src/bringup/launch/vision_launch.py @@ -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)), + ]) \ No newline at end of file diff --git a/src/bringup/package.xml b/src/bringup/package.xml new file mode 100644 index 0000000..a060eb4 --- /dev/null +++ b/src/bringup/package.xml @@ -0,0 +1,29 @@ + + + + + + bringup + 0.1.0 + Top-level bringup launching both py and cpp pubsub + xs + Apache-2.0 + + + py_pubsub + cpp_pubsub + + launch + launch_ros + + + + ament_python + + diff --git a/src/bringup/resource/bringup b/src/bringup/resource/bringup new file mode 100644 index 0000000..e69de29 diff --git a/src/bringup/setup.cfg b/src/bringup/setup.cfg new file mode 100644 index 0000000..c876447 --- /dev/null +++ b/src/bringup/setup.cfg @@ -0,0 +1,6 @@ +[develop] +# ament_python 期望 console scripts 安装到 install/lib//; +# 本包没有 entry_points,只留默认即可。 +script_dir=$base/lib/bringup +[install] +install_scripts=$base/lib/bringup diff --git a/src/bringup/setup.py b/src/bringup/setup.py new file mode 100644 index 0000000..57df964 --- /dev/null +++ b/src/bringup/setup.py @@ -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 一致("bringup",避免和 +# ROS 自带的 'launch' 包同名产生 ament 索引冲突)。 +# ============================================================================= + +# glob:用于展开当前目录下的 launch/*.py,自动收集所有 launch 文件。 +from glob import glob + +# setuptools.setup:标准 Python 安装入口。 +from setuptools import setup + +# 必须与 package.xml 一致。 +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//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', +) diff --git a/src/cpp_pubsub/CMakeLists.txt b/src/cpp_pubsub/CMakeLists.txt new file mode 100644 index 0000000..291ccfd --- /dev/null +++ b/src/cpp_pubsub/CMakeLists.txt @@ -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() \ No newline at end of file diff --git a/src/cpp_pubsub/launch/pubsub_launch.py b/src/cpp_pubsub/launch/pubsub_launch.py new file mode 100644 index 0000000..79b120a --- /dev/null +++ b/src/cpp_pubsub/launch/pubsub_launch.py @@ -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, + }], + ), + ]) diff --git a/src/cpp_pubsub/package.xml b/src/cpp_pubsub/package.xml new file mode 100644 index 0000000..04ddc73 --- /dev/null +++ b/src/cpp_pubsub/package.xml @@ -0,0 +1,31 @@ + + + + + cpp_pubsub + 0.1.0 + C++ talker/listener demo for ROS2 Humble + xs + Apache-2.0 + + + rclcpp + std_msgs + + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + launch_testing_ros + + + + ament_cmake + + diff --git a/src/cpp_pubsub/src/publisher_member_function.cpp b/src/cpp_pubsub/src/publisher_member_function.cpp new file mode 100644 index 0000000..3ad9451 --- /dev/null +++ b/src/cpp_pubsub/src/publisher_member_function.cpp @@ -0,0 +1,113 @@ +// ============================================================================= +// talker_cpp —— ROS2 C++ 发布者节点(Publisher) +// +// 用途:与 Python 版本的 talker_py 形成"跨语言通信"演示,二者通过同一 Topic +// 'chatter' 都用 std_msgs/String,体现 ROS2 的多语言互操作性 +// (因为底层都是 DDS,语言只是同一套消息定义的视图)。 +// +// ROS2 概念速览: +// Node —— 节点,所有发布/订阅/服务/参数都挂在节点上。 +// Publisher —— 发布者,T 是消息类型,publish(msg) 异步投递到 DDS。 +// Timer —— 周期性回调,WallTimer / SteadyTimer 等类型。 +// Parameter —— 节点配置,declare_parameter() + 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 // std::chrono::milliseconds,用来描述 timer 的周期。 +#include // std::shared_ptr / std::make_shared,ROS2 句柄大量用到。 +#include // 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(name, default):模板版的参数声明方式,比通用 + // 指针版更类型安全。这里声明两个参数: + // period_ms —— 发布周期(整数,毫秒) + // topic —— 目标 topic 名(字符串) + this->declare_parameter("period_ms", 500); + this->declare_parameter("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(topic, qos_depth):创建发布者。 + // T = std_msgs::msg::String —— 模板参数告诉编译器消息类型。 + // qos_depth = 10,队列最多缓存 10 条未送达样本(慢消费者场景)。 + publisher_ = this->create_publisher(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::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():把节点对象交给 shared_ptr, + // 这是 rclcpp::spin 的强制要求,Node 内部要拿到 shared_ptr 计数。 + rclcpp::spin(std::make_shared()); + + // spin() 返回后做清理。 + rclcpp::shutdown(); + return 0; +} diff --git a/src/cpp_pubsub/src/subscriber_member_function.cpp b/src/cpp_pubsub/src/subscriber_member_function.cpp new file mode 100644 index 0000000..64a6ac0 --- /dev/null +++ b/src/cpp_pubsub/src/subscriber_member_function.cpp @@ -0,0 +1,68 @@ +// ============================================================================= +// listener_cpp —— ROS2 C++ 订阅者节点(Subscriber) +// +// 与 talker_cpp / talker_py 配对使用,演示 C++ 侧订阅 + 跨语言互联互通。 +// +// 关键概念: +// Subscription:由 node.create_subscription(...) 创建, +// 每收到一条 T 类型消息,ROS2 自动调用回调函数,回调签名: +// void cb(const T::SharedPtr msg) +// SharedPtr 指 std::shared_ptr,可 0 拷贝访问 msg 字段。 +// +// 工具: +// ros2 topic info chatter -v 看 publisher/subscriber 列表 + 消息类型。 +// ros2 topic echo chatter 命令行实时打印消息内容。 +// ============================================================================= + +#include // std::placeholders,std::bind 会用到。 +#include // 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("topic", "chatter"); + std::string topic = this->get_parameter("topic").as_string(); + + // create_subscription(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( + 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::SharedPtr subscription_; +}; + +/// @brief C++ 节点标准 main。 +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/cpp_pubsub/test/test_pub_sub.cpp b/src/cpp_pubsub/test/test_pub_sub.cpp new file mode 100644 index 0000000..65c2c87 --- /dev/null +++ b/src/cpp_pubsub/test/test_pub_sub.cpp @@ -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 +#include +#include + +#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( + "chatter_unit_test", 10, + [this](const std_msgs::msg::String::SharedPtr msg) { + (void)msg; + received_++; + }); + } + + int received_count() const { return received_; } + +private: + rclcpp::Subscription::SharedPtr subscription_; + int received_; +}; + +// Test 1:验证 publisher 在 1s 内能发出至少 1 条消息, +// 通过 ros2 topic hz 等价手段(同进程订阅计数)做侧面验证。 +TEST(PubsubTest, TalkerPublishesAtLeastOnce) +{ + auto node = std::make_shared(); + auto publisher = node->create_publisher("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(); + 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; +} \ No newline at end of file diff --git a/src/cpp_robot_tf2/CMakeLists.txt b/src/cpp_robot_tf2/CMakeLists.txt new file mode 100644 index 0000000..fc19913 --- /dev/null +++ b/src/cpp_robot_tf2/CMakeLists.txt @@ -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//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() \ No newline at end of file diff --git a/src/cpp_robot_tf2/launch/robot_tf2_launch.py b/src/cpp_robot_tf2/launch/robot_tf2_launch.py new file mode 100644 index 0000000..a8352cd --- /dev/null +++ b/src/cpp_robot_tf2/launch/robot_tf2_launch.py @@ -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}], + ), + ]) \ No newline at end of file diff --git a/src/cpp_robot_tf2/package.xml b/src/cpp_robot_tf2/package.xml new file mode 100644 index 0000000..e561d27 --- /dev/null +++ b/src/cpp_robot_tf2/package.xml @@ -0,0 +1,25 @@ + + + + cpp_robot_tf2 + 0.1.0 + C++ robot URDF + TF2 + JointState demo (3-link arm) + xs + Apache-2.0 + + rclcpp + geometry_msgs + sensor_msgs + tf2 + tf2_ros + tf2_geometry_msgs + + ament_cmake_gtest + launch_testing_ros + + + ament_cmake + + \ No newline at end of file diff --git a/src/cpp_robot_tf2/src/joint_state_publisher.cpp b/src/cpp_robot_tf2/src/joint_state_publisher.cpp new file mode 100644 index 0000000..a640600 --- /dev/null +++ b/src/cpp_robot_tf2/src/joint_state_publisher.cpp @@ -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 +#include +#include +#include + +#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_names_ = {"joint1", "joint2", "joint3"}; + + publisher_ = this->create_publisher("/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 joint_names_; + rclcpp::Publisher::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()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/cpp_robot_tf2/src/tf2_listener.cpp b/src/cpp_robot_tf2/src/tf2_listener.cpp new file mode 100644 index 0000000..1164563 --- /dev/null +++ b/src/cpp_robot_tf2/src/tf2_listener.cpp @@ -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 +#include + +#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(this->get_clock()); + + // TransformListener 在节点构造函数里会订阅 /tf 与 /tf_static。 + listener_ = std::make_shared(*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 buffer_; + std::shared_ptr listener_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/cpp_robot_tf2/test/test_tf2_lookup.cpp b/src/cpp_robot_tf2/test/test_tf2_lookup.cpp new file mode 100644 index 0000000..320a56c --- /dev/null +++ b/src/cpp_robot_tf2/test/test_tf2_lookup.cpp @@ -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 +#include + +#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(); + 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(); + 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(); +} \ No newline at end of file diff --git a/src/cpp_robot_tf2/urdf/simple_arm.urdf b/src/cpp_robot_tf2/urdf/simple_arm.urdf new file mode 100644 index 0000000..9b55c59 --- /dev/null +++ b/src/cpp_robot_tf2/urdf/simple_arm.urdf @@ -0,0 +1,95 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/py_action_demo/launch/action_launch.py b/src/py_action_demo/launch/action_launch.py new file mode 100644 index 0000000..461df73 --- /dev/null +++ b/src/py_action_demo/launch/action_launch.py @@ -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', + ), + ]) \ No newline at end of file diff --git a/src/py_action_demo/package.xml b/src/py_action_demo/package.xml new file mode 100644 index 0000000..0fe3957 --- /dev/null +++ b/src/py_action_demo/package.xml @@ -0,0 +1,24 @@ + + + + py_action_demo + 0.1.0 + Python Action demo: Fibonacci server/client + launch + xs + Apache-2.0 + + rclpy + action_msgs + example_interfaces + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + \ No newline at end of file diff --git a/src/py_action_demo/py_action_demo/__init__.py b/src/py_action_demo/py_action_demo/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/py_action_demo/py_action_demo/fibonacci_client.py b/src/py_action_demo/py_action_demo/fibonacci_client.py new file mode 100644 index 0000000..8a3a40a --- /dev/null +++ b/src/py_action_demo/py_action_demo/fibonacci_client.py @@ -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() \ No newline at end of file diff --git a/src/py_action_demo/py_action_demo/fibonacci_server.py b/src/py_action_demo/py_action_demo/fibonacci_server.py new file mode 100644 index 0000000..b21523b --- /dev/null +++ b/src/py_action_demo/py_action_demo/fibonacci_server.py @@ -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 ...' 调用; + # - 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() \ No newline at end of file diff --git a/src/py_action_demo/resource/py_action_demo b/src/py_action_demo/resource/py_action_demo new file mode 100644 index 0000000..e69de29 diff --git a/src/py_action_demo/setup.cfg b/src/py_action_demo/setup.cfg new file mode 100644 index 0000000..ac7650e --- /dev/null +++ b/src/py_action_demo/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/py_action_demo +[install] +install_scripts=$base/lib/py_action_demo \ No newline at end of file diff --git a/src/py_action_demo/setup.py b/src/py_action_demo/setup.py new file mode 100644 index 0000000..03961b8 --- /dev/null +++ b/src/py_action_demo/setup.py @@ -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', + ], + }, +) \ No newline at end of file diff --git a/src/py_action_demo/test/test_action.py b/src/py_action_demo/test/test_action.py new file mode 100644 index 0000000..cfdee52 --- /dev/null +++ b/src/py_action_demo/test/test_action.py @@ -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}' \ No newline at end of file diff --git a/src/py_pubsub/launch/pubsub_launch.py b/src/py_pubsub/launch/pubsub_launch.py new file mode 100644 index 0000000..2831442 --- /dev/null +++ b/src/py_pubsub/launch/pubsub_launch.py @@ -0,0 +1,54 @@ +""" +pubsub_launch.py —— 启动一个 talker_py + listener_py 的最小 launch 文件。 + +什么是 launch? + ROS2 里,launch = 一种"启动多个节点 + 配置参数 + 跨机器调度"的 + Python 描述文件(取代了 ROS1 的 XML launch)。 + 由 `ros2 launch ` 调用。 + 关键概念: + 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', + }], + ), + ]) diff --git a/src/py_pubsub/package.xml b/src/py_pubsub/package.xml new file mode 100644 index 0000000..8c4628e --- /dev/null +++ b/src/py_pubsub/package.xml @@ -0,0 +1,35 @@ + + + + + + py_pubsub + 0.1.0 + Python talker/listener demo for ROS2 Humble + xs + Apache-2.0 + + + rclpy + std_msgs + + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + launch_testing + launch_testing_ros + + + + ament_python + + diff --git a/src/py_pubsub/py_pubsub/__init__.py b/src/py_pubsub/py_pubsub/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/py_pubsub/py_pubsub/publisher_member_function.py b/src/py_pubsub/py_pubsub/publisher_member_function.py new file mode 100644 index 0000000..65539ad --- /dev/null +++ b/src/py_pubsub/py_pubsub/publisher_member_function.py @@ -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() diff --git a/src/py_pubsub/py_pubsub/subscriber_member_function.py b/src/py_pubsub/py_pubsub/subscriber_member_function.py new file mode 100644 index 0000000..3a5ecb1 --- /dev/null +++ b/src/py_pubsub/py_pubsub/subscriber_member_function.py @@ -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() diff --git a/src/py_pubsub/resource/py_pubsub b/src/py_pubsub/resource/py_pubsub new file mode 100644 index 0000000..e69de29 diff --git a/src/py_pubsub/setup.cfg b/src/py_pubsub/setup.cfg new file mode 100644 index 0000000..3f886b9 --- /dev/null +++ b/src/py_pubsub/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/py_pubsub +[install] +install_scripts=$base/lib/py_pubsub diff --git a/src/py_pubsub/setup.py b/src/py_pubsub/setup.py new file mode 100644 index 0000000..02b57c7 --- /dev/null +++ b/src/py_pubsub/setup.py @@ -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//share//,被 colcon 和 ros2 工具识别。 +# +# colcon build 调用链: colcon → ament_python build_type → python setup.py build +# install → install/bin + install/lib/... +# + install//share// +# ============================================================================= + +# find_packages:setuptools 工具,自动扫描当前目录下含 __init__.py 的目录, +# 收录为要安装的 Python 包。 +# setup:setuptools 的入口函数,描述包元数据 / 文件 / 入口点。 +from setuptools import find_packages, setup + +# 这个 ROS 包的"名字"——必须与 package.xml 中的 一致,这是 ament +# 索引文件 resource/ 用到的标识。 +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/ + # 2) 元数据 package.xml ← share// + # 3) launch 文件 ← share//launch/ + data_files=[ + # 让 `ros2 pkg prefix py_pubsub` 能找到本包路径(ament_index 必备)。 + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + # 把 package.xml 拷到 share//,ros2 工具靠它识别包元数据。 + ('share/' + package_name, ['package.xml']), + # launch 文件必须装到 share//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', + ], + }, +) diff --git a/src/py_pubsub/test/test_pubsub_launch.py b/src/py_pubsub/test/test_pubsub_launch.py new file mode 100644 index 0000000..2b5a116 --- /dev/null +++ b/src/py_pubsub/test/test_pubsub_launch.py @@ -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]}' \ No newline at end of file diff --git a/src/py_srv/launch/srv_launch.py b/src/py_srv/launch/srv_launch.py new file mode 100644 index 0000000..da24106 --- /dev/null +++ b/src/py_srv/launch/srv_launch.py @@ -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', + ), + ]) \ No newline at end of file diff --git a/src/py_srv/package.xml b/src/py_srv/package.xml new file mode 100644 index 0000000..9f3c000 --- /dev/null +++ b/src/py_srv/package.xml @@ -0,0 +1,23 @@ + + + + py_srv + 0.1.0 + Python service demo: AddTwoInts server + client + xs + Apache-2.0 + + rclpy + example_interfaces + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + \ No newline at end of file diff --git a/src/py_srv/py_srv/__init__.py b/src/py_srv/py_srv/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/py_srv/py_srv/add_two_ints_client.py b/src/py_srv/py_srv/add_two_ints_client.py new file mode 100644 index 0000000..2803476 --- /dev/null +++ b/src/py_srv/py_srv/add_two_ints_client.py @@ -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() \ No newline at end of file diff --git a/src/py_srv/py_srv/add_two_ints_server.py b/src/py_srv/py_srv/add_two_ints_server.py new file mode 100644 index 0000000..91f7e0c --- /dev/null +++ b/src/py_srv/py_srv/add_two_ints_server.py @@ -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() \ No newline at end of file diff --git a/src/py_srv/resource/py_srv b/src/py_srv/resource/py_srv new file mode 100644 index 0000000..e69de29 diff --git a/src/py_srv/setup.cfg b/src/py_srv/setup.cfg new file mode 100644 index 0000000..6d3f497 --- /dev/null +++ b/src/py_srv/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/py_srv +[install] +install_scripts=$base/lib/py_srv \ No newline at end of file diff --git a/src/py_srv/setup.py b/src/py_srv/setup.py new file mode 100644 index 0000000..879b7a8 --- /dev/null +++ b/src/py_srv/setup.py @@ -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', + ], + }, +) \ No newline at end of file diff --git a/src/py_srv/test/test_srv.py b/src/py_srv/test/test_srv.py new file mode 100644 index 0000000..b4bc92e --- /dev/null +++ b/src/py_srv/test/test_srv.py @@ -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) \ No newline at end of file diff --git a/src/py_vision_demo/launch/vision_launch.py b/src/py_vision_demo/launch/vision_launch.py new file mode 100644 index 0000000..7a71092 --- /dev/null +++ b/src/py_vision_demo/launch/vision_launch.py @@ -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', + ), + ]) \ No newline at end of file diff --git a/src/py_vision_demo/package.xml b/src/py_vision_demo/package.xml new file mode 100644 index 0000000..bbc1e1d --- /dev/null +++ b/src/py_vision_demo/package.xml @@ -0,0 +1,25 @@ + + + + py_vision_demo + 0.1.0 + Python vision demo: simulated camera publishes Image, subscriber processes + xs + Apache-2.0 + + rclpy + sensor_msgs + cv_bridge + python3-numpy + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + \ No newline at end of file diff --git a/src/py_vision_demo/py_vision_demo/__init__.py b/src/py_vision_demo/py_vision_demo/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/py_vision_demo/py_vision_demo/fake_camera.py b/src/py_vision_demo/py_vision_demo/fake_camera.py new file mode 100644 index 0000000..7e68dbc --- /dev/null +++ b/src/py_vision_demo/py_vision_demo/fake_camera.py @@ -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() \ No newline at end of file diff --git a/src/py_vision_demo/py_vision_demo/image_processor.py b/src/py_vision_demo/py_vision_demo/image_processor.py new file mode 100644 index 0000000..df903f4 --- /dev/null +++ b/src/py_vision_demo/py_vision_demo/image_processor.py @@ -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() \ No newline at end of file diff --git a/src/py_vision_demo/resource/py_vision_demo b/src/py_vision_demo/resource/py_vision_demo new file mode 100644 index 0000000..e69de29 diff --git a/src/py_vision_demo/setup.cfg b/src/py_vision_demo/setup.cfg new file mode 100644 index 0000000..fe907d5 --- /dev/null +++ b/src/py_vision_demo/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/py_vision_demo +[install] +install_scripts=$base/lib/py_vision_demo \ No newline at end of file diff --git a/src/py_vision_demo/setup.py b/src/py_vision_demo/setup.py new file mode 100644 index 0000000..97ed590 --- /dev/null +++ b/src/py_vision_demo/setup.py @@ -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', + ], + }, +) \ No newline at end of file diff --git a/src/py_vision_demo/test/test_vision.py b/src/py_vision_demo/test/test_vision.py new file mode 100644 index 0000000..71f869e --- /dev/null +++ b/src/py_vision_demo/test/test_vision.py @@ -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() \ No newline at end of file diff --git a/start.ps1 b/start.ps1 new file mode 100644 index 0000000..0f5d45b --- /dev/null +++ b/start.ps1 @@ -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" diff --git a/start.sh b/start.sh new file mode 100644 index 0000000..30be4b5 --- /dev/null +++ b/start.sh @@ -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" diff --git a/tools/setup_venv.ps1 b/tools/setup_venv.ps1 new file mode 100644 index 0000000..c51055b --- /dev/null +++ b/tools/setup_venv.ps1 @@ -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 测试' \ No newline at end of file diff --git a/tools/setup_venv.sh b/tools/setup_venv.sh new file mode 100644 index 0000000..3911d90 --- /dev/null +++ b/tools/setup_venv.sh @@ -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 测试" \ No newline at end of file