Moveit2 在RViz中可视化

系列文章目录

留空

---

---

前言

自用

---

一、完整代码

#include <memory>

/*第一个C++程序*/
#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
/*在 RViz 中可视化*/
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <thread>

int main(int argc, char * argv[])
{
  // 1.1 初始化 ROS 并创建节点
  rclcpp::init(argc, argv);
  auto const node = std::make_shared<rclcpp::Node>(
    "hello_moveit",
    // 创建名为 "hello_moveit" 的 ROS 2 节点，并自动声明从命令行传入的参数
    rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
  );

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

  // 1.3 创建单线程执行器
  // 创建一个 `SingleThreadedExecutor` 来处理 ROS 的回调函数
  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(node);

  // 1.4 启动一个独立线程运行 `spin` 方法
  // 使用一个独立的线程来运行 executor 的 spin 方法
  // `spin` 方法会处理所有添加到 executor 的节点的回调函数，直到节点被关闭
  auto spinner = std::thread([&executor]() { executor.spin(); });

  // 2.1 创建 MoveGroupInterface
  // 创建 MoveIt 的 `MoveGroupInterface` 接口，用于与机械臂进行交互
  using moveit::planning_interface::MoveGroupInterface;
  auto move_group_interface = MoveGroupInterface(node, "panda_arm");

  // 2.2 初始化 MoveItVisualTools
  // 构建并初始化 MoveItVisualTools 用于在 Rviz 中进行可视化
  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();

  // 2.3 创建用于可视化的闭包函数
  // 创建用于可视化的闭包函数（Lambda 函数）
  auto const draw_title = [&moveit_visual_tools](auto text) {
    auto const text_pose = [] {
      auto msg = Eigen::Isometry3d::Identity(); // 设置文本的初始位置
      msg.translation().z() = 1.0; // 提升文本位置以便在 Rviz 中显示
      return msg;
    }();
    moveit_visual_tools.publishText(text_pose, text, rviz_visual_tools::WHITE,
                                    rviz_visual_tools::XLARGE); // 发布文本标记
  };

  // 等待用户点击 Rviz 中的 "Next" 按钮继续执行的闭包函数
  auto const prompt = [&moveit_visual_tools](auto text) {
    moveit_visual_tools.prompt(text);
  };

  // 在 Rviz 中绘制轨迹的闭包函数
  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); // 发布轨迹线
    };

  // 2.4 设置目标位姿
  // 设置目标位姿
  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); // 将目标位姿设置为机械臂的目标

  // 2.5 创建规划并提示用户
  // 创建一个计划到目标位姿的路径
  prompt("Press 'Next' in the RvizVisualToolsGui window to plan"); // 提示用户继续
  draw_title("Planning"); // 在 Rviz 中显示 "Planning" 字样
  moveit_visual_tools.trigger(); // 触发 Rviz 更新显示
  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); // 返回规划结果和规划的路径
  }();

  // 2.6 执行规划路径
  // 如果规划成功，执行规划
  if (success) {
    draw_trajectory_tool_path(plan.trajectory_); // 在 Rviz 中绘制轨迹
    moveit_visual_tools.trigger(); // 触发 Rviz 更新显示
    prompt("Press 'Next' in the RvizVisualToolsGui window to execute"); // 提示用户继续
    draw_title("Executing"); // 在 Rviz 中显示 "Executing" 字样
    moveit_visual_tools.trigger(); // 触发 Rviz 更新显示
    move_group_interface.execute(plan); // 执行规划的路径
  } else {
    draw_title("Planning Failed!"); // 规划失败，在 Rviz 中显示错误信息
    moveit_visual_tools.trigger(); // 触发 Rviz 更新显示
    RCLCPP_ERROR(logger, "Planning failed!"); // 记录错误日志
  }

  // 1.5 关闭 ROS 节点
  rclcpp::shutdown();  // 这将使 `spin` 函数返回，停止执行回调
  spinner.join();  // 等待线程结束，确保程序安全退出
  return 0;
}

编译

打开一个终端

cd moveit2_ws
colcon build --packages-select learning_moveit2 
source install/setup.sh
ros2 launch moveit2_tutorials demo.launch.py

经过这样那样变成下图（放在 四、补充 的 2）

打开另一个终端

source install/setup.sh
ros2 run learning_moveit2 hello_moveit

点击Next

再点

完美结束

二、编写步骤

1. 初始化和配置 ROS 环境

