$search
00001 # ROS imports 00002 import roslib 00003 roslib.load_manifest('srs_decision_making') 00004 #import copy 00005 import rospy 00006 import smach 00007 #import smach_ros 00008 00009 from std_msgs.msg import String, Bool, Int32 00010 from cob_srvs.srv import Trigger 00011 from math import * 00012 import time 00013 from kinematics_msgs.srv import * 00014 00015 #import actionlib 00016 # ROS imports 00017 import roslib 00018 roslib.load_manifest('srs_states') 00019 import rospy 00020 import smach 00021 # include script server, to move the robot 00022 from simple_script_server import * 00023 sss = simple_script_server() 00024 00025 # msg imports 00026 from geometry_msgs.msg import * 00027 from shared_state_information import * 00028 00029 00030 #from gazebo.srv import * 00031 #import gazebo.msg as gazebo 00032 00033 00034 00035 00036 00037 00038 00039 00040 00041 """ 00042 Below dummy generic states are copied and modified based on IPA examples for testing purpose 00043 They should be replaced by real states from other SRS components in the future 00044 00045 Basic states related to robot includes: 00046 00047 approach_pose() 00048 approach_pose_without_retry() 00049 select_grasp() 00050 grasp_side() 00051 grasp_top() 00052 open_door() 00053 put_object_on_tray() 00054 detect_object() 00055 00056 Only dummy outputs are given for testing purpose 00057 """ 00058 00059 00060 00061 00062 00063 class select_post_table_pose(smach.State): 00064 def __init__(self): 00065 smach.State.__init__(self, outcomes=['succeeded', 'failed','preempted'], input_keys=['post_table_pos'], output_keys=['post_table_pos']) 00066 #self.counter = 0 00067 00068 def execute(self, userdata): 00069 global current_task_info 00070 00071 if current_task_info.get_post_grasp_adjustment_state()==True: # already adjusted 00072 return 'succeeded' 00073 else: 00074 pos=current_task_info.get_robot_pos() 00075 00076 if pos ==None: 00077 userdata.post_table_pos='' 00078 return 'failed' 00079 else: 00080 current_task_info.set_post_grasp_adjustment_state(True) 00081 pos.x = pos.x + 0.15 * cos(pos.theta) 00082 pos.y = pos.y + 0.15 * sin(pos.theta) 00083 userdata.post_table_pos = list() 00084 userdata.post_table_pos.append(pos.x) 00085 userdata.post_table_pos.append(pos.y) 00086 userdata.post_table_pos.append(pos.theta) 00087 return 'succeeded' 00088 00089 00090 class select_pose(smach.State): 00091 def __init__(self): 00092 smach.State.__init__(self, outcomes=['got_to_next_pose', 'no_more_pose', 'failed','preempted'], 00093 input_keys=['target_base_pose_list'], output_keys=['current_target_base_pose']) 00094 def execute(self, userdata): 00095 userdata.current_target_base_pose='home' 00096 return 'got_to_next_pose' 00097 00098 00099 ## Put object on tray side state 00100 # 00101 # This state puts a side grasped object on the tray 00102 class put_object_on_tray(smach.State): 00103 00104 def __init__(self): 00105 smach.State.__init__( 00106 self, 00107 outcomes=['succeeded', 'failed' ,'preempted'], 00108 input_keys=['grasp_categorisation']) 00109 00110 00111 def execute(self, userdata): 00112 #TODO select position on tray depending on how many objects are on the tray already 00113 global current_task_info 00114 00115 if True:#current_task_info.object_in_hand and not current_task_info.object_on_tray: 00116 00117 # move object to frontside 00118 handle_arm = sss.move("arm","grasp-to-tray",False) 00119 sss.sleep(2) 00120 sss.move("tray","up") 00121 handle_arm.wait() 00122 if self.preempt_requested(): 00123 self.service_preempt() 00124 return 'preempted' 00125 00126 # release object 00127 if userdata.grasp_categorisation == 'side' or userdata.grasp_categorisation == 'front': 00128 sss.move("sdh","cylopen") 00129 elif userdata.grasp_categorisation == 'top': 00130 sss.move("sdh","spheropen") 00131 00132 #object is transfered from hand to tray 00133 current_task_info.object_in_hand = False 00134 current_task_info.object_on_tray = True 00135 00136 if self.preempt_requested(): 00137 self.service_preempt() 00138 return 'preempted' 00139 00140 # move arm to backside again 00141 handle_arm = sss.move("arm","tray-to-folded",False) 00142 sss.sleep(3) 00143 sss.move("sdh","home") 00144 handle_arm.wait() 00145 00146 if self.preempt_requested(): 00147 self.service_preempt() 00148 return 'preempted' 00149 else: 00150 return 'succeeded' 00151 00152 00153 #verify_object FROM PRO+IPA, the interface still need to be clarified 00154 class verify_object(smach.State): 00155 00156 def __init__(self): 00157 smach.State.__init__(self, outcomes=['object_verified','no_object_verified','failed','preempted'], 00158 input_keys=['reference_to_map','object_name_list'], 00159 output_keys=['updated_object_list']) 00160 00161 def execute(self,userdata): 00162 # user specify key region on interface device for detection 00163 """ 00164 Extract objects from current point map 00165 """ 00166 updated_object = "" #updated pose information about the object 00167 return 'failed' 00168 00169 #scan environment from IPA, the interface still need to be clarified 00170 class update_env_model(smach.State): 00171 def __init__(self): 00172 smach.State.__init__(self, outcomes=['succeeded', 'not_completed', 'failed', 'preempted']) 00173 00174 def execute(self,userdata): 00175 # user specify key region on interface device for detection 00176 """ 00177 Get current point map 00178 """ 00179 #map_reference = "" 00180 return 'succeeded' 00181 00182 00183 #verify_object FROM PRO+IPA, the interface still need to be clarified 00184 #for integration meeting 00185 class object_verification_simple(smach.State): 00186 00187 def __init__(self): 00188 smach.State.__init__(self, outcomes=['object_verified','no_object_verified','failed','preempted'], 00189 input_keys=['target_object_name','target_object_hh_id','target_object_pose'], 00190 output_keys=['target_object_pose']) 00191 #object verified: the expected object has been found, the revised pose is in the output_put key verified_target_object_pose 00192 #no_object_verified: no object found, given up 00193 00194 00195 def execute(self,userdata): 00196 # user specify key region on interface device for detection 00197 00198 00199 00200 #dummy code for testing 00201 #verified object is in the exact expected position 00202 #return object_verfied 00203 00204 userdata.target_object_pose = userdata.target_object_pose 00205 return 'object_verified' 00206 00207 00208 """ 00209 ## Detect state 00210 # 00211 # This state will try to detect an object. 00212 class detect_object(smach.State): 00213 def __init__(self,object_name = "",max_retries = 1): 00214 smach.State.__init__( 00215 self, 00216 outcomes=['succeeded','retry','no_more_retries','failed'], 00217 input_keys=['object_name'], 00218 output_keys=['object']) 00219 00220 self.object_name = object_name 00221 00222 00223 def execute(self, userdata): 00224 # determine object name 00225 if self.object_name != "": 00226 object_name = self.object_name 00227 elif type(userdata.object_name) is str: 00228 object_name = userdata.object_name 00229 else: # this should never happen 00230 rospy.logerr("Invalid userdata 'object_name'") 00231 self.retries = 0 00232 return 'failed' 00233 00234 return 'succeeded' 00235 00236 00237 00238 00239 00240 class move_head(smach.State): 00241 00242 def __init__(self): 00243 smach.State.__init__( 00244 self, 00245 outcomes=['succeeded', 'failed'], 00246 input_keys=['torso_pose']) 00247 00248 self.torso_poses = [] 00249 self.torso_poses.append("home") 00250 self.torso_poses.append("left") 00251 self.torso_poses.append("right") 00252 00253 00254 def execute(self, userdata): 00255 sss.move("torso",userdata.torso_pose) 00256 return 'succeeded' 00257 00258 00259 00260 00261 ## Deliver object state 00262 # 00263 # This state will deliver an object which should be on the tray. 00264 class deliver_object(smach.State): 00265 def __init__(self): 00266 smach.State.__init__(self, 00267 outcomes=['succeeded', 'retry', 'failed']) 00268 00269 def execute(self, userdata): 00270 00271 #sss.say(["Here is your " + userdata.object_name + ". Please help yourself."],False) 00272 sss.move("torso","nod",False) 00273 00274 try: 00275 rospy.wait_for_service('/tray/check_occupied',10) 00276 except rospy.ROSException, e: 00277 rospy.loginfo("\n\nService not available: %s", e) 00278 rospy.loginfo('\n\nIf the tray has been emptied? Please enter Yes/No - Y/N') 00279 inp = raw_input() 00280 if inp == 'y' or inp == 'Y': 00281 00282 sss.move("tray","down",False) 00283 sss.move("torso","nod",False) 00284 return 'succeeded' 00285 else: 00286 return 'failed' 00287 00288 time = rospy.Time.now().secs 00289 loop_rate = rospy.Rate(5) #hz 00290 while True: 00291 if rospy.Time.now().secs-time > 20: 00292 return 'retry' 00293 try: 00294 tray_service = rospy.ServiceProxy('/tray/check_occupied', CheckOccupied) 00295 req = CheckOccupiedRequest() 00296 res = tray_service(req) 00297 print "waiting for tray to be not occupied any more" 00298 if(res.occupied.data == False): 00299 break 00300 except rospy.ServiceException, e: 00301 print "Service call failed: %s", e 00302 return 'failed' 00303 sss.sleep(2) 00304 00305 sss.move("tray","down",False) 00306 sss.move("torso","nod",False) 00307 00308 return 'succeeded' 00309 """ 00310