本文由 linuxROS 整理发布，首发于 linuxros.cn，转载请注明出处。

MoveIt2可视化模块：RViz2调试与轨迹仿真

导读：MoveIt2 如何做到"所见即所得"的调试体验？RViz2 集成是 MoveIt2 开发调试的利器。本文详解机器人模型显示、规划场景可视化、轨迹调试等核心功能。

原理简析

MoveIt2 与 RViz2 通过 MotionPlanning Display 和 PlanningScene 插件实现深度集成，提供实时的机器人状态显示、障碍物可视化、规划结果预览等功能。所有可视化数据通过 ROS2 话题（/display_robot_state、/display_trajectory、/monitored_planning_scene）传输。

flowchart TB A["MoveIt2 move_group 节点"] --> B["Planning Scene 规划场景"] A --> C["MotionPlanning Display 规划插件"] B --> D["RViz2 3D 可视化"] C --> D D --> E["RobotModel URDF 模型"] D --> F["Octomap 障碍物显示"] D --> G["Trajectory 轨迹回放"]

核心功能：
- 机器人 URDF 模型实时显示
- 关节状态同步（订阅 /joint_states）
- 障碍物 Octomap 可视化
- 规划轨迹动态预览
- 交互式目标姿态拖拽
- 碰撞检测高亮提示

实操步骤

RViz2 MotionPlanning 插件配置流程

flowchart TB A(["启动 RViz2"]) --> B["添加 MotionPlanning Display"] B --> C["配置 Robot Description"] C --> D["加载 PlanningScene"] D --> E["连接 move_group 节点"] E --> F["设置 Planning Group"] F --> G(["开始交互式调试"])

配置步骤：
1. 启动 RViz2：rviz2 -d moveit.rviz（加载预设配置）
2. 添加 MotionPlanning Display 插件
3. 设置 Robot Description 参数为 robot_description
4. 配置 Planning Scene Topic 为 /monitored_planning_scene
5. 选择 Planning Group（如 manipulator）
6. 在 3D 视图中拖拽末端球设定目标姿态

避坑点：MoveIt Setup Assistant 生成的 moveit.rviz 已预置所有插件，建议直接 ros2 launch <robot>_moveit_config demo.launch.py 一键启动，避免手动配置遗漏。
来自 linuxros.cn · linuxROS

代码实现

使用 MoveItVisualTools 发布可视化

// visualization.cpp
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <rviz_visual_tools/rviz_visual_tools.h>

class VisualToolsExample
{
public:
    VisualToolsExample(rclcpp::Node::SharedPtr node,
                       moveit::core::RobotModelConstPtr robot_model)
    {
        namespace rvt = rviz_visual_tools;
        visual_tools_ = std::make_shared<moveit_visual_tools::MoveItVisualTools>(
            node, "base_link", "moveit_visual_tools", robot_model);
        visual_tools_->loadRobotStatePub("/display_robot_state");
        visual_tools_->loadTrajectoryPub("/display_trajectory");
        visual_tools_->loadRemoteControl();  // 启用 RViz 按钮远程控制
    }

    void publishTrajectory(
        const robot_trajectory::RobotTrajectory& trajectory)
    {
        visual_tools_->publishTrajectory(trajectory);
        visual_tools_->trigger();
    }

    void publishPath(const std::vector<Eigen::Isometry3d>& path)
    {
        namespace rvt = rviz_visual_tools;
        visual_tools_->publishPath(path, rvt::ORANGE, 0.02);
        visual_tools_->trigger();
    }

    void publishText(const std::string& text)
    {
        namespace rvt = rviz_visual_tools;
        Eigen::Isometry3d pose = Eigen::Isometry3d::Identity();
        pose.translation().z() = 1.0;
        visual_tools_->publishText(pose, text, rvt::WHITE, rvt::XLARGE);
        visual_tools_->trigger();
    }

private:
    moveit_visual_tools::MoveItVisualTools::SharedPtr visual_tools_;
};

发布自定义标记物

// markers.cpp
void PublishMarkersExample(
    moveit_visual_tools::MoveItVisualTools& visual_tools)
{
    namespace rvt = rviz_visual_tools;

    // 发布球体（标记目标点）
    Eigen::Vector3d center = {0.5, 0.0, 0.2};
    visual_tools.publishSphere(center, rvt::GREEN, rvt::XLARGE);

    // 发布文本标签
    Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
    text_pose.translation() = {0.5, 0.0, 0.4};
    visual_tools.publishText(text_pose, "Target Position",
                              rvt::WHITE, rvt::MEDIUM);

    // 发布路径（笛卡尔路径预览）
    std::vector<Eigen::Isometry3d> path;
    for (double t = 0; t <= 1.0; t += 0.1) {
        Eigen::Isometry3d pose = Eigen::Isometry3d::Identity();
        pose.translation() = {t * 0.5, 0.0, 0.3};
        path.push_back(pose);
    }
    visual_tools.publishPath(path, rvt::ORANGE, 0.02);

    visual_tools.trigger();  // 必须调用，否则标记物不显示
}

注意：trigger() 是批量发布的关键，多个 publish* 调用必须用一次 trigger() 提交，避免每条消息单独发造成 RViz 卡顿。

RViz 关键话题清单

话题	类型	用途
`/display_robot_state`	`DisplayRobotState`	显示指定机器人状态
`/display_trajectory`	`DisplayTrajectory`	回放规划轨迹
`/monitored_planning_scene`	`PlanningScene`	同步规划场景（含障碍物）
`/joint_states`	`JointState`	关节状态实时同步
`/collision_object`	`CollisionObject`	增删碰撞物体

常见问题解决

Q1：机器人模型显示错位？
A：检查 URDF 中 <origin> 的 xyz/rpy 配置；确认 robot_description 参数已正确加载；用 ros2 run tf2_tools view_frames 检查 TF 树完整性。

Q2：Octomap 不显示？
A：确认点云话题（如 /camera/depth/points）有数据；检查 PlanningScene 的 Octomap 订阅配置；调整 octomap_resolution 与点云密度匹配。

Q3：轨迹预览不流畅？
A：减少轨迹点数（增大 eef_step）；降低 RViz 刷新频率（Update Interval）；启用 Loop Animation 复看细节。

Q4：MotionPlanning 插件无响应？
A：确认 move_group 节点正常运行（ros2 node list）；检查 Planning Group 名称是否与 SRDF 一致；查看 ros2 topic echo /move_group/status 的错误反馈。

总结

RViz2 可视化是 MoveIt2 开发调试的核心工具，善用 MotionPlanning 插件和 MoveItVisualTools 库可以大幅提升开发效率。掌握可视化调试技巧，能直观发现规划问题、碰撞位置与轨迹异常，加快项目落地。至此，MoveIt2 六大模块（概述、运动学、规划器、碰撞检测、控制器、可视化）全部讲完，下一篇我们将动手实战，从零搭建一个完整的 MoveIt2 机械臂项目。