MoveIt C++ 接口

简介

MoveItCpp 是一个新的高层接口，提供统一的 C++ API，无需使用 ROS Actions、Services 和 Messages 即可访问 MoveIt 的核心功能。它是现有 MoveGroup API 的一种替代方案（但并非完全替代）。对于需要更实时控制的高级用户或工业应用，我们推荐使用该接口。该接口由 PickNik Robotics 出于其众多商业应用的需求而开发。

入门

如果你还没有完成相关步骤，请先完成 Getting Started 中的步骤。

运行代码

打开一个终端，运行 launch 文件：

ros2 launch moveit2_tutorials moveit_cpp_tutorial.launch.py

稍等片刻后，RViz 窗口就会显示出来，看起来与页面顶部的窗口类似。要逐步推进每个演示步骤，可以点击屏幕底部 RvizVisualToolsGui 面板中的 Next 按钮，或者选择屏幕顶部 Tools 面板中的 Key Tool，然后在 RViz 获得焦点时按下键盘上的 0 键。

完整代码

完整代码可以在 MoveIt GitHub 项目中查看。接下来我们逐段讲解代码，解释其功能。

设置

  static const std::string PLANNING_GROUP = "panda_arm";
  static const std::string LOGNAME = "moveit_cpp_tutorial";

ros2_controllers

  static const std::vector<std::string> CONTROLLERS(1, "panda_arm_controller");

  /* Otherwise robot with zeros joint_states */
  rclcpp::sleep_for(std::chrono::seconds(1));

  RCLCPP_INFO(LOGGER, "Starting MoveIt Tutorials...");

  auto moveit_cpp_ptr = std::make_shared<moveit_cpp::MoveItCpp>(node);
  moveit_cpp_ptr->getPlanningSceneMonitorNonConst()->providePlanningSceneService();

  auto planning_components = std::make_shared<moveit_cpp::PlanningComponent>(PLANNING_GROUP, moveit_cpp_ptr);
  auto robot_model_ptr = moveit_cpp_ptr->getRobotModel();
  auto robot_start_state = planning_components->getStartState();
  auto joint_model_group_ptr = robot_model_ptr->getJointModelGroup(PLANNING_GROUP);

可视化

