srs_generic_states.py
Go to the documentation of this file.
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 import srs_ui_pro.msg as echo_server_msg
00034 
00035 """
00036 This file contains (or import) basic states for SRS high level state machines.
00037 
00038 The following states are designed for generic error handling in unstructured environment  
00039 
00040 Generic states of interventions for unexpected situation include:
00041 
00042     intervention_base_pose()
00043     # this can be used to adjust scan position, grasp position or as a intermediate goal for navigation 
00044     # semi_autonomous_mode == True
00045     # get a new base pose from UI, 
00046     # semi_autonomous_mode == False
00047     # robot has to reason by itself, get a new base pose from KB.
00048         
00049     intervention_key_region()
00050     # get a interested region and then pass it to the object detector
00051     # semi_autonomous_mode == True
00052     # get key_region from UI, 
00053     # semi_autonomous_mode == False
00054     # robot has to reason by itself, get a new key_region from KB.
00055     
00056     intervention_grasp_selection()
00057     # get a grasp configuration and then pass it to the manipulation module
00058     # semi_autonomous_mode == True
00059     # get grasp configuration from UI, 
00060     # semi_autonomous_mode == False
00061     # robot has to reason by itself, get a new grasp configuration from KB.    
00062     
00063     user_intervention_action_sequence()
00064     # get an action sequence from UI and then pass it to the  SRS knowledge service to update the existing action sequence
00065 
00066     semantic_dm()
00067         # This is the heart of the decision making, it monitor and control task execution based on pre-stored knowledge 
00068         
00069         
00070 Others
00071     initialise()
00072     #initialisation for a given task
00073 
00074 """
00075 
00076 """
00077 Dummy states about navigation, detection and grasp are imported from ipa_examples_mod.py for testing purpose
00078 They should be imported from other SRS components in the future  
00079 """
00080 from ipa_examples_mod import *
00081 
00082 
00083 # get a new base pose from UI, this can be used to adjust scan position, grasp position or as a intermediate goal for navigation 
00084 # output is a intermediate pose for navigation or a new scan position
00085 class intervention_base_pose(smach.State):
00086 
00087     def __init__(self):
00088         smach.State.__init__(self, outcomes=['retry','no_more_retry','failed', 'preempted' ],
00089                                 input_keys=['semi_autonomous_mode'],
00090                                 output_keys=['intermediate_pose'])
00091         global current_task_info         
00092         self.pub_fb = current_task_info.pub_fb
00093         self.pub_fb2 = current_task_info.pub_fb2
00094         self.count = 0
00095 
00096     def execute(self,userdata):
00097         
00098         if userdata.semi_autonomous_mode == True:
00099             # user intervention is possible
00100             while not (rospy.is_shutdown() or self.preempt_requested()) :
00101                                 
00102                 global current_task_info
00103                 _feedback=xmsg.ExecutionFeedback()
00104                 _feedback.current_state = "need user intervention for base pose"
00105                 _feedback.solution_required = True
00106                 _feedback.exceptional_case_id = 1
00107                 current_task_info._srs_as._as.publish_feedback(_feedback)
00108                 rospy.sleep(1)
00109                 
00110                 self.count += 1 
00111                 rospy.loginfo("State : need user intervention for base pose")
00112                 rospy.wait_for_service('message_errors')                       
00113                 s = xsrv.errorsResponse()
00114                 
00115                 current_state= 'need user intervention for base pose'
00116                 exceptional_case_id=1 # need user intervention for base pose
00117                 
00118                 try:
00119                     message_errors = rospy.ServiceProxy('message_errors',xsrv.errors)               
00120                     s = message_errors(current_state, exceptional_case_id)
00121                     print s.solution
00122                 except rospy.ServiceException, e:
00123                     print "Service call failed: %s"%e           
00124                     self.pub_fb.publish(False)   
00125                     return 'failed'
00126                 print (s)             
00127                 
00128                 if  (s.giveup == 1):
00129                     return 'no_more_retry'
00130                 else:
00131                     """import json
00132                     tmppos = s.solution.__str__()#"home"#[1.0, 3.0, 0.0]"
00133                     # (temp) -- commented by ze, simpler to use split(sep) directly
00134                     #tmppos = tmppos.replace('[','')
00135                     #tmppos = tmppos.replace(']','')
00136                     #tmppos = tmppos.replace(',',' ')
00137                     #tmppos = tmppos.replace('#','')
00138                     #listtmp = tmppos.split()
00139                     listtmp = tmppos.split('[]#, ')
00140                     #end of comment#
00141                     list_out = list()
00142                     list_out.insert(0, float(listtmp[0]))
00143                     list_out.insert(1, float(listtmp[1]))
00144                     list_out.insert(2, float(listtmp[2]))            
00145                     userdata.intermediate_pose = list_out  
00146                     """
00147                     #userdata.intermediate_pose = eval(s.solution.__str__())
00148                     
00149                     if s.solution.find("[") == -1:
00150                         # not a list (the position is home order etc.)
00151                         userdata.intermediate_pose = s.solution.__str__()
00152                     else:
00153                         # list [1, 2, 3] etc.
00154                         try:
00155                             userdata.intermediate_pose = eval(s.solution.__str__())
00156                         except Exception, e:
00157                             rospy.INFO("%s", e)
00158                             return 'failed'
00159                     
00160                     return 'retry'
00161                     #rospy.loginfo("New intermediate target is :%s", list_out)    
00162                     
00163             return 'failed'
00164         else:
00165             # no user intervention, UI is not connected or not able to handle current situation 
00166             # fully autonomous mode for the current statemachine, robot try to handle error by it self with semantic KB
00167             """
00168             srs knowledge service would find a new scanning or grasping position automatically
00169             """
00170             #userdata.intermediate_pose = "kitchen"   
00171             return 'no_more_retry'
00172 
00173 #################################################################################
00174 #
00175 # The following state has been replaced by new high level detection state machines
00176 #
00177 #################################################################################
00178 
00179 """
00180 #get a interested region from UI or KB and then pass it to the object detector
00181 #intervention to highlight key interested region
00182 class intervention_key_region(smach.State):
00183 
00184     def __init__(self):
00185         smach.State.__init__(self, outcomes=['retry','no_more_retry','failed','preempted'],
00186                                 input_keys=['semi_autonomous_mode'],
00187                                 output_keys=['key_region'])
00188         global current_task_info         
00189         self.pub_fb = current_task_info.pub_fb
00190         self.pub_fb2 = current_task_info.pub_fb2
00191         self.count = 0
00192         
00193     def execute(self,userdata):
00194         
00195         if userdata.semi_autonomous_mode == True:
00196             # user intervention is possible
00197             # user specify key region on interface device for detection
00198             userdata.key_region = ""   
00199             return 'no_more_retry'
00200         else:
00201             # no user intervention, UI is not connected or not able to handle current situation 
00202             # fully autonomous mode for the current statemachine, robot try to handle error by it self with semantic KB
00203             userdata.key_region = ""   
00204             return 'no_more_retry'
00205 """
00206 
00207 #################################################################################
00208 #
00209 # The following state has been replaced by new high level grasp state machines
00210 #
00211 #################################################################################            
00212 
00213 """
00214 # get a grasp configuration from UI or KB and then pass it to the manipulation module
00215 # user intervention on specific configuration for grasp
00216 class intervention_grasp_selection(smach.State):
00217 
00218     def __init__(self):
00219         smach.State.__init__(self, outcomes=['retry','no_more_retry','failed','preempted'],
00220                                 input_keys=['semi_autonomous_mode'],                                
00221                                 output_keys=['grasp_conf'])
00222         global current_task_info         
00223         self.pub_fb = current_task_info.pub_fb
00224         self.pub_fb2 = current_task_info.pub_fb2
00225         self.count = 0
00226         
00227     def execute(self,userdata):
00228         # user specify key region on interface device for detection
00229     
00230         if userdata.semi_autonomous_mode == True:
00231             # user intervention is possible
00232             # user specify key region on interface device for detection
00233             userdata.grasp_conf = "Top"   
00234             return 'retry'
00235         else:
00236             # no user intervention, UI is not connected or not able to handle current situation 
00237             # fully autonomous mode for the current statemachine, robot try to handle error by it self with semantic KB
00238 
00239             userdata.grasp_conf = "Top"   
00240             return 'retry'        
00241 """
00242 
00243 
00244 #################################################################################
00245 #
00246 # The following state has been replaced by new high level detection and grasp state machines
00247 #
00248 #################################################################################
00249 """
00250 # get an action sequence from UI and then pass it to the SRS knowledge service to update the existing action sequence
00251 # 
00252 class user_intervention_action_sequence(smach.State):
00253 
00254     def __init__(self):
00255         smach.State.__init__(self, outcomes=['retry','no_more_retry','failed','preempted'],
00256                                 output_keys=['action_sequence'])
00257         global current_task_info         
00258         self.pub_fb = current_task_info.pub_fb
00259         self.pub_fb2 = current_task_info.pub_fb2
00260         self.count = 0
00261         
00262     def execute(self,userdata):
00263         # user specify key region on interface device for detection
00264         
00265         #updated action sequence for completing current task
00266         
00267         userdata.action_sequence = ""   
00268         return 'no_more_retry'  
00269         
00270 """ 
00271 
00272 #connection to the srs knowledge_ros_service
00273 class semantic_dm(smach.State):
00274 
00275     def __init__(self):
00276         smach.State.__init__(self, 
00277                              outcomes=['succeeded','failed','preempted','navigation','detection','simple_grasp','full_grasp', 'put_on_tray','env_update','reset_robot_after_impossible_task'],
00278                              io_keys=['target_base_pose',
00279                                         'target_object_name',
00280                                         'target_object_pose',
00281                                         'target_object_id',
00282                                         'target_object',
00283                                         'target_workspace_name',
00284                                         'semi_autonomous_mode',
00285                                         'grasp_categorisation',
00286                                         'scan_pose_list'])
00287         
00288         
00289         self.pub_fb = rospy.Publisher('fb_executing_solution', Bool)
00290         self.pub_fb2 = rospy.Publisher('fb_executing_state', String)
00291         self.count = 0
00292 
00293     def execute(self,userdata):     
00294         global current_task_info
00295         
00296         
00297         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':
00298             print 'task stopped'
00299             #nextStep = 'stop'
00300             #return nextStep
00301             #resultLastStep = 3
00302             
00303             current_task_info.set_customised_preempt_acknowledged(False)
00304             current_task_info.set_customised_preempt_required(False)
00305             current_task_info.set_stop_acknowledged(False)
00306             current_task_info.set_stop_required(False)
00307             return 'preempted'
00308         
00309         #call srs ros knowledge service for solution
00310         
00311         #dummy code for testing
00312         userdata.semi_autonomous_mode=True
00313         
00314         if not current_task_info.json_parameters == '':
00315             #clean action information from last step
00316             current_task_info.task_feedback.action_object = ''
00317             current_task_info.task_feedback.action_object_parent = '' 
00318                     
00319 
00320         ##############################################
00321         # get Next Action From Knowledge_ros_service
00322         ##############################################
00323         print 'Plan next Action service'
00324         rospy.wait_for_service('plan_next_action')
00325         try:
00326             next_action = rospy.ServiceProxy('plan_next_action', PlanNextAction)
00327 
00328             # decide result ()
00329             # current_task_info.last_step_info.append(last_step_info)
00330             # current_task_info.session_id = 123456
00331 
00332             print '+++++++++++ action acquired ++++++++++++++'
00333             print current_task_info.last_step_info;
00334             print '+++++++++++ Last Step Info LEN+++++++++++++++'
00335             len_step_info = len(current_task_info.last_step_info)
00336                     
00337             #feedback = []
00338             feedback_in_json = '{}'
00339             if not current_task_info.last_step_info:
00340                 ## first action. does not matter this. just to keep it filled
00341                 resultLastStep = PlanNextActionRequest().LAST_ACTION_SUCCESS
00342             elif current_task_info.last_step_info and current_task_info.last_step_info[len_step_info - 1].outcome == 'succeeded':
00343                 # convert to the format that knowledge_ros_service understands
00344                 resultLastStep = PlanNextActionRequest().LAST_ACTION_SUCCESS
00345                 if current_task_info.last_step_info[len_step_info - 1].step_name == 'sm_srs_detection':
00346                     print userdata.target_object_pose
00347                     #feedback = pose_to_list(userdata)
00348                     feedback_in_json = json_parser.detect_feedback_to_json(userdata)
00349                     
00350                     #rospy.loginfo ("Detected target_object is: %s", userdata.target_object)    
00351             elif current_task_info.last_step_info[len_step_info - 1].outcome == 'not_completed':
00352                 print 'Result return not_completed'
00353                 resultLastStep = PlanNextActionRequest().LAST_ACTION_NOT_COMPLETED
00354 
00355             elif current_task_info.last_step_info[len_step_info - 1].outcome == 'failed':
00356                 print 'Result return failed'
00357                 resultLastStep = PlanNextActionRequest().LAST_ACTION_FAIL
00358             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':
00359                 print 'task stopped'
00360                 #nextStep = 'stop'
00361                 #return nextStep
00362                 #resultLastStep = 3
00363                 return 'preempted'
00364             
00365                 """
00366                 rospy.wait_for_service('task_request')
00367                 try:
00368                     requestNewTask = rospy.ServiceProxy('task_request', TaskRequest)
00369                     #res = requestNewTask(current_task_info.task_name, current_task_info.task_parameter, None, None, None, None)
00370                     res = requestNewTask('stop', None, None)
00371                     print res.sessionId
00372                     current_task_info.session_id = res.sessionId
00373                     if res.result == 1:
00374                         return 'failed'
00375                 except rospy.ServiceException, e:
00376                     print "Service call failed: %s"%e
00377                     return 'failed'
00378                 resultLastStep = 0
00379                 """
00380                 
00381                 
00382                 #if current_task_info.last_step_info[len_step_info - 1].step_name == 'sm_approach_pose_assisted':
00383                 #    result = (1, 0, 0)
00384                 #elif current_task_info.last_step_info[len_step_info - 1].step_name == 'sm_detect_asisted_pose_region':
00385                 #    result = (1, 1, 0)
00386                 #elif current_task_info.last_step_info[len_step_info - 1].step_name == 'sm_pick_object_asisted':
00387                 #    result = (0, 1, 1)
00388                 #else:
00389                 #  move  result = (1, 1, 1)
00390 
00391 
00392             print resultLastStep
00393             print '########## Result ###########'
00394 
00395             toPlanInput = PlanNextActionRequest()
00396             toPlanInput.sessionId = current_task_info.session_id
00397             toPlanInput.resultLastAction = resultLastStep
00398             #toPlanInput.genericFeedBack = feedback   # to be deprecated, replaced by jsonFeedBack
00399             
00400             toPlanInput.jsonFeedback = feedback_in_json
00401             
00402             resp1 = next_action(toPlanInput)
00403             #resp1 = next_action(current_task_info.session_id, resultLastStep, feedback)
00404             if resp1.nextAction.status == 1:
00405                 print 'succeeded'
00406                 return 'succeeded'
00407             elif resp1.nextAction.status == -1:
00408                 #print 'failed'
00409                 #return 'failed'
00410                 print 'impossible task'
00411                 nextStep = 'reset_robot_after_impossible_task'
00412                 return nextStep
00413             
00414             print resp1.nextAction
00415             # else should be 0: then continue executing the following
00416 
00417             if resp1.nextAction.actionType == 'generic':
00418                 actName = json_parser.decode_action(resp1.nextAction.generic.jsonActionInfo)
00419                 
00420                 print "##############"
00421                 print actName
00422                 print "##############"
00423                 #actName = resp1.nextAction.generic.actionInfo[0]
00424                 if actName == 'charging':
00425                     nextStep = 'charging'
00426                     #userdata.target_base_pose = [float(resp1.nextAction.generic.actionInfo[1]), float(resp1.nextAction.generic.actionInfo[2]), float(resp1.nextAction.generic.actionInfo[3])]                    
00427 
00428                     destPos = json_parser.decode_move_parameters(resp1.nextAction.generic.jsonActionInfo)
00429                     if not destPos is None:
00430                         userdata.target_base_pose = [destPos['x'], destPos['y'], destPos['theta']]
00431                         if not current_task_info.json_parameters == '':
00432                             current_task_info.task_feedback.action_object = str([destPos['x'], destPos['y'], destPos['theta']])
00433                             current_task_info.task_feedback.action_object_parent = ''
00434                     
00435                     return nextStep
00436                 elif actName == 'move':
00437                     nextStep = 'navigation'
00438                     #userdata.target_base_pose = [float(resp1.nextAction.generic.actionInfo[1]), float(resp1.nextAction.generic.actionInfo[2]), float(resp1.nextAction.generic.actionInfo[3])]
00439 
00440                     destPos = json_parser.decode_move_parameters(resp1.nextAction.generic.jsonActionInfo)
00441                     if not destPos is None:
00442                         userdata.target_base_pose = [destPos['x'], destPos['y'], destPos['theta']]
00443                         if not current_task_info.json_parameters == '':
00444                             current_task_info.task_feedback.action_object = str([destPos['x'], destPos['y'], destPos['theta']])
00445                             current_task_info.task_feedback.action_object_parent = ''
00446 
00447                     return nextStep
00448                 elif actName == 'put_on_tray':
00449                     nextStep = 'put_on_tray'
00450                     #userdata.grasp_conf = resp1.nextAction.generic.actionInfo[1]
00451                     if not current_task_info.json_parameters == '':
00452                         current_task_info.task_feedback.action_object = 'tray'
00453                         current_task_info.task_feedback.action_object_parent = ''
00454                     return nextStep
00455                 elif actName == 'deliver_object':
00456                     nextStep = 'deliver_object'
00457                     #userdata.target_base_pose = [float(resp1.nextAction.generic.actionInfo[1]), float(resp1.nextAction.generic.actionInfo[2]), float(resp1.nextAction.generic.actionInfo[3])]                    
00458                     
00459                     destPos = json_parser.decode_move_parameters(resp1.nextAction.generic.jsonActionInfo)
00460                     if not destPos is None:
00461                         userdata.target_base_pose = [destPos['x'], destPos['y'], destPos['theta']]
00462                     if not current_task_info.json_parameters == '':
00463                         current_task_info.task_feedback.action_object = str([destPos['x'], destPos['y'], destPos['theta']])
00464                         current_task_info.task_feedback.action_object_parent = ''
00465                     return nextStep
00466                 elif actName == 'finish_success':
00467                     nextStep = 'succeeded'
00468                     return nextStep
00469                 elif actName == 'finish_fail':
00470                     nextStep = 'failed'
00471                     return nextStep
00472                 elif actName == 'detect':
00473                     nextStep = 'detection'
00474                     #TODO should confirm later if name or id used !!!!!!!!
00475 
00476                     ##userdata.target_object_name = 'milk_box'
00477                     #userdata.target_object_name = resp1.nextAction.generic.actionInfo[2]
00478                     #userdata.target_object_id = float(resp1.nextAction.generic.actionInfo[1])
00479                     
00480                     ## name of the workspace
00481                     #userdata.target_workspace_name = resp1.nextAction.generic.actionInfo[3]
00482                     #testing purpose, this value should come from knowledge service
00483                     ##userdata.target_workspace_name='Table0'
00484 
00485                     obj_to_det = json_parser.decode_detect_parameters(resp1.nextAction.generic.jsonActionInfo)
00486                     if not obj_to_det is None:
00487                         userdata.target_object_name = obj_to_det['object_type']
00488                         userdata.target_object_id = obj_to_det['object_id']
00489                         
00490                         # name of the workspace
00491                         userdata.target_workspace_name = obj_to_det['workspace']
00492                         if not current_task_info.json_parameters == '':
00493                             current_task_info.task_feedback.action_object = obj_to_det['object_type']
00494                             current_task_info.task_feedback.action_object_parent = obj_to_det['workspace']   
00495                     
00496                     rospy.loginfo ("target_object_name: %s", userdata.target_object_name)
00497                     rospy.loginfo ("target_object_id: %s", userdata.target_object_id)
00498                     rospy.loginfo ("target_workspace_name: %s", userdata.target_workspace_name)        
00499     
00500                     return nextStep
00501 
00502                 elif actName == 'grasp':
00503                     nextStep = 'simple_grasp'
00504                     
00505                     ##userdata.target_object_name = 'milk_box'
00506                     #userdata.target_object_name = resp1.nextAction.generic.actionInfo[2]
00507                     #userdata.target_object_id = float(resp1.nextAction.generic.actionInfo[1])
00508                     # name of the workspace
00509                     ##userdata.target_workspace_name = resp1.nextAction.generic.actionInfo[???]
00510                     ##testing purpose, this value should come from knowledge service
00511                     ##userdata.target_workspace_name='Table0'
00512                     obj_to_det = json_parser.decode_grasp_parameters(resp1.nextAction.generic.jsonActionInfo)
00513                     if not obj_to_det is None:
00514                         userdata.target_object_name = obj_to_det['object_type']
00515                         userdata.target_object_id = obj_to_det['object_id']
00516                         # name of the workspace
00517                         userdata.target_workspace_name = obj_to_det['workspace']
00518                         if not current_task_info.json_parameters == '':
00519                             current_task_info.task_feedback.action_object = obj_to_det['object_type']
00520                             current_task_info.task_feedback.action_object_parent = obj_to_det['workspace']   
00521                     
00522                     return nextStep
00523                 
00524                 elif actName == 'just_grasp':
00525                     nextStep = 'full_grasp'
00526                     
00527                     ##userdata.target_object_name = 'milk_box'
00528                     #userdata.target_object_name = resp1.nextAction.generic.actionInfo[2]
00529                     #userdata.target_object_id = float(resp1.nextAction.generic.actionInfo[1])
00530                     ## name of the workspace
00531                     #userdata.target_workspace_name = resp1.nextAction.generic.actionInfo[4]
00532 
00533                     obj_to_det = json_parser.decode_grasp_parameters(resp1.nextAction.generic.jsonActionInfo)
00534                     if not obj_to_det is None:
00535                         userdata.target_object_name = obj_to_det['object_type']
00536                         userdata.target_object_id = obj_to_det['object_id']
00537                         # name of the workspace
00538                         userdata.target_workspace_name = obj_to_det['workspace']
00539                         if not current_task_info.json_parameters == '':
00540                             current_task_info.task_feedback.action_object = obj_to_det['object_type']
00541                             current_task_info.task_feedback.action_object_parent = obj_to_det['workspace']   
00542                                         
00543                     return nextStep
00544                 
00545                 elif actName == 'check':
00546                     nextStep = 'env_update'
00547                     
00548                     #scan_base_pose = [float(resp1.nextAction.generic.actionInfo[2]), float(resp1.nextAction.generic.actionInfo[3]), float(resp1.nextAction.generic.actionInfo[4])]                    
00549 
00550                     #userdata.scane_pose_list[0] = scan_base_pose                
00551 
00552                     par = json_parser.decode_check_ws_parameters(resp1.nextAction.generic.jsonActionInfo)
00553                     if not par is None:
00554                         json_pose = par[1]
00555                         scan_base_pose = [json_pose['x'], json_pose['y'], json_pose['theta']]
00556                         userdata.scane_pose_list[0] = scan_base_pose                
00557                         # userdata.target_workspace_name = par[0]
00558                         if not current_task_info.json_parameters == '':
00559                             current_task_info.task_feedback.action_object = ''
00560                             current_task_info.task_feedback.action_object_parent = ''
00561                                        
00562 
00563                     """
00564                     userdata.target_object_hh_id = 1
00565                     
00566                     # userdata.target_object_name = resp1.nextAction.generic.actionInfo[1]
00567             
00568                     userdata.target_base_pose = [float(resp1.nextAction.generic.actionInfo[2]), float(resp1.nextAction.generic.actionInfo[3]), float(resp1.nextAction.generic.actionInfo[4])]                    
00569 
00570                     
00571                     userdata.target_object_pose.position.x = float(resp1.nextAction.generic.actionInfo[5])
00572                     userdata.target_object_pose.position.y = float(resp1.nextAction.generic.actionInfo[6])
00573                     # use height of workspace as a point on the surface
00574                     userdata.target_object_pose.position.z = float(resp1.nextAction.generic.actionInfo[14])
00575                     userdata.target_object_pose.orientation.x = float(resp1.nextAction.generic.actionInfo[8])
00576                     userdata.target_object_pose.orientation.y =float(resp1.nextAction.generic.actionInfo[9])
00577                     userdata.target_object_pose.orientation.z = float(resp1.nextAction.generic.actionInfo[10]) 
00578                     userdata.target_object_pose.orientation.w =float(resp1.nextAction.generic.actionInfo[11])
00579                     """
00580                     return nextStep
00581                 
00582                 else:
00583                     print 'No valid action'
00584                     #nextStep = 'failed'
00585                     nextStep = 'reset_robot_after_impossible_task'
00586                     return nextStep
00587                 
00588                     
00589             else:
00590                 print 'No valid actionFlags'
00591                 #print resp1.nextAction.actionFlags
00592                 #nextStep = 'No_corresponding_action???'
00593                 nextStep = 'failed'
00594                 return 'failed'
00595 
00596         except rospy.ServiceException, e:
00597             print "Service call failed: %s"%e        
00598             return 'failed'
00599 
00600         return nextStep
00601 
00602         ##############################################        
00603         ### End of GetNextAction######################
00604         ##############################################
00605 
00606 
00607 
00608 #initialisation for a given task
00609 class initialise(smach.State):
00610     def __init__(self):
00611         smach.State.__init__(self, outcomes=['succeeded', 'failed'])
00612         #self.count=0
00613 
00614         
00615     def execute(self,userdata):
00616                
00617         last_step_info = xmsg.Last_step_info()
00618         last_step_info.step_name = "initialise"
00619         last_step_info.outcome = 'succeeded'
00620         last_step_info.semi_autonomous_mode = False
00621         
00622         #recording the information of last step
00623         global current_task_info
00624         
00625         if len(current_task_info.last_step_info) > 0 :
00626             del current_task_info.last_step_info[:]  #empty the list        
00627         
00628         current_task_info.last_step_info.append(last_step_info)               
00629      
00630         step_id = 1
00631          
00632         _feedback=xmsg.ExecutionFeedback()
00633         _feedback.current_state = last_step_info.step_name + last_step_info.outcome
00634         _feedback.solution_required = False
00635         _feedback.exceptional_case_id = 0
00636         _feedback.json_feedback = ''
00637         
00638         if not current_task_info.json_parameters == '':
00639         
00640             json_feedback_current_action = '"current_action": {"name": "'+ last_step_info.step_name +'", "state": "' + last_step_info.outcome + '", "step_id": '+ str(step_id) +' }'
00641                   
00642             json_feedback_feedback = '"feedback": {"lang": "'+ current_task_info.language_set +'", "message": "'+ current_task_info.feedback_messages[last_step_info.step_name] +'"}'
00643                          
00644             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 +'"}'
00645                         
00646             _feedback.json_feedback = json.dumps ('{' + json_feedback_current_action + ',' + json_feedback_feedback + ',' + json_feedback_task + '}')
00647         
00648         current_task_info._srs_as._as.publish_feedback(_feedback)
00649 
00650         return 'succeeded'
00651     
00652 #prepare the robot for the task
00653 class prepare_robot(smach.State):
00654     def __init__(self):
00655         smach.State.__init__(self, outcomes=['succeeded', 'failed'])
00656         #self.count=0
00657 
00658         
00659     def execute(self,userdata):
00660                
00661         last_step_info = xmsg.Last_step_info()
00662         last_step_info.step_name = "prepare_robot"
00663         last_step_info.outcome = 'succeeded'
00664         last_step_info.semi_autonomous_mode = False
00665         
00666         #recording the information of last step
00667         global current_task_info
00668         current_task_info.last_step_info.append(last_step_info)
00669                 
00670         #current_task_info.session_id = 123456
00671         
00672         #initialisation of the robot
00673         # move to initial positions
00674         global sss
00675         
00676         sss.set_light("yellow")
00677 
00678         # initialize components
00679         handle_head = sss.init("head")
00680         if handle_head.get_error_code() != 0:
00681             return 'failed'
00682 
00683         handle_torso = sss.init("torso")
00684         if handle_torso.get_error_code() != 0:
00685             return 'failed'
00686 
00687         handle_tray = sss.init("tray")
00688         if handle_tray.get_error_code() != 0:
00689             return 'failed'
00690 
00691         #handle_arm = sss.init("arm")
00692         #if handle_arm.get_error_code() != 0:
00693         # return 'failed'
00694 
00695         handle_sdh = sss.init("sdh")
00696         #if handle_sdh.get_error_code() != 0:
00697         # return 'failed'
00698 
00699         handle_base = sss.init("base")
00700         if handle_base.get_error_code() != 0:
00701             return 'failed'
00702 
00703         # recover components
00704         handle_head = sss.recover("head")
00705         if handle_head.get_error_code() != 0:
00706             return 'failed'
00707 
00708         handle_torso = sss.recover("torso")
00709         if handle_torso.get_error_code() != 0:
00710             return 'failed'
00711 
00712         handle_tray = sss.recover("tray")
00713         if handle_tray.get_error_code() != 0:
00714             return 'failed'
00715 
00716         handle_arm = sss.recover("arm")
00717         #if handle_arm.get_error_code() != 0:
00718         # return 'failed'
00719 
00720         #handle_sdh = sss.recover("sdh")
00721         #if handle_sdh.get_error_code() != 0:
00722         # return 'failed'
00723 
00724         handle_base = sss.recover("base")
00725         if handle_base.get_error_code() != 0:
00726             return 'failed'
00727         
00728         # set light
00729         sss.set_light("green")
00730         
00731         return 'succeeded'
00732         
00733 #prepare the robot for the task
00734 class reset_robot(smach.State):
00735     def __init__(self):
00736         smach.State.__init__(self, outcomes=['completed', 'failed'])
00737         #self.count=0
00738 
00739         
00740     def execute(self,userdata):
00741                
00742         last_step_info = xmsg.Last_step_info()
00743         last_step_info.step_name = "reset_robot"
00744         last_step_info.outcome = 'succeeded'
00745         last_step_info.semi_autonomous_mode = False
00746         
00747         #recording the information of last step
00748         global current_task_info
00749         current_task_info.last_step_info.append(last_step_info)
00750                 
00751         #current_task_info.session_id = 123456
00752         
00753         #initialisation of the robot
00754         # move to initial positions
00755         global sss
00756         
00757         sss.set_light("yellow")
00758   
00759 
00760         # reset components
00761         handle_head = sss.move("head","front",False)
00762         #if handle_head.get_error_code() != 0:
00763         #    return 'failed'
00764 
00765         handle_torso = sss.move("torso","home",False)
00766         #if handle_torso.get_error_code() != 0:
00767         #    return 'failed'
00768         
00769         sss.sleep(2)
00770         sss.say(["The existing task can not be completed, I will reset myself now"],False)
00771 
00772         
00773         # check if tray service is available
00774         service_full_name = '/tray_monitor/occupied'
00775         try:
00776             #rospy.wait_for_service(service_full_name,rospy.get_param('server_timeout',3))
00777             rospy.wait_for_service(service_full_name,3)
00778         except rospy.ROSException, e:
00779             error_message = "%s"%e
00780             rospy.logerr("<<%s>> service not available, error: %s",service_full_name, error_message)
00781             return 'failed'
00782 
00783         # check if service can be called
00784         try:
00785             is_ocuppied = rospy.ServiceProxy(service_full_name,Trigger)
00786             resp = is_ocuppied()
00787         except rospy.ServiceException, e:
00788             error_message = "%s"%e
00789             rospy.logerr("calling <<%s>> service not successfull, error: %s",service_full_name, error_message)
00790             return 'failed'
00791         
00792         #if tray is not occupied
00793         if resp != True :
00794             handle_tray = sss.move("tray","down")
00795             #if handle_tray.get_error_code() != 0:
00796             #    return 'failed'
00797 
00798         handle_sdh = sss.move("sdh","home")
00799         #if handle_sdh.get_error_code() != 0:
00800         #    return 'failed'
00801 
00802 
00803         handle_arm = sss.move("arm","folded")
00804         #if handle_arm.get_error_code() != 0:
00805         #    return 'failed'
00806         handle_arm.wait()
00807         
00808         # set light
00809         sss.set_light("green")
00810         
00811         return 'completed'        
00812         
00813 
00814 # enable intervention through UI_PRO
00815 # completed: the task is completed fully by the remote user via ui_PRO
00816 # give_up: remote user 
00817 # failed: soft or hard ware failure during the intervention 
00818 class remote_user_intervention(smach.State):
00819     def __init__(self):
00820         smach.State.__init__(self, 
00821                              outcomes=['completed', 'give_up' ,'failed'],
00822                              input_keys=['semi_autonomous_mode'])       
00823         
00824         #self.count=0
00825          # values from srs_ui_pro echo_server
00826         self.server_current_status = ""
00827         self.server_json_feedback = ""
00828         self.server_output = ""
00829         self.server_json_result = ""
00830         self.flag = False
00831         
00832     def give_up(self, last_action):
00833         
00834         #recovery logic for last action
00835         global sss
00836         ipa_arm_navigation = 'false'         
00837         try:
00838             ipa_arm_navigation = rospy.get_param("srs/ipa_arm_navigation")
00839         except Exception, e:
00840             print('can not read parameter of srs/ipa_arm_navigation, use the default value planned arm navigation disabled')
00841         
00842         
00843         if last_action == 'sm_srs_grasp' :
00844             # if there is no user intervention and the grasp was failed, move arm back to the hold position 
00845             if ipa_arm_navigation.lower() == 'true':
00846                 handle_arm = sss.move('arm','hold',False, 'Planned')
00847             else:
00848                 handle_arm = sss.move('arm','hold',False)
00849             sss.sleep(2)
00850             handle_arm.wait()
00851         
00852         return 'give_up'
00853         
00854    
00855     def execute(self,userdata):
00856         
00857         print ('CHECKING IF USER INTERVENTION IS REQUIRED')
00858         # check if user intervention is required. 
00859         # give_up if the task is in fully autonomous mode
00860         if userdata.semi_autonomous_mode == False:
00861             return 'give_up'
00862         
00863         global current_task_info
00864         #name of the overall task
00865         #the_task_name = current_task_info.task_feedback.task_name 
00866         
00867         #if not the_task_name:
00868             #return 'give_up'
00869         
00870         the_task_feedback = current_task_info.task_feedback
00871         
00872         if not the_task_feedback:
00873             return 'give_up'
00874         
00875         #parameter of the overall task
00876         the_task_parameter = current_task_info.task_feedback.task_parameter
00877         
00878         step_id = len (current_task_info.last_step_info)         
00879         
00880         print "### user intervention is going on..."
00881         
00882         if step_id > 0:
00883             #name of the current step 
00884             the_action_name = current_task_info.last_step_info[step_id-1].step_name 
00885             
00886             #out come of the last action
00887             the_action_outcome = current_task_info.last_step_info[step_id-1].outcome
00888         
00889             #object of the action
00890             the_action_object = current_task_info.task_feedback.action_object
00891         
00892             #parent of the object
00893             the_action_object_parent = current_task_info.task_feedback.action_object_parent
00894             
00895             try:
00896                 print "### action client of echo server is working..."
00897                 client = actionlib.SimpleActionClient('srs_ui_pro/echo_server', echo_server_msg.dm_serverAction)
00898                 
00899                 if client.wait_for_server(timeout=rospy.Duration(5)) is False:
00900                     rospy.loginfo ("there is no response from srs_ui_pro, this intervention action cannot be executed now...")
00901                     return self.give_up(the_action_name)
00902                 else:
00903                     global sss      
00904                     rospy.sleep(6)            
00905                     sss.say(["I can not finish the task"])
00906                     sss.say(["Remote Operators are Online Should we ask them for help"])
00907                     
00908                     rospy.wait_for_service('answer_yes_no')
00909                     try:
00910                         # call ui_pri_topic_yes_no
00911                         # if the answer is "no", then return 'give_up'
00912                         answer_yes_no = rospy.ServiceProxy('answer_yes_no', xsrv.answer_yes_no)
00913                         resp = answer_yes_no()
00914                         if resp.answer == "No":
00915                             rospy.loginfo ("the use refused to get any remote assistance...")
00916                             # in this stage, the anser_yes_no is not used
00917                             # to use it, just make sure the following line is available
00918                             #return 'give_up'
00919                     except rospy.ServiceException, e:
00920                         print "Service call failed: %s"%e
00921                 
00922                 goal = echo_server_msg.dm_serverGoal()
00923                 
00924                 # see the json_parser file
00925                 try:
00926                     json_decoded = json.loads(current_task_info.json_parameters)
00927                 except Exception:
00928                     rospy.loginfo ("current_task_info.json_parameters is invalid...")
00929                     return self.give_up(the_action_name)
00930                     
00931                 current_tasks = json_decoded['tasks']
00932                 
00933                 # value for "exception_id" in goal
00934                 exception_id = 1
00935                 
00936                 time_schedule = 'null'
00937                 current_task = 'null'
00938                 deliver_destination = 'null'
00939                 
00940                 
00941                 # value for "time_schedule" in goal
00942                 try:
00943                     time_schedule = current_tasks[0]['time_schedule']
00944                 except Exception:
00945                     rospy.loginfo ("no time schedule in this task set null")
00946                 
00947                 
00948                 # value for "task" in goal
00949                 try:
00950                     current_task = current_tasks[0]['task']
00951                 except Exception:
00952                     rospy.loginfo ("no name in this task set null")
00953                 
00954                 # value for "deliver_destination" in goal
00955                 try:
00956                     deliver_destination = current_tasks[0]['deliver_destination']
00957                 except Exception:
00958                     rospy.loginfo ("no destination in this task set null")    
00959                 
00960                 # value for "additional_information" in goal
00961                 additional_information = "this is a test message"
00962                 
00963                 current_goal = {"exception_id": exception_id, "tasks": [{"time_schedule": time_schedule, "task": current_task, "deliver_destination": deliver_destination}], "additional_information": additional_information}
00964                 
00965                  #convert current goal to json object
00966                 json_input = json.dumps(current_goal)
00967                
00968                 # construct a goal
00969                 goal.json_input = json_input
00970                 
00971                 server_feedback = echo_server_msg.dm_serverFeedback()
00972                 server_result = echo_server_msg.dm_serverResult()
00973                 
00974                 # send the goal to echo server
00975                 client.send_goal(goal, self.result_callback, self.active_callback, self.feedback_callback)
00976                 #rospy.sleep(25)
00977                 
00978                 timeout = 6000
00979                 while(self.flag != True and timeout > 0):
00980                     rospy.sleep(1)
00981                     timeout = timeout - 1
00982                 
00983                 if self.server_json_result == "" :
00984                     rospy.loginfo ("*******")
00985                     rospy.loginfo ("there is no response from srs_ui_pro, the current intervention action has been given up...")
00986                     rospy.loginfo ("*******")
00987                     rospy.sleep(3)
00988                     return self.give_up(the_action_name)
00989                 
00990                 _feedback = xmsg.ExecutionFeedback()
00991                 _feedback.current_state =  self.server_current_status + ": started"
00992                 _feedback.solution_required = False
00993                 _feedback.exceptional_case_id = exception_id
00994                 _feedback.json_feedback = self.server_json_feedback
00995                 current_task_info._srs_as._as.publish_feedback(_feedback)
00996             
00997                 json_decoded = json.loads(self.server_json_result)
00998                 result = json_decoded['result']
00999                 # result should be succeeded
01000                 if result == "succeeded":           
01001                     sss.say(["With the help of remote Operators, The task has been completed "])
01002                     return 'completed'
01003                 elif result == "failed":
01004                     sss.say(["This task is impossible, I have to give up"])
01005                     return "failed"
01006                 else:
01007                     return "give_up"
01008             except rospy.ROSInterruptException:
01009                 print "error before completion"
01010                 return "failed"
01011         else:
01012             #the task has not been started yet, not need for intervention 
01013             return 'give_up'
01014          
01015                 
01016         #
01017         # get confirmation from UI_LOC
01018         #
01019         
01020         
01021         #
01022         # processing user intervention from UI_PRO
01023         # current_task_info._srs_as._as.publish_feedback(_feedback)
01024         #
01025         
01026         
01027         #return 'give_up'
01028 
01029     def feedback_callback(self, server_feedback):
01030         #self.server_current_status = server_feedback.current_status
01031         self.server_json_feedback = server_feedback.json_feedback
01032         #rospy.loginfo ("server_feedback.current_status is: %s", server_feedback.current_status)
01033         rospy.loginfo ("server_feedback.json_feedback is: %s",server_feedback.json_feedback)
01034             
01035     def active_callback(self):
01036         rospy.loginfo ("goal has been sent to the echo_server...")
01037     
01038     def result_callback(self, state, server_result):
01039         self.server_json_result = server_result.json_result
01040         rospy.loginfo ("server_feedback.state is: %s",state)
01041         rospy.loginfo ("server_feedback.result_callback is: %s",server_result.json_result)
01042         self.flag = True
01043         
01044 
01045 def pose_to_list(userdata):
01046     # userdata.target_object_name
01047     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))
01048     
01049     return poseList


srs_decision_making
Author(s): Renxi Qiu
autogenerated on Sun Jan 5 2014 12:08:52