srs_common_high_level_statemachines.py
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 # ROS imports
00054 import roslib; roslib.load_manifest('srs_decision_making')
00055 
00056 #import states within srs_decision_making
00057 from srs_generic_states import *
00058 
00059 
00060 #import states from srs_states package
00061 from mapping_states import *
00062 from navigation_states import *
00063 from detection_states  import *
00064 from simple_grasp_states import *
00065 
00066 #from generic_grasp_state import *
00067 
00068 # This is come from srs_object_verification, the name should be updated to avoid confusion
00069 #from generic_states import *
00070 
00071 """
00072 This file contains high level state machines for decision making.
00073 The actual implementation of the states can be found from 
00074     srs_generic_states.py 
00075     or 
00076     imported from other srs components
00077 
00078 Depend on if there is any user client connected and the type of the user client, 
00079 state machines below may switch between semi-autonomous or fully-autonomous mode mode.
00080 
00081 If semi_autonomous_mode=True Generic states of user interventions will be loaded for error handling
00082 Otherwise, generic states based on semantic kb for error handling will be loaded 
00083 
00084 The default mode is semi_autonomous_mode=False assuming no UI connected to the robot
00085 
00086     sm_approach_pose_assisted()
00087     #assisted navigation, operator could specify intermediate position for final goal
00088 
00089     sm_detect_asisted_pose_region()
00090     #detect object, both base pose and region can be adjusted 
00091     
00092     sm_pick_object_asisted()
00093     #pick object with user intervention for error handling
00094     
00095     sm_get_object_on_tray()
00096     #transfer object to tray after pick
00097     
00098     sm_enviroment_object_update
00099     #update the environment object information
00100     
00101     
00102     
00103 """
00104 
00105 ####################################################################################
00106 #Navigation state machine
00107 #        
00108 #assisted navigation, operator or semantic KB could specify intermediate position for final goal
00109 #alternatively, use the approach_pose directly, where robot will re-retry by itself
00110 #
00111 ####################################################################################
00112 class sm_approach_pose_assisted(smach.StateMachine):
00113         
00114     def __init__(self):    
00115         smach.StateMachine.__init__(self, outcomes=['succeeded', 'not_completed', 'failed', 'preempted'],
00116                                     input_keys=['target_base_pose','semi_autonomous_mode'])
00117         self.userdata.intermediate_pose=""
00118         
00119         with self:
00120             smach.StateMachine.add('APPROACH_POSE', approach_pose_without_retry(),
00121                                         transitions={'succeeded':'succeeded', 'not_completed':'not_completed', 'failed':'failed', 'preempted':'preempted'},
00122                                         remapping={'base_pose':'target_base_pose'})
00123             
00124             smach.StateMachine.add('INTERVENTION', intervention_base_pose(),
00125                                         transitions={'retry':'INTERMEDIATE_MOVE', 'no_more_retry':'not_completed','failed':'failed','preempted':'preempted'},
00126                                         remapping={'semi_autonomous_mode':'semi_autonomous_mode','intermediate_pose':'intermediate_pose'})
00127             
00128             """
00129                         smach.StateMachine.add('INTERMEDIATE_MOVE', approach_pose_without_retry(),
00130                                         transitions={'succeeded':'succeeded', 'not_completed':'INTERVENTION', 'failed':'failed', 'preempted':'preempted'},
00131                                         remapping={'base_pose':'intermediate_pose'})
00132             """
00133             
00134             smach.StateMachine.add('INTERMEDIATE_MOVE', approach_pose_without_retry(),
00135                     transitions={'succeeded':'succeeded', 'not_completed':'INTERVENTION', 'failed':'failed', 'preempted':'preempted'},
00136                     remapping={'base_pose':'intermediate_pose'})
00137             
00138 
00139 ################################################################################
00140 #Old grasp state machine
00141 #
00142 #grasp assisted by remote operator or KB
00143 ################################################################################
00144 class sm_pick_object_asisted(smach.StateMachine):
00145     def __init__(self):
00146         smach.StateMachine.__init__(self,
00147             outcomes=['succeeded', 'not_completed', 'failed', 'stopped', 'preempted'],
00148                                     input_keys=['target_object_name','semi_autonomous_mode'],
00149                                     output_keys=['target_object', 'target_object_old_pose', 'grasp_categorisation'])
00150         
00151         self.userdata.grasp_categorisation=""
00152         self.userdata.target_object_old_pose=""
00153         self.userdata.grasp_configuration = ""
00154         
00155         with self:
00156             smach.StateMachine.add('DETECT_OBJECT-simple', detect_object(max_retries=5),
00157                     transitions={'succeeded':'SELECT_GRASP', 'retry':'DETECT_OBJECT-simple', 'no_more_retries':'not_completed', 'failed':'failed', 'preempted':'preempted'},
00158                     remapping={'object_name':'target_object_name', 'object':'target_object', 'object_pose':'target_object_old_pose' })
00159             
00160             
00161             smach.StateMachine.add('SELECT_GRASP', select_simple_grasp(),
00162                     transitions={'succeeded':'GRASP_GENERAL', 'failed':'failed', 'preempted':'preempted'},
00163                     remapping={'grasp_categorisation':'grasp_categorisation', 'object':'target_object'})
00164                 
00165             smach.StateMachine.add('GRASP_GENERAL', simple_grasp(max_retries = 5),
00166                     transitions={'succeeded':'succeeded', 'retry':'DETECT_OBJECT-simple', 'no_more_retries':'not_completed', 'failed':'failed','preempted':'preempted'},
00167                     remapping={'object':'target_object', 'grasp_categorisation':'grasp_categorisation'})
00168  
00169  
00170 ################################################################################
00171 #transfer object to tray state machine
00172 #
00173 #
00174 ################################################################################
00175 class sm_transfer_object_to_tray(smach.StateMachine):
00176     def __init__(self):    
00177         smach.StateMachine.__init__(self, 
00178             outcomes=['succeeded', 'not_completed', 'failed', 'preempted'],
00179                                     input_keys=['grasp_categorisation','surface_distance'])
00180         
00181         self.userdata.post_table_pos=""
00182     
00183         with self:
00184             smach.StateMachine.add("SELECT_POST_TABLE_POS", select_post_table_pose(),
00185                 transitions={'succeeded':'MOVE_TO_POST_TABLE_POS','failed':'failed','preempted':'preempted'},
00186                 remapping={'post_table_pos': 'post_table_pos'})               
00187             smach.StateMachine.add('MOVE_TO_POST_TABLE_POS', approach_pose_without_retry(),
00188                 transitions={'succeeded':'PUT_OBJECT_ON_TRAY','failed':'failed','preempted':'preempted'},
00189                 remapping={'base_pose':'post_table_pos'})                              
00190             smach.StateMachine.add('PUT_OBJECT_ON_TRAY', put_object_on_tray(),
00191                 transitions={'succeeded':'succeeded', 'failed':'failed','preempted':'preempted', 'not_completed':'not_completed'},
00192                 remapping={'grasp_categorisation':'grasp_categorisation', 'surface_distance':'surface_distance'})       
00193             
00194 ################################################################################
00195 #environment object update state machine
00196 #
00197 #
00198 ################################################################################    
00199 
00200 class sm_enviroment_model_update(smach.StateMachine):
00201     def __init__(self):    
00202         smach.StateMachine.__init__(self,
00203                                     outcomes=['succeeded', 'not_completed', 'failed', 'preempted'],
00204                                     input_keys=['target_base_pose_list'],
00205                                     output_keys=['reference_to_map']
00206                                     )
00207         self.userdata.reference_to_map=""
00208         
00209         with self:
00210             smach.StateMachine.add('SELECT_POSE', select_pose(),
00211                     transitions={'got_to_next_pose':'APPROACH_POSE', 'no_more_pose':'succeeded', 'failed':'failed', 'preempted':'preempted'},
00212                     remapping={'base_pose':'current_target_base_pose','target_base_pose_list':'target_base_pose_list'})   
00213             smach.StateMachine.add('APPROACH_POSE', approach_pose_without_retry(),
00214                     transitions={'succeeded':'UPDATE_ENVIROMENT', 'not_completed':'not_completed', 'failed':'failed', 'preempted':'preempted'},
00215                     remapping={'base_pose':'target_base_pose'})     
00216             smach.StateMachine.add('UPDATE_ENVIROMENT', update_env_model(),
00217                     transitions={'succeeded':'SELECT_POSE', 'not_completed':'not_completed', 'failed':'failed', 'preempted':'preempted'},
00218                     remapping={'reference_to_map':'reference_to_map'})   
00219 
00220             
00221 class sm_enviroment_object_update(smach.StateMachine):
00222     def __init__(self):    
00223         smach.StateMachine.__init__(self,
00224                                     outcomes=['succeeded', 'not_completed', 'failed', 'preempted'],
00225                                     input_keys=['target_object_name_list','scan_pose_list'],
00226                                     output_keys=['target_object_pose_list']
00227                                     )
00228         self.userdata.reference_to_map=""
00229         self.userdata.target_object_pose=""
00230         
00231         with self:
00232             
00233             smach.StateMachine.add('MOVE_AND_UPDATE_ENVIROMENT', sm_enviroment_model_update(),
00234                     transitions={'succeeded':'VERIFY_OBJECT', 'not_completed':'not_completed', 'failed':'failed', 'preempted':'preempted'},
00235                     remapping={'scan_pose_list':'scan_pose_list','reference_to_map':'reference_to_map'})     
00236             
00237             smach.StateMachine.add('VERIFY_OBJECT', verify_object(),
00238                     transitions={'object_verified':'succeeded', 'no_object_verified':'not_completed', 'failed':'failed', 'preempted':'preempted'},
00239                     remapping={'reference_to_map':'reference_to_map','object_name_list':'target_object_name_list','updated_object_list':'target_object_pose_list'})      
00240             
00241 ################################################################################
00242 #verify an object update at a single position
00243 #
00244 #
00245 ################################################################################    
00246 
00247             
00248 class sm_enviroment_object_verification_simple(smach.StateMachine):
00249     def __init__(self):    
00250         smach.StateMachine.__init__(self,
00251                                     outcomes=['succeeded', 'not_completed', 'failed', 'preempted'],
00252                                     input_keys=['target_object_hh_id', 'target_object_pose'],
00253                                     output_keys=['verified_target_object_pose']
00254                                     )
00255         self.userdata.angle_range=0.4
00256         
00257         with self:    
00258             smach.StateMachine.add('UPDATE_ENVIROMENT', UpdateEnvMap(),
00259                     transitions={'succeeded':'VERIFY_OBJECT', 'failed':'failed', 'preempted':'preempted'},
00260                     remapping={'angle_range':'angle_range'})  
00261             smach.StateMachine.add('VERIFY_OBJECT', VerifyObject(),
00262                     transitions={'succeeded':'succeeded', 'not_completed':'not_completed', 'failed':'failed', 'preempted':'preempted'},
00263                     remapping={'object_id':'target_object_hh_id',
00264                                'target_object_pose':'target_object_pose',
00265                                'verified_target_object_pose':'verified_target_object_pose'
00266                                })           
00267             
00268 
00269     
00270             


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