MoveItVisualTools 包提供了许多在 RViz 中可视化对象、机器人和轨迹的功能，以及诸如脚本逐步内省（step-by-step introspection）之类的调试工具。

  moveit_visual_tools::MoveItVisualTools visual_tools(node, "panda_link0", "moveit_cpp_tutorial",
                                                      moveit_cpp_ptr->getPlanningSceneMonitorNonConst());
  visual_tools.deleteAllMarkers();
  visual_tools.loadRemoteControl();

  Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
  text_pose.translation().z() = 1.75;
  visual_tools.publishText(text_pose, "MoveItCpp_Demo", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();

开始演示

  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");

使用 MoveItCpp 进行规划

设置规划的起始状态和目标状态有多种方式，下面的规划示例将逐一说明。

规划 1

我们可以将规划的起始状态设置为机器人的当前状态。

  planning_components->setStartStateToCurrentState();

设置规划目标的第一种方式是使用 geometry_msgs::PoseStamped ROS 消息类型，如下所示：

  geometry_msgs::msg::PoseStamped target_pose1;
  target_pose1.header.frame_id = "panda_link0";
  target_pose1.pose.orientation.w = 1.0;
  target_pose1.pose.position.x = 0.28;
  target_pose1.pose.position.y = -0.2;
  target_pose1.pose.position.z = 0.5;
  planning_components->setGoal(target_pose1, "panda_link8");

现在，我们调用 PlanningComponent 计算规划并进行可视化。请注意，我们这里只是进行规划。

  const planning_interface::MotionPlanResponse plan_solution1 = planning_components->plan();

检查 PlanningComponent 是否成功找到规划。

  if (plan_solution1)
  {

在 RViz 中可视化起始位姿。

    visual_tools.publishAxisLabeled(robot_start_state->getGlobalLinkTransform("panda_link8"), "start_pose");

在 RViz 中可视化目标位姿。

    visual_tools.publishAxisLabeled(target_pose1.pose, "target_pose");
    visual_tools.publishText(text_pose, "setStartStateToCurrentState", rvt::WHITE, rvt::XLARGE);

在 RViz 中可视化轨迹。

    visual_tools.publishTrajectoryLine(plan_solution1.trajectory, joint_model_group_ptr);
    visual_tools.trigger();

    /* Uncomment if you want to execute the plan */
    /* bool blocking = true; */
    /* moveit_controller_manager::ExecutionStatus result = moveit_cpp_ptr->execute(plan_solution1.trajectory, blocking, CONTROLLERS); */
  }

规划 1 可视化：

开始下一个规划。

  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
  visual_tools.deleteAllMarkers();
  visual_tools.trigger();

规划 2

这里我们将使用 moveit::core::RobotState 设置规划的起始状态。

  auto start_state = *(moveit_cpp_ptr->getCurrentState());
  geometry_msgs::msg::Pose start_pose;
  start_pose.orientation.w = 1.0;
  start_pose.position.x = 0.55;
  start_pose.position.y = 0.0;
  start_pose.position.z = 0.6;

  start_state.setFromIK(joint_model_group_ptr, start_pose);

  planning_components->setStartState(start_state);

我们将复用之前的目标，并规划到该目标。

  auto plan_solution2 = planning_components->plan();
  if (plan_solution2)
  {
    moveit::core::RobotState robot_state(robot_model_ptr);
    moveit::core::robotStateMsgToRobotState(plan_solution2.start_state, robot_state);

    visual_tools.publishAxisLabeled(robot_state.getGlobalLinkTransform("panda_link8"), "start_pose");
    visual_tools.publishAxisLabeled(target_pose1.pose, "target_pose");
    visual_tools.publishText(text_pose, "moveit::core::RobotState_Start_State", rvt::WHITE, rvt::XLARGE);
    visual_tools.publishTrajectoryLine(plan_solution2.trajectory, joint_model_group_ptr);
    visual_tools.trigger();

    /* Uncomment if you want to execute the plan */
    /* bool blocking = true; */
    /* moveit_cpp_ptr->execute(plan_solution2.trajectory, blocking, CONTROLLERS); */
  }

规划 2 可视化：

开始下一个规划。

  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
  visual_tools.deleteAllMarkers();
  visual_tools.trigger();

规划 3

我们也可以使用 moveit::core::RobotState 设置规划的目标状态。

  auto target_state = *robot_start_state;
  geometry_msgs::msg::Pose target_pose2;
  target_pose2.orientation.w = 1.0;
  target_pose2.position.x = 0.55;
  target_pose2.position.y = -0.05;
  target_pose2.position.z = 0.8;

  target_state.setFromIK(joint_model_group_ptr, target_pose2);

  planning_components->setGoal(target_state);

我们将复用之前的起始状态，并从该状态开始规划。

  auto plan_solution3 = planning_components->plan();
  if (plan_solution3)
  {
    moveit::core::RobotState robot_state(robot_model_ptr);
    moveit::core::robotStateMsgToRobotState(plan_solution3.start_state, robot_state);

    visual_tools.publishAxisLabeled(robot_state.getGlobalLinkTransform("panda_link8"), "start_pose");
    visual_tools.publishAxisLabeled(target_pose2, "target_pose");
    visual_tools.publishText(text_pose, "moveit::core::RobotState_Goal_Pose", rvt::WHITE, rvt::XLARGE);
    visual_tools.publishTrajectoryLine(plan_solution3.trajectory, joint_model_group_ptr);
    visual_tools.trigger();

    /* Uncomment if you want to execute the plan */
    /* bool blocking = true; */
    /* moveit_cpp_ptr->execute(plan_solution3.trajectory, blocking, CONTROLLERS); */
  }

规划 3 可视化：

开始下一个规划。

  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
  visual_tools.deleteAllMarkers();
  visual_tools.trigger();

规划 4

我们可以将规划的起始状态设置为机器人的当前状态，也可以使用命名状态的名称来设置规划的目标状态。对于 panda 机器人，“panda_arm” 规划组有一个名为 “ready” 的命名状态，参见 panda_arm.xacro。

  /* // Set the start state of the plan from a named robot state */
  /* planning_components->setStartState("ready"); // Not implemented yet */

从命名状态设置规划的目标状态。

  planning_components->setGoal("ready");

同样，我们将复用之前的起始状态，并从该状态开始规划。

  auto plan_solution4 = planning_components->plan();
  if (plan_solution4)
  {
    moveit::core::RobotState robot_state(robot_model_ptr);
    moveit::core::robotStateMsgToRobotState(plan_solution4.start_state, robot_state);

    visual_tools.publishAxisLabeled(robot_state.getGlobalLinkTransform("panda_link8"), "start_pose");
    visual_tools.publishAxisLabeled(robot_start_state->getGlobalLinkTransform("panda_link8"), "target_pose");
    visual_tools.publishText(text_pose, "Goal_Pose_From_Named_State", rvt::WHITE, rvt::XLARGE);
    visual_tools.publishTrajectoryLine(plan_solution4.trajectory, joint_model_group_ptr);
    visual_tools.trigger();

    /* Uncomment if you want to execute the plan */
    /* bool blocking = true; */
    /* moveit_cpp_ptr->execute(plan_solution4.trajectory, blocking, CONTROLLERS); */
  }

规划 4 可视化：

开始下一个规划。

  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
  visual_tools.deleteAllMarkers();
  visual_tools.trigger();

规划 5

我们还可以围绕碰撞场景中的物体生成运动规划。

首先，我们创建碰撞物体。

  moveit_msgs::msg::CollisionObject collision_object;
  collision_object.header.frame_id = "panda_link0";
  collision_object.id = "box";

  shape_msgs::msg::SolidPrimitive box;
  box.type = box.BOX;
  box.dimensions = { 0.1, 0.4, 0.1 };

  geometry_msgs::msg::Pose box_pose;
  box_pose.position.x = 0.4;
  box_pose.position.y = 0.0;
  box_pose.position.z = 1.0;

  collision_object.primitives.push_back(box);
  collision_object.primitive_poses.push_back(box_pose);
  collision_object.operation = collision_object.ADD;

将物体添加到规划场景中。

  {  // Lock PlanningScene
    planning_scene_monitor::LockedPlanningSceneRW scene(moveit_cpp_ptr->getPlanningSceneMonitorNonConst());
    scene->processCollisionObjectMsg(collision_object);
  }  // Unlock PlanningScene
  planning_components->setStartStateToCurrentState();
  planning_components->setGoal("extended");

  auto plan_solution5 = planning_components->plan();
  if (plan_solution5)
  {
    visual_tools.publishText(text_pose, "Planning_Around_Collision_Object", rvt::WHITE, rvt::XLARGE);
    visual_tools.publishTrajectoryLine(plan_solution5.trajectory, joint_model_group_ptr);
    visual_tools.trigger();

    /* Uncomment if you want to execute the plan */
    /* bool blocking = true; */
    /* moveit_cpp_ptr->execute(plan_solution5.trajectory, blocking, CONTROLLERS); */
  }

规划 5 可视化：

启动文件

完整的 launch 文件可以在 GitHub上查看。本教程中的所有代码都可以从 moveit2_tutorials 包中运行，该包是 MoveIt 安装的一部分。