srs_decision_making: srs_configuration_statemachines.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/python
00002 #################################################################
00003 ##\file
00004 #
00005 # \note
00006 # Copyright (c) 2011 \n
00007 # Cardiff University \n\n
00008 #
00009 #################################################################
00010 #
00011 # \note
00012 # Project name: Multi-Role Shadow Robotic System for Independent Living
00013 # \note
00014 # ROS stack name: srs
00015 # \note
00016 # ROS package name: srs_decision_making
00017 #
00018 # \author
00019 # Author: Renxi Qiu, email: renxi.qiu@gmail.com
00020 #
00021 # \date Date of creation: Oct 2011
00022 #
00023 # \brief
00024 # Task coordination and interfacing for SRS decision making
00025 #
00026 #################################################################
00027 #
00028 # Redistribution and use in source and binary forms, with or without
00029 # modification, are permitted provided that the following conditions are met:
00030 #
00031 # - Redistributions of source code must retain the above copyright
00032 # notice, this list of conditions and the following disclaimer. \n
00033 #
00034 # - Redistributions in binary form must reproduce the above copyright
00035 # notice, this list of conditions and the following disclaimer in the
00036 # documentation and/or other materials provided with the distribution. \n
00037 #
00038 # This program is free software: you can redistribute it and/or modify
00039 # it under the terms of the GNU Lesser General Public License LGPL as
00040 # published by the Free Software Foundation, either version 3 of the
00041 # License, or (at your option) any later version.
00042 #
00043 # This program is distributed in the hope that it will be useful,
00044 # but WITHOUT ANY WARRANTY; without even the implied warranty of
00045 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
00046 # GNU Lesser General Public License LGPL for more details.
00047 #
00048 # You should have received a copy of the GNU Lesser General Public
00049 # License LGPL along with this program.
00050 # If not, see <http://www.gnu.org/licenses/>.
00051 #
00052 #################################################################
00053 
00054 from srs_monitoring_statemachines import *
00055 from robot_configuration import *
00056 
00057 """
00058 This file contains state machines for robot configuration checking during the operation 
00059 The pause and resume generic state are also included in the file
00060 
00061 The actual robot configuration pre post condition are imported from robot_configuration.py 
00062 
00063 Please note that:
00064 As minimal control unit of the DM is generic state, therefore the pause and resume of the generic states may not be the same as the pause and resume of the real actions of robot.
00065 and, it will be useful to check the pause within the generic state to avoid unnecessary time out.   
00066 
00067 """
00068 
00069 # Checking during robot is paused
00070 class state_checking_during_paused (smach.State):
00071     def __init__(self):
00072         smach.State.__init__(self , outcomes =['resume', 'stopped', 'preempted'])
00073     
00074     
00075     def execute (self, userdata):
00076 
00077         global current_task_info
00078         
00079         
00080         while not self.preempt_requested():
00081             #if stop command has been received during the pause
00082             if current_task_info.get_stop_required():
00083                 #reset the flag to normal
00084                 current_task_info.set_stop_acknowledged(True)
00085                 try:
00086                     sss.say([current_task_info.speaking_language['Stop']],False)
00087                 except:
00088                     print sys.exc_info()
00089                     
00090                 #complete the state and return stop
00091                 return 'stopped'
00092             
00093             #if another command with higher priority received during the pause
00094             elif current_task_info.get_customised_preempt_required():
00095                 #reset the flag to normal
00096                 current_task_info.set_customised_preempt_acknowledged(True)
00097                 #complete the state and return customised_preempted
00098                 return 'preempted'
00099             
00100             #if task is resumed
00101             elif not current_task_info.get_pause_required():
00102                 #return to last operation
00103                 try:
00104                     sss.say([current_task_info.speaking_language['Resume']],False)
00105                 except:
00106                     print sys.exc_info()                
00107                 return 'resume'
00108                         
00109             #elif rospy.is_shutdown:
00110             #    return 'preempted' 
00111             
00112             #sleep 1 sec and check again
00113             rospy.sleep(1)
00114             
00115         self.service_preempt()
00116         
00117         return 'preempted'
00118 
00119 
00120 def robot_configuration(parent, action_name, action_stage):
00121     
00122     global current_task_info
00123     global component_list
00124     global robot_config
00125     global robot_config_need_no_action
00126     global sss;
00127     
00128     handles = list()
00129     sss.set_light("yellow")
00130     sss.sleep(2)
00131     
00132     if action_name == 'navigation':
00133         if current_task_info.object_on_tray: 
00134             if current_task_info.object_in_hand:
00135                 action_name = 'navigation_object_on_tray'
00136             else:
00137                 action_name = 'navigation_object_on_tray_and_sdh'
00138         else:
00139             if current_task_info.object_in_hand:
00140                 action_name = 'navigation_object_in_sdh'
00141             else:
00142                 action_name = 'navigation_no_object'
00143     try:
00144         #bring robot into the pre-configuration state     
00145         if action_stage == 'pre-config':
00146             #initial the sss handles
00147             for index in range(len(component_list)):
00148                 if robot_config_pre[action_name][component_list[index]] in robot_config_need_no_action: 
00149                     handles.append(None)
00150                 else:
00151                     if component_list[index] == "tray":
00152                         try:
00153                             service_full_name = '/tray_monitor/occupied'
00154                             #rospy.wait_for_service(service_full_name,rospy.get_param('server_timeout',3))
00155                             rospy.wait_for_service(service_full_name,3)
00156                              # to check if the tray is ocuppied
00157                             is_ocuppied = rospy.ServiceProxy(service_full_name,Trigger)
00158                             resp = is_ocuppied()
00159                             print "###Checking if there is any component on the tray..." 
00160                             print "###is_ocuppied? ", resp
00161                             
00162                             if(resp is not True):
00163                                 print "###The tray is not ocuppied." 
00164                                 handles.append(sss.move(component_list[index], robot_config_pre[action_name][component_list[index]], False))
00165                                 #handle_tray = sss.move("tray","down")
00166                                 #if handle_tray.get_error_code() != 0:
00167                                 #    return 'failed'
00168                                 #handle_sdh = sss.move("sdh","home")
00169                                 #if handle_sdh.get_error_code() != 0:
00170                                 #    return 'failed'
00171                                 #handle_arm = sss.move("arm","folded")
00172                                 #if handle_arm.get_error_code() != 0:
00173                                 #    return 'failed'
00174                                 #handle_arm.wait()
00175                                 # set light
00176                                 sss.set_light("green")
00177                             else:
00178                                 print "###I cannot fold my tray, as the tray is ocuppied." 
00179                                 handles.append(None) # needs further check
00180                                 error_message = "%s"%e
00181                                 rospy.logerr("calling <<%s>> service not successfull, error: %s",service_full_name, error_message)
00182                                 sss.set_light("yellow")
00183                         except rospy.ROSException, e:
00184                             error_message = "%s"%e
00185                             rospy.logerr("<<%s>> service not available, error: %s",service_full_name, error_message)
00186                             print "the service /tray_monitor/occupied is not available"
00187                     elif component_list[index] == "arm":
00188                         #handles.append(sss.move_planned(component_list[index], robot_config_pre[action_name][component_list[index]], False))
00189                         handles.append(sss.move(component_list[index], robot_config_pre[action_name][component_list[index]], False))
00190                     else:
00191                         handles.append(sss.move(component_list[index], robot_config_pre[action_name][component_list[index]], False))
00192         #bring robot into the post-configuration state     
00193         if action_stage == 'post-config':
00194             #initial the sss handles
00195             for index in range(len(component_list)):
00196                 if robot_config_post[action_name][component_list[index]] in robot_config_need_no_action: 
00197                     handles.append(None)
00198                 else:
00199                     if component_list[index] == "tray":
00200                         # check if tray service is available
00201                         try:
00202                             service_full_name = '/tray_monitor/occupied'
00203                             #rospy.wait_for_service(service_full_name,rospy.get_param('server_timeout',3))
00204                             rospy.wait_for_service(service_full_name,3)
00205                             
00206                             # to check if the tray is ocuppied
00207                             is_ocuppied = rospy.ServiceProxy(service_full_name,Trigger)
00208                             resp = is_ocuppied()
00209                             print "###Checking if there is any component on the tray..." 
00210                             print "###is_ocuppied? ", resp
00211                             
00212                             if(resp is not True):
00213                                 print "The tray is not ocuppied."
00214                                 handles.append(sss.move(component_list[index], robot_config_pre[action_name][component_list[index]], False))
00215                                 # set light
00216                                 sss.set_light("green")
00217                             else:
00218                                 print "###I cannot fold my tray, as the tray is ocuppied." 
00219                                 handles.append(None)
00220                                 error_message = "%s"%e
00221                                 rospy.logerr("calling <<%s>> service not successfull, error: %s",service_full_name, error_message)
00222                         except rospy.ROSException, e:
00223                             error_message = "%s"%e
00224                             rospy.logerr("<<%s>> service not available, error: %s",service_full_name, error_message)
00225                             print "the service /tray_monitor/occupied is not available"
00226                     elif component_list[index] == "arm":
00227                         #handles.append(sss.move_planned(component_list[index], robot_config_pre[action_name][component_list[index]], False))
00228                         handles.append(sss.move(component_list[index], robot_config_pre[action_name][component_list[index]], False))
00229                     else:
00230                         handles.append(sss.move(component_list[index], robot_config_pre[action_name][component_list[index]], False))    
00231                     
00232     except KeyError:
00233         print("dictionary key is not found in the set of existing keys")    
00234         return failed
00235     except IndexError:
00236         print("Index is out of range")
00237         return failed
00238     except:
00239         print("unexpected error during %s and %s",action_name,action_stage)
00240         return failed
00241         
00242     #wait for action to finish
00243     #for index in range(len(component_list)):
00244         #if handles[index] != None:
00245         for handle in handles:
00246             if handle is not None:
00247                 if parent.preempt_requested():  
00248                     parent.service_preempst()    
00249                     return 'preempted'
00250                 else:                
00251                     #handles[index].wait()
00252                     handle.wait()
00253                 ###########################################################################
00254                 #TO DO 
00255                 #need check the state of the handles. return failed after the handles fails.
00256                 #
00257                 ############################################################################
00258     return 'succeeded'
00259 
00260 class pre_conf(smach.State):
00261     def __init__(self):
00262         smach.State.__init__(self , outcomes=['succeeded', 'failed', 'preempted'], input_keys=['action_name'])
00263     
00264     def execute (self, userdata):    
00265         return robot_configuration(self, userdata.action_name, 'pre-config')
00266     
00267         
00268 class post_conf(smach.State):
00269     def __init__(self):
00270         smach.State.__init__(self , outcomes=['succeeded', 'failed',  'preempted'], input_keys=['action_name'])
00271     
00272     def execute (self, userdata):    
00273         return robot_configuration(self, userdata.action_name, 'post-config')
00274 
00275 
00276 co_sm_pre_conf = smach.Concurrence (outcomes=['succeeded', 'failed', 'stopped', 'preempted', 'paused'],
00277                  default_outcome='failed',
00278                  input_keys=['action_name'],
00279                  child_termination_cb = common_child_term_cb,
00280                  outcome_cb = common_out_cb)
00281 with co_sm_pre_conf: 
00282             smach.Concurrence.add('State_Checking_During_Operation', state_checking_during_operation())
00283             smach.Concurrence.add('MAIN_OPERATION', pre_conf(),
00284                                   remapping={'action_name':'action_name'})
00285             
00286 co_sm_post_conf = smach.Concurrence  (outcomes=['succeeded', 'failed', 'stopped', 'preempted', 'paused'],
00287                  default_outcome='failed',
00288                  input_keys=['action_name'],
00289                  child_termination_cb = common_child_term_cb,
00290                  outcome_cb = common_out_cb)
00291 with co_sm_post_conf: 
00292             smach.Concurrence.add('State_Checking_During_Operation', state_checking_during_operation())
00293             smach.Concurrence.add('MAIN_OPERATION', post_conf(),
00294                                   remapping={'action_name':'action_name'})
00295 
00296 """
00297 #It is impossible to reach paused state as the sss used in pre/post conf checking the pause by itself, and will never return time-out 
00298 class co_sm_pre_conf(smach.Concurrence):
00299     def __init__(self, action_name=''):
00300         smach.Concurrence.__init__(outcomes=['succeeded', 'failed', 'stopped', 'preempted', 'paused'],
00301                  default_outcome='failed',
00302                  child_termination_cb = common_child_term_cb,
00303                  outcome_cb = common_out_cb)
00304         self.action_name=action_name
00305                                   
00306         with self: 
00307             smach.Concurrence.add('State_Checking_During_Operation', state_checking_during_operation())
00308             smach.Concurrence.add('MAIN_OPERATION', preconf(self.action_name))
00309     
00310 #It is impossible to reach paused state as the sss used in pre/post conf checking the pause by itself, and will never return time-out 
00311 class co_sm_post_conf(smach.Concurrence):
00312     def __init__(self, action_name=''):
00313         smach.Concurrence.__init__(outcomes=['succeeded', 'failed', 'stopped', 'preempted', 'paused'],
00314                  default_outcome='failed',
00315                  
00316                  child_termination_cb = common_child_term_cb,
00317                  outcome_cb = common_out_cb)
00318         self.action_name=action_name
00319                                   
00320         with self: 
00321             smach.Concurrence.add('State_Checking_During_Operation', state_checking_during_operation())
00322             smach.Concurrence.add('MAIN_OPERATION', post_conf(self.action_name))
00323 """            
00324 
00325 
00326 """
00327 def add_common_states(parent):
00328     with parent:
00329             smach.StateMachine.add('PRE_CONFIG', co_sm_pre_conf,
00330                     transitions={'succeeded':'ACTION', 'paused':'PAUSED_DURING_PRE_CONFIG', 'failed':'failed', 'preempted':'preempted', 'stopped':'stopped'},
00331                     remapping={'action_name':'action_name'})
00332         
00333             smach.StateMachine.add('POST_CONFIG', co_sm_post_conf,
00334                     transitions={'succeeded':'succeeded', 'paused':'PAUSED_DURING_POST_CONFIG', 'failed':'failed', 'preempted':'preempted', 'stopped':'stopped'},
00335                     remapping={'action_name':'action_name'})
00336             
00337             smach.StateMachine.add('PAUSED_DURING_PRE_CONFIG', state_checking_during_paused(),
00338                     transitions={'resume':'PRE_CONFIG','preempted':'preempted', 'stopped':'stopped'})
00339             
00340             smach.StateMachine.add('PAUSED_DURING_ACTION', state_checking_during_paused(),
00341                     transitions={'resume':'ACTION','preempted':'preempted', 'stopped':'stopped'})
00342             
00343             smach.StateMachine.add('PAUSED_DURING_POST_CONFIG', state_checking_during_paused(),
00344                     transitions={'resume':'POST_CONFIG','preempted':'preempted', 'stopped':'stopped'})    
00345 """   
00346 
00347 
00348 class srs_navigation_operation(smach.StateMachine):
00349     
00350     def __init__(self):    
00351         smach.StateMachine.__init__(self, outcomes=['succeeded', 'not_completed', 'failed', 'stopped', 'preempted'],
00352                                     input_keys=['target_base_pose','semi_autonomous_mode'])
00353         #self.action_name = 'navigation'
00354         self.userdata.action_name = 'navigation'
00355         #add_common_states(self)
00356         
00357         with self:              
00358 
00359             smach.StateMachine.add('PRE_CONFIG', co_sm_pre_conf,
00360                     transitions={'succeeded':'ACTION', 'paused':'PAUSED_DURING_PRE_CONFIG', 'failed':'failed', 'preempted':'preempted', 'stopped':'stopped'},
00361                     remapping={'action_name':'action_name'})
00362             
00363             smach.StateMachine.add('ACTION', co_sm_navigation,
00364                     transitions={'succeeded':'POST_CONFIG', 'not_completed':'not_completed', 'paused':'PAUSED_DURING_ACTION', 'failed':'failed', 'preempted':'preempted', 'stopped':'stopped'},
00365                     remapping={'semi_autonomous_mode':'semi_autonomous_mode','target_base_pose':'target_base_pose'}) 
00366             
00367             smach.StateMachine.add('POST_CONFIG', co_sm_post_conf,
00368                     transitions={'succeeded':'succeeded', 'paused':'PAUSED_DURING_POST_CONFIG', 'failed':'failed', 'preempted':'preempted', 'stopped':'stopped'},
00369                     remapping={'action_name':'action_name'})
00370             
00371             smach.StateMachine.add('PAUSED_DURING_PRE_CONFIG', state_checking_during_paused(),
00372                     transitions={'resume':'PRE_CONFIG','preempted':'preempted', 'stopped':'stopped'})
00373             
00374             smach.StateMachine.add('PAUSED_DURING_ACTION', state_checking_during_paused(),
00375                     transitions={'resume':'ACTION','preempted':'preempted', 'stopped':'stopped'})
00376             
00377             smach.StateMachine.add('PAUSED_DURING_POST_CONFIG', state_checking_during_paused(),
00378                     transitions={'resume':'POST_CONFIG','preempted':'preempted', 'stopped':'stopped'})    
00379     
00380 class srs_put_on_tray_operation(smach.StateMachine):
00381     
00382     def __init__(self):    
00383         smach.StateMachine.__init__(self, outcomes=['succeeded', 'not_completed', 'failed', 'stopped', 'preempted'],
00384                                     input_keys=['grasp_categorisation','surface_distance'])
00385         self.userdata.action_name = 'put_on_tray'
00386         #add_common_states(self)
00387         
00388         with self:
00389             smach.StateMachine.add('PRE_CONFIG', co_sm_pre_conf,
00390                     transitions={'succeeded':'ACTION', 'paused':'PAUSED_DURING_PRE_CONFIG', 'failed':'failed', 'preempted':'preempted', 'stopped':'stopped'},
00391                     remapping={'action_name':'action_name'})
00392             
00393             smach.StateMachine.add('ACTION', co_sm_transfer_to_tray,
00394                     transitions={'succeeded':'POST_CONFIG', 'not_completed':'not_completed', 'paused':'PAUSED_DURING_ACTION', 'failed':'failed', 'preempted':'preempted', 'stopped':'stopped'},
00395                     remapping={'grasp_categorisation':'grasp_categorisation','surface_distance':'surface_distance'})
00396 
00397         
00398             smach.StateMachine.add('POST_CONFIG', co_sm_post_conf,
00399                     transitions={'succeeded':'succeeded', 'paused':'PAUSED_DURING_POST_CONFIG', 'failed':'failed', 'preempted':'preempted', 'stopped':'stopped'},
00400                     remapping={'action_name':'action_name'})
00401             
00402             smach.StateMachine.add('PAUSED_DURING_PRE_CONFIG', state_checking_during_paused(),
00403                     transitions={'resume':'PRE_CONFIG','preempted':'preempted', 'stopped':'stopped'})
00404             
00405             smach.StateMachine.add('PAUSED_DURING_ACTION', state_checking_during_paused(),
00406                     transitions={'resume':'ACTION','preempted':'preempted', 'stopped':'stopped'})
00407             
00408             smach.StateMachine.add('PAUSED_DURING_POST_CONFIG', state_checking_during_paused(),
00409                     transitions={'resume':'POST_CONFIG','preempted':'preempted', 'stopped':'stopped'})    
00410 
00411 class srs_enviroment_update_operation(smach.StateMachine):
00412     
00413     def __init__(self):    
00414         smach.StateMachine.__init__(self, outcomes=['succeeded', 'not_completed', 'failed', 'stopped', 'preempted'],
00415                                     input_keys=['scan_pose_list'])
00416         self.userdata.action_name = 'enviroment_update'
00417         #add_common_states(self)
00418         
00419         with self:
00420             smach.StateMachine.add('PRE_CONFIG', co_sm_pre_conf,
00421                     transitions={'succeeded':'ACTION', 'paused':'PAUSED_DURING_PRE_CONFIG', 'failed':'failed', 'preempted':'preempted', 'stopped':'stopped'},
00422                     remapping={'action_name':'action_name'})
00423 
00424             smach.StateMachine.add('ACTION', co_sm_enviroment_update,
00425                     transitions={'succeeded':'POST_CONFIG', 'not_completed':'not_completed', 'paused':'PAUSED_DURING_ACTION', 'failed':'failed', 'preempted':'preempted', 'stopped':'stopped'},
00426                     remapping={'scan_pose_list':'scan_pose_list'})
00427         
00428             smach.StateMachine.add('POST_CONFIG', co_sm_post_conf,
00429                     transitions={'succeeded':'succeeded', 'paused':'PAUSED_DURING_POST_CONFIG', 'failed':'failed', 'preempted':'preempted', 'stopped':'stopped'},
00430                     remapping={'action_name':'action_name'})
00431             
00432             smach.StateMachine.add('PAUSED_DURING_PRE_CONFIG', state_checking_during_paused(),
00433                     transitions={'resume':'PRE_CONFIG','preempted':'preempted', 'stopped':'stopped'})
00434             
00435             smach.StateMachine.add('PAUSED_DURING_ACTION', state_checking_during_paused(),
00436                     transitions={'resume':'ACTION','preempted':'preempted', 'stopped':'stopped'})
00437             
00438             smach.StateMachine.add('PAUSED_DURING_POST_CONFIG', state_checking_during_paused(),
00439                     transitions={'resume':'POST_CONFIG','preempted':'preempted', 'stopped':'stopped'})    
00440 
00441             
00442 
00443 class srs_detection_operation(smach.StateMachine):
00444     
00445     def __init__(self):    
00446         smach.StateMachine.__init__(self, outcomes=['succeeded', 'not_completed', 'failed', 'stopped', 'preempted'],
00447                                     input_keys=['target_object_name','target_object_id', 'target_workspace_name','semi_autonomous_mode'],
00448                                     output_keys=['target_object_pose','target_object'])
00449         self.userdata.action_name = 'detection'
00450         self.userdata.target_object_pose = Pose()
00451         #add_common_states(self)
00452         
00453         with self:
00454                 
00455             smach.StateMachine.add('PRE_CONFIG', co_sm_pre_conf,
00456                     transitions={'succeeded':'ACTION', 'paused':'PAUSED_DURING_PRE_CONFIG', 'failed':'failed', 'preempted':'preempted', 'stopped':'stopped'},
00457                     remapping={'action_name':'action_name'})        
00458             
00459             smach.StateMachine.add('ACTION', co_sm_detection,
00460                     transitions={'succeeded':'POST_CONFIG', 'not_completed':'not_completed', 'paused':'PAUSED_DURING_ACTION', 'failed':'failed', 'preempted':'preempted', 'stopped':'stopped'},
00461                     remapping={'target_object_name':'target_object_name',
00462                                 'target_object_id':'target_object_id',
00463                                 'target_workspace_name':'target_workspace_name',
00464                                 'semi_autonomous_mode':'semi_autonomous_mode',
00465                                 'target_object_pose':'target_object_pose',
00466                                 'target_object':'target_object'})
00467         
00468             smach.StateMachine.add('POST_CONFIG', co_sm_post_conf,
00469                     transitions={'succeeded':'succeeded', 'paused':'PAUSED_DURING_POST_CONFIG', 'failed':'failed', 'preempted':'preempted', 'stopped':'stopped'},
00470                     remapping={'action_name':'action_name'})
00471             
00472             smach.StateMachine.add('PAUSED_DURING_PRE_CONFIG', state_checking_during_paused(),
00473                     transitions={'resume':'PRE_CONFIG','preempted':'preempted', 'stopped':'stopped'})
00474             
00475             smach.StateMachine.add('PAUSED_DURING_ACTION', state_checking_during_paused(),
00476                     transitions={'resume':'ACTION','preempted':'preempted', 'stopped':'stopped'})
00477             
00478             smach.StateMachine.add('PAUSED_DURING_POST_CONFIG', state_checking_during_paused(),
00479                     transitions={'resume':'POST_CONFIG','preempted':'preempted', 'stopped':'stopped'})    
00480 
00481 class srs_grasp_operation(smach.StateMachine):
00482     
00483     def __init__(self):    
00484         smach.StateMachine.__init__(self, outcomes=['succeeded', 'not_completed', 'failed', 'stopped', 'preempted'],
00485                                     input_keys=['target_object_name','target_object_id','target_object','target_workspace_name','semi_autonomous_mode'],
00486                                     output_keys=['grasp_categorisation','surface_distance'])
00487         self.userdata.action_name = 'grasp'
00488         self.userdata.grasp_categorisation = ""
00489         self.userdata.target_object_old_pose = Pose()
00490         
00491         #add_common_states(self)
00492         
00493         with self:
00494             smach.StateMachine.add('PRE_CONFIG', co_sm_pre_conf,
00495                     transitions={'succeeded':'ACTION', 'paused':'PAUSED_DURING_PRE_CONFIG', 'failed':'failed', 'preempted':'preempted', 'stopped':'stopped'},
00496                     remapping={'action_name':'action_name'})
00497             
00498             smach.StateMachine.add('ACTION', co_sm_new_grasp,
00499                     transitions={'succeeded':'POST_CONFIG', 'not_completed':'not_completed', 'paused':'PAUSED_DURING_ACTION', 'failed':'failed', 'preempted':'preempted', 'stopped':'stopped'},
00500                     remapping={'target_object_name':'target_object_name',
00501                                 'semi_autonomous_mode':'semi_autonomous_mode',
00502                                 'target_object_id':'target_object_id',
00503                                 'target_object':'target_object',
00504                                 'target_workspace_name':'target_workspace_name',
00505                                 'grasp_categorisation':'grasp_categorisation',
00506                                 'surface_distance':'surface_distance'})       
00507              
00508             smach.StateMachine.add('POST_CONFIG', co_sm_post_conf,
00509                                    
00510                     transitions={'succeeded':'succeeded', 'paused':'PAUSED_DURING_POST_CONFIG', 'failed':'failed', 'preempted':'preempted', 'stopped':'stopped'},
00511                     remapping={'action_name':'action_name'})
00512             
00513             smach.StateMachine.add('PAUSED_DURING_PRE_CONFIG', state_checking_during_paused(),
00514                     transitions={'resume':'PRE_CONFIG','preempted':'preempted', 'stopped':'stopped'})
00515             
00516             smach.StateMachine.add('PAUSED_DURING_ACTION', state_checking_during_paused(),
00517                     transitions={'resume':'ACTION','preempted':'preempted', 'stopped':'stopped'})
00518             
00519             smach.StateMachine.add('PAUSED_DURING_POST_CONFIG', state_checking_during_paused(),
00520                     transitions={'resume':'POST_CONFIG','preempted':'preempted', 'stopped':'stopped'})    
00521 
00522 
00523 class srs_old_grasp_operation(smach.StateMachine):
00524     
00525     def __init__(self):    
00526         smach.StateMachine.__init__(self, outcomes=['succeeded', 'not_completed', 'failed', 'stopped', 'preempted'],
00527                                     input_keys=['target_object_name','target_object_id','target_object','semi_autonomous_mode'],
00528                                     output_keys=['grasp_categorisation','surface_distance'])
00529         self.userdata.action_name = 'grasp'
00530         self.userdata.grasp_categorisation = ""
00531         self.userdata.target_object_old_pose = Pose()
00532         
00533         #add_common_states(self)
00534         
00535         with self:
00536             smach.StateMachine.add('PRE_CONFIG', co_sm_pre_conf,
00537                     transitions={'succeeded':'ACTION', 'paused':'PAUSED_DURING_PRE_CONFIG', 'failed':'failed', 'preempted':'preempted', 'stopped':'stopped'},
00538                     remapping={'action_name':'action_name'})
00539             
00540             smach.StateMachine.add('ACTION', co_sm_old_grasp,
00541                     transitions={'succeeded':'POST_CONFIG', 'not_completed':'not_completed', 'paused':'PAUSED_DURING_ACTION', 'failed':'failed', 'preempted':'preempted', 'stopped':'stopped'},
00542                     remapping={'target_object_name':'target_object_name',
00543                                 'semi_autonomous_mode':'semi_autonomous_mode',
00544                                 'target_object_id':'target_object_id',
00545                                 'target_object':'target_object',
00546                                 'grasp_categorisation':'grasp_categorisation',
00547                                 'surface_distance':'surface_distance'})       
00548              
00549             smach.StateMachine.add('POST_CONFIG', co_sm_post_conf,
00550                     transitions={'succeeded':'succeeded', 'paused':'PAUSED_DURING_POST_CONFIG', 'failed':'failed', 'preempted':'preempted', 'stopped':'stopped'},
00551                     remapping={'action_name':'action_name'})
00552             
00553             smach.StateMachine.add('PAUSED_DURING_PRE_CONFIG', state_checking_during_paused(),
00554                     transitions={'resume':'PRE_CONFIG','preempted':'preempted', 'stopped':'stopped'})
00555             
00556             smach.StateMachine.add('PAUSED_DURING_ACTION', state_checking_during_paused(),
00557                     transitions={'resume':'ACTION','preempted':'preempted', 'stopped':'stopped'})
00558             
00559             smach.StateMachine.add('PAUSED_DURING_POST_CONFIG', state_checking_during_paused(),
00560                     transitions={'resume':'POST_CONFIG','preempted':'preempted', 'stopped':'stopped'})