ROS2 MoveIt2 Servo

ROS2 MoveIt2 Servo 启动文件

这份是 MoveIt2 Servo 官方示例 launch.py，作用：实时伺服控制 Panda 机械臂（拖拽示教 / 手柄实时控制机械臂），实现不需要规划、直接接收速度指令驱动机械臂。

MoveIt Servo：区别于常规 MoveIt 运动规划，高频实时运动控制，适合遥操作、力控拖拽、手柄控制。

先先说整体架构：

1. 加载机器人模型（URDF/Xacro）
2. 启动 ros2_control 虚拟硬件（仿真 Panda）
3. 启动控制器（joint_state_broadcaster + panda_arm_controller）
4. 启动 MoveIt Servo 核心节点（servo_node_main）
5. 启动手柄 joy 节点 + JoyToServoPub（手柄消息转 Servo 指令）
6. robot_state_publisher、静态 TF、RViz 可视化

一、头部导入模块

python

运行

import os
import yaml
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode
from launch.actions import ExecuteProcess
import xacro
from moveit_configs_utils import MoveItConfigsBuilder

• get_package_share_directory：获取功能包 share 目录路径（ROS2 标准方式，不能写绝对路径）
• MoveItConfigsBuilder：MoveIt2 官方工具，一键加载 moveit 整套配置（机器人描述、SRDF、运动学、组配置等）
• ComposableNodeContainer / ComposableNode：ROS2 组件节点（零拷贝进程内通信，效率更高，多个节点跑在同一个进程）
• yaml：读取 yaml 参数文件

二、两个工具函数

1. load_file（本代码有 BUG！缩进错误，后面修正完整代码）

python

运行

def load_file(package_name, file_path):
    package_path = get_package_share_directory(package_name)
    absolute_file_path = os.path.join(package_path, file_path)

try:
with open(absolute_file_path, "r") as file:
return file.read()
except EnvironmentError:
return None

功能：读取功能包里任意文本文件，返回字符串。

❗原代码严重缩进错误：try、with 没有缩进，直接运行会报语法错误！

2. load_yaml

python

运行

def load_yaml(package_name, file_path):
    package_path = get_package_share_directory(package_name)
    absolute_file_path = os.path.join(package_path, file_path)
try:
with open(absolute_file_path, "r") as file:
return yaml.safe_load(file)
except EnvironmentError:
return None

读取 yaml 配置文件，解析成字典，用来加载 servo 的参数。 同样存在缩进 bug。

三、核心函数 generate_launch_description ()

launch 文件入口函数，必须返回 LaunchDescription 对象。

① 构建 MoveIt 全套配置

python

运行

moveit_config = (
    MoveItConfigsBuilder("moveit_resources_panda")
    .robot_description(file_path="config/panda.urdf.xacro")
    .to_moveit_configs()
)

• moveit_resources_panda：panda 机器人模型包
• 加载 panda.urdf.xacro 机器人模型
• .to_moveit_configs() 自动生成一整套参数：
  • robot_description（URDF）
  • robot_description_semantic（SRDF：规划组、碰撞组、末端执行器）
  • robot_description_kinematics（运动学求解器参数） 后续所有节点直接复用这套参数，不用手动写大量参数。

② 加载 Servo 参数

python

运行

servo_yaml = load_yaml("moveit_servo", "config/panda_simulated_config.yaml")
servo_params = {"moveit_servo": servo_yaml}

panda_simulated_config.yaml 是 Servo 核心配置：

• 控制频率、指令类型（速度 / 位置）
• 命令输入 topic、输出 joint 指令 topic
• 奇异点检测、速度限制、碰撞检测开关 所有 servo_node 的运行参数来源于这里。

③ RViz2 可视化节点

python

运行

rviz_config_file = (
    get_package_share_directory("moveit_servo") + "/config/demo_rviz_config.rviz"
)
rviz_node = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output="log",
arguments=["-d", rviz_config_file],
parameters=[
    moveit_config.robot_description,
    moveit_config.robot_description_semantic,
],
)

• -d：加载预制 rviz 配置，提前配置好机器人模型、TF、MoveIt 插件
• 传入 URDF、SRDF 参数，rviz 才能显示机械臂

④ ros2_control 核心节点（仿真硬件层）

python

运行

ros2_controllers_path = os.path.join(
    get_package_share_directory("moveit_resources_panda_moveit_config"),
    "config",
    "ros2_controllers.yaml",
)
ros2_control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[moveit_config.robot_description, ros2_controllers_path],
output="screen",
)

ros2_control 是 ROS2 机器人硬件驱动框架

• 加载 ros2_controllers.yaml：里面定义硬件接口（FakeSystem 虚拟仿真硬件）、控制器列表
• FakeSystem = 仿真，不需要真实机械臂硬件，纯软件模拟关节
• 所有控制器（joint_state_broadcaster、arm_controller）都由这个节点管理

⑤ 生成控制器 spawner（启动控制器）

python

运行

joint_state_broadcaster_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=[
"joint_state_broadcaster",
"--controller-manager-timeout",
"300",
"--controller-manager",
"/controller_manager",
],
)

