📚 课程目录第2章使用RViz2进行交互式运动规划

2. 使用RViz2进行交互式运动规划

任务目标

在RViz2中加载机器人模型，通过交互标记（Interactive Marker）手动拖动目标位姿，触发MoveIt2运动规划，并在RViz2中可视化规划出的运动轨迹。本案例使用Panda机械臂作为示例机器人。

环境与依赖

Ubuntu 22.04 + ROS2 Humble（或更高版本）
已安装 moveit2、moveit_resources、rviz2
已安装 ros-humble-moveit-resources-panda-description（提供Panda模型）

实现步骤

本案例通过一个launch文件同时启动机器人状态发布、MoveIt2规划节点和RViz2界面。RViz2中已配置好InteractiveMarker显示组件，用户可直接拖动末端执行器目标标记。

1. 创建launch文件

创建文件 panda_interactive_demo.launch.py：

from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import ExecuteProcess
from moveit_configs_utils import MoveItConfigsBuilder

def generate_launch_description():
    moveit_config = (
        MoveItConfigsBuilder("panda", package_name="moveit_resources_panda_moveit_config")
        .robot_description(file_path="config/panda.urdf.xacro")
        .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml")
        .planning_scene_monitor(
            publish_robot_description=True,
            publish_planning_scene=True,
        )
        .to_moveit_configs()
    )

    # 启动MoveIt2规划节点（含交互标记服务器）
    move_group_node = Node(
        package="moveit_ros_move_group",
        executable="move_group",
        output="screen",
        parameters=[moveit_config.to_dict()],
    )

    # 启动RViz2（使用预配置的交互标记显示）
    rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="screen",
        arguments=["-d", "src/moveit_resources/panda_moveit_config/launch/moveit.rviz"],
    )

    # 启动静态虚拟关节（固定机器人基座）
    virtual_joint_node = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        parameters=[moveit_config.robot_description],
    )

    return LaunchDescription([
        virtual_joint_node,
        move_group_node,
        rviz_node,
    ])

2. 运行演示

source /opt/ros/humble/setup.bash
source /path/to/your/workspace/install/setup.bash
ros2 launch moveit_resources_panda_moveit_config demo.launch.py

若使用上述自定义launch文件，则：

ros2 launch your_package panda_interactive_demo.launch.py

RViz2交互操作

启动后，RViz2中显示Panda机械臂模型，末端执行器处有一个带三个箭头和三个圆环的交互标记（InteractiveMarker）。
拖动箭头：沿对应坐标轴平移末端目标位置。
拖动圆环：绕对应坐标轴旋转末端目标姿态。
每次拖动松开后，MoveIt2自动执行运动规划，规划成功后显示规划轨迹（机器人模型会显示规划后的运动路径）。
若规划失败，机器人保持原位，RViz2下方状态栏显示Planning failed。

验证方法与判定标准

启动验证：终端无报错，RViz2中能看到Panda模型和交互标记。
交互验证：拖动交互标记的箭头或圆环，松开后观察机器人是否规划并显示新轨迹。
成功判定：拖动后机器人模型更新到新目标位姿，且规划路径以机器人模型动画形式显示（或显示为轨迹线），即判定通过。
失败判定：若目标位姿超出关节限位或发生自碰撞，RViz2显示Planning failed，机器人不移动，属正常现象。

常见问题

看不到交互标记：检查RViz2左侧Display面板中是否已添加InteractiveMarker显示组件，并确保MoveIt2的move_group节点已启动。
拖动无反应：确认交互标记处于“Translate”或“Rotate”模式（RViz2顶部工具栏可切换），且未误触“Disable”按钮。
规划失败率高：拖动幅度过大导致目标不可达，可尝试小步拖动或使用“Plan”按钮手动触发规划。