MoveIt2-humble----Planning Around Objects

1 添加Planning Scene Interface头文件

#include <moveit/planning_scene_interface/planning_scene_interface.h>

2 改变目标位姿

// Set a target Pose
auto const target_pose = [] {geometry_msgs::msg::Pose msg;msg.orientation.w = 1.0;msg.position.x = 0.28;msg.position.y = 0.4;  // <---- This value was changedmsg.position.z = 0.5;return msg;
}();
move_group_interface.setPoseTarget(target_pose);

3 创建一个碰撞物体

在下一个代码块中，我们创建了一个碰撞物体。

第一个需要注意的是，物体被放置在机器人坐标系下。如果我们有一个感知系统，能够报告场景中障碍物的位置，这就是该系统将要建立的一类信息。在本例子中我们手动来创建它。

另外要注意代码块的结尾处，我们设置该信息的操作为ADD，这样能够允许在后面的操作中将物体添加到碰撞场景。

把此代码块插入到【设置目标位姿】和【创建规划】之间。

// 创建用于避障的碰撞对象
auto const collision_object = [frame_id =move_group_interface.getPlanningFrame()] {moveit_msgs::msg::CollisionObject collision_object;collision_object.header.frame_id = frame_id;collision_object.id = "box1";shape_msgs::msg::SolidPrimitive primitive;// 定义盒子的尺寸（单位：m）primitive.type = primitive.BOX;primitive.dimensions.resize(3);primitive.dimensions[primitive.BOX_X] = 0.5;primitive.dimensions[primitive.BOX_Y] = 0.1;primitive.dimensions[primitive.BOX_Z] = 0.5;// 定义盒子的位姿（相对于 frame_id）geometry_msgs::msg::Pose box_pose;box_pose.orientation.w = 1.0;box_pose.position.x = 0.2;box_pose.position.y = 0.2;box_pose.position.z = 0.25;collision_object.primitives.push_back(primitive);collision_object.primitive_poses.push_back(box_pose);collision_object.operation = collision_object.ADD;return collision_object;
}();

4 添加物体到规划场景中

最后，我们需要添加这个物体到碰撞场景中。为了实现这个操作，我们使用PlanningSceneInterface对象，该对象使用 ROS 接口，将规划场景的变化传递给MoveGroup。

下面的代码块插入在上面代码块的下面：

// 将碰撞物体添加到场景中
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
planning_scene_interface.applyCollisionObject(collision_object);