在 RViz 中可视化

本教程将向您介绍一个工具，通过在 RViz 中呈现可视化效果，该工具可以帮助您更轻松地了解 MoveIt 应用程序正在做什么。

先决条件

如果您还没有这样做，请确保您已完成您的第一个项目中的步骤。

该项目假设您从上一个教程结束的地方“hello_moveit”项目开始。

步骤

1 添加依赖 moveit_visual_tools

在其他“<depend>”语句之后，将此行添加到“hello_moveit”项目中的“package.xml”中：

<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 <moveit_visual_tools/moveit_visual_tools.h>

为了测试这一切是否有效，请在工作区目录中打开一个终端（记住在 opt 中获取 ROS 安装的源代码），然后使用 colcon 进行构建：

cd ~/ws_moveit
colcon build --mixin debug

2 创建 ROS 执行器并在线程上旋转节点

在初始化 MoveItVisualTools 之前，我们需要在 ROS 节点上旋转执行器。这是必要的，因为 MoveItVisualTools 与 ROS 服务和主题交互。首先，将线程库添加到顶部的包含中。

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

通过创建和命名记录器，我们可以保持程序日志井然有序。

// Create a ROS logger
auto const logger = rclcpp::get_logger("hello_moveit");

接下来，在创建 MoveIt MoveGroup 接口之前添加执行器。

  // Spin up a SingleThreadedExecutor for MoveItVisualTools to interact with ROS
  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(node);
  auto spinner = std::thread([&executor]() { executor.spin(); });

  // Create the MoveIt MoveGroup Interface

...

最后，退出之前请确保加入线程。

// Shutdown ROS
rclcpp::shutdown();  // <--- This will cause the spin function in the thread to return
spinner.join();  // <--- Join the thread before exiting
return 0;

进行这些更改后，重建工作区以确保没有任何语法错误。

3 创建并初始化 MoveItVisualTools

接下来，我们将在构造 MoveGroupInterface 之后构造并初始化 MoveItVisualTools。

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

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

我们将以下内容传递到构造函数中：ROS 节点、机器人的基本链接、要使用的标记主题（稍后会详细介绍）以及机器人模型（我们从 move_group_interface 中获取）。接下来，我们调用删除所有标记。这将清除 RViz 中之前运行遗留下来的任何渲染状态。最后，我们加载远程控制。远程控制是一个非常简单的插件，它让我们在 RViz 中有一个按钮可以与我们的程序交互。

4 为可视化编写闭包

在构造和初始化之后，我们现在创建一些闭包（可以访问当前范围内的变量的函数对象），我们稍后可以在程序中使用这些闭包来帮助在 RViz 中渲染可视化。

// Create closures for visualization
auto const draw_title = [&moveit_visual_tools](auto text) {
  auto const text_pose = [] {
    auto msg = Eigen::Isometry3d::Identity();
    msg.translation().z() = 1.0;  // Place text 1m above the base link
    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(
         "manipulator")](auto const trajectory) {
      moveit_visual_tools.publishTrajectoryLine(trajectory, jmg);
    };

三个闭包中的每一个都通过引用捕获“moveit_visual_tools”，最后一个捕获指向我们正在规划的关节模型组对象的指针。

每个闭包都调用“moveit_visual_tools”上的函数，该函数会更改 RViz 中的某些内容。

第一个“draw_title”在机器人底座上方一米处添加文本。这是一种从高层次显示程序状态的有用方法。
第二个调用一个名为“prompt”的函数。此函数会阻止您的程序，直到用户按下 RViz 中的“next”按钮。这有助于在调试时逐步执行程序。
最后一个绘制我们计划的轨迹的工具路径。这通常有助于从工具的角度理解计划的轨迹。

您可能会问自己为什么要创建这样的 lambda，原因只是为了使后面的代码更易于阅读和理解。在编写软件时，将功能分解为可轻松重用和单独测试的命名函数通常很有帮助。您将在下一节中看到我们如何使用我们创建的这些函数。

5 可视化程序的步骤

现在我们将扩充程序中间的代码。更新您的规划和执行代码以包含以下新功能：

// 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, "Planning failed!");
}

您很快就会注意到的一件事是，每次调用“moveit_visual_tools”来更改 RViz 中呈现的内容后，我们都必须调用“trigger”方法。这样做的原因是，当您调用“trigger”时，发送到 RViz 的消息会被分批发送，以减少标记主题的带宽。

最后，再次构建您的项目，以确保所有代码添加都是正确的。

cd ~/ws_moveit
source /opt/ros/rolling/setup.bash
colcon build --mixin debug

6 在 RViz 中启用可视化

打开一个新终端，获取工作区，然后启动打开 RViz 的演示启动文件。

cd ~/ws_moveit
source install/setup.bash
ros2 launch moveit2_tutorials demo.launch.py

取消选中“显示”选项卡中的“MotionPlanning”以将其隐藏。我们不会在下一部分中使用“MotionPlanning”插件。

../../../_images/uncheck_motion_planning.png

../../../_images/unchecked_motion_planning.png

要添加按钮来与我们添加到程序中的提示进行交互，请使用“面板/添加新面板”菜单打开对话框：

然后选择“RvizVisualToolsGui”并单击“确定”。这将在左下角创建一个新面板，其中有一个我们稍后会使用的“下一步”按钮。

最后，我们需要添加一个“标记数组”来呈现我们添加的可视化效果。单击“显示”面板中的“添加”按钮。

选择“标记数组”并单击“确定”。

滚动到显示面板中项目的底部，并将新标记阵列正在使用的主题编辑为“/rviz_visual_tools”。

您现在可以运行带有可视化效果的新程序了。

7 运行程序

在新终端中，转到工作区，获取工作区，然后运行“hello_moveit”：

cd ~/ws_moveit
source install/setup.bash
ros2 run hello_moveit hello_moveit

您会注意到您的程序已停止并显示如下日志：

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

单击 RViz 中的“下一步”按钮，查看您的应用程序进展。

单击下一步按钮后，您将看到您的应用程序已规划，在机器人上方添加了一个标题，并绘制了一条代表工具路径的线。要继续，请再次按“下一步”以查看您的机器人执行计划。

摘要

您扩展了使用 MoveIt 编写的程序，使其与 RViz 中的 Gui 进行交互，从而允许您使用按钮逐步执行程序，在机器人上方渲染一些文本，并显示您计划的工具路径。

进一步阅读

MoveItVisualTools 具有更多用于可视化机器人运动的有用功能。您可以在此处阅读更多信息。
在 MoveItCpp 教程中还有更多使用 MoveItVisualTools 的示例。
这是完整 hello_moveit.cpp 源代码的副本。

下一步

在下一个教程：doc: `围绕对象规划</doc/tutorials/planning_around_objects/planning_around_objects>`中，您将扩展在此处构建的程序以添加到碰撞环境并查看机器人计划中的这些变化。