ROS API 规划场景

参考：ROS API Planning Scene Tutorial

• 在世界环境中添加或移除对象
• 为机器人附加或拆卸对象

ROS API

ROS API对规划场景发布消息是使用“diffs”通过一个话题接口实现的。planning scene diff是当前规划场景与用户使用的新的规划场景不同的地方。

广播所需要的话题

这个话题可能需要在launch文件中重新映射。

ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
while (planning_scene_diff_publisher.getNumSubscribers() < 1)
{
  ros::WallDuration sleep_t(0.5);
  sleep_t.sleep();
}

定义附加对象消息

moveit_msgs::AttachedCollisionObject attached_object;
attached_object.link_name = "r_wrist_roll_link";
/* The header must contain a valid TF frame*/
attached_object.object.header.frame_id = "r_wrist_roll_link";
/* The id of the object */
attached_object.object.id = "box";

/* A default pose */
geometry_msgs::Pose pose;
pose.orientation.w = 1.0;

/* Define a box to be attached */
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.1;
primitive.dimensions[1] = 0.1;
primitive.dimensions[2] = 0.1;

attached_object.object.primitives.push_back(primitive);
attached_object.object.primitive_poses.push_back(pose);

附加对象需要有一个添加操作

attached_object.object.operation = attached_object.object.ADD;

添加对象到场景

将对象添加到环境中，方法是将其添加到计划场景的“世界”部分中的一组碰撞对象中。 请注意，我们仅在这里使用attached_object消息的“对象”字段。

ROS_INFO("Adding the object into the world at the location of the right wrist.");
moveit_msgs::PlanningScene planning_scene;
planning_scene.world.collision_objects.push_back(attached_object.object);
planning_scene.is_diff = true;
planning_scene_diff_publisher.publish(planning_scene);
sleep_time.sleep();

同步更新与异步更新

使用 diffs与move_group进行交互有两种不同的机制，

• 一种方式是通过rosservice调用发送一个diff，直到diff被应用（同步机制）
• 另一种是通过话题发送diff，无论是否应用都继续进行

将对象附着到机器人上

当机器人从环境中拾取物体时，我们需要将物体“附着”到机器人上，以便处理机器人模型的任何部件知道对附着的物体进行说明，比如进行碰撞检查。

• 首先删除原对象
• 然后将对象附着到机器人上

/* First, define the REMOVE object message*/
moveit_msgs::CollisionObject remove_object;
remove_object.id = "box";
remove_object.header.frame_id = "odom_combined";
remove_object.operation = remove_object.REMOVE;

注意 diff 消息是怎样确定没有包含其他对象的

/* Carry out the REMOVE + ATTACH operation */
ROS_INFO("Attaching the object to the right wrist and removing it from the world.");
planning_scene.world.collision_objects.clear();
planning_scene.world.collision_objects.push_back(remove_object);
planning_scene.robot_state.attached_collision_objects.push_back(attached_object);
planning_scene_diff_publisher.publish(planning_scene);

sleep_time.sleep();

从机器人解除附着对象

也需要两步

• 解除对象
• 重新将对象引入到环境中

/* First, define the DETACH object message*/
moveit_msgs::AttachedCollisionObject detach_object;
detach_object.object.id = "box";
detach_object.link_name = "r_wrist_roll_link";
detach_object.object.operation = attached_object.object.REMOVE;

/* Carry out the DETACH + ADD operation */
ROS_INFO("Detaching the object from the robot and returning it to the world.");
planning_scene.robot_state.attached_collision_objects.clear();
planning_scene.robot_state.attached_collision_objects.push_back(detach_object);
planning_scene.robot_state.is_diff = true;
planning_scene.world.collision_objects.clear();
planning_scene.world.collision_objects.push_back(attached_object.object);
planning_scene.is_diff = true;
planning_scene_diff_publisher.publish(planning_scene);

sleep_time.sleep();

将对象从碰撞环境中移除

ROS_INFO("Removing the object from the world.");
planning_scene.robot_state.attached_collision_objects.clear();
planning_scene.world.collision_objects.clear();
planning_scene.world.collision_objects.push_back(remove_object);
planning_scene_diff_publisher.publish(planning_scene);

运行命令

roslaunch moveit_tutorials planning_scene_ros_api_tutorial.launch

---