srs_decision_making: srs_decision_making: srs_generic

00001 #!/usr/bin/python
00002 #################################################################
00003 # \note
00004 #   Project name: srs
00005 # \author
00006 #   Renxi Qiu, email:renxi.qiu@googlemail.com
00007 #
00008 # \date Date of creation: Oct 2011
00009 #################################################################
00010 # ROS imports
00011 import roslib; roslib.load_manifest('srs_decision_making')
00012 #roslib.load_manifest('knowledge_ros_service')
00013 
00014 import rospy
00015 import smach
00016 import smach_ros
00017 import time
00018 import tf
00019 import actionlib
00020 import gc
00021 import json
00022 
00023 from std_msgs.msg import String, Bool, Int32
00024 from cob_srvs.srv import Trigger
00025 from geometry_msgs.msg import *
00026 
00027 import srs_decision_making.msg as xmsg
00028 import srs_decision_making.srv as xsrv
00029 from srs_knowledge.srv import *
00030 from srs_knowledge.msg import *
00031 import util.json_parser as json_parser
00032 
00033 """
00034 This file contains (or import) basic states for SRS high level state machines.
00035 
00036 The following states are designed for generic error handling in unstructured environment  
00037 
00038 Generic states of interventions for unexpected situation include:
00039 
00040     intervention_base_pose()
00041     # this can be used to adjust scan position, grasp position or as a intermediate goal for navigation 
00042     # semi_autonomous_mode == True
00043     # get a new base pose from UI, 
00044     # semi_autonomous_mode == False
00045     # robot has to reason by itself, get a new base pose from KB.
00046         
00047     intervention_key_region()
00048     # get a interested region and then pass it to the object detector
00049     # semi_autonomous_mode == True
00050     # get key_region from UI, 
00051     # semi_autonomous_mode == False
00052     # robot has to reason by itself, get a new key_region from KB.
00053     
00054     intervention_grasp_selection()
00055     # get a grasp configuration and then pass it to the manipulation module
00056     # semi_autonomous_mode == True
00057     # get grasp configuration from UI, 
00058     # semi_autonomous_mode == False
00059     # robot has to reason by itself, get a new grasp configuration from KB.    
00060     
00061     user_intervention_action_sequence()
00062     # get an action sequence from UI and then pass it to the  SRS knowledge service to update the existing action sequence
00063 
00064     semantic_dm()
00065         # This is the heart of the decision making, it monitor and control task execution based on pre-stored knowledge 
00066         
00067         
00068 Others
00069     initialise()
00070     #initialisation for a given task
00071 
00072 """
00073 
00074 """
00075 Dummy states about navigation, detection and grasp are imported from ipa_examples_mod.py for testing purpose
00076 They should be imported from other SRS components in the future  
00077 """
00078 from ipa_examples_mod import *
00079 
00080 
00081 # get a new base pose from UI, this can be used to adjust scan position, grasp position or as a intermediate goal for navigation 
00082 # output is a intermediate pose for navigation or a new scan position
00083 class intervention_base_pose(smach.State):
00084 
00085     def __init__(self):
00086         smach.State.__init__(self, outcomes=['retry','no_more_retry','failed', 'preempted' ],
00087                                 input_keys=['semi_autonomous_mode'],
00088                                 output_keys=['intermediate_pose'])
00089         global current_task_info         
00090         self.pub_fb = current_task_info.pub_fb
00091         self.pub_fb2 = current_task_info.pub_fb2
00092         self.count = 0
00093 
00094     def execute(self,userdata):
00095         
00096         if userdata.semi_autonomous_mode == True:
00097             # user intervention is possible
00098             while not (rospy.is_shutdown() or self.preempt_requested()) :
00099                                 
00100                 global current_task_info
00101                 _feedback=xmsg.ExecutionFeedback()
00102                 _feedback.current_state = "need user intervention for base pose"
00103                 _feedback.solution_required = True
00104                 _feedback.exceptional_case_id = 1
00105                 current_task_info._srs_as._as.publish_feedback(_feedback)
00106                 rospy.sleep(1)
00107                 
00108                 self.count += 1 
00109                 rospy.loginfo("State : need user intervention for base pose")
00110                 rospy.wait_for_service('message_errors')                       
00111                 s = xsrv.errorsResponse()
00112                 
00113                 current_state= 'need user intervention for base pose'
00114                 exceptional_case_id=1 # need user intervention for base pose
00115                 
00116                 try:
00117                     message_errors = rospy.ServiceProxy('message_errors',xsrv.errors)               
00118                     s = message_errors(current_state, exceptional_case_id)
00119                     print s.solution
00120                 except rospy.ServiceException, e:
00121                     print "Service call failed: %s"%e           
00122                     self.pub_fb.publish(False)   
00123                     return 'failed'
00124                 print (s)             
00125                 
00126                 if  (s.giveup == 1):
00127                     return 'no_more_retry'
00128                 else:
00129                     """import json
00130                     tmppos = s.solution.__str__()#"home"#[1.0, 3.0, 0.0]"
00131                     # (temp) -- commented by ze, simpler to use split(sep) directly
00132                     #tmppos = tmppos.replace('[','')
00133                     #tmppos = tmppos.replace(']','')
00134                     #tmppos = tmppos.replace(',',' ')
00135                     #tmppos = tmppos.replace('#','')
00136                     #listtmp = tmppos.split()
00137                     listtmp = tmppos.split('[]#, ')
00138                     #end of comment#
00139                     list_out = list()
00140                     list_out.insert(0, float(listtmp[0]))
00141                     list_out.insert(1, float(listtmp[1]))
00142                     list_out.insert(2, float(listtmp[2]))            
00143                     userdata.intermediate_pose = list_out  
00144                     """
00145                     userdata.intermediate_pose = eval(s.solution.__str__())
00146                     #rospy.loginfo("New intermediate target is :%s", list_out)    
00147                     return 'retry'
00148             return 'failed'
00149         else:
00150             # no user intervention, UI is not connected or not able to handle current situation 
00151             # fully autonomous mode for the current statemachine, robot try to handle error by it self with semantic KB
00152             """
00153             call srs knowledge ros service for a new scanning or grasping position
00154             """
00155             userdata.intermediate_pose = "kitchen"   
00156             return 'no_more_retry'
00157 
00158 #################################################################################
00159 #
00160 # The following state has been replaced by new high level detection state machines
00161 #
00162 #################################################################################
00163 
00164 """
00165 #get a interested region from UI or KB and then pass it to the object detector
00166 #intervention to highlight key interested region
00167 class intervention_key_region(smach.State):
00168 
00169     def __init__(self):
00170         smach.State.__init__(self, outcomes=['retry','no_more_retry','failed','preempted'],
00171                                 input_keys=['semi_autonomous_mode'],
00172                                 output_keys=['key_region'])
00173         global current_task_info         
00174         self.pub_fb = current_task_info.pub_fb
00175         self.pub_fb2 = current_task_info.pub_fb2
00176         self.count = 0
00177         
00178     def execute(self,userdata):
00179         
00180         if userdata.semi_autonomous_mode == True:
00181             # user intervention is possible
00182             # user specify key region on interface device for detection
00183             userdata.key_region = ""   
00184             return 'no_more_retry'
00185         else:
00186             # no user intervention, UI is not connected or not able to handle current situation 
00187             # fully autonomous mode for the current statemachine, robot try to handle error by it self with semantic KB
00188             userdata.key_region = ""   
00189             return 'no_more_retry'
00190 """
00191 
00192 #################################################################################
00193 #
00194 # The following state has been replaced by new high level grasp state machines
00195 #
00196 #################################################################################            
00197 
00198 """
00199 # get a grasp configuration from UI or KB and then pass it to the manipulation module
00200 # user intervention on specific configuration for grasp
00201 class intervention_grasp_selection(smach.State):
00202 
00203     def __init__(self):
00204         smach.State.__init__(self, outcomes=['retry','no_more_retry','failed','preempted'],
00205                                 input_keys=['semi_autonomous_mode'],                                
00206                                 output_keys=['grasp_conf'])
00207         global current_task_info         
00208         self.pub_fb = current_task_info.pub_fb
00209         self.pub_fb2 = current_task_info.pub_fb2
00210         self.count = 0
00211         
00212     def execute(self,userdata):
00213         # user specify key region on interface device for detection
00214     
00215         if userdata.semi_autonomous_mode == True:
00216             # user intervention is possible
00217             # user specify key region on interface device for detection
00218             userdata.grasp_conf = "Top"   
00219             return 'retry'
00220         else:
00221             # no user intervention, UI is not connected or not able to handle current situation 
00222             # fully autonomous mode for the current statemachine, robot try to handle error by it self with semantic KB
00223 
00224             userdata.grasp_conf = "Top"   
00225             return 'retry'        
00226 """
00227 
00228 
00229 #################################################################################
00230 #
00231 # The following state has been replaced by new high level detection and grasp state machines
00232 #
00233 #################################################################################
00234 """
00235 # get an action sequence from UI and then pass it to the SRS knowledge service to update the existing action sequence
00236 # 
00237 class user_intervention_action_sequence(smach.State):
00238 
00239     def __init__(self):
00240         smach.State.__init__(self, outcomes=['retry','no_more_retry','failed','preempted'],
00241                                 output_keys=['action_sequence'])
00242         global current_task_info         
00243         self.pub_fb = current_task_info.pub_fb
00244         self.pub_fb2 = current_task_info.pub_fb2
00245         self.count = 0
00246         
00247     def execute(self,userdata):
00248         # user specify key region on interface device for detection
00249         
00250         #updated action sequence for completing current task
00251         
00252         userdata.action_sequence = ""   
00253         return 'no_more_retry'  
00254         
00255 """ 
00256 
00257 #connection to the srs knowledge_ros_service
00258 class semantic_dm(smach.State):
00259 
00260     def __init__(self):
00261         smach.State.__init__(self, 
00262                              outcomes=['succeeded','failed','preempted','navigation','detection','simple_grasp','full_grasp', 'put_on_tray','env_update'],
00263                              io_keys=['target_base_pose',
00264                                         'target_object_name',
00265                                         'target_object_pose',
00266                                         'target_object_id',
00267                                         'target_object',
00268                                         'target_workspace_name',
00269                                         'semi_autonomous_mode',
00270                                         'grasp_categorisation',
00271                                         'scan_pose_list'])
00272         
00273         
00274         self.pub_fb = rospy.Publisher('fb_executing_solution', Bool)
00275         self.pub_fb2 = rospy.Publisher('fb_executing_state', String)
00276         self.count = 0
00277 
00278     def execute(self,userdata):     
00279         global current_task_info
00280         
00281         
00282         if current_task_info.last_step_info[len(current_task_info.last_step_info) - 1].outcome == 'stopped' or current_task_info.last_step_info[len(current_task_info.last_step_info) - 1].outcome == 'preempted':
00283             print 'task stopped'
00284             #nextStep = 'stop'
00285             #return nextStep
00286             #resultLastStep = 3
00287             
00288             current_task_info.set_customised_preempt_acknowledged(False)
00289             current_task_info.set_customised_preempt_required(False)
00290             current_task_info.set_stop_acknowledged(False)
00291             current_task_info.set_stop_required(False)
00292             return 'preempted'
00293         
00294         #call srs ros knowledge service for solution
00295         
00296         #dummy code for testing
00297         userdata.semi_autonomous_mode=True
00298         
00299         if not current_task_info.json_parameters == '':
00300             #clean action information from last step
00301             current_task_info.task_feedback.action_object = ''
00302             current_task_info.task_feedback.action_object_parent = '' 
00303                     
00304 
00305         ##############################################
00306         # get Next Action From Knowledge_ros_service
00307         ##############################################
00308         print 'Plan next Action service'
00309         rospy.wait_for_service('plan_next_action')
00310         try:
00311             next_action = rospy.ServiceProxy('plan_next_action', PlanNextAction)
00312 
00313             # decide result ()
00314             # current_task_info.last_step_info.append(last_step_info)
00315             # current_task_info.session_id = 123456
00316 
00317             print '+++++++++++ action acquired ++++++++++++++'
00318             print current_task_info.last_step_info;
00319             print '+++++++++++ Last Step Info LEN+++++++++++++++'
00320             len_step_info = len(current_task_info.last_step_info)
00321                     
00322             #feedback = []
00323             feedback_in_json = '{}'
00324             if not current_task_info.last_step_info:
00325                 ## first action. does not matter this. just to keep it filled
00326                 resultLastStep = 0
00327             elif current_task_info.last_step_info and current_task_info.last_step_info[len_step_info - 1].outcome == 'succeeded':
00328                 # convert to the format that knowledge_ros_service understands
00329                 resultLastStep = 0
00330                 if current_task_info.last_step_info[len_step_info - 1].step_name == 'sm_srs_detection':
00331                     print userdata.target_object_pose
00332                     #feedback = pose_to_list(userdata)
00333                     feedback_in_json = json_parser.detect_feedback_to_json(userdata)
00334                     
00335                     #rospy.loginfo ("Detected target_object is: %s", userdata.target_object)    
00336             elif current_task_info.last_step_info[len_step_info - 1].outcome == 'not_completed':
00337                 print 'Result return not_completed'
00338                 resultLastStep = 1
00339 
00340             elif current_task_info.last_step_info[len_step_info - 1].outcome == 'failed':
00341                 print 'Result return failed'
00342                 resultLastStep = 2
00343             elif current_task_info.last_step_info[len_step_info - 1].outcome == 'stopped' or current_task_info.last_step_info[len_step_info - 1].outcome == 'preempted':
00344                 print 'task stopped'
00345                 #nextStep = 'stop'
00346                 #return nextStep
00347                 #resultLastStep = 3
00348                 return 'preempted'
00349             
00350                 """
00351                 rospy.wait_for_service('task_request')
00352                 try:
00353                     requestNewTask = rospy.ServiceProxy('task_request', TaskRequest)
00354                     #res = requestNewTask(current_task_info.task_name, current_task_info.task_parameter, None, None, None, None)
00355                     res = requestNewTask('stop', None, None)
00356                     print res.sessionId
00357                     current_task_info.session_id = res.sessionId
00358                     if res.result == 1:
00359                         return 'failed'
00360                 except rospy.ServiceException, e:
00361                     print "Service call failed: %s"%e
00362                     return 'failed'
00363                 resultLastStep = 0
00364                 """
00365                 
00366                 
00367                 #if current_task_info.last_step_info[len_step_info - 1].step_name == 'sm_approach_pose_assisted':
00368                 #    result = (1, 0, 0)
00369                 #elif current_task_info.last_step_info[len_step_info - 1].step_name == 'sm_detect_asisted_pose_region':
00370                 #    result = (1, 1, 0)
00371                 #elif current_task_info.last_step_info[len_step_info - 1].step_name == 'sm_pick_object_asisted':
00372                 #    result = (0, 1, 1)
00373                 #else:
00374                 #  move  result = (1, 1, 1)
00375 
00376 
00377             print resultLastStep
00378             print '########## Result ###########'
00379 
00380             toPlanInput = PlanNextActionRequest()
00381             toPlanInput.sessionId = current_task_info.session_id
00382             toPlanInput.resultLastAction = resultLastStep
00383             #toPlanInput.genericFeedBack = feedback   # to be deprecated, replaced by jsonFeedBack
00384             
00385             toPlanInput.jsonFeedback = feedback_in_json
00386             
00387             resp1 = next_action(toPlanInput)
00388             #resp1 = next_action(current_task_info.session_id, resultLastStep, feedback)
00389             if resp1.nextAction.status == 1:
00390                 print 'succeeded'
00391                 return 'succeeded'
00392             elif resp1.nextAction.status == -1:
00393                 print 'failed'
00394                 return 'failed'
00395             
00396             print resp1.nextAction
00397             # else should be 0: then continue executing the following
00398 
00399             if resp1.nextAction.actionType == 'generic':
00400                 actName = json_parser.decode_action(resp1.nextAction.generic.jsonActionInfo)
00401                 
00402                 print "##############"
00403                 print actName
00404                 print "##############"
00405                 #actName = resp1.nextAction.generic.actionInfo[0]
00406                 if actName == 'charging':
00407                     nextStep = 'charging'
00408                     #userdata.target_base_pose = [float(resp1.nextAction.generic.actionInfo[1]), float(resp1.nextAction.generic.actionInfo[2]), float(resp1.nextAction.generic.actionInfo[3])]                    
00409 
00410                     destPos = json_parser.decode_move_parameters(resp1.nextAction.generic.jsonActionInfo)
00411                     if not destPos is None:
00412                         userdata.target_base_pose = [destPos['x'], destPos['y'], destPos['theta']]
00413                         if not current_task_info.json_parameters == '':
00414                             current_task_info.task_feedback.action_object = str([destPos['x'], destPos['y'], destPos['theta']])
00415                             current_task_info.task_feedback.action_object_parent = ''
00416                     
00417                     return nextStep
00418                 elif actName == 'move':
00419                     nextStep = 'navigation'
00420                     #userdata.target_base_pose = [float(resp1.nextAction.generic.actionInfo[1]), float(resp1.nextAction.generic.actionInfo[2]), float(resp1.nextAction.generic.actionInfo[3])]
00421 
00422                     destPos = json_parser.decode_move_parameters(resp1.nextAction.generic.jsonActionInfo)
00423                     if not destPos is None:
00424                         userdata.target_base_pose = [destPos['x'], destPos['y'], destPos['theta']]
00425                         if not current_task_info.json_parameters == '':
00426                             current_task_info.task_feedback.action_object = str([destPos['x'], destPos['y'], destPos['theta']])
00427                             current_task_info.task_feedback.action_object_parent = ''
00428 
00429                     return nextStep
00430                 elif actName == 'put_on_tray':
00431                     nextStep = 'put_on_tray'
00432                     #userdata.grasp_conf = resp1.nextAction.generic.actionInfo[1]
00433                     if not current_task_info.json_parameters == '':
00434                         current_task_info.task_feedback.action_object = 'tray'
00435                         current_task_info.task_feedback.action_object_parent = ''
00436                     return nextStep
00437                 elif actName == 'deliver_object':
00438                     nextStep = 'deliver_object'
00439                     #userdata.target_base_pose = [float(resp1.nextAction.generic.actionInfo[1]), float(resp1.nextAction.generic.actionInfo[2]), float(resp1.nextAction.generic.actionInfo[3])]                    
00440                     
00441                     destPos = json_parser.decode_move_parameters(resp1.nextAction.generic.jsonActionInfo)
00442                     if not destPos is None:
00443                         userdata.target_base_pose = [destPos['x'], destPos['y'], destPos['theta']]
00444                     if not current_task_info.json_parameters == '':
00445                         current_task_info.task_feedback.action_object = str([destPos['x'], destPos['y'], destPos['theta']])
00446                         current_task_info.task_feedback.action_object_parent = ''
00447                     return nextStep
00448                 elif actName == 'finish_success':
00449                     nextStep = 'succeeded'
00450                     return nextStep
00451                 elif actName == 'finish_fail':
00452                     nextStep = 'failed'
00453                     return nextStep
00454                 elif actName == 'detect':
00455                     nextStep = 'detection'
00456                     #TODO should confirm later if name or id used !!!!!!!!
00457 
00458                     ##userdata.target_object_name = 'milk_box'
00459                     #userdata.target_object_name = resp1.nextAction.generic.actionInfo[2]
00460                     #userdata.target_object_id = float(resp1.nextAction.generic.actionInfo[1])
00461                     
00462                     ## name of the workspace
00463                     #userdata.target_workspace_name = resp1.nextAction.generic.actionInfo[3]
00464                     #testing purpose, this value should come from knowledge service
00465                     ##userdata.target_workspace_name='Table0'
00466 
00467                     obj_to_det = json_parser.decode_detect_parameters(resp1.nextAction.generic.jsonActionInfo)
00468                     if not obj_to_det is None:
00469                         userdata.target_object_name = obj_to_det['object_type']
00470                         userdata.target_object_id = obj_to_det['object_id']
00471                         
00472                         # name of the workspace
00473                         userdata.target_workspace_name = obj_to_det['workspace']
00474                         if not current_task_info.json_parameters == '':
00475                             current_task_info.task_feedback.action_object = obj_to_det['object_type']
00476                             current_task_info.task_feedback.action_object_parent = obj_to_det['workspace']   
00477                     
00478                     rospy.loginfo ("target_object_name: %s", userdata.target_object_name)
00479                     rospy.loginfo ("target_object_id: %s", userdata.target_object_id)
00480                     rospy.loginfo ("target_workspace_name: %s", userdata.target_workspace_name)        
00481     
00482                     return nextStep
00483 
00484                 elif actName == 'grasp':
00485                     nextStep = 'simple_grasp'
00486                     
00487                     ##userdata.target_object_name = 'milk_box'
00488                     #userdata.target_object_name = resp1.nextAction.generic.actionInfo[2]
00489                     #userdata.target_object_id = float(resp1.nextAction.generic.actionInfo[1])
00490                     # name of the workspace
00491                     ##userdata.target_workspace_name = resp1.nextAction.generic.actionInfo[???]
00492                     ##testing purpose, this value should come from knowledge service
00493                     ##userdata.target_workspace_name='Table0'
00494                     obj_to_det = json_parser.decode_grasp_parameters(resp1.nextAction.generic.jsonActionInfo)
00495                     if not obj_to_det is None:
00496                         userdata.target_object_name = obj_to_det['object_type']
00497                         userdata.target_object_id = obj_to_det['object_id']
00498                         # name of the workspace
00499                         userdata.target_workspace_name = obj_to_det['workspace']
00500                         if not current_task_info.json_parameters == '':
00501                             current_task_info.task_feedback.action_object = obj_to_det['object_type']
00502                             current_task_info.task_feedback.action_object_parent = obj_to_det['workspace']   
00503                     
00504                     return nextStep
00505                 
00506                 elif actName == 'just_grasp':
00507                     nextStep = 'full_grasp'
00508                     
00509                     ##userdata.target_object_name = 'milk_box'
00510                     #userdata.target_object_name = resp1.nextAction.generic.actionInfo[2]
00511                     #userdata.target_object_id = float(resp1.nextAction.generic.actionInfo[1])
00512                     ## name of the workspace
00513                     #userdata.target_workspace_name = resp1.nextAction.generic.actionInfo[4]
00514 
00515                     obj_to_det = json_parser.decode_grasp_parameters(resp1.nextAction.generic.jsonActionInfo)
00516                     if not obj_to_det is None:
00517                         userdata.target_object_name = obj_to_det['object_type']
00518                         userdata.target_object_id = obj_to_det['object_id']
00519                         # name of the workspace
00520                         userdata.target_workspace_name = obj_to_det['workspace']
00521                         if not current_task_info.json_parameters == '':
00522                             current_task_info.task_feedback.action_object = obj_to_det['object_type']
00523                             current_task_info.task_feedback.action_object_parent = obj_to_det['workspace']   
00524                                         
00525                     return nextStep
00526                 
00527                 elif actName == 'check':
00528                     nextStep = 'env_update'
00529                     
00530                     #scan_base_pose = [float(resp1.nextAction.generic.actionInfo[2]), float(resp1.nextAction.generic.actionInfo[3]), float(resp1.nextAction.generic.actionInfo[4])]                    
00531 
00532                     #userdata.scane_pose_list[0] = scan_base_pose                
00533 
00534                     par = json_parser.decode_check_ws_parameters(resp1.nextAction.generic.jsonActionInfo)
00535                     if not par is None:
00536                         json_pose = par[1]
00537                         scan_base_pose = [json_pose['x'], json_pose['y'], json_pose['theta']]
00538                         userdata.scane_pose_list[0] = scan_base_pose                
00539                         # userdata.target_workspace_name = par[0]
00540                         if not current_task_info.json_parameters == '':
00541                             current_task_info.task_feedback.action_object = ''
00542                             current_task_info.task_feedback.action_object_parent = ''
00543                                        
00544 
00545                     """
00546                     userdata.target_object_hh_id = 1
00547                     
00548                     # userdata.target_object_name = resp1.nextAction.generic.actionInfo[1]
00549             
00550                     userdata.target_base_pose = [float(resp1.nextAction.generic.actionInfo[2]), float(resp1.nextAction.generic.actionInfo[3]), float(resp1.nextAction.generic.actionInfo[4])]                    
00551 
00552                     
00553                     userdata.target_object_pose.position.x = float(resp1.nextAction.generic.actionInfo[5])
00554                     userdata.target_object_pose.position.y = float(resp1.nextAction.generic.actionInfo[6])
00555                     # use height of workspace as a point on the surface
00556                     userdata.target_object_pose.position.z = float(resp1.nextAction.generic.actionInfo[14])
00557                     userdata.target_object_pose.orientation.x = float(resp1.nextAction.generic.actionInfo[8])
00558                     userdata.target_object_pose.orientation.y =float(resp1.nextAction.generic.actionInfo[9])
00559                     userdata.target_object_pose.orientation.z = float(resp1.nextAction.generic.actionInfo[10]) 
00560                     userdata.target_object_pose.orientation.w =float(resp1.nextAction.generic.actionInfo[11])
00561                     """
00562                     return nextStep
00563                 
00564                 else:
00565                     print 'No valid action'
00566                     nextStep = 'failed'
00567                     return nextStep
00568                 
00569                     
00570             else:
00571                 print 'No valid actionFlags'
00572                 #print resp1.nextAction.actionFlags
00573                 #nextStep = 'No_corresponding_action???'
00574                 nextStep = 'failed'
00575                 return 'failed'
00576 
00577         except rospy.ServiceException, e:
00578             print "Service call failed: %s"%e        
00579             return 'failed'
00580 
00581         return nextStep
00582 
00583         ##############################################        
00584         ### End of GetNextAction######################
00585         ##############################################
00586 
00587 
00588 
00589 #initialisation for a given task
00590 class initialise(smach.State):
00591     def __init__(self):
00592         smach.State.__init__(self, outcomes=['succeeded', 'failed'])
00593         #self.count=0
00594 
00595         
00596     def execute(self,userdata):
00597                
00598         last_step_info = xmsg.Last_step_info()
00599         last_step_info.step_name = "initialise"
00600         last_step_info.outcome = 'succeeded'
00601         last_step_info.semi_autonomous_mode = False
00602         
00603         #recording the information of last step
00604         global current_task_info
00605         
00606         if len(current_task_info.last_step_info) > 0 :
00607             del current_task_info.last_step_info[:]  #empty the list        
00608         
00609         current_task_info.last_step_info.append(last_step_info)               
00610      
00611         step_id = 1
00612          
00613         _feedback=xmsg.ExecutionFeedback()
00614         _feedback.current_state = last_step_info.step_name + last_step_info.outcome
00615         _feedback.solution_required = False
00616         _feedback.exceptional_case_id = 0
00617         _feedback.json_feedback = ''
00618         
00619         if not current_task_info.json_parameters == '':
00620         
00621             json_feedback_current_action = '"current_action": {"name": "'+ last_step_info.step_name +'", "state": "' + last_step_info.outcome + '", "step_id": '+ str(step_id) +' }'
00622                   
00623             json_feedback_feedback = '"feedback": {"lang": "'+ current_task_info.language_set +'", "message": "'+ current_task_info.feedback_messages[last_step_info.step_name] +'"}'
00624                          
00625             json_feedback_task = '"task": {"task_id": "'+ str(current_task_info.task_feedback.task_id) +'", "task_initializer": "'+ current_task_info.task_feedback.task_initializer +'","task_initializer_type": "'+ current_task_info.task_feedback.task_initializer_type +'", "task_name": "'+ current_task_info.task_feedback.task_name +'","task_parameter": "'+ current_task_info.task_feedback.task_parameter +'"}'
00626                         
00627             _feedback.json_feedback = json.dumps ('{' + json_feedback_current_action + ',' + json_feedback_feedback + ',' + json_feedback_task + '}')
00628         
00629         current_task_info._srs_as._as.publish_feedback(_feedback)
00630 
00631         return 'succeeded'
00632     
00633 #prepare the robot for the task
00634 class prepare_robot(smach.State):
00635     def __init__(self):
00636         smach.State.__init__(self, outcomes=['succeeded', 'failed'])
00637         #self.count=0
00638 
00639         
00640     def execute(self,userdata):
00641                
00642         last_step_info = xmsg.Last_step_info()
00643         last_step_info.step_name = "prepare_robot"
00644         last_step_info.outcome = 'succeeded'
00645         last_step_info.semi_autonomous_mode = False
00646         
00647         #recording the information of last step
00648         global current_task_info
00649         current_task_info.last_step_info.append(last_step_info)
00650                 
00651         #current_task_info.session_id = 123456
00652         
00653         #initialisation of the robot
00654         # move to initial positions
00655         global sss
00656         
00657         sss.set_light("yellow")
00658 
00659         # initialize components
00660         handle_head = sss.init("head")
00661         if handle_head.get_error_code() != 0:
00662             return 'failed'
00663 
00664         handle_torso = sss.init("torso")
00665         if handle_torso.get_error_code() != 0:
00666             return 'failed'
00667 
00668         handle_tray = sss.init("tray")
00669         if handle_tray.get_error_code() != 0:
00670             return 'failed'
00671 
00672         #handle_arm = sss.init("arm")
00673         #if handle_arm.get_error_code() != 0:
00674         # return 'failed'
00675 
00676         handle_sdh = sss.init("sdh")
00677         #if handle_sdh.get_error_code() != 0:
00678         # return 'failed'
00679 
00680         handle_base = sss.init("base")
00681         if handle_base.get_error_code() != 0:
00682             return 'failed'
00683 
00684         # recover components
00685         handle_head = sss.recover("head")
00686         if handle_head.get_error_code() != 0:
00687             return 'failed'
00688 
00689         handle_torso = sss.recover("torso")
00690         if handle_torso.get_error_code() != 0:
00691             return 'failed'
00692 
00693         handle_tray = sss.recover("tray")
00694         if handle_tray.get_error_code() != 0:
00695             return 'failed'
00696 
00697         handle_arm = sss.recover("arm")
00698         #if handle_arm.get_error_code() != 0:
00699         # return 'failed'
00700 
00701         #handle_sdh = sss.recover("sdh")
00702         #if handle_sdh.get_error_code() != 0:
00703         # return 'failed'
00704 
00705         handle_base = sss.recover("base")
00706         if handle_base.get_error_code() != 0:
00707             return 'failed'
00708         
00709         # set light
00710         sss.set_light("green")
00711         
00712         return 'succeeded'
00713         
00714         
00715         """
00716         
00717         
00718         handle_torso = sss.move("torso", "home", False)
00719         handle_tray = sss.move("tray", "down", False)
00720         handle_arm = sss.move("arm", "folded", False)
00721         handle_sdh = sss.move("sdh", "cylclosed", False)
00722         handle_head = sss.move("head", "front", False)
00723     
00724         
00725         # wait for initial movements to finish
00726         handle_torso.wait()
00727         handle_tray.wait()
00728         handle_arm.wait()
00729         handle_sdh.wait()
00730         handle_head.wait()
00731         
00732         
00733 
00734         return 'succeeded'
00735         """
00736 
00737 def pose_to_list(userdata):
00738     # userdata.target_object_name
00739     poseList = ('detect', userdata.target_object_name, str(userdata.target_object_pose.pose.position.x), str(userdata.target_object_pose.pose.position.y), str(userdata.target_object_pose.pose.position.z), str(userdata.target_object_pose.pose.orientation.x), str(userdata.target_object_pose.pose.orientation.y), str(userdata.target_object_pose.pose.orientation.z), str(userdata.target_object_pose.pose.orientation.w))
00740     
00741     return poseList
srs_generic_states.py
