跑一个ROS2 机器人系统

先看最终目标ROS2机器人系统 │ ros2 launch │ ┌──────────┼──────────┐ ↓ ↓ ↓ Node A Node B Node C │ │ │ └────── Topic / Service ──────┘ │ 数据和命令流动 │ ┌──────────────┼──────────────┐ ↓ ↓ ↓ TF2 URDF ros2_control │ │ │ ↓ ↓ ↓ 坐标关系 机器人模型 控制器 │ ↓ Hardware Interface │ ↓ EtherCAT/CAN │ ↓ 电机你现在先不要研究每一个节点怎么写。先搞清楚启动 → 数据 → 坐标 → 控制 → 硬件这条主线。1、第一层Bringup真实机器人 ROS2 项目里面你以后经常看到robot_bringup或者xxx_bringup这个名字非常值得记。它通常不是一个核心算法包而是负责把整个机器人系统启动起来。比如robot_bringup/ ├── launch/ │ ├── robot.launch.py │ ├── simulation.launch.py │ └── hardware.launch.py │ └── config/ ├── controller.yaml └── robot.yaml于是ros2 launch robot_bringup robot.launch.py可能一次启动robot_state_publisher controller_manager joint_state_broadcaster robot_controller RViz hardware_interface所以以后看到bringup你脑子里直接翻译成系统启动入口。2、第二层URDF——机器人“长什么样”现在假设机器人已经启动ROS2 需要知道机器人有多少个关节 每个关节连接什么 每个 link 多大 坐标系在哪里 关节怎么运动这就是URDF例如一个简单机械臂base_link │ joint1 │ link1 │ joint2 │ link2 │ joint3 │ link3URDF 描述的是Link Joint 几何模型 惯量 碰撞模型 关节限制真实机器人 URDF 往往很长。例如几十个关节left_hip left_knee left_ankle right_hip right_knee right_ankle ...直接写 URDF 很麻烦。所以工程里经常看到.xacro你可以暂时把它理解成更方便生成 URDF 的模板语言。因此以后看到urdf/ ├── robot.urdf.xacro ├── leg.xacro ├── arm.xacro └── sensor.xacro不要懵。本质上Xacro ↓ 生成 ↓ URDF ↓ 机器人描述3、第三层robot_state_publisher这是一个非常重要的 Node。它的任务可以简单理解为根据机器人模型和关节状态告诉 ROS2 每个 link 当前在哪里。例如base_link ↓ left_hip ↓ left_thigh ↓ left_knee ↓ left_shank如果膝关节转了joint position变化 ↓ robot_state_publisher ↓ TF变化 ↓ RViz里的机器人姿态变化4、TF2机器人坐标系的“关系网络”你可以把 TF2 理解成机器人所有坐标系之间的关系管理系统。例如人形机器人base_link │ ├── torso │ ├── left_leg │ ├── left_thigh │ ├── left_shin │ └── left_foot │ └── right_leg ├── right_thigh ├── right_shin └── right_foot相机head │ └── camera_linkIMUtorso │ └── imu_linkTF2 就是在维护camera_link ↓ head ↓ torso ↓ base_link这样的坐标关系。5、ros2_control到这里URDF ↓ 机器人是什么样TF2 ↓ 机器人各部分在哪里但是还缺一个东西怎么让机器人动这就是ros2_control你可以先把它理解成ROS2 和真实机器人硬件之间的标准控制框架。整体ROS2 │ Controller │ ↓ ros2_control │ Hardware Interface │ ┌───────┴───────┐ ↓ ↓ EtherCAT CAN ↓ ↓ 电机 电机所以Controller负责我要机器人怎么运动。而Hardware Interface负责怎么把这个运动命令真正送给电机。现在把整条链真正串起来你现在应该能看懂下面这个架构robot_bringup │ Launch启动 │ ┌──────────────┼──────────────┐ ↓ ↓ ↓ GMR robot_state controller Node publisher manager │ │ │ ↓ ↓ ↓ /joint_command TF2 ros2_control │ ↓ Hardware Interface │ ┌───────┴───────┐ ↓ ↓ MuJoCo EtherCAT │ │ ↓ ↓ 仿真 真实机器人旁边URDF/Xacro ↓ 机器人模型 ↓ robot_state_publisher ↓ TF2而参数YAML ↓ Controller / GMR / Driver最终Launch ↓ 把所有东西启动起来6、你现在看真实 ROS2 仓库应该按照这个顺序以后打开一个陌生机器人项目不要从.cpp开始。按照① README ↓ ② Workspace ↓ ③ Package ↓ ④ bringup ↓ ⑤ launch ↓ ⑥ config/YAML ↓ ⑦ URDF/Xacro ↓ ⑧ Node ↓ ⑨ Topic ↓ ⑩ TF2 ↓ ⑪ Controller ↓ ⑫ ros2_control ↓ ⑬ Hardware这就是你现在最需要建立的读 ROS2 项目的方法论。ROS2 为什么需要 URDF因为 ROS2 本身并不知道“你的机器人到底长什么样”比如宇树 G1head │ torso │ pelvis / \ left_leg right_leg │ │ foot footROS2 需要知道有哪些关节有哪些身体部件谁连接谁关节能不能转能转多少每个部件的坐标系在哪里碰撞模型是什么质量、惯量是什么这就是URDF主要解决的问题。你只需要先记住URDF 机器人结构说明书它描述的是机器人本身。例如机器人 │ ├── base_link │ ├── pelvis │ ├── torso │ ├── left_arm │ ├── right_arm │ ├── left_leg │ └── right_leg这些叫Link也就是机器人的“身体部件”。7、Link 是什么非常简单Link 一块刚体例如大腿 小腿 脚 手臂 前臂 手 躯干都可以看成 Link。例如大腿 ↓ 小腿 ↓ 脚可以理解成Link ↓ Joint ↓ Link ↓ Joint ↓ Link8、Joint 是什么Joint 就是两个 Link 之间怎么连接、怎么运动。例如机器人腿大腿 │ │ 髋关节 ↓ 小腿 │ │ 膝关节 ↓ 脚对应thigh │ hip_joint │ shin │ knee_joint │ foot所以Link 是东西Joint 是东西之间的连接和运动关系。这个一定记住。Joint 有哪些类型目前最需要知道三个1. Fixed固定连接torso │ fixed │ camera摄像头固定在头上就可以是 Fixed。2. Revolute旋转关节。机器人绝大部分关节都属于这一类。例如大腿 │ 髋关节 ↻ │ 小腿3. Prismatic直线运动←────→机器人传统机械臂里可能比较常见。对于你现在的 humanoid 运控工作重点理解 Fixed Revolute 就够了。Xacro 帮你生成 URDF 的模板语言你可以理解成Xacro ↓ 生成 ↓ URDF所以URDF 是最终机器人模型Xacro 是更方便写 URDF 的方式。9、robot_state_publisher现在我们有了URDF ↓ 知道机器人有哪些 Link / Joint但是还有一个问题机器人现在的关节到底转到了哪里比如左膝关节 30°那么小腿应该在哪里这时候就需要robot_state_publisher它的作用可以简单理解成URDF Joint State ↓ 计算 ↓ 各个 Link 的空间关系 ↓ 发布 TF10、TF2TF2 管理机器人各个坐标系之间空间关系的系统。例如base_link ↓ pelvis ↓ torso ↓ head ↓ camera_linkTF2 就是在告诉 ROS2camera_link 相对于 head 在哪里 head 相对于 torso 在哪里 torso 相对于 base_link 在哪里机器人学ROS2连杆Link关节Joint坐标系TF Frame坐标变换TF Transform机器人结构URDF参数化机器人模型Xacro根据关节状态计算 TFrobot_state_publisher这个地方非常重要。Joint State告诉你左膝 position 0.5 rad velocity ... effort ...它描述的是关节状态TF告诉你left_foot 相对于 base_link 在哪里它描述的是坐标系之间的位置和姿态关系所以Joint State ↓ 关节转了多少 ↓ robot_state_publisher ↓ TF ↓ 各个坐标系在哪里11、RViz以后打开rviz2你会看到机器人模型。背后的逻辑其实就是URDF ↓ Robot Model ↓ TF ↓ RViz ↓ 显示机器人所以 RViz 不是什么“神奇的机器人软件”。它主要是在把 ROS2 中的数据可视化出来。URDF / Xacro │ 机器人结构模型 │ Link Joint │ ↓ robot_state_publisher │ ↓ TF2 │ 各坐标系之间的空间关系 │ ↓ RViz Joint State ───────┘而控制链是另外一条GMR ↓ Joint Command ↓ Controller ↓ ros2_control ↓ Hardware Interface ↓ EtherCAT ↓ 电机两条线最后共同组成一个机器人系统。12、Static TF 和 Dynamic TF这个也不用学复杂。Static TF不会变化。例如head ↓ camera_link摄像头牢牢固定在头上。那么head → camera_link基本不会变化。这就是静态 TF。Dynamic TF会随着机器人运动变化。例如torso ↓ upper_arm ↓ forearm ↓ hand手臂动起来以后torso → hand一直在变化。所以这是动态 TF以后调机器人系统你会经常用。查看 TFros2 topic echo /tf静态 TFros2 topic echo /tf_static查看节点ros2 node list查看机器人状态ros2 topic echo /joint_states查看 TF 树ros2 run tf2_tools view_frames查看两个坐标系之间的关系ros2 run tf2_ros tf2_echo base_link camera_link不用死记以后真正调系统的时候再查命令即可。13、ros2_controlROS2 │ ↓ Controller │ ↓ ros2_control │ ┌────────┴────────┐ ↓ ↓ Command Interface State Interface ↓ ↑ └──── Hardware ───┘ │ ↓ Hardware Interface │ ┌────────┴────────┐ ↓ ↓ EtherCAT MuJoCo ↓ ↓ 电机 仿真上面 控制算法 ↓ Controller ↓ ros2_control ↓ Hardware Interface ↓ 具体硬件所以上面的控制逻辑尽量不要关心底层电机到底是 EtherCAT、CAN 还是仿真。底层通过 Hardware Interface 对接。Controller它不希望知道“下面到底是 EtherCAT 还是 MuJoCo”所以Controller ↓ ros2_control ↓ Hardware Interface ↓ 具体硬件例如真机Controller ↓ ros2_control ↓ G1 Hardware Interface ↓ EtherCAT ↓ 电机仿真Controller ↓ ros2_control ↓ Simulation Interface ↓ MuJoCo上面的 Controller 可以保持基本一致。这就是 ros2_control 很重要的意义。14、 Hardware Interface简单理解Hardware Interface ROS2 和真实硬件之间的翻译层。上面说左膝目标位置 0.5 radHardware Interface 负责把这个东西变成底层硬件能理解的东西。例如ROS2 ↓ position command ↓ Hardware Interface ↓ EtherCAT PDO ↓ 电机驱动器 ↓ 电机然后电机反馈编码器 ↓ EtherCAT ↓ Hardware Interface ↓ ROS2 ↓ joint_state于是形成闭环Command ↓ ROS2 → Hardware → 电机 ↑ FeedbackJoint Command ↓ Controller ↓ ros2_control ↓ Hardware Interface ↓ EtherCAT ↓ Motor ↓ Encoder ↓ Hardware Interface ↓ Joint State ↓ ROS2所以你会发现ROS2 并没有改变机器人控制的本质。它主要是在把整个机器人软件系统组织起来。名称大白话Controller决定机器人应该怎么动ros2_control控制框架Hardware InterfaceROS2 和硬件之间的翻译层Command Interface给硬件的目标State Interface硬件反馈回来的状态这五个概念搞明白后面学习就会非常顺。15、 Controller 到底怎么控制关节你现在最需要搞明白的是这一条上层算法 ↓ Controller ↓ Command Interface ↓ Hardware Interface ↓ 电机以及反馈编码器 ↓ Hardware Interface ↓ State Interface ↓ ControllerController 不等于“电机控制器”这是刚接触 ros2_control 很容易混淆的地方。这里的Controller更接近ROS2 中负责产生关节控制目标的软件模块。例如Joint Trajectory Controller收到左膝 → 0.5 rad 右膝 → 0.6 rad然后产生对应的关节命令。真正和电机通信的是后面的Hardware Interface所以Controller 算“我要什么” Hardware Interface 想办法把这个要求送到硬件16、 Command Interface假设一个关节left_knee可能存在position velocity effort这就是不同的Command Interface。例如position command意思“我希望这个关节到这个位置。”或者velocity command意思“我希望这个关节以这个速度运动。”或者effort command意思“我希望给这个关节这个力/力矩控制量。”你可以把它理解成Controller ↓ “给左膝 0.5 rad” ↓ position command interface17、 State Interface反过来机器人会告诉 ROS2left_knee position 0.48 left_knee velocity 0.12 left_knee effort ...这些就是State Interface所以Command Interface ↓ 输出 ↓ Hardware ↑ 反馈 ↑ State Interface这是 ros2_control 最核心的一组概念。18、 Controller Manager现在再加入一个东西Controller Manager它可以理解成Controller 的管理者。例如你的机器人可能同时有joint_state_broadcaster arm_controller leg_controller head_controllerController Manager 负责加载 Controller启动 Controller停止 Controller切换 Controller管理 Controller 使用哪些接口所以结构变成Controller Manager / | \ ↓ ↓ ↓ Controller Controller Controller ↓ ros2_control ↓ Hardware Interface假设控制频率500 Hz一个周期大致可以理解成① 读取硬件反馈 ↓ ② 更新 State Interface ↓ ③ Controller 读取状态 ↓ ④ Controller 计算 ↓ ⑤ 产生 Command Interface ↓ ⑥ Hardware Interface 写入硬件 ↓ ⑦ 电机执行 ↓ ⑧ 编码器反馈 ↓ 回到①形成闭环┌──────────────────────┐ │ ↓ 反馈 → State → Controller → Command ↑ ↓ └──── Hardware ←──────────┘这就是你之前学的闭环控制在 ROS2 中的一个软件实现形式。上层算法 ↓ Joint Target ↓ Controller ↓ Command Interface ↓ ros2_control ↓ Hardware Interface ↓ EtherCAT/CAN ↓ 电机 ↑ Encoder ↑ State Interface ↑ ros2_controlController 是什么→ 产生控制目标的软件模块。Command Interface 是什么→ 向硬件发送什么类型的控制命令。State Interface 是什么→ 从硬件读取什么状态。Controller Manager 是什么→ 管理 Controller 的加载、启动、停止和切换。19、 Hardware Interface你可以把它理解成一个“适配器”ROS2世界 ↓ Command / State Interface ↓ Hardware Interface ↓ 机器人硬件世界上面只认识position velocity effort下面可能是EtherCAT PDO CAN 串口 SDK MuJoCo所以 Hardware Interface 的任务就是把 ROS2 的标准接口翻译成具体硬件能理解的通信。read()从机器人读取状态电机 ↓ EtherCAT ↓ Hardware Interface ↓ read() ↓ State Interface ↓ ROS2例如position 0.52 velocity 0.13 effort ...write()把 ROS2 的目标写给机器人Controller ↓ Command Interface ↓ write() ↓ Hardware Interface ↓ EtherCAT ↓ 电机例如left_knee position command 0.6所以你可以直接记read 从硬件读write 向硬件写Controller │ ↓ Command Interface │ ↓ Hardware Interface │ write │ ↓ EtherCAT │ ↓ 电机驱动 │ ↓ 电机 │ 编码器 │ ↓ EtherCAT │ read │ ↓ Hardware Interface │ ↓ State Interface │ ↓ Controller20、 以后遇到机器人“不动”你应该怎么想不要第一反应就怀疑电机。按照链路检查① Controller启动了吗 ↓ ② Controller有没有产生Command ↓ ③ Command Interface存在吗 ↓ ④ Hardware Interface激活了吗 ↓ ⑤ write()有没有执行 ↓ ⑥ EtherCAT有没有发送 ↓ ⑦ 驱动器有没有收到 ↓ ⑧ 电机有没有执行反馈则反过来电机 ↓ 编码器 ↓ EtherCAT ↓ read() ↓ State Interface ↓ ROS2沿着数据链逐段定位而不是一上来猜问题。21、 Noitom和宇树G1完整机器人系统先看最终全貌人体 ↓ Motion Capture / \ Xsens Noitom \ / ↓ ↓ MotionInput ↓ NoitomFrame ↓ GMR ↓ Joint Target ↓ Controller ↓ ros2_control ↓ Hardware Interface ↓ ┌───────┴────────┐ ↓ ↓ EtherCAT MuJoCo ↓ ↓ G1 真机 仿真1. Motion Capture 层Xsens / Noitom ↓ 人体骨骼运动数据例如人体左手在哪里 人体膝盖怎么弯 人体躯干怎么旋转这些数据本质上是人体运动状态。2. MotionInput你之前已经做过这个抽象XsensUdpSource ↓ NoitomFrame或者NoitomMocapApiSource ↓ NoitomFrame所以 GMR 不需要关心“数据到底来自 Xsens 还是 Noitom”它只需要NoitomFrame这就是软件工程里面非常典型的接口抽象 多种实现。3. GMR然后NoitomFrame ↓ GMR ↓ Robot Joint Target这里发生的是人体动作 → 机器人动作例如人体膝盖角度 ↓ 机器人腿部目标关节角所以 GMR 属于运动重定向 / IK / 运动映射层。它不是EtherCAT电机驱动ControllerHardware Interface这些东西不要混。4. Joint Target 到 ControllerGMR 算出来q_target [ q1, q2, q3, ... ]这个东西进入控制系统。可以理解为GMR ↓ “我希望机器人每个关节到这些位置”然后 Controller 再负责目标 ↓ 控制逻辑 ↓ 输出控制命令5. ros2_control接下来Controller ↓ ros2_control这里的核心作用已经学过把上层控制和底层硬件解耦。于是上面不需要关心EtherCAT PDO CAN 驱动器寄存器 编码器协议这些交给下面。6. Hardware Interface继续ros2_control ↓ Hardware InterfaceHardware Interface 把position command velocity command effort command转换成底层硬件需要的数据。例如Command ↓ write() ↓ EtherCAT PDO反馈EtherCAT PDO ↓ read() ↓ Joint State7. 真机路线于是完整真机链路就是人体 ↓ 动捕 ↓ NoitomFrame ↓ GMR ↓ Joint Target ↓ Controller ↓ ros2_control ↓ Hardware Interface ↓ EtherCAT ↓ 电机 ↓ 编码器 ↓ EtherCAT ↓ Hardware Interface ↓ Joint State ↓ Controller这就是一个完整闭环。8. MuJoCo 路线如果换成仿真人体 ↓ 动捕 ↓ GMR ↓ Joint Target ↓ Controller ↓ ros2_control ↓ Simulation Hardware ↓ MuJoCo因此你之前使用 MuJoCo 的经验现在就接上了。你可以把真实 Hardware Interface换成仿真 Hardware Interface上层控制逻辑仍然可以保持一致。9. 那么 ROS2 在整个系统中到底是什么ROS2 更像整个机器人软件系统的通信 节点组织 参数配置 启动管理 控制框架集成平台。所以你可以把整个系统看成┌─────────────────────────────┐ │ ROS2 │ │ │ │ Motion Capture │ │ ↓ │ │ GMR │ │ ↓ │ │ Controller │ │ ↓ │ │ ros2_control │ │ ↓ │ │ Hardware Interface │ └────────────┬────────────────┘ ↓ EtherCAT ↓ G122、 以后拿到一个陌生 ROS2 机器人项目怎么办这是这套学习路线最终要达到的能力。不要一上来就看源码。先找这几个东西第一启动入口找launch/ bringup/问系统怎么启动第二机器人模型找urdf/ xacro/问机器人结构是什么第三控制器找controller/ config/ yaml问谁产生控制命令第四硬件找hardware/ hardware_interface/问ROS2 怎么连接真实机器人第五上层算法找gmr/ ik/ motion/ planner/问机器人为什么要这么运动第六通信找msg/ srv/ action/ topic问数据怎么流动看到robot_bringup robot_description robot_controller robot_hardware gmr xxx_msgs你应该能大概判断robot_bringup → 系统启动 robot_description → URDF/Xacro robot_controller → 控制器 robot_hardware → 硬件接口 gmr → 运动重定向 xxx_msgs → ROS2数据接口然后再进入源码。