【MoveIt2-humble】入门教程（翻译自官方文档）二：在 RViz 中实现可视化

前言

本系列教程共四节，环境为：

Ubuntu22.04
ros2-humble
MoveIt2-humble
官方文档上的教程，从moveit1的melodic到moveit2的foxy基本一致，但是从最新的humble开始有了很大的变化，其中之一便是 lambda表达式 的广泛使用。

本节为教程的第二节，会介绍一个工具（moveit_visual_tools），它能够通过在 rviz 中呈现可视化，从而帮助你更容易理解你的 Moveit 应用在做什么。

前提是需要先完成之前内容，可以看我的这篇博文：https://blog.csdn.net/qq_43557907/article/details/125679824
四节教程会手把手带你写一个完整的 Moveit 控制程序，包括轨迹规划、RViz可视化、添加碰撞物体、抓取和放置。

更多信息可以去官方文档查看：https://moveit.picknik.ai/humble/index.html
本系列博客为我个人翻译，方便自己学习使用。

准备

请先完成第一节教程

步骤

1 添加 moveit_visual_tools 依赖包

把下面这行添加到 hello_moveit 项目下 package.xml 中其它<depend>的下面：

<depend>moveit_visual_tools</depend>

然后在 CMakeLists.txt中添加下面这行到find_package声明中：

find_package(moveit_visual_tools REQUIRED)

在该文件后面，扩展ament_target_dependencies宏调用来包含这个新的依赖，如：

ament_target_dependencies(
  hello_moveit
  "moveit_ros_planning_interface"
  "moveit_visual_tools"
  "rclcpp"
)

为了核实依赖已经正确添加，在hello_moveit.cpp中添加include路径：

#include <moveit_visual_tools/moveit_visual_tools.h>

保存后打开一个新终端，编译看是否报错。

2 创建 ROS 执行器并在线程中循环节点

在初始化 MoveItVisualTools 之前，需要有一个执行器在 ROS 节点上不断循环。

这是必要的操作，来让MoveItVisualTools与 ROS服务和话题交互。

#include <thread>  // <---- add this to the set of includes at the top

  ...

  // 创建一个 ROS logger
  auto const logger = rclcpp::get_logger("hello_moveit");

  // 循环一个单线程执行器来让 MoveItTools 与 ROS 交互
  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(node);
  // 实例化一个线程对象，使用 lambda表达式来构造
  auto spinner = std::thread([&executor]() { executor.spin(); });

  // Create the MoveIt MoveGroup Interface
  ...

  // Shutdown ROS
  rclcpp::shutdown();  // <--- 这将会使线程中的回调函数返回
  spinner.join();  // <--- 在结束前加入子线程，等待子线程结束，主进程才可以退出
  return 0;
}

然后重新编译工作空间确保没有语法错误。

3 创建并初始化 MoveItVisualTools

在创建 MoveGroupInterface 后创建和初始化 MoveItVisualTools：

// Create the MoveIt MoveGroup Interface
using moveit::planning_interface::MoveGroupInterface;
auto move_group_interface = MoveGroupInterface(node, "panda_arm");

// Construct and initialize MoveItVisualTools
auto moveit_visual_tools = moveit_visual_tools::MoveItVisualTools{
    node, "panda_link0", rviz_visual_tools::RVIZ_MARKER_TOPIC,
    move_group_interface.getRobotModel()};
moveit_visual_tools.deleteAllMarkers();
moveit_visual_tools.loadRemoteControl();

我们传递以下参数到构造函数中：

1. ROS节点
2. 机械臂的 base link
3. 要使用的 marker 话题（稍后介绍）
4. 机器人模型 robot model（从 move_group_interface 中获得）

下一步，我们调用其成员函数去删除所有的标记，这会清除 Rviz 中所有以前运行遗留下来的绘制状态。

最后，加载远程控制。远程控制是一个简单的插件，它能在 Rviz 中提供一个按钮来与我们的程序进行交互。

4 为可视化编写闭包

在初始化后，我们创建一些闭包（闭包是引用了自由变量的函数），这样我们就能在程序的后面调用这些闭包来帮助在 Rviz 中描述可视化。

