2. 加载机器人模型到MoveIt2

任务目标

本案例将完成一个最小可复现的MoveIt2模型加载闭环：从XACRO机器人描述文件出发，通过MoveIt2的move_group节点加载机器人模型，并在RViz中可视化。最终你将看到机器人的完整模型显示在RViz界面中，且move_group节点能够正确识别机器人的规划组。

环境与依赖

Ubuntu 22.04 + ROS 2 Humble（或更高版本ROS 2发行版）
已安装moveit2（包含move_group、moveit_ros_visualization）
已安装urdf、xacro、robot_state_publisher、joint_state_publisher_gui
已安装rviz2

实现方案

采用最小可复现结构：一个自定义ROS 2包moveit2_demo，包含机器人XACRO描述文件、launch启动文件，以及一个用于加载MoveIt2配置的YAML文件。机器人模型使用一个简单的6自由度机械臂（包含基座、6个连杆和6个旋转关节），足以演示MoveIt2的规划组识别功能。

文件结构与完整代码

创建ROS 2包并添加以下文件：

moveit2_demo/
├── CMakeLists.txt
├── package.xml
├── config/
│   └── moveit_controllers.yaml
├── launch/
│   └── demo.launch.py
├── robots/
│   ├── arm.urdf.xacro
│   └── arm.urdf
└── src/
    └── (空，无需源码)

1. 机器人描述文件 robots/arm.urdf.xacro

<?xml version="1.0"?>
<robot name="arm" xmlns:xacro="http://www.ros.org/wiki/xacro">
  <link name="base_link">
    <visual>
      <geometry><box size="0.2 0.2 0.1"/></geometry>
      <material name="blue"/>
    </visual>
  </link>
  <link name="link1">
    <visual>
      <geometry><cylinder radius="0.05" length="0.3"/></geometry>
      <origin xyz="0 0 0.15"/>
      <material name="gray"/>
    </visual>
  </link>
  <joint name="joint1" type="revolute">
    <parent link="base_link"/>
    <child link="link1"/>
    <origin xyz="0 0 0.1"/>
    <axis xyz="0 0 1"/>
    <limit lower="-3.14" upper="3.14" effort="10" velocity="1"/>
  </joint>
  <!-- 为节省篇幅，link2~link6与link1结构类似，关节依次连接 -->
  <!-- 完整代码见下方说明 -->
</robot>

为保持代码简洁，此处省略link2~link6的重复定义。完整6自由度机械臂XACRO文件请按以下模式补齐：每个link包含一个圆柱体visual，每个joint为revolute类型，依次连接。关节轴交替为z和y方向，以形成可动结构。

2. 生成URDF（在包目录下执行）

source /opt/ros/humble/setup.bash
cd moveit2_demo/robots
xacro arm.urdf.xacro > arm.urdf

3. MoveIt2控制器配置 config/moveit_controllers.yaml

moveit_controller_manager: moveit_simple_controller_manager
moveit_simple_controller_manager:
  controller_names:
    - arm_controller
  arm_controller:
    type: FollowJointTrajectory
    joints:
      - joint1
      - joint2
      - joint3
      - joint4
      - joint5
      - joint6
    action_ns: follow_joint_trajectory
    default: true

4. Launch文件 launch/demo.launch.py

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess
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('moveit2_demo')
    urdf_path = os.path.join(pkg_share, 'robots', 'arm.urdf')
    srdf_path = os.path.join(pkg_share, 'config', 'arm.srdf')
    moveit_controllers = os.path.join(pkg_share, 'config', 'moveit_controllers.yaml')

    robot_description = {'robot_description': open(urdf_path).read()}

    return LaunchDescription([
        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            output='screen',
            parameters=[robot_description]
        ),
        Node(
            package='joint_state_publisher_gui',
            executable='joint_state_publisher_gui',
            output='screen'
        ),
        Node(
            package='moveit_ros_move_group',
            executable='move_group',
            output='screen',
            parameters=[{
                'robot_description': robot_description['robot_description'],
                'robot_description_semantic': open(srdf_path).read(),
                'use_sim_time': False,
                'moveit_controller_manager': 'moveit_simple_controller_manager',
            }],
            arguments=['--ros-args', '--params-file', moveit_controllers]
        ),
        Node(
            package='rviz2',
            executable='rviz2',
            output='screen',
            arguments=['-d', os.path.join(pkg_share, 'config', 'moveit.rviz')]
        )
    ])

5. SRDF文件 config/arm.srdf（需使用MoveIt Setup Assistant生成，或手动创建）

<robot name="arm">
  <group name="arm_group">
    <joint name="joint1"/>
    <joint name="joint2"/>
    <joint name="joint3"/>
    <joint name="joint4"/>
    <joint name="joint5"/>
    <joint name="joint6"/>
  </group>
  <group_state name="home" group="arm_group">
    <joint name="joint1" value="0"/>
    <joint name="joint2" value="0"/>
    <joint name="joint3" value="0"/>
    <joint name="joint4" value="0"/>
    <joint name="joint5" value="0"/>
    <joint name="joint6" value="0"/>
  </group_state>
</robot>

6. RViz配置 config/moveit.rviz（可通过RViz手动保存，或使用MoveIt2默认配置）

7. CMakeLists.txt与package.xml（标准ROS 2包配置，需安装robots、launch、config目录）

编译与运行

cd ~/ros2_ws
colcon build --packages-select moveit2_demo
source install/setup.bash
ros2 launch moveit2_demo demo.launch.py

验证方法

RViz显示：启动后RViz窗口应显示完整的6自由度机械臂模型。在RViz左侧Displays面板中，确认RobotModel的Description Topic为/robot_description，且模型正常显示无报错。
关节控制：在joint_state_publisher_gui窗口中拖动关节滑块，RViz中的机械臂应实时响应运动。
MoveIt2识别：在终端中执行以下命令，确认move_group已加载规划组：

ros2 param get /move_group robot_description_semantic | grep arm_group

预期输出包含arm_group的规划组定义，说明MoveIt2已正确加载机器人模型。

常见问题

RViz无模型显示：检查robot_state_publisher是否正常运行，执行ros2 topic echo /robot_description确认有数据发布。
move_group启动失败：确认SRDF文件中的组名与URDF关节名一致，且moveit_controllers.yaml中的关节列表与URDF匹配。
joint_state_publisher_gui无法打开：确认已安装joint_state_publisher_gui包，且ROS 2环境已source。

📚 课程目录