00001
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()