1. 初始化ROS2并创建节点：初始化ROS2 系统，创建一个名为 "hello_moveit" 的 ROS 2 节点。
2. 创建 ROS2 日志记录器：用于记录日志信息。
3. 创建单线程执行器：用于处理 ROS2 的回调函数。
4. 启动独立线程：运行执行器的 spin 方法，以便处理回调函数。
5. 关闭 ROS2 节点：停止 ROS2 节点并等待线程结束。

2. 使用 MoveIt 进行机械臂控制和可视化

1. 创建 MoveGroupInterface：用于与机械臂进行交互。
2. 初始化 MoveItVisualTools：用于在 Rviz 中可视化机械臂的状态和轨迹。
3. 定义可视化闭包函数：
   • 绘制标题：在 Rviz 中显示文本标记。
   • 提示用户：在 Rviz 中提示用户操作。
   • 绘制轨迹：在 Rviz 中绘制机械臂的轨迹。
4. 设置目标位姿：定义机械臂的目标位置和姿态。
5. 创建并执行规划：
   • 创建规划：计算机械臂从当前位置到目标位置的路径。
   • 执行规划：沿着规划的路径移动机械臂。如果规划失败，记录错误并在 Rviz 中显示错误信息。

三、代码详细解析

1. 初始化和配置 ROS2 环境

#include <thread>  // 引入线程库

int main(int argc, char * argv[])
{
  // 1.1 初始化 ROS2 并创建节点
  rclcpp::init(argc, argv);
  auto const node = std::make_shared<rclcpp::Node>(
    "hello_moveit", 
    rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
  );

• rclcpp::init(argc, argv);: 初始化 ROS 2 系统，准备处理命令行参数。这里 argc 和 argv 分别表示参数的个数和具体的参数值，这些参数会传递给 ROS 进行配置。
• std::make_shared<rclcpp::Node>("hello_moveit", ...);: 使用 std::make_shared 创建一个共享指针，生成名为 "hello_moveit" 的 ROS 2 节点。
• NodeOptions().automatically_declare_parameters_from_overrides(true) 设置节点的参数选项，允许从命令行传入的参数自动声明。

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

• rclcpp::get_logger("hello_moveit");: 创建一个日志记录器，用于输出日志信息，帮助调试和监控程序的运行。"hello_moveit" 是日志记录器的名称，用来标识日志信息的来源。

  // 1.3 创建单线程执行器
  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(node);

• rclcpp::executors::SingleThreadedExecutor executor;: 创建一个 SingleThreadedExecutor 对象，用于处理 ROS 节点的回调函数。SingleThreadedExecutor 是 ROS 2 中的一种执行器，表示在单线程中顺序处理回调函数。

• executor.add_node(node);: 将之前创建的节点 node 添加到执行器中。这样执行器就可以管理该节点，并处理该节点的回调函数。

  // 1.4 启动一个独立线程运行 `spin` 方法
  auto spinner = std::thread([&executor]() { executor.spin(); });

• std::thread([&executor]() { executor.spin(); });: 使用 std::thread 创建一个新线程，在这个线程中执行 executor.spin(); 方法。executor.spin() 会持续运行，处理与节点相关的回调函数。使用新线程的目的是让回调函数在后台持续运行，而主线程可以继续执行其他任务。

• Lambda 表达式 [&executor]() { executor.spin(); }: 这个 Lambda 表达式定义了一个无参数、无返回值的匿名函数，并捕获了外部变量 executor。在这个表达式中，executor.spin() 会在新线程中运行。

  // 1.5 关闭 ROS 节点
  rclcpp::shutdown();  // 停止 ROS 节点
  spinner.join();  // 等待线程结束
  return 0;
}

• rclcpp::shutdown();: 停止 ROS 系统，关闭所有的 ROS 节点。这个调用会使 executor.spin() 方法退出，从而停止执行回调函数。
• spinner.join();: 调用 join() 方法等待线程 spinner 结束。这样可以确保后台线程在主线程退出前已经完成其任务，避免线程未正确关闭引发的资源泄露或未定义行为。
• return 0;: 结束 main 函数并返回 0，表示程序成功执行完毕。

2. 使用 MoveIt 进行机械臂控制和可视化

理解了你的需求，以下是对代码的详细解释，按步骤分解并解释每部分的功能和使用的专业术语。

2.1 创建 MoveGroupInterface

