srs_decision_making: srs_decision_making: srs_high_level

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 import json
00056 #import states within srs_decision_making
00057 from srs_generic_states import *
00058 
00059 
00060 #import states machines
00061 from srs_common_high_level_statemachines import * 
00062 from srs_detection_high_level_statemachines import *
00063 from srs_grasp_high_level_statemachines import *
00064 
00065 #from generic_grasp_state import *
00066 
00067 # This is come from srs_object_verification, the name should be updated to avoid confusion
00068 #from generic_states import *
00069 
00070 """
00071 This file contains high level state machines for decision making.
00072 The actual implementation of the states can be found from 
00073     srs_generic_states.py 
00074     or 
00075     imported from other srs components
00076 
00077 
00078     
00079     
00080 """
00081 
00082 class SRS_StateMachine(smach.StateMachine):
00083     #This container inherits functionality from smach.StateMachine and adds
00084     #some auto-generated call that share information among SRS states 
00085     #
00086     
00087     def __init__(self, outcomes, input_keys=[], output_keys=[]):
00088         super(SRS_StateMachine,self).__init__(outcomes, input_keys, output_keys)
00089     
00090     def customised_initial (self, name_of_the_state):
00091         #initialise userdata for goal
00092         self.userdata.current_sub_task_name=name_of_the_state
00093         
00094         #add necessary cbs
00095         self.register_termination_cb (self.termination_cb, [])
00096         self.register_transition_cb(self.transition_cb, [])
00097         self.register_start_cb(self.start_cb, [])
00098 
00099     def start_cb(self, userdata, intial_state):
00100         global current_task_info
00101         
00102         _feedback=xmsg.ExecutionFeedback()
00103         _feedback.current_state = userdata.current_sub_task_name + ": started"
00104         _feedback.solution_required = False
00105         _feedback.exceptional_case_id = 0
00106         _feedback.json_feedback = ''
00107         if not current_task_info.json_parameters == '':
00108             _feedback.json_feedback = self.get_json_feedback(userdata.current_sub_task_name, "started")         
00109         
00110         current_task_info._srs_as._as.publish_feedback(_feedback)
00111 
00112         rospy.sleep(1)
00113     
00114     def transition_cb (self, userdata, active_states):
00115         pass
00116                       
00117     def termination_cb (self, userdata, active_states, outcome ):
00118         #update the task execution status of the last state / state machine container
00119         global current_task_info      
00120        
00121         _feedback=xmsg.ExecutionFeedback()
00122         _feedback.json_feedback = ''
00123         if not current_task_info.json_parameters == '':
00124             _feedback.json_feedback = self.get_json_feedback(userdata.current_sub_task_name, outcome)        
00125         _feedback.current_state = userdata.current_sub_task_name + ":" + outcome
00126         _feedback.solution_required = False
00127         _feedback.exceptional_case_id = 0
00128                
00129         current_task_info._srs_as._as.publish_feedback(_feedback)
00130         
00131         last_step_info = xmsg.Last_step_info()
00132         last_step_info.step_name = userdata.current_sub_task_name
00133         last_step_info.outcome = outcome
00134         current_task_info.last_step_info.append(last_step_info)
00135         
00136         rospy.sleep(1)
00137 
00138 
00139     def get_json_feedback (self, name_of_the_action, state_of_the_action):
00140         
00141         global current_task_info
00142         
00143         step_id = len (current_task_info.last_step_info) 
00144         
00145         print step_id
00146         
00147         json_feedback_current_action = '"current_action": {"name": "'+ name_of_the_action +'", "state": "' + state_of_the_action + '", "step_id": "'+ str(step_id+1) +'", "target_object": "'+ current_task_info.task_feedback.action_object +'", "parent_object": "'+ current_task_info.task_feedback.action_object_parent +'"'+' }'
00148         
00149         json_feedback_last_action =''
00150         
00151         json_feedback_feedback = '"feedback": {"lang": "'+ current_task_info.language_set +'", "message": "'+ current_task_info.feedback_messages[name_of_the_action] +'"}'
00152                      
00153         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 +'"}'
00154 
00155         if step_id > 0:
00156             json_feedback_last_action = '"last_action": {"name": "'+ current_task_info.last_step_info[step_id-1].step_name +'","outcome": "'+ current_task_info.last_step_info[step_id-1].outcome +'", "step_id": '+ str(step_id) +'}'
00157             return json.dumps ('{' + json_feedback_current_action + ',' + json_feedback_feedback + ',' + json_feedback_last_action + ',' + json_feedback_task + '}')
00158         
00159         else:
00160             return json.dumps ('{' + json_feedback_current_action + ',' + json_feedback_feedback + ',' + json_feedback_task + '}')
00161         
00162         
00163 
00164 ####################################################################################
00165 #Navigation state machine
00166 #        
00167 #assisted navigation, operator or semantic KB could specify intermediate position for final goal
00168 #alternatively, use the approach_pose directly, where robot will re-retry by itself
00169 #
00170 ####################################################################################
00171 class sm_srs_navigation(SRS_StateMachine):
00172         
00173     def __init__(self):    
00174         smach.StateMachine.__init__(self, outcomes=['succeeded', 'not_completed', 'failed', 'preempted'],
00175                                     input_keys=['target_base_pose','semi_autonomous_mode'])
00176         self.customised_initial("sm_srs_navigation")
00177         
00178         with self:
00179                         smach.StateMachine.add('SRS_NAVIGATION', sm_approach_pose_assisted(),
00180                                         transitions={'succeeded':'succeeded', 'not_completed':'not_completed', 'failed':'failed', 'preempted':'preempted'},
00181                                         remapping={'target_base_pose':'target_base_pose', 'semi_autonomous_mode':'semi_autonomous_mode'})
00182                                     
00183 ####################################################################################
00184 #Detection state machine
00185 #        
00186 #assisted or simple with or without environment confirmation
00187 #
00188 ####################################################################################
00189 #detection assisted by remote operator or KB, operator could specify region of interest and scanning pose for detection
00190 
00191 class sm_srs_detection(SRS_StateMachine):
00192     def __init__(self):    
00193         smach.StateMachine.__init__(self, 
00194                                     outcomes=['succeeded', 'not_completed', 'failed', 'preempted'],
00195                                     input_keys=['target_object_name','target_object_id', 'target_workspace_name','semi_autonomous_mode'],
00196                                     output_keys=['target_object','target_object_pose'])
00197         self.customised_initial("sm_srs_detection") 
00198         self.detection_type = 'simple'
00199         self.enviroment_confimation_required = False  
00200         try:
00201             self.detection_type = rospy.get_param("srs/detection_type")
00202             self.enviroment_confimation_required  = rospy.get_param("srs/enviroment_confirmation")
00203         except Exception, e:
00204             rospy.loginfo("Parameter Server not ready, use default value for detection and environment update")        
00205       
00206         # states related to detection
00207         if self.detection_type.lower() == 'assisted':
00208             if self.enviroment_confimation_required == 'False':
00209                 #assisted detection with environment confirmation
00210                 with self:
00211                     smach.StateMachine.add('SM_DETECTION-assisted', sm_assisted_detection(),
00212                                    transitions={'succeeded':'succeeded', 'not_completed':'not_completed', 'failed':'failed','preempted':'preempted'},
00213                                    remapping={'target_object_name':'target_object_name',
00214                                               'target_object_id':'target_object_id',
00215                                               'target_workspace_name':'target_workspace_name',
00216                                               'semi_autonomous_mode':'semi_autonomous_mode',
00217                                                'target_object_pose':'target_object_pose',
00218                                                'target_object':'target_object'})
00219             else:
00220                 #assisted detection without environment confirmation
00221                 with self:
00222                     smach.StateMachine.add('SM_DETECTION-assisted-env', sm_assisted_detection_env(),
00223                                    transitions={'succeeded':'succeeded', 'not_completed':'not_completed', 'failed':'failed','preempted':'preempted'},
00224                                    remapping={'target_object_name':'target_object_name',
00225                                               'target_object_id':'target_object_id',
00226                                               'target_workspace_name':'target_workspace_name',
00227                                               'semi_autonomous_mode':'semi_autonomous_mode',
00228                                                'target_object_pose':'target_object_pose',
00229                                                'target_object':'target_object'})
00230 
00231         else:
00232             if self.enviroment_confimation_required == 'False':
00233                 #assisted detection with environment confirmation
00234                 with self:
00235                     smach.StateMachine.add('SM_DETECTION-simple', sm_simple_detection(),
00236                                    transitions={'succeeded':'succeeded', 'not_completed':'not_completed', 'failed':'failed','preempted':'preempted'},
00237                                    remapping={'target_object_name':'target_object_name',
00238                                               'target_object_id':'target_object_id',
00239                                               'target_workspace_name':'target_workspace_name',
00240                                               'semi_autonomous_mode':'semi_autonomous_mode',
00241                                                'target_object_pose':'target_object_pose',
00242                                                'target_object':'target_object' })            
00243                 
00244             else:
00245                 #assisted detection without environment confirmation
00246                 with self:
00247                     smach.StateMachine.add('SM_DETECTION-simple-env', sm_simple_detection_env(),
00248                                    transitions={'succeeded':'succeeded', 'not_completed':'not_completed', 'failed':'failed','preempted':'preempted'},
00249                                    remapping={'target_object_name':'target_object_name',
00250                                               'target_object_id':'target_object_id',
00251                                               'target_workspace_name':'target_workspace_name',
00252                                               'semi_autonomous_mode':'semi_autonomous_mode',
00253                                                'target_object_pose':'target_object_pose',
00254                                                'target_object':'target_object' })
00255             
00256             
00257 
00258 
00259 
00260 ####################################################################################
00261 #Grasp state machine
00262 #        
00263 #assisted planned or simple grasp (The grasp does not involve detection)
00264 #
00265 #
00266 ####################################################################################
00267 
00268 
00269 class sm_srs_new_grasp(SRS_StateMachine):
00270     def __init__(self):
00271         smach.StateMachine.__init__(self,
00272             outcomes=['succeeded', 'not_completed', 'failed', 'stopped', 'preempted'],
00273             input_keys=['target_object_name','target_object_id','target_object','target_workspace_name','semi_autonomous_mode'],
00274             output_keys=['grasp_categorisation'])
00275         
00276         self.customised_initial("sm_srs_grasp")
00277         
00278         #environment switches for development purpose. Should be assisted by default when the project is completed  
00279         self.grasp_type = 'assisted'
00280         try:
00281             self.grasp_type = rospy.get_param("/srs/grasping_type")
00282         except Exception, e:
00283             rospy.loginfo("Parameter Server not ready, use default value for grasp")
00284         
00285 
00286         if self.grasp_type.lower() == 'planned':
00287             #assisted grasp
00288             with self:
00289                 smach.StateMachine.add('SM_GRASP-planned', sm_srs_grasp_planned(),
00290                                    transitions={'succeeded':'succeeded', 'not_completed':'not_completed', 'failed':'failed','preempted':'preempted'},
00291                                    remapping={'target_object_name':'target_object_name',
00292                                               'semi_autonomous_mode':'semi_autonomous_mode',
00293                                               'target_object_id':'target_object_id',
00294                                               'target_object':'target_object',
00295                                               'target_workspace_name':'target_workspace_name',
00296                                               'grasp_categorisation':'grasp_categorisation'})
00297         
00298         if self.grasp_type.lower() == 'assisted':
00299             #assisted grasp
00300             with self:
00301                 smach.StateMachine.add('SM_GRASP-assisted', sm_srs_grasp_assisted(),
00302                                    transitions={'succeeded':'succeeded', 'not_completed':'not_completed', 'failed':'failed','preempted':'preempted'},
00303                                    remapping={'target_object_name':'target_object_name',
00304                                               'semi_autonomous_mode':'semi_autonomous_mode',    
00305                                               'target_object_id':'target_object_id',
00306                                               'target_object':'target_object',
00307                                               'target_workspace_name':'target_workspace_name',
00308                                               'grasp_categorisation':'grasp_categorisation'})
00309         else:
00310             #simple grasp    
00311             with self:
00312                 smach.StateMachine.add('SM_GRASP-simple', sm_srs_grasp_simple(),
00313                                    transitions={'succeeded':'succeeded', 'not_completed':'not_completed', 'failed':'failed','preempted':'preempted'},
00314                                    remapping={'target_object_name':'target_object_name',
00315                                               'semi_autonomous_mode':'semi_autonomous_mode',
00316                                               'target_object_id':'target_object_id',
00317                                               'target_object':'target_object',
00318                                               'target_workspace_name':'target_workspace_name',
00319                                               'grasp_categorisation':'grasp_categorisation'})
00320 
00321 ################################################################################
00322 #Old grasp for backward compatibility (the grasp include the detection)
00323 #
00324 #
00325 ################################################################################
00326 class sm_srs_old_grasp(SRS_StateMachine):
00327     def __init__(self):
00328         smach.StateMachine.__init__(self,
00329             outcomes=['succeeded', 'not_completed', 'failed', 'stopped', 'preempted'],
00330             input_keys=['target_object_name','target_object_id','target_object','semi_autonomous_mode'],
00331             output_keys=['grasp_categorisation'])
00332         
00333         self.customised_initial("sm_srs_grasp")
00334         
00335         with self:
00336                  smach.StateMachine.add('SM_GRASP-old', sm_pick_object_asisted(),
00337                                    transitions={'succeeded':'succeeded', 'not_completed':'not_completed', 'failed':'failed','preempted':'preempted'},
00338                                    remapping={'target_object_name':'target_object_name',
00339                                               'semi_autonomous_mode':'semi_autonomous_mode',
00340                                               'target_object_id':'target_object_id',
00341                                               'target_object':'target_object',
00342                                               'grasp_categorisation':'grasp_categorisation'})
00343 ################################################################################
00344 #Transfer object to tray state machine
00345 #
00346 #
00347 ################################################################################
00348 class sm_srs_put_on_tray(SRS_StateMachine):
00349     def __init__(self):    
00350         smach.StateMachine.__init__(self, 
00351             outcomes=['succeeded', 'not_completed', 'failed', 'preempted'],
00352             input_keys=['grasp_categorisation'])
00353         
00354         self.customised_initial("sm_put_object_on_tray")
00355         self.userdata.post_table_pos=""
00356     
00357         with self:
00358             smach.StateMachine.add("PUT_ON_TRAY", sm_transfer_object_to_tray(),
00359                 transitions={'succeeded':'succeeded', 'not_completed':'not_completed', 'failed':'failed','preempted':'preempted'},
00360                 remapping={'grasp_categorisation': 'grasp_categorisation'})                  
00361             
00362 ################################################################################
00363 #3D environment update state machine
00364 #
00365 #
00366 ################################################################################    
00367           
00368 class sm_enviroment_update(SRS_StateMachine):
00369     def __init__(self):    
00370         smach.StateMachine.__init__(self,
00371                                     outcomes=['succeeded', 'not_completed', 'failed', 'preempted'],
00372                                     input_keys=['scan_pose_list'],
00373                                     )
00374         self.customised_initial("sm_enviroment_update")
00375         
00376         with self:
00377             
00378             smach.StateMachine.add('UPDATE_ENVIROMENT', sm_enviroment_model_update(),
00379                     transitions={'succeeded':'succeeded', 'not_completed':'not_completed', 'failed':'failed', 'preempted':'preempted'},
00380                     remapping={'scan_pose_list':'scan_pose_list'})     
00381                
00382
srs_high_level_statemachines.py