panda_arm_controller_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["panda_arm_controller", "-c", "/controller_manager"],
)

spawner 作用：向 controller_manager 请求启动控制器

1. joint_state_broadcaster：广播 /joint_states 话题，发布所有关节当前位置，robot_state_publisher 依靠它生成 TF
2. panda_arm_controller：手臂轨迹控制器，接收关节速度 / 位置指令，驱动仿真机械臂

Servo 最终输出关节速度指令发给这个控制器执行

⑥ ComposableNodeContainer 组件容器

python

运行

container = ComposableNodeContainer(
name="moveit_servo_demo_container",
namespace="/",
package="rclcpp_components",
executable="component_container_mt",
composable_node_descriptions=[
    ComposableNode(
        package="robot_state_publisher",
        plugin="robot_state_publisher::RobotStatePublisher",
        name="robot_state_publisher",
        parameters=[moveit_config.robot_description],
    ),
    ComposableNode(
        package="tf2_ros",
        plugin="tf2_ros::StaticTransformBroadcasterNode",
        name="static_tf2_broadcaster",
        parameters=[{"child_frame_id": "/panda_link0", "frame_id": "/world"}],
    ),
    ComposableNode(
        package="moveit_servo",
        plugin="moveit_servo::JoyToServoPub",
        name="controller_to_servo_node",
    ),
    ComposableNode(
        package="joy",
        plugin="joy::Joy",
        name="joy_node",
    ),
],
output="screen",
)

组件节点 = 多个节点编译成动态库，运行在同一个进程内，进程内消息通信无需序列化，延迟更低！

容器内 4 个组件：

1. robot_state_publisher：根据 /joint_states + URDF，实时计算所有连杆 TF 变换
2. static_tf2_broadcaster：静态坐标变换 world → panda_link0，把机械臂基座绑定到世界坐标系
3. joy::Joy：手柄驱动节点，读取游戏手柄硬件，发布 /joy 话题
4. JoyToServoPub：消息转换器
   • 订阅手柄 /joy
   • 将摇杆按键信号转换为 Servo 需要的 Twist 指令（笛卡尔速度指令）发给 servo_node

注意：代码里 Servo 本身没有放进组件，是单独独立节点运行。

⑦ Servo 独立节点（核心实时控制）

python

运行

servo_node = Node(
package="moveit_servo",
executable="servo_node_main",
parameters=[
    servo_params,
    moveit_config.robot_description,
    moveit_config.robot_description_semantic,
    moveit_config.robot_description_kinematics,
],
output="screen",
)

整个工程最核心节点 servo_node_main 工作流程：

1. 接收笛卡尔速度指令（Twist，来自 JoyToServoPub 或者自己写的程序）
2. 调用机械臂雅可比矩阵，笛卡尔速度 → 各关节速度（微分逆运动学）
3. 做限速、奇异点校验、碰撞检查
4. 输出关节速度指令 → 发给 panda_arm_controller 控制机械臂运动

⑧ 启动列表

python

运行

return LaunchDescription(
    [
        rviz_node,
        ros2_control_node,
        joint_state_broadcaster_spawner,
        panda_arm_controller_spawner,
        servo_node,
        container,
    ]
)

LaunchDescription 内所有节点会同时启动（launch2 有简单时序，spawner 会等待 controller_manager 就绪）

完整数据流（重点！）

plaintext

游戏手柄 → joy_node(/joy) → JoyToServoPub → Twist消息 → servo_node
                                                         ↓
servo_node逆解 → 关节速度指令 → panda_arm_controller → ros2_control(Fake仿真硬件)
                                                         ↓
                    joint_state_broadcaster ← 仿真关节状态
                            ↓
               robot_state_publisher 计算TF树 → RViz显示机械臂

发现原代码的重大问题：缩进错误！

load_file 和 load_yaml 里面 try/with 没有正确缩进，直接运行会报语法错误。

修正后的工具函数

python

运行

def load_file(package_name, file_path):
    package_path = get_package_share_directory(package_name)
    absolute_file_path = os.path.join(package_path, file_path)
    try:
        with open(absolute_file_path, "r") as file:
            return file.read()
    except EnvironmentError:
        return None

def load_yaml(package_name, file_path):
    package_path = get_package_share_directory(package_name)
    absolute_file_path = os.path.join(package_path, file_path)
    try:
        with open(absolute_file_path, "r") as file:
            return yaml.safe_load(file)
    except EnvironmentError:
        return None

补充几个常见疑问

1. Servo 和普通 MoveIt 规划区别？ MoveIt Plan：给定目标点，路径规划；慢速、非实时 MoveIt Servo：接收实时速度指令，微分逆解；高频实时，适合遥操作、拖拽

2. ComposableNode 和普通 Node 区别？ Composable：同进程，消息无拷贝，延迟低；缺点一个节点崩溃整个容器挂掉。 Node：独立进程，隔离性好，延迟略高。

3. 可以不用手柄吗？ 可以。自己发布 /servo/delta_twist_cmds（默认 topic）Twist 消息，不需要 joy 和 JoyToServoPub。