📚 课程目录第4章加载机器人到MoveIt2

4. 加载机器人到MoveIt2

任务目标

在ROS2环境中，通过MoveIt2的launch系统加载一个机器人模型（URDF/Xacro），启动MoveIt2的规划与可视化界面，最终在RViz2中看到机器人模型并能够操作MoveIt2的交互式标记（InteractiveMarker）。

环境与依赖

Ubuntu 22.04 + ROS2 Humble（或更高版本）
已安装 moveit2（建议通过二进制包安装：sudo apt install ros-humble-moveit）
已安装 rviz2、xacro、robot_state_publisher
一个可用的机器人URDF/Xacro文件（本案例使用MoveIt2自带的Panda机械臂示例）

实现方案

本案例采用最小可复现方式：直接使用MoveIt2官方提供的Panda机器人描述包，通过自定义launch文件启动robot_state_publisher（发布机器人模型）和move_group节点（提供规划能力），并打开RViz2加载MoveIt2的MotionPlanning插件进行可视化。不修改机器人模型本身，只演示加载与可视化闭环。

完整代码

创建launch文件 load_panda_moveit.launch.py：

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    # 参数声明
    use_sim_time = LaunchConfiguration('use_sim_time', default='false')

    # Panda机器人URDF（xacro格式）
    panda_xacro = os.path.join(
        get_package_share_directory('moveit_resources_panda_description'),
        'urdf', 'panda.urdf.xacro'
    )

    # 机器人状态发布器：加载xacro并发布TF和robot_description
    robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{
            'robot_description': Command(['xacro ', panda_xacro]),
            'use_sim_time': use_sim_time,
        }],
        output='screen'
    )

    # MoveIt2的move_group节点（使用Panda的SRDF和配置）
    move_group_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(
                get_package_share_directory('moveit_resources_panda_moveit_config'),
                'launch', 'move_group.launch.py'
            )
        ),
        launch_arguments={
            'robot_description': Command(['xacro ', panda_xacro]),
            'use_sim_time': use_sim_time,
        }.items()
    )

    # RViz2 + MoveIt2 MotionPlanning插件
    rviz_config = os.path.join(
        get_package_share_directory('moveit_resources_panda_moveit_config'),
        'rviz', 'moveit.rviz'
    )
    rviz2 = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        arguments=['-d', rviz_config],
        parameters=[{'use_sim_time': use_sim_time}],
        output='screen'
    )

    return LaunchDescription([
        DeclareLaunchArgument('use_sim_time', default_value='false'),
        robot_state_publisher,
        move_group_launch,
        rviz2,
    ])

注意：上述代码中使用了 Command，需在文件头部补充导入：

from launch.substitutions import Command

运行方式

确保已安装依赖包：

sudo apt install ros-humble-moveit-resources-panda-description ros-humble-moveit-resources-panda-moveit-config

编译工作空间（若在src下创建了launch文件）：
```
cd ~/your_ws && colcon build --packages-select your_pkg
```

source环境并启动：

source install/setup.bash
ros2 launch your_pkg load_panda_moveit.launch.py

验证方法与通过标准

观察RViz2窗口：启动后约3~5秒内，RViz2中应显示Panda机械臂模型（红色/灰色连杆），左侧Displays面板中MotionPlanning插件已加载。
检查TF树：在另一个终端运行 ros2 run tf2_tools view_frames，生成的frames.pdf中应包含从panda_link0到panda_hand的完整TF链。
验证move_group响应：在RViz2中点击机械臂末端的InteractiveMarker并拖动，机械臂应跟随运动（绿色圆弧显示规划轨迹）。
命令行确认：运行 ros2 node list，应看到 /move_group 和 /robot_state_publisher 节点存在。

满足以上4点即表示机器人成功加载到MoveIt2并完成可视化。

常见问题

RViz2中无模型：检查终端是否有xacro解析错误；确认robot_description参数已正确传入（可在RViz2的MotionPlanning插件中查看Robot Description是否非空）。
move_group启动失败：确认SRDF文件与URDF匹配，且moveit_resources_panda_moveit_config包已安装。
拖动InteractiveMarker无反应：检查/move_group是否正常发布规划场景，运行ros2 topic echo /planning_scene应有数据输出。