// 创建用于可视化的闭包
auto const draw_title = [&moveit_visual_tools](auto text) {
  auto const text_pose = [] {
    auto msg = Eigen::Isometry3d::Identity();
    msg.translation().z() = 1.0;
    return msg;
  }();
  moveit_visual_tools.publishText(text_pose, text, rviz_visual_tools::WHITE,
                                  rviz_visual_tools::XLARGE);
};
auto const prompt = [&moveit_visual_tools](auto text) {
  moveit_visual_tools.prompt(text);
};
auto const draw_trajectory_tool_path =
    [&moveit_visual_tools,
     jmg = move_group_interface.getRobotModel()->getJointModelGroup(
         "panda_arm")](auto const trajectory) {
      moveit_visual_tools.publishTrajectoryLine(trajectory, jmg);
    };

这三个闭包都引用了 moveit_visual_tools，最后一个闭包引用了一个指向我们所规划的关节组对象的指针。

这些闭包调用了一个moveit_visual_tools上的函数，改变了 Rviz 中的一些东西：

1. 第一个闭包draw_title：在 RViz 中机械臂的上方一米处添加了文本，方便看运行状态；
2. 第二个闭包prompt：这个函数会阻止程序进行，直到用户按下 RViz 中的next按钮，方便 debug；
3. 第三个闭包draw_trajectory_tool_path ：画出了我们规划的轨迹路径。

你可能会好奇我们为什么要用 lambda 表达式。原因是它能更简单地编码，便于以后阅读和理解。当你写软件时，把功能分解为命名函数，有助于复用和测试。

5 Visualize the steps of your program

在程序中更新规划和计算部分的代码，以包括以上创建的新特性：

// Set a target Pose
auto const target_pose = [] {
  geometry_msgs::msg::Pose msg;
  msg.orientation.w = 1.0;
  msg.position.x = 0.28;
  msg.position.y = -0.2;
  msg.position.z = 0.5;
  return msg;
}();
move_group_interface.setPoseTarget(target_pose);

// Create a plan to that target pose
prompt("Press 'Next' in the RvizVisualToolsGui window to plan");
draw_title("Planning");
moveit_visual_tools.trigger();
auto const [success, plan] = [&move_group_interface] {
  moveit::planning_interface::MoveGroupInterface::Plan msg;
  auto const ok = static_cast<bool>(move_group_interface.plan(msg));
  return std::make_pair(ok, msg);
}();

// Execute the plan
if (success) {
  draw_trajectory_tool_path(plan.trajectory_);
  moveit_visual_tools.trigger();
  prompt("Press 'Next' in the RvizVisualToolsGui window to execute");
  draw_title("Executing");
  moveit_visual_tools.trigger();
  move_group_interface.execute(plan);
} else {
  draw_title("Planning Failed!");
  moveit_visual_tools.trigger();
  RCLCPP_ERROR(logger, "Planing failed!");
}

我们会注意到，在每次调用后，我们必须调用moveit_visual_tools中的trigger方法，才能改变 RViz 中呈现的东西。原因是发送给 RViz 的消息是批处理的，并在调用trigger时发送，以减少 marker 话题的带宽。

最后，再次编译程序并确保所有添加的代码是正确的。

6 在 RViz 中启用可视化

打开一个新终端，和之前一样，先启动 demo launch 来打开 RViz：

source install/setup.bash
ros2 launch moveit2_tutorials demo.launch.py

在 RViz 中，取消选择 “Displays”中的 “MotionPlanning”，这节中用不到这个插件

为了添加用于交互的按钮，我们打开“Panels/Add New Panel”菜单

选中RvizVisualToolsGui并点击 OK。这会在左下角创建一个新的面板，带有Next按钮，我们在后面会用到。


最后，我们需要添加Marker Array去描绘我们添加的可视化。在“Displays”面板中点击“Add”按钮：

选中Marker Array并点击 OK

在Marker Array面板中找到 Topic，编辑为/rviz_visual_tools

7 运行程序

在一个新终端中运行 hello_moveit 节点，你会发现程序停住了，并显示了如下的提示：

[INFO] [1652822889.492940200] [hello_moveit.remote_control]: Waiting to continue: Press 'Next' in the RvizVisualToolsGui window to plan

在 RViz 中点击 Next 按钮，会发现完成规划，并显示 title 和规划出的轨迹线。再点击 Next 按钮，机械臂开始执行规划。

扩展阅读

• MoveItVisualTools 有许多有用的特性来可视化机械臂的运动. You can read more about it here.
• There are also more examples of using MoveItVisualTools in MoveItCpp Tutorial.
• Here is a copy of the full hello_moveit.cpp source.

下一步的内容

下一节，你将会扩展本程序，添加碰撞环境并观察改变后的机械臂规划