Environment Representation/ROS API

In this tutorial, we will explore the ROS API to the planning scene monitor. The monitor exposes an API for the following tasks:


 * Specifying a whole planning scene using the ROS PlanningScene message.
 * Specifying diffs to the current planning scene.
 * Adding or removing objects from the environment.
 * Attaching or detaching objects from the robot.

Setup the Planning Scene Monitor
The planning scene monitor can be easily setup using just the robot description parameter. In this tutorial, we will not setup the monitor directly but instead use the move_group node which instantiates the monitor internally. If you would like to setup the planning scene monitor yourself, these two lines will do that for you:

If your robot_description is mapped to a different parameter (other than "robot_description") on the param server, modify that argument accordingly.

Topic-level Interface
The planning scene monitor listens on the "/attached_collision_object" topic for new objects in the environment or for removing objects from the environment. The planning scene monitor also listens on the "collision_object" topic for objects that should be added to the environment or removed from the environment.

Define the Attached Object Message and Publish
In this case, we will define a simple box to be attached to the r_wrist_roll_link of the PR2 right arm and publish this information on the "attached_collision_object" topic. Note how you have to specify the ADD flag for the operation on the object.

Detach an Object from the Robot
Detaching the object from the robot is easy - you need to define a similar message and fill in only:
 * the id of the object
 * the link it is attached to and
 * the "REMOVE" operation flag

Note that once the object is detached, it will be added back into the environment

Create the CollisionObject message and publish
Now, you can create the CollisionObject message and publish it on the topic with the "REMOVE" operation flag set. Note that the AttachedCollisionObject message used earlier to specify what connects to the robot also uses the CollisionObject message.

Using the PlanningScene Message as a Diff
You can use the ROS PlanningScene message as a diff to specify what the world looks like for the robot instead of using the separate topics above. Here, we will go through a similar set of operations, adding, removing, attaching and detaching an object to show how this works.

Add a CollisionObject into the Environment
Adding a collision object into the environment using the PlanningScene message is easy: just add the object in the appropriate place in the PlanningScene message. Then, specify that the message is a diff (by setting the is_diff flag to true) and publish.

Attach an Object to the Robot
Attaching an object to the robot is also very easy. You have to remove it from the environment while at the same time attaching it to the robot. Remember to clear out any old objects in the collision_objects and attached_collision_objects messages before applying any new operations to the planning scene. Also remember that since you are reusing the planning scene message, the type of planning scene is still set to a diff.

Detach the Object from the Robot
Detaching an object from the robot is also very easy. You have to detach it while simultaneously adding it to the environment. Again, remember to clear out any old objects in the collision_objects and attached_collision_objects messages before applying any new operations to the planning scene. Also remember that since you are reusing the planning scene message, the type of planning scene is still set to a diff.

Remove the Object from the Environment
Removing the object completely from the environment is also very easy. Again, remember to clear out any old objects in the collision_objects and attached_collision_objects messages before applying any new operations to the planning scene. Also remember that since you are reusing the planning scene message, the type of planning scene is still set to a diff.

The Entire Code
The entire code can be seen here in the moveit_pr2 github project.

Compiling the code
Follow the instructions for compiling code from source.

Visualizing the results
When you launch Rviz, it will automatically bring up the MotionPlanning plugin. This plugin will let you visualize all the changes in the scene and the robot.

Running the code
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 bring up MoveIt! with the PR2 robot:


 * Now, run the code to test out the ROS API for the Planning Scene:


 * You should see the following series of changes as the tutorial runs (note that the colors of the object in the environment and when it is attached to the robot depends on the colors that you have set in Rviz).



Links

 * Back to Planning Scene