【MoveIt 2】直接通过 C++ API 使用 MoveIt : 运动规划 API-优快云博客

本文链接：https://blog.youkuaiyun.com/cxyhjl/article/details/140923727

在 MoveIt 中，运动规划器是使用插件基础设施 plugin infrastructure 加载的。这允许 MoveIt 在运行时加载运动规划器。在这个例子中，我们将运行所需的 C++代码来实现这一点。

入门

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

运行演示

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

ros2 launch moveit2_tutorials move_group.launch.py

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

ros2 launch moveit2_tutorials motion_planning_api_tutorial.launch.py

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

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

预期输出

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

机器人将其手臂移动到第一个姿势目标，

机器人将其手臂移动到关节目标，

机器人将其手臂移回到原始姿势目标，

机器人在保持末端执行器水平的同时，将其手臂移动到一个新的姿态目标。

整个代码

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

这段代码展示了如何使用MoveIt和ROS 2进行运动规划。以下是代码的主要步骤和功能：

初始化和节点创建：

初始化ROS 2。
创建一个名为motion_planning_api_tutorial的节点，并使用单线程执行器运行该节点。

加载机器人模型和规划场景：

使用RobotModelLoader从ROS参数服务器加载机器人描述，并创建RobotModel对象。
创建RobotState和JointModelGroup对象以跟踪当前的机器人姿态和规划组。
使用RobotModel创建PlanningScene对象，该对象维护世界的状态（包括机器人）。

加载规划器插件：

使用ROS的pluginlib库加载规划器插件。
从ROS参数服务器获取规划插件的名称，并实例化规划器。

MoveGroupInterface和可视化工具：

创建MoveGroupInterface对象以与规划组进行交互。
使用MoveItVisualTools进行可视化，包括在RViz中显示文本、轴和轨迹。

姿态目标规划：

创建一个运动规划请求，指定末端执行器的目标姿态。
设置位置和方向的容差。
调用规划器并解决规划问题。
可视化规划结果并在RViz中显示轨迹。

关节空间目标规划：

设置关节空间目标，并创建相应的运动规划请求。
调用规划器并解决规划问题。
可视化规划结果并在RViz中显示轨迹。

路径约束规划：

创建一个新的姿态目标，并添加路径约束（例如，保持末端执行器水平）。
设置规划空间的边界。
调用规划器并解决规划问题。
可视化规划结果并在RViz中显示轨迹。

结束和清理：

等待用户输入以继续演示或退出。
关闭ROS 2节点。

这段代码展示了如何使用MoveIt和ROS 2进行复杂的运动规划和可视化，包括姿态目标、关节空间目标和路径约束。

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


// MoveIt 
#include <moveit/robot_model_loader/robot_model_loader.h> 
// 机器人模型加载器
#include <moveit/planning_interface/planning_interface.h> 
// 规划接口
#include <moveit/planning_scene/planning_scene.h> 
// 规划场景
#include <moveit/kinematic_constraints/utils.h> 
// 运动学约束工具
#include <moveit_msgs/msg/display_trajectory.hpp> 
// 显示轨迹消息
#include <moveit_msgs/msg/planning_scene.h> 
// 规划场景消息
#include <moveit_visual_tools/moveit_visual_tools.h> 
// MoveIt可视化工具
#include <moveit/move_group_interface/move_group_interface.h> 
// MoveIt移动组接口


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


int main(int argc, char** argv) 
{
  rclcpp::init(argc, argv); 
  // 初始化ROS
  rclcpp::NodeOptions node_options; 
  // 定义节点选项
  node_options.automatically_declare_parameters_from_overrides(true); 
  // 自动声明参数
  std::shared_ptr<rclcpp::Node> motion_planning_api_tutorial_node = 
      rclcpp::Node::make_shared("motion_planning_api_tutorial", node_options); 
  // 创建节点


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


  // BEGIN_TUTORIAL 
  // 开始教程
  // ^^^^^ 
  // 开始使用规划器的设置非常简单。规划器作为插件在MoveIt中设置，
  // 你可以使用ROS的插件库接口加载任何你想使用的规划器。
  // 在我们加载规划器之前，我们需要两个对象，一个是RobotModel，另一个是PlanningScene。
  // 我们首先实例化一个RobotModelLoader对象，该对象会在ROS参数服务器上查找机器人描述，
  // 并为我们构建一个RobotModel以供使用。
  const std::string PLANNING_GROUP = "panda_arm"; 
  // 规划组名称
  robot_model_loader::RobotModelLoader robot_model_loader(motion_planning_api_tutorial_node, "robot_description"); 
  // 加载机器人模型
  const moveit::core::RobotModelPtr& robot_model = robot_model_loader.getModel(); 
  // 获取机器人模型
  /* 创建一个RobotState和JointModelGroup来跟踪当前的机器人姿态和规划组 */
  moveit::core::RobotStatePtr robot_state(new moveit::core::RobotState(robot_model)); 
  // 创建机器人状态
  const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP); 
  // 获取关节模型组


  // 使用RobotModel，我们可以构建一个PlanningScene，它维护世界的状态（包括机器人）。
  planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model)); 
  // 创建规划场景


  // 配置一个有效的机器人状态
  planning_scene->getCurrentStateNonConst().setToDefaultValues(joint_model_group, "ready"); 
  // 设置默认机器人状态


  // 现在我们将构建一个加载器来按名称加载一个规划器。
  // 请注意，我们在这里使用的是ROS插件库。
  std::unique_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager>> planner_plugin_loader; 
  // 定义规划器插件加载器
  planning_interface::PlannerManagerPtr planner_instance; 
  // 定义规划器实例
  std::vector<std::string> planner_plugin_names; 
  // 定义规划器插件名称列表


  // 我们将从ROS参数服务器获取我们想要加载的规划插件的名称，然后加载规划器，并确保捕获所有异常。
  if (!motion_planning_api_tutorial_node->get_parameter("ompl.planning_plugins", planner_plugin_names)) 
    RCLCPP_FATAL(LOGGER, "无法找到规划器插件名称"); 
  // 获取规划器插件名称，若获取失败则打印错误信息


  try 
  { 
    planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::