Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Cabinet Handle Detection

Downloading the Code

If you already have already checked out the mit-ros-pkg, you can skip this step.

$ svn co `roslocate svn tabletop_for_cabinet`

After you do that, make sure package is compiled on your local machine by:

$ roscd tabletop_for_cabinet
$ rosmake

Starting the robot and Rviz

To start the robot, ssh onto the PR2 and run

$ sudo robot start

Start the dashboard and rviz.

$ rosrun rviz rviz

In rviz, download this configuration file and open it in rviz using File->Open Config:

rvizConfig.vcg (This can also be found in the tabletop_for_cabinet package)

Adjust the Robot

Maneuver the robot to 2-3 feet in front of the cabinet door. Its arms should be out of the way. The robot should be in a similar configuration to this:

Launch the detection

Launch open_cupboard_launch file

$ roslaunch open_cupboard_launch opencupboard.launch

Adjust the Head

Now you should be able to see the cabinet point cloud in rviz. To adjust where the head is looking, teleop the head to be facing the cabinet. Use rviz to get a good view of the door. The point cloud should include the door as the main plane in the cloud, the handle should be visible, and other objects (this includes the top of the cabinet and the table the cabinet is resting on) should be avoided as much as possible.The point cloud in rviz should look similar to this:

Run detection client

The Code

Within tabletop_for_cabinet, create the scripts/object_processing_client.py file and paste the following inside it:

   1 import roslib; roslib.load_manifest('tabletop_for_cabinet')
   2 import sys
   3 import rospy
   4 from tabletop_for_cabinet.srv import *
   5 from geometry_msgs.msg import Pose, PoseStamped
   6 from tabletop_object_detector.msg import TableIt uses the tabletop returned from the PickAndPlaceManager and returns a resp1 which is the pose of the handle.
   7 from pr2_pick_and_place_demos.pick_and_place_manager import PickAndPlaceManager
   8 import actionlib
   9 import opendoors.msg
  10 
  11 def get_table():
  12     ppm = PickAndPlaceManager()
  13     (things,table) = ppm.call_tabletop_detection(take_static_collision_map=1, update_table=1)
  14     print table
  15     return table
  16 
  17 def door_processing_client():
  18     rospy.wait_for_service('cabinet_table')
  19     try:
  20         door_processing = rospy.ServiceProxy('cabinet_table', cabinet_table)
  21         resp1 = door_processing(get_table())
  22         return resp1.pose
  23     except rospy.ServiceException, e:
  24         print "Service call failed: %s"%e
  25 
  26 def object_processing_client():
  27     rospy.wait_for_service('cabinet_table_result')
  28     try:
  29         object_processing = rospy.ServiceProxy('cabinet_table_result', cabinet_table_result)
  30         resp1 = object_processing(door_processing_client().detection)
  31         return resp1.pose
  32     except rospy.ServiceException, e:
  33         print "Service call failed: %s"%e
  34 
  35 if __name__ == "__main__":
  36     rospy.init_node("tabletop_detection_cabinet")
  37     pose = object_processing_client()
  38     pose_stamped = geometry_msgs.msg.PoseStamped()
  39     pose_stamped.header.frame_id = 'torso_lift_link'
  40     pose_stamped.pose.position.x = 0
  41     pose_stamped.pose.position.y = -0.7
  42     pose_stamped.pose.position.z = 0.0
  43     pose_stamped.pose.orientation.x = 0.0
  44     pose_stamped.pose.orientation.y = 0.0
  45     pose_stamped.pose.orientation.z = 0.0
  46     pose_stamped.pose.orientation.w = 1.0

Examining the Code

Now, let's break the code down.

   1 ppm = PickAndPlaceManager()
   2 (things,table) = ppm.call_tabletop_detection(take_static_collision_map=1 update_table=1)

This initializes the PickAndPlaceManager which manages table detection. PickAndPlaceManager's tabletop detection returns a plane which is the tabletop and a list of 'things' on top of the table. In our case, the tabletop is the cabinet door.

   1 door_processing = rospy.ServiceProxy('cabinet_table', cabinet_table)
   2 resp1 = door_processing(get_table())

This uses the door_processing_server to find the table and the clusters on the table. It uses the tabletop returned from the PickAndPlaceManager and returns the table with the clusters.

   1 object_processing = rospy.ServiceProxy('cabinet_table_result', cabinet_table_result)
   2 resp1 = object_processing(door_processing_client().detection)

This uses the object_processing_server to find the handle on the cabinet door. It selects the handle from the clusters returned in the detection of the door_processing_server.

Running the Code

You should rosmake

$ rosmake tabletop_for_cabinet

Launch the tabletop detection

$ roslaunch open_cupboard_launch opencupboard.launch

Launch door_processing_server

$ rosrun tabletop_for_cabinet door_processing_server

Launch object_processing_server

$ rosrun tabletop_for_cabinet object_processing_server

From tabletop_for_cabinet, run the client

$ python scripts/object_processing_client.py 

In rviz, a marker for the handle and boxes for the other possible handles should appear(If they do not, you may need to refresh the topic for /handle_maker in rviz before you run your client).

Rviz should show you markers similar to this: