srs_grasp_high_level_statemachines.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 #################################################################
00003 ##\file
00004 #
00005 # \note
00006 # Copyright (c) 2012 \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: April 2012
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 os
00056 
00057 #import states within srs_decision_making
00058 from srs_generic_states import *
00059 
00060 #import states from srs_states package
00061 #simple grasp from IPA
00062 from simple_grasp_states import *
00063 #grasp based on configuration from ROB
00064 
00065 #arm manipulation from IPA
00066 from move_arm_states import *
00067 
00068 try:
00069     #grasp state from ROB, need working openrave
00070     from srs_grasp_states import *
00071 except ImportError:
00072     rospy.logwarn ('Package srs_grasping NOT ready! You can only use simple grasp NOT srs grasp')
00073     # the package is not ready use dummy function instead
00074     from srs_grasp_dummy_states import *
00075     
00076 try:
00077     #assisted arm manipulation from BUT
00078     from arm_manip_states import *
00079 except ImportError:
00080     rospy.logwarn ('Package srs_assisted_arm_navigation NOT ready, You can NOT use srs assisted grasp')
00081     # the package is not working use dummy function instead
00082     from arm_manip_dummy_states import *
00083 
00084 
00085 #import service form semantic KB
00086 from srs_knowledge.srv import *
00087 
00088 #import other state machines
00089 from srs_common_high_level_statemachines import * 
00090 from srs_detection_high_level_statemachines import *
00091 
00092 #from generic_grasp_state import *
00093 
00094 # This is come from srs_object_verification, the name should be updated to avoid confusion
00095 #from generic_states import *
00096 
00097 """
00098 This file contains high level state machines for grasp in decision making.
00099 The actual implementation of the states can be found from 
00100     srs_states packages
00101 
00102 Depend on if there is any user client connected and the type of the user client, 
00103 state machines below may switch between semi-autonomous or fully-autonomous mode mode.
00104 
00105 If semi_autonomous_mode=True Generic states of user interventions will be loaded for error handling
00106 Otherwise, generic states based on semantic kb for error handling will be loaded 
00107 
00108 The default mode is semi_autonomous_mode=False assuming no UI connected to the robot
00109 
00110     sm_simple_grasp()
00111     #simple detection with no user intervention and no environment confirmation
00112 
00113     sm_simple_detection_env()
00114     #simple detection with no user intervention but with environment confirmation
00115     
00116     sm_assisted_detection()
00117     #assisted detection with user intervention and no environment confirmation 
00118     
00119     sm_assisted_detection_env()
00120     #assisted detection with user intervention and environment confirmation    
00121 """
00122 
00123 
00124 ################################################################################
00125 #Simple detection state machine
00126 #
00127 #robot would detect the object autonomously
00128 #no environment confirmation is needed for this detection
00129 #
00130 #if the name/id is empty, then the robot will search all the objects in the field of view
00131 #if the environment confirmation/verification is not needed, the target workspace name is not used
00132 #in the simple detection, only the nearest object to the camera is detected
00133 #
00134 #the id of the object is not used by the detection state yet, this should be changed
00135 ################################################################################
00136 
00137 ################################################################################
00138 #grasp state machine
00139 #
00140 #grasp simple
00141 ################################################################################
00142 class sm_srs_grasp_simple (smach.StateMachine):
00143     def __init__(self):    
00144         smach.StateMachine.__init__(self, 
00145             outcomes=['succeeded', 'not_completed', 'failed', 'preempted'],
00146             input_keys=['target_object_name','target_object_id', 'target_object','target_workspace_name','semi_autonomous_mode'],
00147             output_keys=['grasp_categorisation', 'surface_distance'])
00148         self.userdata.surface_distance = -1000 # default value for surface_distance, if the value is negative, the parameter will be ignored. 
00149               
00150         self.max_retries = 5  # default value for max retries 
00151         try:
00152             self.max_retries = rospy.get_param("srs/common/grasp_max_retries")
00153         except Exception, e:
00154             rospy.INFO('can not read parameter of max retries, use the default value')
00155         
00156         
00157         with self:         
00158             smach.StateMachine.add('SELECT_GRASP-simple', select_simple_grasp(),
00159                     transitions={'succeeded':'GRASP-simple', 'failed':'failed', 'preempted':'preempted'},
00160                     remapping={'grasp_categorisation':'grasp_categorisation', 'object':'target_object'})
00161                 
00162             smach.StateMachine.add('GRASP-simple', simple_grasp(self.max_retries),
00163                     transitions={'succeeded':'succeeded', 'retry':'GRASP-simple', 'no_more_retries':'not_completed', 'failed':'failed','preempted':'preempted'},
00164                     remapping={'object':'target_object', 'grasp_categorisation':'grasp_categorisation'})
00165 
00166 
00167 ################################################################################
00168 #grasp state machine
00169 #
00170 #grasp assisted by remote operator or KB
00171 ################################################################################
00172 class sm_srs_grasp_assisted (smach.StateMachine):
00173     def __init__(self):    
00174         smach.StateMachine.__init__(self, 
00175             outcomes=['succeeded', 'not_completed', 'failed', 'stopped', 'preempted'],
00176             input_keys=['target_object_name','target_object_id', 'target_object','target_workspace_name','semi_autonomous_mode'],
00177             output_keys=['grasp_categorisation', 'surface_distance'])
00178         
00179         self.userdata.grasp_configuration = ""   #list of possible grasp configuration
00180         self.userdata.poses = ""                 #list of pre grasp pose of the above configuration
00181         self.userdata.base_pose = ""             #best base pose for grasp
00182         self.userdata.index_of_the_selected_pose = 1   #the id of the pre grasp which has been reached
00183         self.pose_of_the_target_object = ''
00184         self.bb_of_the_target_object = ''
00185         
00186         self.max_retries = 5  # default value for max retries 
00187         self.detection_type = 'simple'
00188 
00189         try:
00190             self.detection_type = rospy.get_param("srs/detection_type")
00191         except Exception, e:
00192             rospy.INFO('can not read parameter of detection type, use the default value')
00193 
00194         try:
00195             self.max_retries = rospy.get_param("srs/common/grasp_max_retries")
00196         except Exception, e:
00197             rospy.INFO('can not read parameter of max retries, use the default value')
00198         
00199         
00200         with self:          
00201                 #guided grasp with simple detection      
00202                 smach.StateMachine.add('PREPARE', assisted_arm_navigation_prepare(),
00203                     transitions={'succeeded':'GRASP_SELECT', 'failed':'failed', 'preempted':'preempted'},
00204                     remapping={'pose_of_the_target_object':'pose_of_the_target_object', 'bb_of_the_target_object':'bb_of_the_target_object', 'object':'target_object'})
00205                                 
00206                 smach.StateMachine.add('GRASP_SELECT', select_srs_grasp(),
00207                     transitions={'succeeded':'GRASP_MOVE_ARM', 'not_possible':'GRASP_MOVE_ARM', 'failed':'failed', 'preempted':'preempted'},
00208                     remapping={'grasp_configuration':'grasp_configuration', 'poses':'poses', 'object':'target_object', 'target_object_id':'target_object_id'})
00209             
00210                 smach.StateMachine.add('GRASP_MOVE_ARM', move_arm_to_given_positions_assisted(),
00211                     transitions={'succeeded':'GRASP_SRS_GRASP', 'not_completed':'not_completed', 'failed':'failed', 'preempted':'preempted'},
00212                     remapping={'name_of_the_target_object':'target_object_name', 'list_of_target_positions':'poses', 'pose_of_the_target_object':'pose_of_the_target_object', 'bb_of_the_target_object':'bb_of_the_target_object'})
00213             
00214                 smach.StateMachine.add('GRASP_SRS_GRASP', srs_grasp(),
00215                     transitions={'succeeded':'succeeded', 'not_completed':'not_completed', 'failed':'failed','preempted':'preempted'},
00216                     remapping={'grasp_configuration_id':'index_of_the_selected_pose', 'grasp_configuration':'grasp_configuration', 'grasp_categorisation':'grasp_categorisation', 'surface_distance':'surface_distance'})
00217             
00218 
00219 ################################################################################
00220 #grasp state machine
00221 #
00222 #grasp planned by planner
00223 ################################################################################
00224 class sm_srs_grasp_planned (smach.StateMachine):
00225     def __init__(self):    
00226         smach.StateMachine.__init__(self, 
00227             outcomes=['succeeded', 'not_completed', 'failed', 'stopped', 'preempted'],
00228             input_keys=['target_object_name','target_object_id', 'target_object','target_workspace_name','semi_autonomous_mode'],
00229             output_keys=['grasp_categorisation','surface_distance'])
00230         
00231         self.userdata.grasp_configuration = ""   #list of possible grasp configuration
00232         self.userdata.poses = ""                 #list of pre grasp pose of the above configuration
00233         self.userdata.base_pose = ""             #best base pose for grasp
00234         self.userdata.index_of_the_selected_pose = 0   #the id of the pre grasp which has been reached, the default value is the first one
00235         self.userdata.grasp_categorisation = ""
00236         self.max_retries = 5  # default value for max retries 
00237         self.detection_type = 'simple'
00238                    
00239         try:
00240             self.detection_type = rospy.get_param("srs/detection_type")
00241         except Exception, e:
00242             rospy.INFO('can not read parameter of detection type, use the default value')
00243 
00244         try:
00245             self.max_retries = rospy.get_param("srs/common/grasp_max_retries")
00246         except Exception, e:
00247             rospy.INFO('can not read parameter of max retries, use the default value')
00248             
00249         
00250         step_after_grasp_select = 'GRASP_SRS_GRASP'
00251         
00252         self.ipa_arm_navigation = 'False'
00253         
00254         #default value is failed, would be replaced by proper value below:
00255         self.step_after_grasp_select = 'failed'
00256         
00257         try:
00258             self.ipa_arm_navigation = rospy.get_param("srs/ipa_arm_navigation")
00259         except Exception, e:
00260             rospy.INFO('can not read parameter of srs/ipa_arm_navigation, use the default value planned arm navigation disabled')
00261         
00262         if self.ipa_arm_navigation.lower() == 'true':
00263             #move arm planned before grasp
00264             step_after_grasp_select = 'GRASP_MOVE_ARM'
00265         else:
00266             #move arm unplanned before grasp
00267             step_after_grasp_select = 'GRASP_SRS_GRASP'
00268         
00269         
00270         if self.detection_type.lower() == 'simple':
00271             with self:          
00272                 #guided grasp with simple detection        
00273                 smach.StateMachine.add('GRASP_SELECT', select_srs_grasp(),
00274                     transitions={'succeeded':step_after_grasp_select, 'not_possible':'GRASP_BEST_BASE_POSE_ESTIMATION', 'failed':'failed', 'preempted':'preempted'},
00275                     remapping={'grasp_configuration':'grasp_configuration', 'poses':'poses', 'object':'target_object', 'target_object_id':'target_object_id'})
00276             
00277                 smach.StateMachine.add('GRASP_MOVE_ARM', move_arm_planned(),
00278                     transitions={'succeeded':'GRASP_SRS_GRASP', 'not_completed':'GRASP_SRS_GRASP', 'failed':'failed', 'preempted':'preempted'},
00279                     remapping={'poses':'poses', 'index_of_the_selected_pose':'pose_id'})
00280             
00281                 smach.StateMachine.add('GRASP_SRS_GRASP', srs_grasp(),
00282                     transitions={'succeeded':'succeeded', 'not_completed':'not_completed', 'failed':'failed','preempted':'preempted', 'retry':'GRASP_BEST_BASE_POSE_ESTIMATION'},
00283                     remapping={'grasp_configuration_id':'index_of_the_selected_pose', 'grasp_configuration':'grasp_configuration', 'grasp_categorisation':'grasp_categorisation', 'surface_distance':'surface_distance' })
00284             
00285                 smach.StateMachine.add('GRASP_BEST_BASE_POSE_ESTIMATION', grasp_base_pose_estimation(),
00286                     transitions={'retry':'GRASP_MOVE_BASE', 'not_retry':'not_completed', 'failed':'failed','preempted':'preempted'},
00287                     remapping={'object':'target_object', 'target_workspace_name':'target_workspace_name', 'base_pose':'base_pose'})
00288                 
00289                 smach.StateMachine.add('GRASP_MOVE_BASE', approach_pose_without_retry(),
00290                     transitions={'succeeded':'GRASP_DETECT_SIMPLE', 'not_completed':'not_completed', 'failed':'failed','preempted':'preempted'},
00291                     remapping={'object':'target_object', 'grasp_configuration':'grasp_configuration', 'base_pose':'base_pose'})
00292                 
00293                 smach.StateMachine.add('GRASP_DETECT_SIMPLE', sm_simple_detection(),
00294                     transitions={'succeeded':'GRASP_SELECT', 'not_completed':'not_completed', 'failed':'failed','preempted':'preempted'},
00295                     remapping={'target_object':'target_object', 'target_object_name':'target_object_name', 'target_object_id':'target_object_id', 'target_workspace_name':'target_workspace_name'})
00296 
00297         else:
00298             with self: 
00299                 # guided grasp with assisted detection
00300                 smach.StateMachine.add('GRASP_SELECT', select_srs_grasp(),
00301                     transitions={'succeeded':step_after_grasp_select, 'not_possible':'GRASP_BEST_BASE_POSE_ESTIMATION', 'failed':'failed', 'preempted':'preempted'},
00302                     remapping={'grasp_configuration':'grasp_configuration', 'poses':'poses', 'object':'target_object', 'target_object_id':'target_object_id'})
00303             
00304                 smach.StateMachine.add('GRASP_MOVE_ARM', move_arm_planned(),
00305                     transitions={'succeeded':'GRASP_SRS_GRASP', 'not_completed':'GRASP_SRS_GRASP', 'failed':'failed', 'preempted':'preempted'},
00306                     remapping={'poses':'poses', 'index_of_the_selected_pose':'pose_id'})
00307             
00308                 smach.StateMachine.add('GRASP_SRS_GRASP', srs_grasp(),
00309                     transitions={'succeeded':'succeeded', 'not_completed':'not_completed', 'failed':'failed','preempted':'preempted', 'retry':'GRASP_BEST_BASE_POSE_ESTIMATION'},
00310                     remapping={'grasp_configuration_id':'index_of_the_selected_pose', 'grasp_configuration':'grasp_configuration', 'grasp_categorisation':'grasp_categorisation', 'surface_distance':'surface_distance' })
00311             
00312                 smach.StateMachine.add('GRASP_BEST_BASE_POSE_ESTIMATION', grasp_base_pose_estimation(),
00313                     transitions={'retry':'GRASP_MOVE_BASE', 'not_retry':'not_completed', 'failed':'failed','preempted':'preempted'},
00314                     remapping={'object':'target_object', 'target_workspace_name':'target_workspace_name', 'base_pose':'base_pose'})
00315                 
00316                 smach.StateMachine.add('GRASP_MOVE_BASE', approach_pose_without_retry(),
00317                     transitions={'succeeded':'GRASP_DETECT_ASSISTED', 'not_completed':'not_completed', 'failed':'failed','preempted':'preempted'},
00318                     remapping={'object':'target_object', 'grasp_configuration':'grasp_configuration', 'base_pose':'base_pose'})
00319                 
00320                 smach.StateMachine.add('GRASP_DETECT_ASSISTED', sm_assisted_detection(),
00321                     transitions={'succeeded':'GRASP_SELECT', 'not_completed':'not_completed', 'failed':'failed','preempted':'preempted'},
00322                     remapping={'target_object':'target_object', 'target_object_name':'target_object_name', 'target_object_id':'target_object_id', 'target_workspace_name':'target_workspace_name'})
00323             
00324 
00325 


srs_decision_making
Author(s): Renxi Qiu
autogenerated on Mon Oct 6 2014 08:38:32