InteractiveRobot/CollisionContact

This section walks you through some C++ code which allows you to see collision contact points between the robot and itself and the world, as you interactively move the robot arm around in Rviz.

Classes
The code for this tutorial borrows heavily from the InteractivityTutorial. The InteractiveRobot class is described in that tutorial and will not be covered here. (Note: You do not need to understand that tutorial to follow this one. You can use the InteractiveRobot class as is.)

Code Description
We will walk through the code in the order that it is run in the program, starting with the main function in collision_contact_tutorial.cpp: After the standard ros initialization we create an instance of the InteractiveRobot class. This class is covered in the InteractivityTutorial and will not be covered here.

Next we create a planning scene: Among other things the PlanningScene maintains collision information for the robot and the world (in this tutorial the "world" is a single yellow cube). You can read more about PlanningScenes here.

We have to tell the PlanningScene about the world geometry: getWorldGeometry gets the size and pose of the cube from the InteractiveRobot class. g_world_cube_shape is a shared pointer to a new box shape describing the cube. addToObject adds the cube to the CollisionWorld so it can be checked for collisions.

The g_marker_array_publisher is used to publish collision contact points for display in Rviz.

Here we set the callback which will be called when the interactive markers are manipulated, and then enter the ros::spin infinite loop.

The rest of the main function is just cleanup:

The interesting work all happens in the callback function: Here we tell the CollisionWorld the new location of the yellow world cube.

Next we prepare to check for collisions:
 * group_name (set above to "right_arm") indicates which part of the robot to check for collisions. (Remove this line to check the entire robot instead of just the right arm.)
 * We ask for up to 100 collision points (up to 5 from each pair of colliding links/objects).
 * verbose can be set to true to print extra debug info

And then actually run the collision check: This checks for collisions between the "right_arm" and the world as well as between the "right_arm" and the rest of the robot.

If a collision occurred (c_res.collision is true) then we display the collision points: getCollisionMarkersFromContacts</tt> is a helper function that adds the collision contact points into a MarkerArray message. If you want to use the contact points for something other than displaying them you can iterate through c_res.contacts</tt> which is a std::map</tt> of contact points. Look at the implementation of getCollisionMarkersFromContacts</tt> in collision_tools.cpp for how.

And finally we publish the markers to Rviz:

If no collision occurred we just erase any collision contact point markers that we may have placed there last time the callback was called:

The publishMarkers</tt> function deletes any old markers and then adds new ones:

The entire code
The entire code can be seen here in the moveit_pr2 github project.
 * collision_contact_tutorial.cpp - the main function
 * interactive_robot.h - InteractiveRobot class
 * interactive_robot.cpp - InteractiveRobot class implementation
 * imarker.h - IMarker class
 * imarker.cpp - IMarker class implementation
 * pose_string.h - helper for pretty-printing poses
 * pose_string.cpp - helper for pretty-printing poses
 * collision_tools.cpp - helper for placing collision points into MarkerArray

Compiling the code
Follow the instructions for compiling code from source. If you install the moveit_pr2 package from github then you can simply type catkin_make</tt> from the top level directory.

Launch file
A launch file is located here. It loads the URDF and SRDF parameters for the PR2 robot, launches Rviz, and runs the collision_contact_tutorial</tt> program described above. If moveit_pr2 is in your ROS_PACKAGE_PATH</tt> then launch it by typing: roslaunch pr2_moveit_tutorials collision_contact_tutorial.launch

Rviz setup
When rviz starts up you will have to add some displays to see the objects your code is publishing. This is done in the "Displays" panel in rviz.
 * Under GlobalOptions set FixedFrame to /base_footprint</tt>
 * Click Add and (under moveit_ros_visualization) add a RobotState display.
 * Set the RobotState::RobotDescription to robot_description</tt>
 * Set the RobotState::RobotStateTopic to interactive_robot_state</tt>
 * Set the RobotState::RobotAlpha to 0.3 (to make the robot transparent and see the collision points)
 * Click Add and (under rviz) add a Marker display.
 * Set the Marker::MarkerTopic to interactive_robot_markers</tt>
 * Click Add and (under rviz) add a InteractiveMarkers display.
 * Set the Marker::UpdateTopic to interactive_robot_imarkers/update</tt>
 * Click Add and (under rviz) add a MarkerArray display.
 * Set the Marker::UpdateTopic to interactive_robot_marray</tt>

You should now see the PR2 robot with 2 interactive markers which you can drag around.



Interacting
In Rviz you will see 2 sets of Red/Green/Blue interactive marker arrows. Drag these around with the mouse.

Move the right arm so it is in contact with the left arm. You will see magenta spheres marking the contact points.

If you do not see the magenta spheres be sure that you added the MarkerArray display with interactive_robot_marray</tt>'' topic as described above. Also be sure to set RobotAlpha to 0.3</tt> (or some other value less than 1) so the robot is transparent and the spheres can be seen.''

Move the right arm so it is in contact with the yellow cube (you may also move the yellow cube). You will see magenta spheres marking the contact points.