版权声明:本文为博主原创文章,未经博主允许不得转载。 https://blog.csdn.net/wxflamy/article/details/79205735
参考: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