【MoveIt 2】围绕对象进行规划

本教程将向您介绍如何将对象插入到规划场景中并围绕它们进行规划。

 先决条件 

如果您还没有完成，请确保您已经完成了在 RViz 中可视化的步骤。该项目假设您从上一个教程结束的 hello_moveit 项目开始。如果您只想运行教程，可以按照 Docker 指南启动一个包含完整教程的容器。

 步骤 

1 添加包含用于规划场景接口 

在你的源文件顶部，将此添加到包含列表中：

#include <moveit/planning_scene_interface/planning_scene_interface.h>

2 更改目标姿势 

首先，通过以下更改更新目标姿态，使机器人规划到不同的位置：

// 设置目标位姿，并使用更新的值
auto const target_pose = [] {
  geometry_msgs::msg::Pose msg; // 创建一个Pose消息对象
  msg.orientation.y = 0.8; // 设置目标位姿的四元数中的y分量
  msg.orientation.w = 0.6; // 设置目标位姿的四元数中的w分量
  msg.position.x = 0.1; // 设置目标位姿的X坐标
  msg.position.y = 0.4; // 设置目标位姿的Y坐标
  msg.position.z = 0.4; // 设置目标位姿的Z坐标
  return msg; // 返回定义好的Pose消息
}();


// 将定义好的目标位姿设置为MoveGroup的目标
move_group_interface.setPoseTarget(target_pose);

3 创建碰撞对象 

在下一个代码块中，我们创建了一个碰撞对象。首先要注意的是，它被放置在机器人的坐标系中。如果我们有一个感知系统报告我们场景中障碍物的位置，那么这就是它会构建的消息。因为这只是一个示例，所以我们手动创建它。需要注意的是，在这个代码块的末尾，我们将此消息的操作设置为 ADD 。这会导致对象被添加到碰撞场景中。将此代码块放在设置目标姿势和创建计划之间。

// 创建一个机器人需要避开的碰撞物体
auto const collision_object = [frame_id = move_group_interface.getPlanningFrame()] {
  // 创建一个CollisionObject对象，用于描述碰撞物体
  moveit_msgs::msg::CollisionObject collision_object;
  collision_object.header.frame_id = frame_id; // 设置参考坐标系的ID
  collision_object.id = "box1"; // 给碰撞物体一个唯一的ID


  shape_msgs::msg::SolidPrimitive primitive;
  // 定义箱子的大小（单位：米）
  primitive.type = primitive.BOX; // 设置物体类型为BOX
  primitive.dimensions.resize(3); // 为三个维度分配空间
  primitive.dimensions[primitive.BOX_X] = 0.5; // 设置箱子的X方向长度为0.5米
  primitive.dimensions[primitive.BOX_Y] = 0.1; // 设置箱子的Y方向长度为0.1米
  primitive.dimensions[primitive.BOX_Z] = 0.5; // 设置箱子的Z方向长度为0.5米


  // 定义箱子的位姿（相对于frame_id）
  geometry_msgs::msg::Pose box_pose;
  box_pose.orientation.w = 1.0; // 设置箱子的四元数旋转部分
  box_pose.position.x = 0.2; // 设置箱子的X坐标
  box_pose.position.y &