00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 import random
00019 import numpy
00020 from math import pi
00021
00022 import rospy
00023 import actionlib
00024 from geometry_msgs.msg import PoseStamped
00025 from tf.transformations import *
00026 import simple_moveit_interface as smi_
00027 import cob_pick_place_action.msg
00028
00029
00030 def gen_pose(frame_id="/base_link", pos=[0,0,0], euler=[0,0,0]):
00031 pose = PoseStamped()
00032 pose.header.frame_id = frame_id
00033 pose.header.stamp = rospy.Time.now()
00034 pose.pose.position.x, pose.pose.position.y, pose.pose.position.z = pos
00035 pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w = quaternion_from_euler(*euler)
00036 return pose
00037
00038 def setup_environment():
00039 psi = smi_.get_planning_scene_interface()
00040 rospy.sleep(1.0)
00041
00042
00043 smi_.clear_objects("arm_left_7_link")
00044
00045
00046 smi_.add_ground()
00047
00048
00049 pose = PoseStamped()
00050 pose.header.frame_id = "/base_footprint"
00051 pose.header.stamp = rospy.Time.now()
00052 pose.pose.position.x = -0.9
00053 pose.pose.position.y = 0
00054 pose.pose.position.z = 0.39
00055 pose.pose.orientation.x = 0
00056 pose.pose.orientation.y = 0
00057 pose.pose.orientation.z = 0
00058 pose.pose.orientation.w = 1
00059 psi.add_box("bookcase", pose, size=(0.5, 1.5, 0.78))
00060 rospy.sleep(1.0)
00061
00062
00063
00064 def cob_pick_action_client(object_class, object_name, object_pose):
00065 pick_action_client = actionlib.SimpleActionClient('cob_pick_action', cob_pick_place_action.msg.CobPickAction)
00066
00067 pick_action_client.wait_for_server()
00068
00069
00070 goal = cob_pick_place_action.msg.CobPickGoal()
00071 goal.object_class = object_class
00072 goal.object_name = object_name
00073 goal.object_pose = object_pose
00074
00075 goal.gripper_type = "sdh"
00076
00077 goal.gripper_side = "left"
00078
00079
00080
00081 goal.grasp_database = "OpenRAVE"
00082 goal.support_surface = "bookcase"
00083
00084
00085
00086 pick_action_client.send_goal(goal)
00087
00088
00089 finished_before_timeout=pick_action_client.wait_for_result(rospy.Duration(300, 0))
00090
00091 if finished_before_timeout:
00092 state=pick_action_client.get_state()
00093 print "Action finished: %s"%state
00094
00095 return state
00096
00097
00098 def cob_place_action_client(object_class, object_name, destinations):
00099
00100
00101 place_action_client = actionlib.SimpleActionClient('cob_place_action', cob_pick_place_action.msg.CobPlaceAction)
00102
00103
00104
00105 place_action_client.wait_for_server()
00106
00107
00108 goal = cob_pick_place_action.msg.CobPlaceGoal()
00109 goal.object_class = object_class
00110 goal.object_name = object_name
00111 goal.destinations = destinations
00112 goal.support_surface = "bookcase"
00113
00114
00115
00116 place_action_client.send_goal(goal)
00117
00118
00119 finished_before_timeout=place_action_client.wait_for_result(rospy.Duration(300, 0))
00120
00121 if finished_before_timeout:
00122 state=place_action_client.get_state()
00123 print "Action finished: %s"%state
00124
00125 return state
00126
00127
00128 if __name__ == '__main__':
00129 try:
00130
00131
00132 rospy.init_node('CobPickAction_client_py')
00133
00134 setup_environment()
00135
00136 psi = smi_.get_planning_scene_interface()
00137 rospy.sleep(1.0)
00138
00139 filename = roslib.packages.get_pkg_dir('cob_grasp_generation')+"/files/meshes/yellowsaltcube.stl"
00140 pose1 = gen_pose(pos=[-0.7, 0.0, 0.85], euler=[random.uniform(-pi, pi), random.uniform(-pi, pi), random.uniform(-pi, pi)])
00141 psi.add_mesh("cube1", pose1, filename)
00142 rospy.sleep(1.0)
00143
00144 pose2 = gen_pose(pos=[-0.7, 0.2, 0.85], euler=[random.uniform(-pi, pi), random.uniform(-pi, pi), random.uniform(-pi, pi)])
00145 psi.add_mesh("cube2", pose2, filename)
00146 rospy.sleep(1.0)
00147
00148 destinations = []
00149 for x in numpy.arange(-0.9, -0.6, 0.1):
00150 for y in numpy.arange(-0.3, -0.2, 0.1):
00151 for theta in numpy.arange(-pi, pi, pi/2):
00152 destination = gen_pose(pos=[x,y,0.78], euler=[0,0,theta])
00153 destinations.append(destination)
00154
00155
00156 result = cob_pick_action_client(18, "cube1", pose1)
00157
00158
00159
00160
00161 result = cob_place_action_client(18, "cube1", destinations)
00162
00163 result = cob_pick_action_client(18, "cube2", pose2)
00164
00165
00166
00167
00168
00169 result = cob_place_action_client(18, "cube2", destinations)
00170
00171
00172 except rospy.ROSInterruptException:
00173 print "program interrupted before completion"