从0到1独立搭建机器人软件系统

从0到1独立搭建机器人软件系统 我们接下来按这条主线学机器人系统工程师 │ ├── ① ROS2基础 │ ├── Workspace / Package │ ├── Node │ ├── Topic │ ├── Service │ ├── Action │ ├── Parameter │ ├── Launch │ └── rosbag │ ├── ② ROS2机器人基础设施 │ ├── TF / TF2 │ ├── URDF / Xacro │ ├── RViz │ └── diagnostics │ ├── ③ C工程化 │ ├── 类 / 接口 │ ├── RAII │ ├── 智能指针 │ ├── 多线程 │ ├── Strategy │ ├── Factory │ ├── Adapter │ └── Observer │ ├── ④ 标准机器人驱动 │ ├── CAN / EtherCAT设备 │ ├── 初始化 │ ├── 状态机 │ ├── 心跳 │ ├── 超时 │ ├── 重连 │ └── diagnostics │ ├── ⑤ ros2_control │ ├── Hardware Interface │ ├── Controller Manager │ ├── Joint State │ └── Controller │ ├── ⑥ SDK产品化 │ ├── 统一接口 │ ├── 配置 │ ├── 插件 │ ├── 错误码 │ ├── API │ └── 安装发布 │ ├── ⑦ 网络与DDS │ ├── UDP │ ├── TCP │ ├── RS485 │ ├── IP / Route / DNS │ ├── Docker网络 │ └── DDS QoS │ ├── ⑧ 应用层 │ ├── 状态机 │ ├── 任务队列 │ ├── 优先级 │ ├── Cancel / Pause / Resume │ └── 异常恢复 │ ├── ⑨ 软件测试 │ ├── GoogleTest │ ├── Mock │ ├── CTest │ ├── Coverage │ └── CI/CD │ └── ⑩ 最终项目 └── ROS2多设备统一SDK与机器人驱动平台这条路线的关键是不要把 ROS2 当作一套命令去背。你真正要理解的是机器人硬件 ↓ 驱动层 ↓ SDK层 ↓ ROS2通信层 ↓ 机器人功能模块 ↓ 应用/任务层ROS2只是贯穿其中的一套“机器人软件基础设施”。ROS2到底是什么我们先不敲代码先把脑子里的模型建立起来很多刚学 ROS2 的人会觉得ros2 topic list ros2 node list ros2 run ros2 launch colcon build命令很多很乱。其实 ROS2 的核心非常简单让很多彼此独立的程序能够方便地交换数据。例如你现在做人形机器人动捕可以想成动捕程序 ↓ GMR ↓ 机器人控制如果全部写在一个程序int main() { read_mocap(); run_gmr(); send_robot(); }开始的时候很舒服但是以后你想增加MuJoCo 日志 RViz 状态监控 录包 机器人真机 调试工具程序就会越来越大ROS2的思想是Mocap Node ↓ ROS2 ↓ GMR Node ↓ ROS2 ↓ Robot Node每个程序只负责自己的事情这种独立程序在 ROS2 里叫Node也就是节点。Node到底是什么不要把 Node 想复杂。它本质就是一个运行中的程序模块例如xsens_node gmr_node robot_driver_node imu_node logger_node每个节点干一件事情。例如xsens_node 职责 读取动捕数据然后gmr_node 职责 人体动作 ↓ 机器人动作再例如robot_driver_node 职责 机器人指令 ↓ CAN / EtherCAT ↓ 电机所以第一条非常重要的工程思想就是一个大型机器人程序不是一个巨大 main()而是很多职责清晰的模块。那这些Node怎么交流ROS2主要给你四种方式。Topic Service Action Parameter暂时先记住这个关系持续不断的数据 ↓ Topic 问一次回答一次 ↓ Service 执行一个需要时间的任务 ↓ Action 程序的配置 ↓ Parameter这是后面非常重要的一张图。Topic假设 IMU 一直发送姿态 角速度 加速度50 Hz第1帧 第2帧 第3帧 第4帧 ...这就是典型的Topic比如/imu/dataIMU节点imu_driver │ │ publish ↓ /imu/data其他节点想要这个数据state_estimator ↑ │ subscribe │ /imu/data因此会看到两个极其重要的词Publisher SubscriberPublisher发布数据。Subscriber接收数据。你可以把它类比成广播。电台 ↓ 广播频道 ↓ 无数收音机ROS2Publisher ↓ Topic ↓ Subscriber Subscriber Subscriber这也是为什么 Topic 特别适合IMU数据 关节状态 激光雷达 相机 机器人状态 动捕数据 控制指令Service但是有些东西不应该不停发送。例如“机器人现在是否已经初始化”你问一次请求 ↓ 机器人驱动 ↓ 回答类似Client │ │ Request ↓ Server │ │ Response ↓ Client这就是Service例如以后我们的驱动可以有/reset_motor /enable_motor /clear_error你发送enable true机器人回答success true所以Topic 流式通信Service 请求/响应ActionAction刚开始最容易搞不懂。比如“让机器人走到目标位置。”这个任务可能需要10秒这期间你希望知道现在走到哪了甚至取消任务。于是发送Goal ↓ 机器人执行 ↓ Feedback ↓ Feedback ↓ Feedback ↓ Result而且可以Cancel这就是 Action。因此以后做机器人任务层回零 导航 机械臂移动 执行动作 抓取Action非常常见。Parameter再比如你的机器人程序里面有double max_speed 1.0; double timeout 0.5; std::string can_interface can0;如果全写死max_speed 1.0;每次修改都要改代码 ↓ 重新编译 ↓ 重新运行很麻烦。ROS2允许max_speed: 1.0 timeout: 0.5 can_interface: can0这些就是Parameter以后甚至可以配置robot: can_interface: can0 baudrate: 1000000 timeout_ms: 100 motor: max_velocity: 3.0 max_torque: 20.0这就开始有“产品化软件”的味道了。把今天这几个概念放进真实机器人假设以后我们自己做ROS2多设备统一SDK与机器人驱动平台最初架构可能长这样ROS2 Application │ ┌────────┴────────┐ │ │ Service Action │ │ ↓ ↓ Robot Driver Node ↑ ↓ Topic Topic │ │ command joint_state │ │ └──────┬───────┘ ↓ Device SDK ↓ ┌────────┴────────┐ ↓ ↓ CAN Motor Sim Sensor看到这里你应该开始发现我们实际上要做的是把原来 C程序 ↓ CAN ↓ 设备升级成ROS2 ↓ Driver ↓ SDK ↓ CAN / EtherCAT ↓ 设备这正是机器人系统工程师的重要能力。Workspace、Package、colcon 到底是什么这几个词刚接触 ROS2 时特别容易混先不看定义直接看最终我们的项目humanoid_ros2_system/ ├── mocap_driver ├── gmr_retarget ├── robot_output ├── humanoid_description ├── mujoco_bridge ├── ethercat_driver ├── ethercat_ros2_control └── system_tests你以后不会把这么多东西写进一个 C 工程。ROS2 会把它们组织成Workspace │ └── src/ │ ├── Package A │ ├── Package B │ ├── Package C │ └── Package D先记一句Workspace 是整个开发工作区Package 是里面一个个相对独立的软件模块。一、Workspace 是什么假设我们建立~/humanoid_ros2_ws这个就是一个ROS2 Workspace。你可以把它理解成一个“大项目文件夹”。以后你的动捕、GMR、MuJoCo、EtherCAT都可以放在这个 Workspace 里面。比如humanoid_ros2_ws/ │ ├── src/ │ ├── mocap_driver/ │ ├── gmr_retarget/ │ ├── mujoco_bridge/ │ └── ethercat_ros2_control/ │ ├── build/ ├── install/ └── log/这四个目录非常重要。二、src 是干什么的src最容易理解src source也就是你的源代码放这里。比如src/ ├── mocap_driver ├── gmr_retarget └── robot_output这些目录通常就是一个个ROS2 Package官方 ROS2 教程也建议 Workspace 下使用src目录组织各个 Package。(Cyberlab FTP)三、Package 又是什么Package 可以理解成ROS2 项目中的一个功能模块。例如mocap_driver负责PNLink / Xsens / Axis ↓ 读取人体姿态 ↓ 发布 ROS2 Topic那么我们就把它做成一个 Package。另一个gmr_retarget负责人体姿态 ↓ GMR ↓ 机器人关节目标它又是一个 Package。所以Workspace │ ├── Packagemocap_driver │ ├── Packagegmr_retarget │ └── Packagerobot_output非常像一家公司 │ ├── 动捕部门 ├── GMR部门 └── 输出部门Workspace 是公司。Package 是部门。Node 则可以理解成部门里面实际干活的人。四、Package 和 Node 不一样这是新手特别容易混淆的地方。比如mocap_driver这是Package里面可能有mocap_driver_node mocap_monitor_node mocap_test_node三个 Node。关系是Package │ ├── Node ├── Node └── Node所以Package 是代码组织单位。而Node 是程序运行单位。这一点非常重要。五、那 colcon 是什么现在我们已经写了mocap_driver gmr_retarget robot_output这些都是 C 代码C 代码不能直接运行必须经历.cpp ↓ 编译 ↓ 可执行程序普通 C 项目可能使用cmake makeROS2 项目一般使用colcon build你可以先把colcon理解成ROS2 工作空间的统一编译工具。官方 ROS2 教程也是使用colcon构建整个 Workspace。(ROS 2 Repository)六、colcon 不等于 CMake这里顺手把一个以后经常遇到的问题讲清楚。你会同时看到CMake ament_cmake colcon很容易乱。先建立这个关系colcon ↓ 找到所有 Package ↓ 调用构建系统 ↓ ament_cmake ↓ CMake ↓ g ↓ C程序先不用深究内部机制可以粗略理解CMake负责告诉编译器有哪些.cpp 链接哪些库 生成什么程序ament_cmakeROS2 在 CMake 上面加的一套 ROS 工程规则例如find_package(rclcpp REQUIRED)告诉系统我要使用 ROS2 的 C 库。官方 ROS2 C Package 使用ament_cmake作为常见构建类型而colcon用于构建工作空间。(Cyberlab FTP)colcon站得更高假设src/ ├── A ├── B ├── C ├── D ├── E └── F你不用进入每一个目录cd A cmake ... make cd B cmake ... make而是在 Workspace 根目录colcon build它帮你组织构建所以记CMake ↓ 构建一个C工程 ament_cmake ↓ 让CMake适应ROS2 Package colcon ↓ 管理、构建整个ROS2 Workspace现在知道这个程度完全够。七、我们现在真正创建自己的 Workspace因为真机环境是 Ubuntu 22.04我们这条实战主线优先按Ubuntu 22.04 ROS2 Humble C来做先打开终端检查 ROS2echo $ROS_DISTRO理想情况下应该看到humble如果没有输出可以source /opt/ros/humble/setup.bash再执行echo $ROS_DISTRO八、创建我们的正式学习工作区不要叫官方教程里的dev_ws我们直接为以后项目准备mkdir -p ~/humanoid_ros2_ws/src进入cd ~/humanoid_ros2_ws看看tree如果没有treesudo apt install tree应该类似humanoid_ros2_ws/ └── src/现在这里还什么都没有。九、创建第一个 Package进入cd ~/humanoid_ros2_ws/src执行ros2 pkg create --build-type ament_cmake mocap_demo --dependencies rclcpp std_msgs这条命令暂时不用背。拆开看。ros2 pkg create意思创建 ROS2 Package。--build-type ament_cmake意思我要写 C ROS2 Package。mocap_demoPackage 名字。--dependencies rclcpp std_msgs告诉它我的项目依赖rclcpp std_msgs其中rclcpp非常重要它就是ROS2 的 C 客户端库。以后我们写#include rclcpp/rclcpp.hpp基本都会碰到它。十、现在看看 Package 长什么样执行cd ~/humanoid_ros2_ws tree src应该类似src/ └── mocap_demo ├── CMakeLists.txt ├── include │ └── mocap_demo ├── package.xml └── src这里先认识两个文件。package.xml可以理解成这个 Package 的身份证。里面写我叫什么 版本多少 作者是谁 依赖什么比如namemocap_demo/name dependrclcpp/depend dependstd_msgs/depend以后安装依赖、发布 Package 都会用它。十一、CMakeLists.txt 是什么它是告诉 CMake 怎么编译这个 Package。以后我们会逐渐看到find_package(rclcpp REQUIRED) add_executable(pose_publisher src/pose_publisher.cpp) ament_target_dependencies( pose_publisher rclcpp ) install( TARGETS pose_publisher DESTINATION lib/${PROJECT_NAME} )现在先不要背。你只要知道.cpp代码 ↓ CMakeLists.txt ↓ 告诉编译器怎么编 ↓ 生成程序即可。十二、第一次 colcon build现在cd ~/humanoid_ros2_ws注意不要在src里面执行。执行colcon build正常情况下会出现类似Starting mocap_demo Finished mocap_demo Summary: 1 package finished然后再执行tree -L 1你会突然发现humanoid_ros2_ws/ ├── build ├── install ├── log └── src这就是为什么我前面先给你看这四个目录。十三、build / install / log 是什么buildbuild/可以理解成编译过程中产生的中间文件。一般不要自己改。installinstall/非常重要。最终编译好的程序 库 配置 环境文件很多东西会被安装到这里。loglog/记录colcon构建日志编译失败时这里可以帮助排查。所以src 你写的东西 build 编译中间产物 install 编译后的可用结果 log 构建日志这一组一定要理解。十四、为什么 build 完还要 source这是 ROS2 新手的又一个大坑。你编译colcon build之后并不代表当前终端马上就认识你新编译的软件。通常还要source install/setup.bash于是整个流程变成修改代码 ↓ colcon build ↓ source install/setup.bash ↓ ros2 run ...以后你会执行这个流程无数次。十五、为什么还要 source /opt/ros/humble/setup.bash这里其实有两层环境。第一层source /opt/ros/humble/setup.bash意思类似把系统安装的 ROS2 Humble 环境加载进来。这叫Underlay然后source ~/humanoid_ros2_ws/install/setup.bash意思再把我自己写的项目环境叠上去。这叫Overlay直观来看Ubuntu ↓ ROS2 Humble ↓ 你的 humanoid_ros2_ws所以以后你的 GMR、动捕、EtherCAT Package 都相当于建立在 ROS2 Humble 之上。十六、把它和你原来的 C 项目对应起来你以前可能是gmr_project/ ├── src/ ├── include/ └── CMakeLists.txt然后mkdir build cd build cmake .. make现在变成 ROS2humanoid_ros2_ws │ └── src │ ├── mocap_driver │ ├── src │ ├── include │ ├── CMakeLists.txt │ └── package.xml │ ├── gmr_retarget │ └── robot_output然后你不再分别编译 mocap 编译 GMR 编译 robot_output而是cd ~/humanoid_ros2_ws colcon build这就是 ROS2 工程组织方式开始给你带来的价值。这一课真正需要记住的只有5句话Workspace 整个ROS2开发工作区Package 一个相对独立的软件功能模块Node Package里面运行起来的功能程序ament_cmake ROS2 C Package常用构建体系colcon build 构建整个Workspace再加上最重要的目录humanoid_ros2_ws/ │ ├── src ← 我写的 ├── build ← 编译中间文件 ├── install ← 编译结果 └── log ← 编译日志今天你的实际练习在 Ubuntu 22.04 机器上依次执行source /opt/ros/humble/setup.bash mkdir -p ~/humanoid_ros2_ws/src cd ~/humanoid_ros2_ws/src ros2 pkg create \ --build-type ament_cmake \ mocap_demo \ --dependencies rclcpp std_msgs cd ~/humanoid_ros2_ws colcon build source install/setup.bash tree -L 3如果这一步成功下一课我们就不再讲概念了直接写第一个真正属于你项目的 C ROS2 节点pose_publisher │ │ 20ms ↓ /mocap/human_pose然后再写/mocap/human_pose ↓ pose_subscriber ↓ 帧号 频率 延迟到那里你会第一次真正把C → Node → Publisher → Topic → Subscriber整条链跑起来。如果你想按学习计划稳定推进我也可以每天提醒你继续当天的 ROS2 任务。