srs_decision_making: srs_decision_making: srs_grasp_high_level

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'])
00148               
00149         self.max_retries = 5  # default value for max retries 
00150         try:
00151             self.max_retries = rospy.get_param("srs/common/grasp_max_retries")
00152         except Exception, e:
00153             rospy.INFO('can not read parameter of max retries, use the default value')
00154         
00155         
00156         with self:         
00157             smach.StateMachine.add('SELECT_GRASP-simple', select_simple_grasp(),
00158                     transitions={'succeeded':'GRASP-simple', 'failed':'failed', 'preempted':'preempted'},
00159                     remapping={'grasp_categorisation':'grasp_categorisation', 'object':'target_object'})
00160                 
00161             smach.StateMachine.add('GRASP-simple', simple_grasp(self.max_retries),
00162                     transitions={'succeeded':'succeeded', 'retry':'GRASP-simple', 'no_more_retries':'not_completed', 'failed':'failed','preempted':'preempted'},
00163                     remapping={'object':'target_object', 'grasp_categorisation':'grasp_categorisation'})
00164 
00165 
00166 ################################################################################
00167 #grasp state machine
00168 #
00169 #grasp assisted by remote operator or KB
00170 ################################################################################
00171 class sm_srs_grasp_assisted (smach.StateMachine):
00172     def __init__(self):    
00173         smach.StateMachine.__init__(self, 
00174             outcomes=['succeeded', 'not_completed', 'failed', 'stopped', 'preempted'],
00175             input_keys=['target_object_name','target_object_id', 'target_object','target_workspace_name','semi_autonomous_mode'],
00176             output_keys=['grasp_categorisation'])
00177         
00178         self.userdata.grasp_configuration = ""   #list of possible grasp configuration
00179         self.userdata.poses = ""                 #list of pre grasp pose of the above configuration
00180         self.userdata.base_pose = ""             #best base pose for grasp
00181         self.userdata.index_of_the_selected_pose = 1   #the id of the pre grasp which has been reached
00182         self.pose_of_the_target_object = ''
00183         self.bb_of_the_target_object = ''
00184         
00185         self.max_retries = 5  # default value for max retries 
00186         self.detection_type = 'simple'
00187 
00188         try:
00189             self.detection_type = rospy.get_param("srs/detection_type")
00190         except Exception, e:
00191             rospy.INFO('can not read parameter of detection type, use the default value')
00192 
00193         try:
00194             self.max_retries = rospy.get_param("srs/common/grasp_max_retries")
00195         except Exception, e:
00196             rospy.INFO('can not read parameter of max retries, use the default value')
00197         
00198         
00199         with self:          
00200                 #guided grasp with simple detection      
00201                 smach.StateMachine.add('PREPARE', assisted_arm_navigation_prepare(),
00202                     transitions={'succeeded':'GRASP_SELECT', 'failed':'failed', 'preempted':'preempted'},
00203                     remapping={'pose_of_the_target_object':'pose_of_the_target_object', 'bb_of_the_target_object':'bb_of_the_target_object', 'object':'target_object'})
00204                                 
00205                 smach.StateMachine.add('GRASP_SELECT', select_srs_grasp(),
00206                     transitions={'succeeded':'GRASP_MOVE_ARM', 'not_possible':'GRASP_MOVE_ARM', 'failed':'failed', 'preempted':'preempted'},
00207                     remapping={'grasp_configuration':'grasp_configuration', 'poses':'poses', 'object':'target_object', 'target_object_id':'target_object_id'})
00208             
00209                 smach.StateMachine.add('GRASP_MOVE_ARM', move_arm_to_given_positions_assisted(),
00210                     transitions={'succeeded':'GRASP_SRS_GRASP', 'not_completed':'not_completed', 'failed':'failed', 'preempted':'preempted'},
00211                     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'})
00212             
00213                 smach.StateMachine.add('GRASP_SRS_GRASP', srs_grasp(),
00214                     transitions={'succeeded':'succeeded', 'not_completed':'not_completed', 'failed':'failed','preempted':'preempted'},
00215                     remapping={'grasp_configuration_id':'index_of_the_selected_pose', 'grasp_configuration':'grasp_configuration', 'grasp_categorisation':'grasp_categorisation' })
00216             
00217 
00218 ################################################################################
00219 #grasp state machine
00220 #
00221 #grasp planned by planner
00222 ################################################################################
00223 class sm_srs_grasp_planned (smach.StateMachine):
00224     def __init__(self):    
00225         smach.StateMachine.__init__(self, 
00226             outcomes=['succeeded', 'not_completed', 'failed', 'stopped', 'preempted'],
00227             input_keys=['target_object_name','target_object_id', 'target_object','target_workspace_name','semi_autonomous_mode'],
00228             output_keys=['grasp_categorisation'])
00229         
00230         self.userdata.grasp_configuration = ""   #list of possible grasp configuration
00231         self.userdata.poses = ""                 #list of pre grasp pose of the above configuration
00232         self.userdata.base_pose = ""             #best base pose for grasp
00233         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
00234         self.userdata.grasp_categorisation = ""
00235         self.max_retries = 5  # default value for max retries 
00236         self.detection_type = 'simple'
00237                    
00238         try:
00239             self.detection_type = rospy.get_param("srs/detection_type")
00240         except Exception, e:
00241             rospy.INFO('can not read parameter of detection type, use the default value')
00242 
00243         try:
00244             self.max_retries = rospy.get_param("srs/common/grasp_max_retries")
00245         except Exception, e:
00246             rospy.INFO('can not read parameter of max retries, use the default value')
00247             
00248         
00249         step_after_grasp_select = 'GRASP_SRS_GRASP'
00250         
00251         self.ipa_arm_navigation = 'False'
00252         
00253         #default value is failed, would be replaced by proper value below:
00254         self.step_after_grasp_select = 'failed'
00255         
00256         try:
00257             self.ipa_arm_navigation = rospy.get_param("srs/ipa_arm_navigation")
00258         except Exception, e:
00259             rospy.INFO('can not read parameter of srs/ipa_arm_navigation, use the default value planned arm navigation disabled')
00260         
00261         if self.ipa_arm_navigation.lower() == 'true':
00262             #move arm planned before grasp
00263             step_after_grasp_select = 'GRASP_MOVE_ARM'
00264         else:
00265             #move arm unplanned before grasp
00266             step_after_grasp_select = 'GRASP_SRS_GRASP'
00267         
00268         
00269         if self.detection_type.lower() == 'simple':
00270             with self:          
00271                 #guided grasp with simple detection        
00272                 smach.StateMachine.add('GRASP_SELECT', select_srs_grasp(),
00273                     transitions={'succeeded':step_after_grasp_select, 'not_possible':'GRASP_BEST_BASE_POSE_ESTIMATION', 'failed':'failed', 'preempted':'preempted'},
00274                     remapping={'grasp_configuration':'grasp_configuration', 'poses':'poses', 'object':'target_object', 'target_object_id':'target_object_id'})
00275             
00276                 smach.StateMachine.add('GRASP_MOVE_ARM', move_arm_planned(),
00277                     transitions={'succeeded':'GRASP_SRS_GRASP', 'not_completed':'GRASP_SRS_GRASP', 'failed':'failed', 'preempted':'preempted'},
00278                     remapping={'poses':'poses', 'index_of_the_selected_pose':'pose_id'})
00279             
00280                 smach.StateMachine.add('GRASP_SRS_GRASP', srs_grasp(),
00281                     transitions={'succeeded':'succeeded', 'not_completed':'not_completed', 'failed':'failed','preempted':'preempted'},
00282                     remapping={'grasp_configuration_id':'index_of_the_selected_pose', 'grasp_configuration':'grasp_configuration', 'grasp_categorisation':'grasp_categorisation' })
00283             
00284                 smach.StateMachine.add('GRASP_BEST_BASE_POSE_ESTIMATION', grasp_base_pose_estimation(),
00285                     transitions={'retry':'GRASP_MOVE_BASE', 'not_retry':'not_completed', 'failed':'failed','preempted':'preempted'},
00286                     remapping={'object':'target_object', 'target_workspace_name':'target_workspace_name', 'base_pose':'base_pose'})
00287                 
00288                 smach.StateMachine.add('GRASP_MOVE_BASE', approach_pose_without_retry(),
00289                     transitions={'succeeded':'GRASP_DETECT_SIMPLE', 'not_completed':'not_completed', 'failed':'failed','preempted':'preempted'},
00290                     remapping={'object':'target_object', 'grasp_configuration':'grasp_configuration', 'base_pose':'base_pose'})
00291                 
00292                 smach.StateMachine.add('GRASP_DETECT_SIMPLE', sm_simple_detection(),
00293                     transitions={'succeeded':'GRASP_SELECT', 'not_completed':'not_completed', 'failed':'failed','preempted':'preempted'},
00294                     remapping={'target_object':'target_object', 'target_object_name':'target_object_name', 'target_object_id':'target_object_id', 'target_workspace_name':'target_workspace_name'})
00295 
00296         else:
00297             with self: 
00298                 # guided grasp with assisted detection
00299                 smach.StateMachine.add('GRASP_SELECT', select_srs_grasp(),
00300                     transitions={'succeeded':step_after_grasp_select, 'not_possible':'GRASP_BEST_BASE_POSE_ESTIMATION', 'failed':'failed', 'preempted':'preempted'},
00301                     remapping={'grasp_configuration':'grasp_configuration', 'poses':'poses', 'object':'target_object', 'target_object_id':'target_object_id'})
00302             
00303                 smach.StateMachine.add('GRASP_MOVE_ARM', move_arm_planned(),
00304                     transitions={'succeeded':'GRASP_SRS_GRASP', 'not_completed':'GRASP_SRS_GRASP', 'failed':'failed', 'preempted':'preempted'},
00305                     remapping={'poses':'poses', 'index_of_the_selected_pose':'pose_id'})
00306             
00307                 smach.StateMachine.add('GRASP_SRS_GRASP', srs_grasp(),
00308                     transitions={'succeeded':'succeeded', 'not_completed':'not_completed', 'failed':'failed','preempted':'preempted'},
00309                     remapping={'grasp_configuration_id':'index_of_the_selected_pose', 'grasp_configuration':'grasp_configuration', 'grasp_categorisation':'grasp_categorisation' })
00310             
00311                 smach.StateMachine.add('GRASP_BEST_BASE_POSE_ESTIMATION', grasp_base_pose_estimation(),
00312                     transitions={'retry':'GRASP_MOVE_BASE', 'not_retry':'not_completed', 'failed':'failed','preempted':'preempted'},
00313                     remapping={'object':'target_object', 'target_workspace_name':'target_workspace_name', 'base_pose':'base_pose'})
00314                 
00315                 smach.StateMachine.add('GRASP_MOVE_BASE', approach_pose_without_retry(),
00316                     transitions={'succeeded':'GRASP_DETECT_ASSISTED', 'not_completed':'not_completed', 'failed':'failed','preempted':'preempted'},
00317                     remapping={'object':'target_object', 'grasp_configuration':'grasp_configuration', 'base_pose':'base_pose'})
00318                 
00319                 smach.StateMachine.add('GRASP_DETECT_ASSISTED', sm_assisted_detection(),
00320                     transitions={'succeeded':'GRASP_SELECT', 'not_completed':'not_completed', 'failed':'failed','preempted':'preempted'},
00321                     remapping={'target_object':'target_object', 'target_object_name':'target_object_name', 'target_object_id':'target_object_id', 'target_workspace_name':'target_workspace_name'})
00322             
00323 
00324
srs_grasp_high_level_statemachines.py
