【MoveIt 2】在RViz中可视化

十年一梦实验室

于 2024-07-31 21:59:31 发布

阅读量116

点赞数 2

本文链接：https://blog.csdn.net/cxyhjl/article/details/140835845

版权

Visualizing In RViz

本教程将向您介绍一个工具，该工具可以通过在RViz中渲染可视化效果，帮助您更容易地理解您的MoveIt应用程序正在做什么。

前提条件

如果您还没有完成，确保您已经完成了“您的第一个项目”中的步骤。本项目假设您是从hello_moveit项目开始的，这是之前教程的结束点。

步骤

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

为了测试这一切是否正常工作，请在工作区目录中打开终端（记得source您的ROS安装），然后使用colcon构建：

cd ~/ws_moveit
colcon build --mixin debug

cxy@cxy-Ubuntu2404:~/ws_moveit$ colcon build --packages-select hello_moveit --mixin debug
Starting >>> hello_moveit
Finished <<< hello_moveit [22.7s]                       


Summary: 1 package finished [23.1s]

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 Interface之前添加您的执行器。

// 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获取）。接下来，我们调用deleteAllMarkers()来清除RViz中我们从先前运行中留下的任何渲染状态。最后，我们加载远程控件。

远程控件是一个非常简单的插件，它让我们在RViz中有一个按钮与我们的程序交互。

4 编写可视化的闭包

在我们构建和初始化之后，我们现在创建一些闭包（具有访问我们当前作用域中变量的函数对象），我们稍后可以在程序中使用它们来帮助在RViz中渲染可视化效果。

// Create closures for visualization
// 创建用于可视化的闭包


auto const draw_title = [&moveit_visual_tools](auto text) {
  // 声明一个常量闭包 draw_title，使用 moveit_visual_tools 来执行


  auto const text_pose = [] {
    // 声明一个常量闭包 text_pose，用于生成文本位置


    auto msg = Eigen::Isometry3d::Identity();
    // 创建一个 3D 变换矩阵 msg，初始化为单位矩阵


    msg.translation().z() = 1.0;  // Place text 1m above the base link
    // 将文本位置的 z 坐标设为 1.0，即将文本放置在基座上方 1 米处


    return msg;
    // 返回这个变换矩阵 msg
  }();
  // 立即调用闭包 text_pose，并获得返回的变换矩阵


  moveit_visual_tools.publishText(text_pose, text, rviz_visual_tools::WHITE,
                                  rviz_visual_tools::XLARGE);
  // 使用 moveit_visual_tools 在指定的位置（text_pose）以白色和特大号显示文本
};


auto const prompt = [&moveit_visual_tools](auto text) {
  // 声明一个常量闭包 prompt，使用 moveit_visual_tools 来执行


  moveit_visual_tools.prompt(text);
  // 使用 moveit_visual_tools 显示一个提示信息
};


auto const draw_trajectory_tool_path =
    [&moveit_visual_tools,
     jmg = move_group_interface.getRobotModel()->getJointModelGroup(
         "manipulator")](auto const trajectory) {
      // 声明一个常量闭包 draw_trajectory_tool_path，使用 moveit_visual_tools 和机器人模型中的关节组 jmg 来执行
      // 这里的 jmg 是通过获取机器人模型的 "manipulator" 关节组来初始化的


      moveit_visual_tools.publishTrajectoryLine(trajectory, jmg);
      // 使用 moveit_visual_tools 绘制给定轨迹的路径线
    };

这三个闭包都通过引用捕获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!");
}

您很快就会注意到，在每次调用更改RViz中渲染内容的方法后，我们必须调用moveit_visual_tools上的一个叫做trigger的方法。这是因为发送到RViz的消息是批量的，并且在调用trigger时发送，以减少标志主题的带宽。

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

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

cxy@cxy-Ubuntu2404:~/ws_moveit$ colcon build --packages-select hello_moveit --mixin debug
Starting >>> hello_moveit
Finished <<< hello_moveit [14.0s]                       


Summary: 1 package finished [14.3s]

6 在RViz中启用可视化

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

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

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

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

然后选择RvizVisualToolsGui并点击确定。这将在左下角创建一个新的面板，上面有一个我们稍后将使用的Next按钮。

最后，我们需要添加一个Marker Array来渲染我们添加的可视化效果。在“显示”面板上点击“添加”按钮。

选择Marker Array并点击OK。

