Ros2 Moveit2 之 在Riz2中可视化

 

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

先决条件

如果你还没有完成，请确保你已经完成了你的第一个项目hello_moveit中的步骤。这个项目假设你从上一个教程结束的地方开始。

步骤

1 添加依赖 moveit_visual_tools

package.xml在项目中的hello_moveit其他语句后面添加此行<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 <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。此函数会阻止您的程序，直到用户按下nextRViz 中的按钮。这对于在调试时逐步执行程序很有帮助。

• 最后一个绘制了我们规划的轨迹的轨迹路径。这通常有助于从轨迹的角度理解规划的轨迹。

您可能会问自己，我们为什么要创建这样的 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!");
}

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

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

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

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

 

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

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

 

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

选择并单击。Marker ArrayOK

滚动到显示面板中的项目底部并编辑新标记阵列正在使用的主题/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

单击NextRViz 中的按钮并查看您的应用程序进展。

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