simple_pick_example.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 
00003 import roslib; roslib.load_manifest('cotesys_ros_grasping')
00004 import rospy
00005 
00006 import actionlib
00007 from cotesys_ros_grasping.msg import MoveArmRelativeCartesianPointAction, MoveArmRelativeCartesianPointGoal 
00008 from cotesys_ros_grasping.msg import MoveArmToPositionGoal, MoveArmToPositionAction
00009 from cotesys_ros_grasping.msg import TakeStaticCollisionMapAction, TakeStaticCollisionMapGoal
00010 from cotesys_ros_grasping.msg import AttachBoundingBoxAction, AttachBoundingBoxGoal
00011 from table_objects.srv import GetObjects, GetObjectsRequest
00012 from sensor_msgs.msg import PointCloud2
00013 from pr2_controllers_msgs.msg import Pr2GripperCommandAction, Pr2GripperCommandGoal
00014 
00015 def close_gripper(action):
00016     goal = Pr2GripperCommandGoal()
00017     goal.command.position = 0.0
00018     goal.command.max_effort = 1000
00019     action.send_goal(goal)
00020     action.wait_for_result(rospy.Duration(1.0))
00021 
00022 def open_gripper(action):
00023     goal = Pr2GripperCommandGoal()
00024     goal.command.position = 0.08
00025     goal.command.max_effort = 1000
00026     action.send_goal(goal)
00027     action.wait_for_result(rospy.Duration(1.0))
00028 
00029 latest_point_cloud = PointCloud2()
00030 got_cloud = False
00031 
00032 def point_cloud_cb(data):
00033     global latest_point_cloud
00034     
00035     latest_point_cloud = data
00036 
00037     got_cloud = True
00038 
00039 if __name__ == '__main__':
00040 
00041       rospy.init_node('simple_pick_example')
00042 
00043       move_to_rel_pos_client = actionlib.SimpleActionClient('/move_arm_relative_cartesian_point', MoveArmRelativeCartesianPointAction)
00044       move_to_rel_pos_client.wait_for_server()
00045 
00046       take_static_client = actionlib.SimpleActionClient('/take_static_collision_map', TakeStaticCollisionMapAction)
00047       take_static_client.wait_for_server()
00048 
00049       move_to_arm_pos_client = actionlib.SimpleActionClient('/move_arm_to_position', MoveArmToPositionAction)
00050       move_to_arm_pos_client.wait_for_server()
00051 
00052       right_gripper_client_ = actionlib.SimpleActionClient('/r_gripper_controller/gripper_action', Pr2GripperCommandAction)
00053       right_gripper_client_.wait_for_server()
00054 
00055       left_gripper_client_ = actionlib.SimpleActionClient('/l_gripper_controller/gripper_action', Pr2GripperCommandAction)
00056       left_gripper_client_.wait_for_server()
00057 
00058       rospy.wait_for_service("/get_table_objects")
00059       get_table_objects_service_ = rospy.ServiceProxy("/get_table_objects", GetObjects)
00060 
00061       attach_object_client = actionlib.SimpleActionClient('attach_bounding_box', AttachBoundingBoxAction)
00062       attach_object_client.wait_for_server()
00063 
00064       rospy.Subscriber("/extract_objects_indices/output", PointCloud2, point_cloud_cb)
00065 
00066       print 'Connected to all clients'
00067 
00068       move_rarm_to_side_goal = MoveArmToPositionGoal()
00069       move_rarm_to_side_goal.arm_name = "right_arm"
00070       move_rarm_to_side_goal.grasp_name = "top_level"
00071       move_rarm_to_side_goal.point.x = .52
00072       move_rarm_to_side_goal.point.y = -.7
00073       move_rarm_to_side_goal.point.z = .9
00074 
00075       move_to_arm_pos_client.send_goal(move_rarm_to_side_goal)
00076       print 'sent right arm goal'
00077       move_to_arm_pos_client.wait_for_result()
00078 
00079       move_larm_to_side_goal = MoveArmToPositionGoal()
00080       move_larm_to_side_goal.arm_name = "left_arm"
00081       move_rarm_to_side_goal.grasp_name = "top_level"
00082       move_larm_to_side_goal.point.x = .52
00083       move_larm_to_side_goal.point.y = .7
00084       move_larm_to_side_goal.point.z = .9
00085 
00086       move_to_arm_pos_client.send_goal(move_larm_to_side_goal)
00087       print 'sent left arm goal'
00088       move_to_arm_pos_client.wait_for_result()
00089 
00090       open_gripper(right_gripper_client_)
00091       open_gripper(left_gripper_client_)
00092 
00093       stat_goal = TakeStaticCollisionMapGoal()
00094       take_static_client.send_goal(stat_goal)
00095       take_static_client.wait_for_result()
00096 
00097       if(not got_cloud):
00098           print 'No point cloud'
00099           exit
00100 
00101       get_table_objects = GetObjectsRequest()
00102       get_table_objects.data = latest_point_cloud
00103       table_objects = get_table_objects_service_.call(get_table_objects)
00104 
00105       if(len(table_objects.table_objects) == 0):
00106           print 'no objects'
00107           exit
00108       else:
00109           print 'got objects ', len(table_objects.table_objects)
00110 
00111       right_arm_pos_goal = MoveArmToPositionGoal()
00112       right_arm_pos_goal.arm_name = "right_arm"
00113       right_arm_pos_goal.grasp_name = "top"
00114       right_arm_pos_goal.point.x = table_objects.table_objects[0].position.x
00115       right_arm_pos_goal.point.y = table_objects.table_objects[0].position.y
00116       right_arm_pos_goal.point.z = table_objects.table_objects[0].position.z+.2
00117 
00118       move_to_arm_pos_client.send_goal(right_arm_pos_goal)
00119       move_to_arm_pos_client.wait_for_result()
00120 
00121       move_rarm_down_goal = MoveArmRelativeCartesianPointGoal()
00122       move_rarm_down_goal.arm_name = "right_arm"
00123       move_rarm_down_goal.rel_point.z = -.1
00124       move_to_rel_pos_client.send_goal(move_rarm_down_goal)
00125       move_to_rel_pos_client.wait_for_result()
00126 
00127       close_gripper(right_gripper_client_)
00128 
00129       attach_goal = AttachBoundingBoxGoal()
00130       attach_goal.arm_name = "right_arm"
00131       attach_goal.x_size = table_objects.table_objects[0].dimension[0]
00132       attach_goal.y_size = table_objects.table_objects[0].dimension[1]
00133       attach_goal.z_size = table_objects.table_objects[0].dimension[2]
00134       attach_object_client.send_goal(attach_goal)
00135       attach_object_client.wait_for_result()
00136 
00137       move_rarm_up_goal = MoveArmRelativeCartesianPointGoal()
00138       move_rarm_up_goal.arm_name = "right_arm"
00139       move_rarm_up_goal.rel_point.z = .2
00140       move_to_rel_pos_client.send_goal(move_rarm_up_goal)
00141       move_to_rel_pos_client.wait_for_result()
00142 
00143       move_to_arm_pos_client.send_goal(move_rarm_to_side_goal)
00144       move_to_arm_pos_client.wait_for_result()
00145    
00146       stat_goal = TakeStaticCollisionMapGoal()
00147       take_static_client.send_goal(stat_goal)
00148       take_static_client.wait_for_result()
00149       
00150       move_to_arm_pos_client.send_goal(right_arm_pos_goal)
00151       move_to_arm_pos_client.wait_for_result()
00152 
00153       move_rarm_down_goal = MoveArmRelativeCartesianPointGoal()
00154       move_rarm_down_goal.arm_name = "right_arm"
00155       move_rarm_down_goal.rel_point.z = -.1
00156       move_to_rel_pos_client.send_goal(move_rarm_down_goal)
00157       move_to_rel_pos_client.wait_for_result()
00158 
00159       open_gripper(right_gripper_client_)
00160 
00161       attach_goal.remove = True
00162       attach_object_client.send_goal(attach_goal)
00163       attach_object_client.wait_for_result()
00164 
00165       move_rarm_up_goal = MoveArmRelativeCartesianPointGoal()
00166       move_rarm_up_goal.arm_name = "right_arm"
00167       move_rarm_up_goal.rel_point.z = .2
00168       move_to_rel_pos_client.send_goal(move_rarm_up_goal)
00169       move_to_rel_pos_client.wait_for_result()
00170       
00171       move_to_arm_pos_client.send_goal(move_rarm_to_side_goal)
00172       move_to_arm_pos_client.wait_for_result()
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


cotesys_ros_grasping
Author(s): Gil Jones
autogenerated on Thu May 23 2013 10:22:37