// 包含 MoveIt 的运动规划接口
#include <moveit/move_group_interface/move_group_interface.h>

// 创建 MoveGroupInterface 对象，用于控制机械臂运动
auto move_group_interface = moveit::planning_interface::MoveGroupInterface(node, "panda_arm");

• MoveGroupInterface：MoveIt 提供的一个接口类，用于与机械臂进行交互。通过它，可以发送目标位姿、获取机械臂状态、规划和执行运动等。
• "panda_arm"：指定要控制的机械臂组的名称。

2.2 初始化 MoveItVisualTools

// 包含 MoveIt Visual Tools，用于在 Rviz 中可视化机械臂状态和轨迹
#include <moveit_visual_tools/moveit_visual_tools.h>

// 初始化 MoveItVisualTools，设置基座链接和 Rviz 中的标记主题
auto moveit_visual_tools = moveit_visual_tools::MoveItVisualTools(
    node, "panda_link0", rviz_visual_tools::RVIZ_MARKER_TOPIC, move_group_interface.getRobotModel());

这段代码创建了一个名为 moveit_visual_tools 的对象，它是 MoveItVisualTools 类型的实例。这个对象的作用是在ROS2可视化工具Rviz中展示机械臂的状态和运动轨迹。具体来说：

• node: 是一个ROS节点，用于与ROS系统通信。
• "panda_link0": 是机械臂的基座链接，即机械臂的起始点。
• rviz_visual_tools::RVIZ_MARKER_TOPIC: 是一个ROS主题，用于发布标记消息到Rviz中，以便在3D场景中显示机械臂的运动和规划路径。
• move_group_interface.getRobotModel(): 获取当前机械臂的模型，这样可以确保在Rviz中正确显示机械臂的运动轨迹。

总之，这行代码的目的是初始化一个工具，使得机械臂在Rviz中的运动和规划结果可以直观地展示出来。

// 删除所有旧的标记
moveit_visual_tools.deleteAllMarkers();

// 加载远程控制界面，允许用户在 Rviz 中进行交互操作
moveit_visual_tools.loadRemoteControl();

这两行代码的作用是在使用 moveit_visual_tools 对象时进行一些准备工作，具体解释如下：

1. moveit_visual_tools.deleteAllMarkers();

   • 这行代码调用了 deleteAllMarkers() 方法，用于删除在之前可能已经在 Rviz 中显示的所有标记。这样做可以确保在执行后续操作时，Rviz 中不会混杂之前的标记，从而保持界面的清晰度和准确性。

2. moveit_visual_tools.loadRemoteControl();

   • 这行代码调用了 loadRemoteControl() 方法，该方法允许在 Rviz 中加载一个远程控制界面。这个界面使用户可以通过 Rviz 来交互地控制和调整机械臂的运动和显示，例如选择动作、设置目标位姿等操作。

这两行代码确保在使用 moveit_visual_tools 对象进行机械臂运动规划和可视化时，能够清除之前的标记，并提供一个交互界面，使用户可以方便地在 Rviz 中进行操作和观察。

2.3 创建用于可视化的闭包函数

// 定义闭包函数，用于在 Rviz 中显示标题文本
auto draw_title = [&moveit_visual_tools](const std::string& text) {
    auto text_pose = Eigen::Isometry3d::Identity(); // 设置文本的初始位置
    text_pose.translation().z() = 1.0; // 提升文本位置以便在 Rviz 中显示
    moveit_visual_tools.publishText(text_pose, text, rviz_visual_tools::WHITE, rviz_visual_tools::XLARGE); // 发布文本标记
};

// 定义闭包函数，提示用户进行下一步操作
auto prompt = [&moveit_visual_tools](const std::string& text) {
    moveit_visual_tools.prompt(text); // 在 Rviz 中显示提示信息
};

// 定义闭包函数，绘制机械臂轨迹
auto draw_trajectory_tool_path = [&moveit_visual_tools, 
                                  jmg = move_group_interface.getRobotModel()->getJointModelGroup("panda_arm")]
    (const moveit::planning_interface::MoveGroupInterface::Plan& plan) {
    moveit_visual_tools.publishTrajectoryLine(plan.trajectory_, jmg); // 发布轨迹线
};

这段代码定义了三个闭包函数（Lambda 函数），用于在 MoveItVisualTools 中进行操作和可视化：

