In this tutorial, we will examine the use of planning scene diffs to perform two operations:
- Adding and removing objects into the world
- Attaching and detaching objects to the robot
The ROS API to the planning scene publisher is through a topic interface using “diffs”. A planning scene diff is the difference between the current planning scene (maintained by the move_group node) and the new planning scene desired by the user.
Note that this topic may need to be remapped in the launch file
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();
}
We will use this message to add or subtract the object from the world and to attach the object to the robot
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);
Note that attaching an object to the robot requires the corresponding operation to be specified as an ADD operation
attached_object.object.operation = attached_object.object.ADD;
Add the object into the environment by adding it to the set of collision objects in the “world” part of the planning scene. Note that we are using only the “object” field of the attached_object message here.
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();
There are two separate mechanisms available to interact with the move_group node using diffs:
While most of this tutorial uses the latter mechanism (given the long sleeps inserted for visualization purposes asynchronous updates do not pose a problem), it would is perfectly justified to replace the planning_scene_diff_publisher by the following service client:
ros::ServiceClient planning_scene_diff_client = node_handle.serviceClient<moveit_msgs::ApplyPlanningScene>("apply_planning_scene");
planning_scene_diff_client.waitForExistence();
and send the diffs to the planning scene via a service call:
moveit_msgs::ApplyPlanningScene srv;
srv.request.scene = planning_scene;
planning_scene_diff_client.call(srv);
Note that this does not continue until we are sure the diff has been applied.
When the robot picks up an object from the environment, we need to “attach” the object to the robot so that any component dealing with the robot model knows to account for the attached object, e.g. for collision checking.
/* 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;
Note how we make sure that the diff message contains no other attached objects or collisions objects by clearing those fields first.
/* 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;
Note how we make sure that the diff message contains no other attached objects or collisions objects by clearing those fields first.
/* 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.world.collision_objects.clear();
planning_scene.world.collision_objects.push_back(attached_object.object);
planning_scene_diff_publisher.publish(planning_scene);
sleep_time.sleep();
Removing the object from the collision world just requires using the remove object message defined earlier. Note, also how we make sure that the diff message contains no other attached objects or collisions objects by clearing those fields first.
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);
The entire code can be seen here in the moveit_pr2 github project.
Follow the instructions for compiling code from source.
The entire launch file is here on github. All the code in this tutorial can be compiled and run from the pr2_moveit_tutorials package that you have as part of your MoveIt! setup.
Roslaunch the launch file to run the code directly from pr2_moveit_tutorials:
roslaunch pr2_moveit_tutorials planning_scene_ros_api_tutorial.launch
- Back to Moveit Tutorials