【MoveIt 2】直接通过 C++ API 使用 MoveIt : 运动规划管道

在 MoveIt 中，运动规划器被设置为规划路径。然而，有时我们可能希望对运动规划请求进行预处理或对规划的路径进行后处理（例如，进行时间参数化）。在这种情况下，我们使用规划管道，它将运动规划器与预处理和后处理阶段链接起来。预处理和后处理阶段称为规划请求适配器，可以通过 ROS 参数服务器按名称进行配置。在本教程中，我们将通过 C++ 代码向您介绍如何实例化和调用这样的规划管道。

 入门

如果您还没有这样做，请确保您已完成入门中的步骤。

 运行代码

打开两个 shell。在第一个 shell 中启动 RViz 并等待所有内容加载完成：

ros2 launch moveit2_tutorials move_group.launch.py

在第二个 shell 中，运行启动文件：

ros2 launch moveit2_tutorials motion_planning_pipeline_tutorial.launch.py

注意：本教程使用 RvizVisualToolsGui 面板逐步演示。要将此面板添加到 RViz，请按照可视化教程中的说明进行操作。

片刻之后，RViz 窗口应该会出现，并且看起来与此页面顶部的窗口类似。要通过每个演示步骤，请按屏幕底部的 RvizVisualToolsGui 面板中的 Next 按钮，或在屏幕顶部的 Tools 面板中选择 Key Tool，然后在 RViz 聚焦时按键盘上的 N 键。

预期输出

在 RViz 中，我们最终应该能够看到三个轨迹被重放：

1. 机器人将其右臂移动到前方的目标姿势，

2. 机器人将其右臂移动到侧面的关节目标，

3. 机器人将其右臂移回到它前面的原始姿势目标，

整个代码

整个代码可以在 MoveIt GitHub 项目中看到。https://github.com/moveit/moveit2_tutorials/blob/main/doc/examples/motion_planning_pipeline

#include <pluginlib/class_loader.hpp> // 插件库类加载器


// MoveIt
#include <moveit/robot_model_loader/robot_model_loader.h> // 机器人模型加载器
#include <moveit/robot_state/conversions.h> // 机器人状态转换
#include <moveit/planning_pipeline/planning_pipeline.h> // 规划管道
#include <moveit/planning_interface/planning_interface.h> // 规划接口
#include <moveit/planning_scene_monitor/planning_scene_monitor.h> // 规划场景监视器
#include <moveit/kinematic_constraints/utils.h> // 运动学约束工具
#include <moveit_msgs/msg/display_trajectory.hpp> // 显示轨迹消息
#include <moveit_msgs/msg/planning_scene.hpp> // 规划场景消息
#include <moveit_visual_tools/moveit_visual_tools.h> // MoveIt可视化工具


static const rclcpp::Logger LOGGER = rclcpp::get_logger("motion_planning_pipeline"); // 日志记录器


int main(int argc, char** argv)
{
  rclcpp::init(argc, argv); // 初始化ROS
  rclcpp::NodeOptions node_options; // 节点选项
  node_options.automatically_declare_parameters_from_overrides(true); // 自动声明参数
  auto node = rclcpp::Node::make_shared("motion_planning_pipeline_tutorial", node_options); // 创建共享节点


  rclcpp::executors::SingleThreadedExecutor executor; // 单线程执行器
  executor.add_node(node); // 添加节点到执行器
  std::thread([&executor]() { executor.spin(); }).detach(); // 分离线程执行


  // BEGIN_TUTORIAL
  // 开始
  // ^^^^^
  // 设置使用规划管道非常简单。在加载规划器之前，我们需要两个对象，
  // 一个机器人模型和一个规划场景。
  //
  // 我们将通过实例化一个
  // :moveit_codedir:`RobotModelLoader<moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h>`
  // 对象，该对象将在ROS参数服务器上查找机器人描述并构建一个
  // :moveit_codedir:`RobotModel<moveit_core/robot_model/include/moveit/robot_model/robot_model.h>`
  // 供我们使用。
  robot_model_loader::RobotModelLoaderPtr robot_model_loader(
      new robot_model_loader::RobotModelLoader(node, "robot_description")); // 机器人模型加载器


  // 使用RobotModelLoader，我们可以构建一个规划场景监视器，
  // 它将创建一个规划场景，监视规划场景差异，并将差异应用到其内部规划场景。
  // 然后我们调用startSceneMonitor、startWorldGeometryMonitor和
  // startStateMonitor来完全初始化规划场景监视器
  planning_scene_monitor::PlanningSceneMonitorPtr psm(
      new planning_scene_monitor::PlanningSceneMonitor(node, robot_model_loader)); // 规划场景监视器


  /* 监听主题/XXX上的规划场景消息并将其应用到内部规划场景
                       相应地更新内部规划场景 */
  psm->startSceneMonitor(); // 启动场景监视器
  /* 监听世界几何、碰撞对象和（可选）八叉树地图的变化
                                世界几何、碰撞对象和可选的八叉树地图 */
  psm->startWorldGeometryMonitor(); // 启动世界几何监视器
  /* 监听关节状态更新以及附加碰撞对象的变化
                        并相应地更新内部规划场景 */
  psm->startStateMonitor(); // 启动状态监视器


  /* 我们还可以使用RobotModelLoader获取包含机器人运动学信息的机器人模型 */
  moveit::core::RobotModelPtr robot_model = robot_model_loader->getModel(); // 获取机器人模型


  /* 我们可以通过锁定内部规划场景来从PlanningSceneMonitor获取最新的机器人状态
     以进行读取。此锁定确保在我们读取其状态时，底层场景不会更新。
     RobotState在计算机器人的正向和逆向运动学等许多方面非常有用 */
  moveit::core::RobotStatePtr robot_state(
      new moveit::core::RobotState(planning_scene_monitor::LockedPlanningSceneRO(psm)->getCurrentState())); // 获取机器人状态


  /* 创建一个JointModelGroup来跟踪当前的机器人姿态和规划组。关节模型
     组在处理一组关节时非常有用，例如左臂或末端执行器 */
  const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup("panda_arm"); // 获取关节模型组


  // 我们现在可以设置PlanningPipeline对象，它将使用ROS参数服务器
  // 来确定请求适配器和规划插件的集合
  planning_pipeline::PlanningPipelinePtr planning_pipeline(
      new planning_pipeline::PlanningPipeline(robot_model, node, "ompl")); // 设置规划管道


  // 可视化
  // ^^^^^^^^^^^^^
  // MoveItVisualTools包提供了许多可视化对象、机器人和轨迹的功能，
  // 以及调试工具，例如逐步检查脚本。
  namespace rvt = rviz_visual_tools;
  moveit_visual_tools::MoveItVisualTools visual_tools(node, "panda_link0", "move_group_tutorial", psm); // MoveIt可视化工具
  visual_tools.deleteAllMarkers(); // 删除所有标记


  /* 远程控制是一个检查工具，允许用户通过按钮和键盘快捷键在RViz中逐步执