auto xxx = [...]：使用 auto 关键字定义了一个闭包函数 xxx。[&moveit_visual_tools]：闭包函数通过引用捕获了外部的 moveit_visual_tools 对象。

1. draw_title 在 Rviz 中绘制标题文本。
   • 闭包函数 draw_title 接受一个字符串参数 text，用于在 Rviz 中显示标题文本。
   • Eigen::Isometry3d::Identity() 创建一个单位变换矩阵，用于设置文本的初始位置。
   • text_pose.translation().z() = 1.0; 提升文本位置，使其在 Rviz 中更清晰可见。
   • moveit_visual_tools.publishText(...); 发布文本标记，显示指定文本内容、颜色和大小。
2. prompt 在 Rviz 中显示用户提示。
   • 闭包函数 prompt 接受一个字符串参数 text，用于在 Rviz 中显示提示信息，让用户进行下一步操作。
   • moveit_visual_tools.prompt(text); 在 Rviz 中显示给定的提示文本，等待用户操作。
3. draw_trajectory_tool_path 在 Rviz 中绘制机械臂的运动轨迹。
   • 闭包函数 draw_trajectory_tool_path 在（）中接受一个 MoveGroupInterface::Plan 对象 plan，用于在 Rviz 中绘制机械臂的规划轨迹。
   • [&moveit_visual_tools, jmg = ...]：这里的 &moveit_visual_tools 表示闭包函数中引用了外部的 moveit_visual_tools 对象，闭包还引入了一个名为 jmg 的局部变量。这个 jmg 是通过 move_group_interface.getRobotModel()->getJointModelGroup("panda_arm") 获取的机械臂模型中的关节模型组 “panda_arm”。
   • (const moveit::planning_interface::MoveGroupInterface::Plan& plan)：这是闭包函数的参数列表，它接受一个类型为 moveit::planning_interface::MoveGroupInterface::Plan 的引用参数 plan。这个 plan 包含了机械臂的运动规划信息。
   • moveit_visual_tools.publishTrajectoryLine(...); 调用 moveit_visual_tools 对象的 publishTrajectoryLine 方法，用于在 Rviz 中发布机械臂的运动轨迹线，使用给定的关节模型组 jmg，展示规划的运动路径。
   • 其中，plan.trajectory_：通过 plan 参数访问机械臂的规划路径 trajectory_。jmg：作为第二个参数，指定机械臂的关节模型组，确保轨迹线的正确显示和适配。

2.4 设置目标位姿

// 设置目标位姿，定义机械臂末端的目标位置和姿态
geometry_msgs::msg::Pose target_pose;
target_pose.orientation.w = 1.0;  // 设置四元数，表示没有旋转
target_pose.position.x = 0.28;
target_pose.position.y = -0.2;
target_pose.position.z = 0.5;

// 将目标位姿设置为机械臂的运动目标
move_group_interface.setPoseTarget(target_pose);

• geometry_msgs::msg::Pose：定义机械臂末端执行器的位置和姿态。
• setPoseTarget：设置目标位姿，机械臂将朝这个目标进行运动。

2.5 创建和执行规划

// 提示用户开始规划机械臂运动
prompt("Press 'Next' in the RvizVisualToolsGui window to plan");
draw_title("Planning");

// 触发 Rviz 更新以显示最新的可视化内容
moveit_visual_tools.trigger();

