3.12 MoveIt 与真实机械臂通信
一、核心问题：MoveIt 如何控制真实机械臂？
在前面的章节中，我们学会了用 MoveIt 在 RViz 中规划轨迹。但你可能会问：MoveIt 规划好的轨迹是怎样发送给真实机械臂的？机械臂又如何告诉 MoveIt 自己现在的位置？
本章的答案是：双向通信。
- 下行（MoveIt → 机械臂）：MoveIt 通过
FollowJointTrajectoryAction 发送轨迹 - 上行（机械臂 → MoveIt）：机械臂通过
/joint_statesTopic 实时反馈当前关节角度
没有这两条线，MoveIt 和机械臂就是"形同陌路"的两个系统。
二、MoveIt 配置包的关键文件详解
在 episode1_urdf_student_moveit/config/ 目录下，有几个关键配置文件需要理解：
2.1 文件对比表
| 配置文件 | 作用 | 层级 |
|---|---|---|
moveit_controllers.yaml |
告诉 MoveIt 把轨迹发送给谁（Action topic） | 上层（MoveIt） |
ros2_controllers.yaml |
配置实际执行轨迹的控制器软件 | 下层（执行层） |
episode1_urdf_1113.ros2_control.xacro |
定义硬件接口（Demo 用模拟硬件） | 底层（硬件抽象） |
2.2 moveit_controllers.yaml（MoveIt 发送轨迹的配置）
文件路径： config/moveit_controllers.yaml
# MoveIt uses this configuration for controller management
moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
moveit_simple_controller_manager:
controller_names:
- episode_arm_controller # 控制器名称（必须与 ros2_controllers.yaml 一致）
episode_arm_controller:
type: FollowJointTrajectory # Action 类型（固定值）
joints: # 控制哪些关节
- joint1
- joint2
- joint3
- joint4
- joint5
- joint6
action_ns: follow_joint_trajectory # Action 命名空间
default: true
逐行解读：
| 配置项 | 含义 | 对应的 ROS2 实体 |
|---|---|---|
controller_names |
MoveIt 要连接的控制器列表 | 需要与 ros2_controllers.yaml 中的控制器名匹配 |
type: FollowJointTrajectory |
使用标准的轨迹执行 Action | control_msgs/action/FollowJointTrajectory |
action_ns: follow_joint_trajectory |
Action 的命名空间 | 最终 topic: /episode_arm_controller/follow_joint_trajectory |
joints |
需要控制的关节名称 | 必须与 URDF 中的关节名一致 |
重要概念：MoveIt 是 Action Client，不是 Server！
┌─────────────────────────────────────────────────────────────────────────┐
│ Action 通信模型 │
├─────────────────────────────────────────────────────────────────────────┤
│ │
│ ┌──────────────┐ Goal (轨迹) ┌──────────────────────┐ │
│ │ │ ─────────────────────────▶ │ │ │
│ │ MoveIt │ │ episode_arm_ │ │
│ │ │ Feedback │ controller │ │
│ │ (Action │ ◀───────────────────────── │ │ │
│ │ Client) │ │ (Action Server) │ │
│ │ │ Result │ │ │
│ │ "请求者" │ ◀───────────────────────── │ "执行者" │ │
│ └──────────────┘ └──────────────────────┘ │
│ │
└─────────────────────────────────────────────────────────────────────────┘
为什么 MoveIt 是 Client？
回忆 Action 的设计原则：
- Action Client：发起请求的一方（"我想让你做某事"）
- Action Server：执行请求的一方（"我来帮你执行"）
在机械臂控制场景中：
- MoveIt 是"大脑"：负责规划轨迹，但它自己不知道怎么控制电机
- 控制器是"手脚"：负责执行轨迹，它知道怎么控制电机
所以逻辑是：
MoveIt (Client): "我规划好了一条轨迹，请你执行" → 发送 Goal
Controller (Server): "好的，我正在执行..." → 返回 Feedback
Controller (Server): "执行完成！" → 返回 Result
类比理解：
| 角色 | 类比 | 职责 |
|---|---|---|
| MoveIt (Client) | 乘客 | "请送我去机场"（发出请求，不管怎么开车） |
| Controller (Server) | 出租车司机 | 实际驾驶，定期反馈位置，最后报告"到达" |
代码验证（在真实控制节点中我们创建的是 Server）：
# trajectory_printer_node.py 中创建的是 Action Server
self._action_server = ActionServer(
self,
FollowJointTrajectory,
'episode_arm_controller/follow_joint_trajectory', # topic 名
self.follow_joint_trajectory_callback # 处理请求
)
这正是因为我们的节点替代了控制器的角色，需要接收并"执行"MoveIt 发来的轨迹
2.3 ros2_controllers.yaml（实际执行轨迹的控制器）
文件路径： config/ros2_controllers.yaml
# This config file is used by ros2_control
controller_manager:
ros__parameters:
update_rate: 100 # Hz - 控制器更新频率
episode_arm_controller:
type: joint_trajectory_controller/JointTrajectoryController
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
episode_arm_controller:
ros__parameters:
joints:
- joint1
- joint2
- joint3
- joint4
- joint5
- joint6
command_interfaces:
- position # 发送位置命令
state_interfaces:
- position # 读取位置反馈
- velocity # 读取速度反馈
allow_nonzero_velocity_at_trajectory_end: true
逐行解读：
| 配置项 | 含义 |
|---|---|
update_rate: 100 |
Controller Manager 每秒更新 100 次 |
episode_arm_controller |
轨迹执行控制器，接收并执行 MoveIt 发送的轨迹 |
joint_state_broadcaster |
关节状态广播器，发布 /joint_states Topic |
command_interfaces: position |
控制器通过"位置命令"控制电机 |
state_interfaces: position, velocity |
控制器可以读取电机的位置和速度状态 |
2.4 ros2_control.xacro（硬件接口定义）
文件路径： config/episode1_urdf_1113.ros2_control.xacro
<ros2_control name="${name}" type="system">
<hardware>
<!-- By default, set up controllers for simulation. This won't work on real hardware -->
<plugin>mock_components/GenericSystem</plugin>
</hardware>
<joint name="joint1">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['joint1']}</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<!-- joint2 ~ joint6 类似配置 -->
</ros2_control>
关键点：
| 配置项 | 含义 |
|---|---|
<plugin>mock_components/GenericSystem</plugin> |
模拟硬件插件，Demo 模式专用，不连接真实硬件 |
<command_interface name="position"/> |
该关节支持接收位置命令 |
<state_interface name="position"> |
该关节可以反馈当前位置 |
initial_value |
初始位置（从 initial_positions.yaml 读取） |
重要提示：
mock_components/GenericSystem是一个"假硬件"，它在内存中模拟关节状态。真实机械臂需要替换为自定义硬件插件或绕过 ros2_control 框架。
2.5 三者之间的关系图
┌──────────────────────────────────────────────────────────────┐
│ MoveIt2 │
│ ┌────────────────────────────────────────────────────────┐ │
│ │ moveit_controllers.yaml 配置: │ │
│ │ • 控制器名: episode_arm_controller │ │
│ │ • Action: /episode_arm_controller/follow_joint_trajectory│
│ └────────────────────────────────────────────────────────┘ │
│ │ │
│ ▼ 发送轨迹 │
└──────────────────────────────────────────────────────────────┘
│
▼
┌──────────────────────────────────────────────────────────────┐
│ Controller Manager │
│ ┌────────────────────────────────────────────────────────┐ │
│ │ ros2_controllers.yaml 配置: │ │
│ │ • episode_arm_controller (执行轨迹) │ │
│ │ • joint_state_broadcaster (发布 /joint_states) │ │
│ └────────────────────────────────────────────────────────┘ │
│ │ │
│ ▼ 读写硬件 │
└──────────────────────────────────────────────────────────────┘
│
▼
┌──────────────────────────────────────────────────────────────┐
│ 硬件接口层 │
│ ┌────────────────────────────────────────────────────────┐ │
│ │ ros2_control.xacro 配置: │ │
│ │ • Demo: mock_components/GenericSystem (模拟硬件) │ │
│ │ • 真实: 自定义驱动 或 绕过 ros2_control │ │
│ └────────────────────────────────────────────────────────┘ │
│ │ │
│ ▼ │
│ ┌────────────────────────────────────────────────────────┐ │
│ │ → 发送电机命令 (position command) │ │
│ │ ← 读取电机状态 (position/velocity state) │ │
│ └────────────────────────────────────────────────────────┘ │
└──────────────────────────────────────────────────────────────┘
│
▼
┌──────────────┐
│ 真实机械臂 │
│ 6个电机驱动 │
└──────────────┘
2.6 Demo 启动文件解析
文件路径： launch/demo.launch.py
from moveit_configs_utils import MoveItConfigsBuilder
from moveit_configs_utils.launches import generate_demo_launch
def generate_launch_description():
moveit_config = MoveItConfigsBuilder(
"episode1_urdf_1113", # 机器人名称
package_name="episode1_urdf_student_moveit" # 配置包名
).to_moveit_configs()
return generate_demo_launch(moveit_config)
generate_demo_launch() 会自动启动多个组件。怎么知道它启动了什么？
方法 1：查看 launch/ 目录下的拆分文件
MoveIt Setup Assistant 自动生成了这些独立的 launch 文件：
launch/
├── demo.launch.py # 一键启动所有（调用 generate_demo_launch）
├── rsp.launch.py # Robot State Publisher
├── static_virtual_joint_tfs.launch.py # 静态 TF 发布
├── spawn_controllers.launch.py # 启动控制器（包含 mock_components）
├── move_group.launch.py # MoveIt 核心节点
└── moveit_rviz.launch.py # RViz 可视化
每个文件都是 generate_demo_launch() 内部调用的一个子模块。例如：
# spawn_controllers.launch.py
from moveit_configs_utils.launches import generate_spawn_controllers_launch
def generate_launch_description():
moveit_config = MoveItConfigsBuilder(...).to_moveit_configs()
return generate_spawn_controllers_launch(moveit_config) # 启动控制器
方法 2：运行 Demo 后用命令行验证
# 启动 Demo 后，查看所有运行的节点
ros2 node list
实际输出：
/controller_manager ← Controller Manager (ros2_control 核心)
/episode_arm_controller ← 轨迹执行控制器 (Action Server)
/fakesystem ← mock_components 模拟硬件
/joint_state_broadcaster ← 发布 /joint_states
/move_group ← MoveIt 核心规划节点
/move_group/moveit ← MoveIt 内部节点
/moveit_simple_controller_manager ← MoveIt 控制器管理器 (Action Client)
/robot_state_publisher ← 发布 URDF 模型
/rviz ← RViz 可视化
/static_transform_publisher0 ← 发布 world → base_link 静态 TF
/interactive_marker_display_... ← RViz 交互标记（用于拖拽末端执行器）
/move_group_private_... ← MoveIt 内部私有节点
/rviz_private_... ← RViz 内部私有节点
注意：带有随机数字后缀的节点（如
/moveit_1951110333）是 MoveIt/RViz 内部自动创建的，每次启动数字会变化，可以忽略。
总结：generate_demo_launch() 实际上组合调用了这些子 launch：
| 子 Launch 文件 | 启动的节点 | 作用 |
|---|---|---|
rsp.launch.py |
robot_state_publisher |
发布 URDF 模型到 /robot_description |
static_virtual_joint_tfs.launch.py |
static_transform_publisher |
发布 world → base_link 静态 TF |
spawn_controllers.launch.py |
controller_manager + 控制器 |
加载 mock_components 模拟硬件，启动 episode_arm_controller 和 joint_state_broadcaster |
move_group.launch.py |
move_group |
MoveIt 核心规划节点 |
moveit_rviz.launch.py |
rviz2 |
可视化界面 |
2.7 joint_limits.yaml（关节速度和加速度限制）
文件路径： config/joint_limits.yaml
这个文件用于覆盖或增强 URDF 中定义的运动学限制，主要配置每个关节的最大速度和加速度。
# joint_limits.yaml 允许覆盖或增强 URDF 中指定的动力学属性
# 对于初学者，我们降低速度和加速度限制。
# 您可以在运动请求中指定更高的缩放因子（<= 1.0）。将下面的值增加到 1.0 以始终以最大速度移动。
default_velocity_scaling_factor: 1.0
default_acceleration_scaling_factor: 1.0
# 可以使用以下键更改特定关节属性 [max_position, min_position, max_velocity, max_acceleration]
# 可以通过 [has_velocity_limits, has_acceleration_limits] 关闭关节限制
# 每个电机的 RPM 限制，恩培机械臂经验值： [100, 60, 200, 200, 200, 100] (示例值)
# 实际关节的RPM限制（除以减速比）
# joint_max_speeds = [100/25, 60/20, 200/25, 200/10, 200/4, 100/1] = [4, 3, 8, 20, 50, 100]
# max_velocity单位：rad/s = RPM * 2pi/ 60
# max_acceleration 设置为 max_velocity 的 2-3 倍作为合理值
joint_limits:
joint1:
has_velocity_limits: true
max_velocity: 0.4188790205
has_acceleration_limits: true
max_acceleration: 2.0
joint2:
has_velocity_limits: true
max_velocity: 0.3141592654
has_acceleration_limits: true
max_acceleration: 1.0
joint3:
has_velocity_limits: true
max_velocity: 0.837758
has_acceleration_limits: true
max_acceleration: 2.0
joint4:
has_velocity_limits: true
max_velocity: 2.094395
has_acceleration_limits: true
max_acceleration: 5.0
joint5:
has_velocity_limits: true
max_velocity: 5.235988
has_acceleration_limits: true
max_acceleration: 10.0
joint6:
has_velocity_limits: true
max_velocity: 10.471976
has_acceleration_limits: true
max_acceleration: 20.0
关键配置项说明：
| 配置项 | 含义 | 恩培机械臂实际值 |
|---|---|---|
default_velocity_scaling_factor |
全局速度缩放因子（0.0~1.0） | 1.0（全速运行） |
default_acceleration_scaling_factor |
全局加速度缩放因子（0.0~1.0） | 1.0（全加速度） |
max_velocity |
关节最大角速度（弧度/秒） | 根据电机 RPM 和减速比计算 |
max_acceleration |
关节最大角加速度（弧度/秒 ²） | 通常设为 max_velocity 的 2-3 倍 |
速度限制的计算方法：
恩培机械臂每个关节的电机和减速比不同，需要分别计算：
步骤1：获取电机 RPM 限制（厂商提供）
电机 RPM = [100, 60, 200, 200, 200, 100] (示例值)
步骤2：除以减速比，得到关节输出轴的 RPM
减速比 = [25, 20, 25, 10, 4, 1]
关节 RPM = [100/25, 60/20, 200/25, 200/10, 200/4, 100/1]
= [4, 3, 8, 20, 50, 100]
步骤3：转换为弧度/秒
max_velocity (rad/s) = RPM × (2π / 60)
joint1: 4 × (2π / 60) = 0.4189 rad/s
joint2: 3 × (2π / 60) = 0.3142 rad/s
joint3: 8 × (2π / 60) = 0.8378 rad/s
joint4: 20 × (2π / 60) = 2.0944 rad/s
joint5: 50 × (2π / 60) = 5.2360 rad/s
joint6: 100 × (2π / 60) = 10.4720 rad/s
为什么需要配置这些限制？
- 安全保护：防止 MoveIt 规划出超过硬件能力的轨迹
- 规划质量：让 MoveIt 在合理的速度范围内规划，生成更平滑的轨迹
- 硬件适配：不同机械臂的电机性能不同，需要自定义限制
与其他配置文件的关系：
┌────────────────────────────────────────────────┐
│ URDF (episode1_urdf_1113.urdf.xacro) │
│ 定义关节的位置限制 (position limits) │
│ <limit lower="-3.14" upper="3.14" ... /> │
└────────────────────────────────────────────────┘
▼ 可被覆盖
┌────────────────────────────────────────────────┐
│ joint_limits.yaml │
│ 覆盖/增强速度和加速度限制 │
│ max_velocity: 0.4189 │
│ max_acceleration: 2.0 │
└────────────────────────────────────────────────┘
▼ 应用于
┌────────────────────────────────────────────────┐
│ MoveIt 轨迹规划 │
│ 生成的轨迹不会超过这些限制 │
└────────────────────────────────────────────────┘
实践建议：
| 场景 | velocity_scaling_factor |
acceleration_scaling_factor |
|---|---|---|
| 调试/测试阶段 | 0.1 ~ 0.3 |
0.1 ~ 0.3 |
| 正常运行 | 0.5 ~ 0.8 |
0.5 ~ 0.8 |
| 紧急/高速任务 | 1.0 |
1.0 |
注意：初学者建议从低速（0.1）开始测试，确认轨迹正确后再逐步提高到 1.0。
三、启动 Demo 模式并验证双向通信
本节目的：我们还没有连接真实机械臂！这一节的目标是：
- 用 Demo 模式启动一个完整的 MoveIt 环境（使用模拟硬件）
- 用命令行工具亲眼看看双向通信到底在传输什么数据
- 理解 Action 和 Topic 的实际内容，为后续连接真实机械臂打基础
3.1 启动 Demo
cd ~/ros2_ws
source install/setup.bash
ros2 launch episode1_urdf_student_moveit demo.launch.py
3.2 验证下行通信（MoveIt → 机械臂）
对应配置文件：moveit_controllers.yaml
这个文件告诉 MoveIt "我要把轨迹发送给谁"：
# 文件路径: config/moveit_controllers.yaml
moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
moveit_simple_controller_manager:
controller_names:
- episode_arm_controller # 控制器名称
episode_arm_controller:
type: FollowJointTrajectory # Action 类型（标准轨迹跟踪）
action_ns: follow_joint_trajectory # Action 命名空间
joints: # 控制哪些关节
- joint1
- joint2
- joint3
- joint4
- joint5
- joint6
default: true
关键配置项说明：
| 配置项 | 含义 | 实际效果 |
|---|---|---|
controller_names |
MoveIt 要连接的控制器列表 | MoveIt 会尝试连接 episode_arm_controller |
type: FollowJointTrajectory |
使用标准的轨迹执行 Action | 消息类型：control_msgs/action/FollowJointTrajectory |
action_ns: follow_joint_trajectory |
Action 的命名空间 | 最终 topic：/episode_arm_controller/follow_joint_trajectory |
joints |
需要控制的关节名称 | 必须与 URDF 中的关节名一致 |
打开新终端，查看 Action 话题：
# 列出所有 Action
ros2 action list
# 输出应该包含：
# /episode_arm_controller/follow_joint_trajectory
查看 Action 接口定义：
ros2 interface show control_msgs/action/FollowJointTrajectory
# 输出（Goal 部分）：
# Goal
trajectory_msgs/JointTrajectory trajectory
std_msgs/Header header
string[] joint_names # 关节名称列表
JointTrajectoryPoint[] points # 轨迹点列表
float64[] positions # 每个点的关节角度（弧度）
float64[] velocities # 每个点的关节速度（弧度/秒）
float64[] accelerations # 每个点的关节加速度
builtin_interfaces/Duration time_from_start # 相对起点的时间
# ...
查看 Action 的具体信息：
ros2 action info /episode_arm_controller/follow_joint_trajectory
# 输出：
Action: /episode_arm_controller/follow_joint_trajectory
Action clients: 1
/moveit_simple_controller_manager
Action servers: 1
/episode_arm_controller
解读这个输出：
| 角色 | 节点名称 | 身份 | 职责 |
|---|---|---|---|
| Action Client | /moveit_simple_controller_manager |
MoveIt 的控制器管理器 | 发送轨迹请求（"请执行这个轨迹"） |
| Action Server | /episode_arm_controller |
轨迹执行控制器 | 接收并执行轨迹，返回执行结果 |
验证了前面的理论：MoveIt 确实是 Client（请求者），控制器是 Server（执行者）。
查看 Action 的轨迹数据结构：
注意：直接用
ros2 topic echo监控 Action 内部 topic 不太实用，因为时机难以把握。更好的方法是使用后文的"轨迹打印节点"来查看完整的轨迹数据。
不过我们可以先了解轨迹的数据结构：
# 查看 Action 的 Goal 类型定义
ros2 interface show control_msgs/action/FollowJointTrajectory
轨迹数据示例（来自 MoveIt 规划结果）：
# Goal 消息结构
trajectory:
joint_names:
- joint1
- joint2
- joint3
- joint4
- joint5
- joint6
points:
- positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
time_from_start:
sec: 0
nanosec: 0
- positions: [0.1234, 0.2345, 0.1567, -0.0456, 0.3421, 0.0234]
velocities: [0.5, 0.3, 0.4, 0.2, 0.6, 0.1]
time_from_start:
sec: 0
nanosec: 500000000 # 0.5秒后到达
# ... MoveIt 规划出的更多轨迹点 ...
这些数据从哪里来？对照 moveit_controllers.yaml 配置：
| Action 消息字段 | 实际值 | 对应的 moveit_controllers.yaml 配置 |
|---|---|---|
| Action Topic | /episode_arm_controller/follow_joint_trajectory |
controller_names: [episode_arm_controller] + action_ns: follow_joint_trajectory |
| joint_names | joint1 ~ joint6 |
joints: 列表（6 个关节名） |
| positions | 弧度值（如 0.1234） |
MoveIt 规划结果，单位必须是弧度 |
| velocities | 弧度/秒（如 0.5） |
MoveIt 规划的速度曲线 |
| time_from_start | 相对时间（如 0.5秒） |
MoveIt 规划的时间序列 |
理解：MoveIt 根据
moveit_controllers.yaml的配置，知道要把轨迹发送到/episode_arm_controller/follow_joint_trajectory这个 Action Server。轨迹包含多个点，每个点定义了关节角度、速度和时间戳。
集成真实机械臂时，你需要做同样的事情：
| Demo 模式（模拟硬件） | 真实机械臂（你需要实现） |
|---|---|
episode_arm_controller 自动接收并"执行"轨迹 |
创建 Action Server 接收轨迹（如 robot_interface_node.py） |
| 模拟硬件自动更新关节位置 | 从轨迹中提取关节角度，通过 CAN 总线发送给电机驱动 |
接收弧度制的 positions |
同样需要处理弧度制数据 |
按照 time_from_start 时序执行 |
同样需要按时间同步执行每个轨迹点 |
| 角度范围检查 | 同样需要做坐标系转换（URDF 坐标系 → 电机坐标系） |
总结：Demo 模式帮你"假装"有一个控制器在接收并执行轨迹。连接真实机械臂时，你必须自己实现这个 Action Server——这就是
robot_interface_node.py中follow_joint_trajectory_callback()方法做的事情。
3.3 验证上行通信（机械臂 → MoveIt）
为什么 MoveIt 需要机械臂反馈这些信息？
MoveIt 本质上是一个规划器，不是执行器。它就像一个导航软件：
| 导航软件 GPS | MoveIt /joint_states |
|---|---|
| 需要知道"我现在在哪" | 需要知道"机械臂各关节现在的角度" |
| 规划从当前位置到目的地的路线 | 规划从当前姿态到目标姿态的轨迹 |
| 实时追踪是否偏离路线 | 实时追踪机械臂是否按轨迹运动 |
| 确认是否到达目的地 | 确认机械臂是否到达目标位置 |
具体用途：
- 规划起点：MoveIt 规划轨迹时，必须知道起点在哪。如果没有
/joint_states，MoveIt 不知道从哪里开始规划，会提示 "Current state is not set" 错误 - 碰撞检测：MoveIt 需要实时知道机械臂当前位置，才能判断规划的轨迹是否会与环境或自身碰撞
- 执行监控：轨迹执行时，MoveIt 订阅
/joint_states监控实际位置，判断是否成功到达目标或是否需要重新规划 - RViz 可视化：RViz 中显示的机械臂实时位置，就是从
/joint_states读取的。如果没有这个 Topic，RViz 中的机械臂模型会"死"在原地不动
类比理解：如果把 MoveIt 比作"大脑"，
/joint_states就是"视觉和触觉"——没有反馈，大脑无法感知肢体位置，也就无法协调运动。
对应配置文件：ros2_controllers.yaml
这个文件配置了 joint_state_broadcaster 控制器，负责发布关节状态：
# 文件路径: config/ros2_controllers.yaml
controller_manager:
ros__parameters:
update_rate: 100 # Hz - 控制器更新频率
episode_arm_controller:
type: joint_trajectory_controller/JointTrajectoryController
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
episode_arm_controller:
ros__parameters:
joints:
- joint1
- joint2
- joint3
- joint4
- joint5
- joint6
command_interfaces:
- position # 发送位置命令
state_interfaces:
- position # 读取位置反馈
- velocity # 读取速度反馈
关键配置项说明：
| 配置项 | 含义 | 实际效果 |
|---|---|---|
update_rate: 100 |
Controller Manager 更新频率 | /joint_states 发布频率约为 100Hz |
joint_state_broadcaster |
关节状态广播器 | 负责发布 /joint_states Topic |
state_interfaces: position |
读取位置状态接口 | /joint_states 的 position 字段有值 |
state_interfaces: velocity |
读取速度状态接口 | /joint_states 的 velocity 字段有值 |
查看 /joint_states 话题：
# 列出所有 Topic
ros2 topic list | grep joint
# 输出应该包含：
# /joint_states
实时查看关节状态（100Hz）：
ros2 topic echo /joint_states
# 输出示例（不断更新）：
header:
stamp:
sec: 1770181181
nanosec: 430544653
frame_id: base_link
name:
- joint1
- joint2
- joint3
- joint4
- joint5
- joint6
position:
- 9.486499267457113e-06
- -0.01768203011056477
- 0.512994335327643
- -2.278372309775389e-05
- -0.4953049000658073
- 7.960733048048388e-05
velocity:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
effort:
- .nan
- .nan
- .nan
- .nan
- .nan
- .nan
---
查看 Topic 发布频率：
ros2 topic hz /joint_states
# 输出示例：
average rate: 100.004
这些数据从哪里来？对照 ros2_controllers.yaml 配置：
/joint_states 字段 |
实际值 | 对应的 ros2_controllers.yaml 配置 |
|---|---|---|
| 发布频率 | 100.004 Hz |
update_rate: 100 |
| name | joint1 ~ joint6 |
joints: 列表（6 个关节名） |
| position | 有值（弧度） | state_interfaces: - position ✓ |
| velocity | 有值（0.0） | state_interfaces: - velocity ✓ |
| effort | .nan（无效值） |
未配置 - effort，所以无数据 |
理解：
joint_state_broadcaster从硬件接口读取state_interfaces配置的数据（position、velocity），然后发布到/joint_states。因为我们没有配置effort接口，所以 effort 字段显示为.nan。
集成真实机械臂时，你需要做同样的事情：
| Demo 模式（模拟硬件） | 真实机械臂（你需要实现） |
|---|---|
mock_components 自动维护关节状态 |
从 CAN 总线/串口读取电机编码器数据 |
joint_state_broadcaster 发布 /joint_states |
自己写代码发布 /joint_states |
| 100Hz 发布频率 | 同样需要 ≥50Hz 的发布频率 |
| position、velocity 字段有值 | 同样需要填充 position（必须）、velocity（可选） |
| 角度单位是弧度 | 同样需要转换为弧度再发布 |
总结：Demo 模式帮你"假装"有一个机械臂在发布状态。连接真实机械臂时，你必须自己实现这个发布逻辑——这就是后面
robot_interface_node.py中publish_joint_states()方法做的事情。
四、创建"轨迹打印节点"
现在我们创建一个最小化的节点来理解 MoveIt 如何发送轨迹。这个节点：
- 实现
FollowJointTrajectoryAction Server - 打印每个收到的轨迹点（不执行真实硬件命令）
- 发布假的
/joint_states（让 MoveIt 能规划）
4.1 创建打印节点文件
创建文件 ~/ros2_ws/src/py_episode/py_episode/trajectory_printer_node.py：
#!/usr/bin/env python3
"""
轨迹打印节点 - 用于学习 MoveIt 如何发送轨迹
功能：
1. 接收 MoveIt 的 FollowJointTrajectory Action 请求
2. 打印轨迹数据（不执行真实硬件命令）
3. 发布假的 /joint_states（让 MoveIt 能规划）
用途：理解 MoveIt 与机械臂的通信机制
"""
import rclpy
from rclpy.node import Node
from rclpy.action import ActionServer
from rclpy.callback_groups import ReentrantCallbackGroup
from control_msgs.action import FollowJointTrajectory
from sensor_msgs.msg import JointState
import math
import time
class TrajectoryPrinterNode(Node):
"""轨迹打印节点"""
def __init__(self):
super().__init__('trajectory_printer')
# 使用 ReentrantCallbackGroup 允许并发处理
self.callback_group = ReentrantCallbackGroup()
# 关节名称（必须与 URDF 中的定义一致）
self.joint_names = ['joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6']
# 当前关节角度（初始为全零，执行轨迹后更新为目标位置）
self.current_positions = [0.0] * 6
# 创建 Action Server 接收 MoveIt 的轨迹
self._action_server = ActionServer(
self,
FollowJointTrajectory,
'episode_arm_controller/follow_joint_trajectory',
self.follow_joint_trajectory_callback,
callback_group=self.callback_group
)
# 创建 joint_states 发布器（发布假数据）
self._joint_states_pub = self.create_publisher(
JointState,
'joint_states',
10
)
# 创建定时器发布假的 joint_states（全零角度）
self._timer = self.create_timer(1.0 / 120.0, self.publish_fake_joint_states)
self.get_logger().info('轨迹打印节点已启动')
self.get_logger().info('等待 MoveIt 发送轨迹...')
def publish_fake_joint_states(self):
"""发布假的关节状态（跟随轨迹目标位置）"""
msg = JointState()
msg.header.stamp = self.get_clock().now().to_msg()
msg.header.frame_id = ''
msg.name = self.joint_names
msg.position = self.current_positions # 使用当前位置（初始全零，执行后更新）
msg.velocity = []
msg.effort = []
self._joint_states_pub.publish(msg)
def follow_joint_trajectory_callback(self, goal_handle):
"""处理 MoveIt 发送的轨迹"""
self.get_logger().info('=' * 80)
self.get_logger().info('收到 MoveIt 轨迹！')
self.get_logger().info('=' * 80)
# 获取轨迹数据
trajectory = goal_handle.request.trajectory
# 打印轨迹基本信息
self.get_logger().info(f'轨迹包含 {len(trajectory.points)} 个点')
self.get_logger().info(f'关节顺序: {trajectory.joint_names}')
# 打印每个轨迹点的详细信息
for i, point in enumerate(trajectory.points):
# 获取时间戳（秒）
time_from_start = point.time_from_start.sec + point.time_from_start.nanosec * 1e-9
# 获取位置（弧度）
positions_rad = point.positions
positions_deg = [math.degrees(p) for p in positions_rad]
# 获取速度（如果有）
if point.velocities:
velocities_rad = point.velocities
velocities_deg = [math.degrees(v) for v in velocities_rad]
else:
velocities_deg = [0.0] * 6
# 打印格式化输出
self.get_logger().info(f'\n点 {i+1}/{len(trajectory.points)}:')
self.get_logger().info(f' 时间: {time_from_start:.3f} 秒')
self.get_logger().info(f' 位置(弧度): [{", ".join([f"{p:.4f}" for p in positions_rad])}]')
self.get_logger().info(f' 位置(度): [{", ".join([f"{p:7.2f}" for p in positions_deg])}]°')
self.get_logger().info(f' 速度(度/s): [{", ".join([f"{v:7.2f}" for v in velocities_deg])}]°/s')
# 轨迹信息总结
total_time = trajectory.points[-1].time_from_start.sec + \
trajectory.points[-1].time_from_start.nanosec * 1e-9
self.get_logger().info(f'\n轨迹总时长: {total_time:.3f} 秒')
# 更新当前位置为轨迹的最后一个点（模拟"执行完成"）
self.current_positions = list(trajectory.points[-1].positions)
self.get_logger().info(f'已更新关节位置到目标点')
# 标记为成功（不实际执行硬件命令）
goal_handle.succeed()
result = FollowJointTrajectory.Result()
result.error_code = FollowJointTrajectory.Result.SUCCESSFUL
self.get_logger().info('轨迹接收并打印完成')
self.get_logger().info('=' * 80 + '\n')
return result
def main(args=None):
rclpy.init(args=args)
node = TrajectoryPrinterNode()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
4.2 添加入口点
编辑 ~/ros2_ws/src/py_episode/setup.py，在 entry_points 中添加：
entry_points={
'console_scripts': [
# ... 之前的入口点 ...
'trajectory_printer = py_episode.trajectory_printer_node:main', # 新增
],
},
4.3 编译并安装
cd ~/ros2_ws
colcon build --packages-select py_episode --symlink-install
source install/setup.bash
五、验证打印节点与 MoveIt 的通信
5.1 启动 Robot State Publisher（发布机器人模型）
终端 1：启动 Robot State Publisher
ros2 launch episode1_urdf_student_moveit rsp.launch.py
重要说明：这个节点负责发布
/robot_descriptiontopic 和 TF 变换树，RViz 需要这些信息才能显示机械臂模型。
5.2 启动打印节点（替代 Demo 的控制层）
终端 2：启动打印节点
ros2 run py_episode trajectory_printer
输出：
[INFO] [trajectory_printer]: 轨迹打印节点已启动
[INFO] [trajectory_printer]: 等待 MoveIt 发送轨迹...
5.3 启动 MoveIt（不启动模拟控制器）
终端 3：启动 MoveIt 核心节点
ros2 launch episode1_urdf_student_moveit move_group.launch.py
5.4 启动 RViz
终端 4：启动 RViz
ros2 launch episode1_urdf_student_moveit moveit_rviz.launch.py
如果 RViz 中仍然看不到机械臂模型，请检查：
-
确认
/robot_description是否发布：ros2 topic list | grep robot_description # 应该输出: /robot_description -
在 RViz 左侧 Displays 面板中检查 RobotModel 插件：
- 如果没有
RobotModel，点击 Add → RobotModel - 如果
RobotModel显示错误（红色），检查Robot Description字段是否设置为robot_description
- 如果没有
-
检查 Fixed Frame 设置：
- 在 RViz 顶部
Global Options→Fixed Frame应该设置为world或base_link
- 在 RViz 顶部
5.5 在 RViz 中测试
- 在 RViz 中拖拽末端执行器到新位置
- 点击 Plan and Execute 按钮
- 查看终端 2（打印节点）的输出
预期输出示例：
[INFO] [1770182572.225622462] [trajectory_printer]: 轨迹打印节点已启动
[INFO] [1770182572.225961882] [trajectory_printer]: 等待 MoveIt 发送轨迹...
[INFO] [1770182581.064092929] [trajectory_printer]: ================================================================================
[INFO] [1770182581.064368922] [trajectory_printer]: 收到 MoveIt 轨迹！
[INFO] [1770182581.064624089] [trajectory_printer]: ================================================================================
[INFO] [1770182581.064889296] [trajectory_printer]: 轨迹包含 105 个点
[INFO] [1770182581.065161159] [trajectory_printer]: 关节顺序: ['joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6']
[INFO] [1770182581.065489167] [trajectory_printer]:
点 1/105:
[INFO] [1770182581.065807672] [trajectory_printer]: 时间: 0.000 秒
[INFO] [1770182581.066135113] [trajectory_printer]: 位置(弧度): [0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000]
[INFO] [1770182581.066483834] [trajectory_printer]: 位置(度): [ 0.00, 0.00, 0.00, 0.00, 0.00, 0.00]°
[INFO] [1770182581.066837572] [trajectory_printer]: 速度(度/s): [ 0.01, 0.01, 0.01, 0.03, 0.01, 0.01]°/s
[INFO] [1770182581.067153266] [trajectory_printer]:
点 2/105:
[INFO] [1770182581.067471218] [trajectory_printer]: 时间: 0.100 秒
[INFO] [1770182581.067795699] [trajectory_printer]: 位置(弧度): [0.0010, 0.0005, 0.0005, 0.0025, 0.0007, 0.0013]
[INFO] [1770182581.068131857] [trajectory_printer]: 位置(度): [ 0.06, 0.03, 0.03, 0.14, 0.04, 0.07]°
[INFO] [1770182581.068502888] [trajectory_printer]: 速度(度/s): [ 1.11, 0.59, 0.60, 2.86, 0.85, 1.46]°/s
[INFO] [1770182581.068816531] [trajectory_printer]:
点 3/105:
[INFO] [1770182581.069125076] [trajectory_printer]: 时间: 0.200 秒
[INFO] [1770182581.069453573] [trajectory_printer]: 位置(弧度): [0.0039, 0.0020, 0.0021, 0.0100, 0.0030, 0.0051]
[INFO] [1770182581.069790439] [trajectory_printer]: 位置(度): [ 0.22, 0.12, 0.12, 0.57, 0.17, 0.29]°
[INFO] [1770182581.070147412] [trajectory_printer]: 速度(度/s): [ 2.22, 1.17, 1.20, 5.73, 1.69, 2.93]°/s
[INFO] [1770182581.070469177] [trajectory_printer]:
点 4/105:...
六、集成完整的机械臂接口节点
前面的 trajectory_printer_node.py 只是一个学习用的简化版本，它只打印轨迹数据，不实际控制硬件。
现在我们要使用完整的生产版本 robot_interface_node.py，它能够：
- 接收 MoveIt 的轨迹并通过 CAN 总线发送给真实电机
- 读取电机编码器数据并发布
/joint_states - 支持轨迹插值，让运动更平滑
6.1 复制代码到 py_episode 包
将完整的 robot_interface_node.py 复制到你的 ROS2 包中：
# 假设你的包在 ~/ros2_ws/src/py_episode/
cp robot_interface_node.py ~/ros2_ws/src/py_episode/py_episode/robot_interface_node_full.py
将 robot_arm_interfaces 也替换为 Episode1 ROS2 包的，因为完整版有很多新的 Services 和 Actions。
6.2 添加入口点
编辑 ~/ros2_ws/src/py_episode/setup.py，在 entry_points 中添加：
entry_points={
'console_scripts': [
# ... 之前的入口点 ...
'trajectory_printer = py_episode.trajectory_printer_node:main', # 学习用
'robot_interface_full = py_episode.robot_interface_node_full:main', # 生产用（新增）
],
},
6.3 编译
cd ~/ros2_ws
colcon build
source install/setup.bash
6.4 启动真实机械臂
安全提示：首次测试时，请确保机械臂周围无障碍物，并准备好急停开关。
终端 1：启动 Robot State Publisher
cd ~/ros2_ws
source install/setup.bash
ros2 launch episode1_urdf_student_moveit rsp.launch.py
重要说明：这个节点负责发布
/robot_description和 TF 变换树，MoveIt 和 RViz 都需要这些信息。
终端 2：启动机械臂接口节点
cd ~/ros2_ws
source install/setup.bash
ros2 run py_episode robot_interface_full
可选参数：
usb_index：CAN 设备索引（默认1，对应/dev/ttyUSB1）init_mode：0=回零校准，1=从当前位置恢复（默认）
# 完整参数示例
ros2 run py_episode robot_interface_full --ros-args -p usb_index:=1 -p init_mode:=1
终端 3：启动 MoveIt 核心节点
cd ~/ros2_ws
source install/setup.bash
ros2 launch episode1_urdf_student_moveit move_group.launch.py
终端 4：启动 RViz
cd ~/ros2_ws
source install/setup.bash
ros2 launch episode1_urdf_student_moveit moveit_rviz.launch.py
6.5 验证连接
检查上行通信（机械臂 → MoveIt）：
# 查看 /joint_states 是否有数据
ros2 topic echo /joint_states --once
# 检查发布频率（应该接近 120Hz）
ros2 topic hz /joint_states
检查下行通信（MoveIt → 机械臂）：
# 查看 Action Server 是否存在
ros2 action list | grep follow_joint_trajectory
# 应该输出:
# /episode_arm_controller/follow_joint_trajectory
在 RViz 中测试：
- RViz 中的机械臂模型应该显示真实机械臂的当前位置
- 拖拽末端执行器到新位置
- 点击 Plan and Execute
- 观察真实机械臂是否按规划轨迹运动
6.6 常见问题排查
| 问题 | 可能原因 | 解决方案 |
|---|---|---|
| RViz 中机械臂不动 | /joint_states 未发布 |
检查 robot_interface 节点是否启动，CAN 连接是否正常 |
| MoveIt 报 "Current state is not set" | /joint_states 未发布或关节名不匹配 |
检查关节名是否与 URDF 一致 |
| 规划成功但机械臂不执行 | Action Server 未连接 | 检查 ros2 action info /episode_arm_controller/follow_joint_trajectory |
| 运动抖动或不平滑 | 插值因子过小或 CAN 通信延迟 | 尝试增大 interpolation_factor 或检查 CAN 总线负载 |
七、Demo 模式 vs 真实机械臂模式
这是最关键的理解点：连接真实机械臂时，我们不启动 spawn_controllers.launch.py，而是用 robot_interface_node 完全替代 ros2_control 层。
7.1 启动组件对比
| 组件 | Demo 模式 | 真实机械臂模式 |
|---|---|---|
robot_state_publisher |
✅ 启动 | ✅ 启动 |
static_transform_publisher |
✅ 启动 | ✅ 启动 |
move_group (MoveIt 核心) |
✅ 启动 | ✅ 启动 |
rviz2 |
✅ 启动 | ✅ 启动 |
controller_manager + spawn_controllers |
✅ 启动 | ❌ 不启动 |
mock_components/GenericSystem |
✅ 使用 | ❌ 不使用 |
robot_interface_node |
❌ 不需要 | ✅ 启动 |
7.2 架构对比图
Demo 模式（模拟硬件）：
┌─────────────────────────────────────────────────────────────────────────────┐
│ Demo 模式架构 │
├─────────────────────────────────────────────────────────────────────────────┤
│ │
│ ┌──────────────┐ Action ┌──────────────────────────────────┐ │
│ │ │ ───────────────▶ │ │ │
│ │ MoveIt │ │ ros2_control 框架 │ │
│ │ move_group │ Topic │ ┌────────────────────────────┐ │ │
│ │ │ ◀─────────────── │ │ episode_arm_controller │ │ │
│ └──────────────┘ │ │ joint_state_broadcaster │ │ │
│ │ └────────────────────────────┘ │ │
│ │ │ │ │
│ │ ▼ │ │
│ │ ┌────────────────────────────┐ │ │
│ │ │ mock_components/ │ │ │
│ │ │ GenericSystem (模拟硬件) │ │ │
│ │ └────────────────────────────┘ │ │
│ └──────────────────────────────────┘ │
│ │
│ 配置文件: ros2_controllers.yaml + ros2_control.xacro │
│ 启动方式: ros2 launch demo.launch.py │
│ │
└─────────────────────────────────────────────────────────────────────────────┘
真实机械臂模式：
┌─────────────────────────────────────────────────────────────────────────────┐
│ 真实机械臂模式架构 │
├─────────────────────────────────────────────────────────────────────────────┤
│ │
│ ┌──────────────┐ Action ┌──────────────────────────────────┐ │
│ │ │ ───────────────▶ │ │ │
│ │ MoveIt │ │ robot_interface_node │ │
│ │ move_group │ Topic │ (替代 ros2_control 框架) │ │
│ │ │ ◀─────────────── │ │ │
│ └──────────────┘ │ ┌────────────────────────────┐ │ │
│ │ │ │ _moveit_action_server │ │ │
│ │ │ │ _joint_states_publisher │ │ │
│ ▼ │ └────────────────────────────┘ │ │
│ ┌──────────────┐ │ │ │ │
│ │ RViz │ │ ▼ │ │
│ │ (可视化) │ │ ┌────────────────────────────┐ │ │
│ └──────────────┘ │ │ CAN 总线通信 │ │ │
│ │ │ MotorControl 类 │ │ │
│ │ └────────────────────────────┘ │ │
│ └──────────────────────────────────┘ │
│ │ │
│ ▼ │
│ ┌──────────────────────────────────┐ │
│ │ 真实机械臂硬件 │ │
│ │ 6个电机驱动 + 编码器反馈 │ │
│ └──────────────────────────────────┘ │
│ │
│ 配置文件: 不需要 ros2_controllers.yaml │
│ 启动方式: 分别启动 robot_interface + move_group + rviz │
│ │
└─────────────────────────────────────────────────────────────────────────────┘
7.3 为什么不需要 ros2_controllers.yaml？
| 问题 | Demo 模式的做法 | 真实机械臂模式的做法 |
|---|---|---|
| 谁提供 Action Server？ | episode_arm_controller（由 ros2_control 框架启动） |
robot_interface_node 的 _moveit_action_server |
谁发布 /joint_states？ |
joint_state_broadcaster（由 ros2_control 框架启动） |
robot_interface_node 的 _joint_states_publisher |
| 谁与硬件通信？ | mock_components/GenericSystem（模拟） |
MotorControl 类（CAN 总线） |
结论：robot_interface_node 完全绕过了 ros2_control 框架，直接实现了 MoveIt 需要的接口。所以不需要加载 ros2_controllers.yaml 和 ros2_control.xacro。
八、轨迹插值机制
8.1 为什么需要插值？
MoveIt 规划的轨迹点间隔较大（通常 100ms 一个点），但电机需要更密集的控制点才能实现平滑运动。
MoveIt 规划的轨迹（稀疏）:
时间: 0.0s ──────── 0.1s ──────── 0.2s ──────── 0.3s
● ● ● ●
点1 点2 点3 点4
插值后的轨迹（密集，interpolation_factor=15）:
时间: 0.0s ─ 0.006s ─ 0.012s ─ ... ─ 0.1s ─ 0.106s ─ ...
● ○ ○ ○ ○ ○ ○ ○ ○ ○ ○ ○ ○ ○ ○ ○ ● ○ ○ ...
原始 原始
点 点
● = 原始轨迹点
○ = 插值生成的中间点
8.2 插值效果
| 指标 | 插值前 | 插值后（factor=15） |
|---|---|---|
| 原始轨迹点数 | 105 个 | 105 个 |
| 总点数 | 105 个 | 105 + 104×15 = 1665 个 |
| 点间隔时间 | ~100ms | ~6.25ms |
| 控制频率 | ~10Hz | ~160Hz |
8.3 代码位置
插值逻辑在 robot_interface_node.py 的 _interpolate_trajectory() 方法中实现，使用线性插值计算中间点的位置、速度和时间戳。
# 插值因子配置（在 _move_trajectory_data 方法中）
interpolation_factor = 15 # 每两个原始点之间插入 15 个中间点
下一节预告
在本章中，我们通过 RViz 的交互界面来控制真实机械臂。但在实际应用中，我们往往需要通过代码来控制机械臂——比如视觉引导抓取、自动化流程等场景。
下一节，我们将学习 MoveIt Python API，掌握如何用 Python 代码：
- 获取机械臂当前位姿
- 设置目标位置并规划轨迹
- 执行轨迹控制真实机械臂