滚动到显示面板底部的项目，并编辑新的Marker Array正在使用的topic为/rviz_visual_tools。

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

7 运行程序

在新的终端中，进入工作区，source工作区，并运行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中的Next按钮，看看您的应用程序如何前进。

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

总结

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

进一步阅读

MoveItVisualTools具有许多其他有用的功能，用于可视化机器人动作。您可以在此处阅读更多信息。https://github.com/ros-planning/moveit_visual_tools/tree/ros2
还有更多使用MoveItVisualTools的MoveItCpp教程示例。https://moveit.picknik.ai/main/doc/examples/moveit_cpp/moveitcpp_tutorial.html
这是完整的hello_moveit.cpp源代码的副本。https://github.com/moveit/moveit2_tutorials/blob/main/doc/tutorials/visualizing_in_rviz/hello_moveit.cpp

下一步

在下一个教程“Planning Around Objects”中https://moveit.picknik.ai/main/doc/tutorials/planning_around_objects/planning_around_objects.html ，您将扩展在这里构建的程序，以添加到碰撞环境中，并看到机器人在这些变化中进行规划。

附录：完整代码

#include <moveit/move_group_interface/move_group_interface.h> // 包含MoveIt移动组接口的头文件
#include <moveit_visual_tools/moveit_visual_tools.h> // 包含MoveIt可视化工具的头文件


#include <memory> // 包含智能指针的头文件
#include <rclcpp/rclcpp.hpp> // 包含ROS 2的C++客户端库头文件
#include <thread> // 包含线程库的头文件


int main(int argc, char* argv[])
{
  // Initialize ROS and create the Node
  rclcpp::init(argc, argv); // 初始化ROS并创建节点
  auto const node = std::make_shared<rclcpp::Node>(
      "hello_moveit", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)); // 创建一个名为"hello_moveit"的节点，并自动声明参数


  // Create a ROS logger
  auto const logger = rclcpp::get_logger("hello_moveit"); // 创建一个ROS日志记录器


  // We spin up a SingleThreadedExecutor for the current state monitor to get
  // information about the robot's state.
  rclcpp::executors::SingleThreadedExecutor executor; // 启动一个单线程执行器，用于当前状态监视器以获取机器人的状态信息
  executor.add_node(node); // 将节点添加到执行器
  auto spinner = std::thread([&executor]() { executor.spin(); }); // 创建一个线程来运行执行器


  // Create the MoveIt MoveGroup Interface
  using moveit::planning_interface::MoveGroupInterface; // 使用MoveIt的移动组接口
  auto move_group_interface = MoveGroupInterface(node, "manipulator"); // 创建MoveIt移动组接口，控制"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可视化工具
  moveit_visual_tools.deleteAllMarkers(); // 删除所有标记
  moveit_visual_tools.loadRemoteControl(); // 加载远程控制


  // Create a closure for updating the text in rviz
  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;
    }(); // 创建一个闭包用于在rviz中更新文本
    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); };// 创建一个闭包用于绘制轨迹


  // 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"); // 提示用户在RvizVisualToolsGui窗口中按'next'以进行规划
  draw_title("Planning"); // 更新文本为"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"); // 提示用户在RvizVisualToolsGui窗口中按'next'以执行
    draw_title("Executing"); // 更新文本为"Executing"
    moveit_visual_tools.trigger(); // 触发可视化工具
    move_group_interface.execute(plan); // 执行计划
  }
  else
  {
    draw_title("Planning Failed!"); // 更新文本为"Planning Failed!"
    moveit_visual_tools.trigger(); // 触发可视化工具
    RCLCPP_ERROR(logger, "Planning failed!"); // 记录规划失败的错误信息
  }


  // Shutdown ROS
  rclcpp::shutdown(); // 关闭ROS
  spinner.join(); // 等待线程结束
  return 0; // 返回0
}

十年一梦实验室

关注

2
点赞
踩
3

收藏

觉得还不错? 一键收藏
0
评论
【MoveIt 2】在RViz中可视化

Visualizing In RViz本教程将向您介绍一个工具，该工具可以通过在RViz中渲染可视化效果，帮助您更容易地理解您的MoveIt应用程序正在做什么。前提条件如果您还没有完成，确保您已经完成了“您的第一个项目”中的步骤。本项目假设您是从hello_moveit项目开始的，这是之前教程的结束点。步骤1 添加依赖项moveit_visual_tools在hello_moveit项目的p...
复制链接

扫一扫