// 进行运动规划，获取规划结果
moveit::planning_interface::MoveGroupInterface::Plan plan;
bool success = (move_group_interface.plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

if (success) {
    // 如果规划成功，绘制运动轨迹并提示用户执行
    draw_trajectory_tool_path(plan); // 调用绘制轨迹的闭包函数，显示机械臂运动路径
    prompt("Press 'Next' in the RvizVisualToolsGui window to execute"); // 提示用户执行下一步操作
    draw_title("Executing"); // 在 Rviz 中显示 "Executing" 字样，表示正在执行

    // 触发 Rviz 更新以显示最新的可视化内容
    moveit_visual_tools.trigger();

    // 执行机械臂运动规划的路径
    move_group_interface.execute(plan); // 调用 MoveGroupInterface 的 execute 方法执行规划好的路径
} else {
    // 如果规划失败，在 Rviz 中显示错误信息
    draw_title("Planning Failed!"); // 在 Rviz 中显示规划失败的提示信息
    moveit_visual_tools.trigger(); // 触发 Rviz 更新显示
    RCLCPP_ERROR(logger, "Failed to plan motion"); // 记录错误日志，提示运动规划失败
}

• 运动规划：

  • move_group_interface.plan(plan)：调用 MoveGroupInterface 对象的 plan 方法，对机械臂从当前位置到目标位置的运动路径进行规划。这个方法会返回一个规划结果，其中包括规划好的运动路径。
  • (... == moveit::planning_interface::MoveItErrorCode::SUCCESS)：将 plan 方法返回的规划结果与 MoveItErrorCode::SUCCESS 进行比较。MoveItErrorCode::SUCCESS 是 MoveIt 提供的一个枚举值，表示规划成功的状态。
  • success = (...);：将比较的结果（布尔值）赋给 success 变量。如果 plan 方法返回的规划结果等于 MoveItErrorCode::SUCCESS（成功），则 success 变量将为 true，表示运动规划成功；否则为 false，表示运动规划失败。

• 执行规划：

  • draw_trajectory_tool_path(plan)：调用之前定义的闭包函数 draw_trajectory_tool_path，传入规划好的路径 plan，在 Rviz 中绘制机械臂的运动轨迹。
  • move_group_interface.execute(plan)：调用 MoveGroupInterface 对象的 execute 方法，执行之前规划好的机械臂运动路径。

• 错误处理：

  • 如果 success 变量为 false，表示运动规划失败。
  • draw_title("Planning Failed!")：在 Rviz 中显示 “Planning Failed!”，提示用户规划失败。
  • RCLCPP_ERROR(logger, "Failed to plan motion")：使用 ROS2 的日志系统记录错误信息，指出运动规划过程中出现问题。

四、补充

1.MoveItVisualTools 功能和语法

MoveItVisualTools 是 是一个用于在 ROS Visualization (Rviz) 中可视化机械臂运动规划和执行过程的工具库。它主要用于帮助开发者和用户实时监测和调试机械臂的运动状态，提供了丰富的功能来简化机械臂操作的可视化和交互。

1. 构造函数：

   moveit_visual_tools::MoveItVisualTools visual_tools("base_link", "/moveit_visual_tools");
   

   • 功能：初始化 MoveItVisualTools 对象。
   • 参数：
     • "base_link"：指定机械臂的基座链接。
     • "/moveit_visual_tools"：Rviz 中的标记主题，用于标记显示。

2. 删除所有旧标记：

   visual_tools.deleteAllMarkers();
   

   • 功能：清除 Rviz 中所有之前发布的标记，确保视图中只显示最新的标记。

3. 发布文本标记：

   visual_tools.publishText(pose, "Hello World", rviz_visual_tools::WHITE, rviz_visual_tools::XLARGE);
   

   • 功能：在 Rviz 中显示文本信息。
   • 参数：
     • pose：文本的显示位置和姿态。
     • "Hello World"：要显示的文本内容。
     • rviz_visual_tools::WHITE：文本的颜色。
     • rviz_visual_tools::XLARGE：文本的大小。

4. 加载远程控制界面：

   visual_tools.loadRemoteControl();
   

   • 功能：在 Rviz 中加载控制界面，允许用户通过点击按钮来控制和交互。

5. 发布轨迹线：

   visual_tools.publishTrajectoryLine(plan.trajectory_, joint_model_group);
   

   • 功能：在 Rviz 中显示机械臂的运动轨迹。
   • 参数：
     • plan.trajectory_：机械臂的运动轨迹。
     • joint_model_group：机械臂的关节模型组，用于正确显示轨迹。

6. 触发 Rviz 更新：

   visual_tools.trigger();
   

   • 功能：更新 Rviz 的显示，确保最新的标记和信息被渲染出来。

7. 显示提示信息：

   visual_tools.prompt("Press 'Next' to continue");
   

   • 功能：在 Rviz 中显示提示信息，让用户知道下一步要做什么。
   • 参数：
     • "Press 'Next' to continue"：提示的内容。

这些功能和语法使 MoveItVisualTools 能够帮助用户在 Rviz 中实时查看和控制机械臂的动作，通过可视化和交互操作简化调试过程。

2.补充步骤

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

要添加按钮以与我们添加到程序中的提示进行交互，请使用 “Panels/Add New Panel” 菜单打开对话框：

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

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

选择并单击 。Marker Array OK

滚动到 Displays 面板中项目的底部，然后编辑新 Marker Array 正在使用的主题。/rviz_visual_tools

这样那样完成！

---

总结

自用