00001
00002 import roslib
00003 roslib.load_manifest('rfid_demos')
00004 roslib.load_manifest('hrl_object_fetching')
00005 roslib.load_manifest('hrl_table_detect')
00006 roslib.load_manifest('hrl_trajectory_playback')
00007 roslib.load_manifest('pr2_overhead_grasping')
00008 import rospy
00009
00010 import smach
00011 import actionlib
00012
00013 from smach_ros import SimpleActionState, ServiceState, IntrospectionServer
00014
00015 from hrl_table_detect.srv import DetectTableInst, DetectTableInstRequest
00016 from hrl_table_detect.srv import DetectTableStart, DetectTableStop
00017 from rfid_behaviors.srv import FloatFloat_Int32 as RotateBackupSrv
00018 from rfid_behaviors.srv import FloatFloat_Int32Request as RotateBackupSrvRequest
00019 from rfid_behaviors.srv import HandoffSrv, HandoffSrvRequest
00020 from hrl_trajectory_playback.srv import TrajPlaybackSrv, TrajPlaybackSrvRequest
00021 import pr2_overhead_grasping.sm_overhead_grasp as sm_overhead_grasp
00022 from pr2_controllers_msgs.msg import PointHeadAction, PointHeadGoal
00023 from geometry_msgs.msg import PointStamped
00024
00025 import sm_rfid_servo_approach
00026 import approach_table.sm_approach as sm_approach
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 def sm_fetch_object():
00038
00039 sm = smach.StateMachine(outcomes=['succeeded','aborted','preempted'],
00040 input_keys = [ 'tagid' ])
00041
00042 with sm:
00043
00044 approach = sm_rfid_servo_approach.sm_rfid_servo_approach()
00045 smach.StateMachine.add(
00046 'SERVO_APPROACH',
00047 approach,
00048 remapping = { 'tagid' : 'tagid'},
00049 transitions = { 'succeeded': 'POINT_HEAD' })
00050
00051
00052
00053 pgoal = PointHeadGoal()
00054 pgoal.target.header.frame_id = '/torso_lift_link'
00055 pgoal.target.point.x = 0.50
00056 pgoal.min_duration = rospy.Duration(0.6)
00057 pgoal.max_velocity = 1.0
00058 smach.StateMachine.add(
00059 'POINT_HEAD',
00060 SimpleActionState( '/head_traj_controller/point_head_action',
00061 PointHeadAction,
00062 goal = pgoal ),
00063 transitions = { 'succeeded' : 'PERCEIVE_TABLE' })
00064
00065
00066
00067
00068
00069 smach.StateMachine.add(
00070 'PERCEIVE_TABLE',
00071 ServiceState( '/table_detect_inst',
00072 DetectTableInst,
00073 request = DetectTableInstRequest( 1.0 ),
00074 response_slots = ['grasp_points']),
00075 transitions = {'succeeded':'PERCEIVE_OBJECT'},
00076 remapping = {'grasp_points':'approach_poses'})
00077
00078
00079 smach.StateMachine.add(
00080 'PERCEIVE_OBJECT',
00081 ServiceState( '/obj_segment_inst',
00082 DetectTableInst,
00083 request = DetectTableInstRequest( 1.0 ),
00084 response_slots = ['grasp_points']),
00085 transitions = {'succeeded':'APPROACH_TABLE'},
00086 remapping = {'grasp_points':'object_poses'})
00087
00088
00089
00090 sm_table = sm_approach.sm_approach_table()
00091 smach.StateMachine.add(
00092 'APPROACH_TABLE',
00093 sm_table,
00094 remapping = {'table_edge_poses':'approach_poses',
00095 'movebase_pose_global':'approach_movebase_pose',
00096 'table_edge_global':'table_edge_global'},
00097 transitions = {'succeeded':'REPOINT_HEAD'})
00098
00099
00100 def repoint_goal_cb(userdata, goal):
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112 pgoal = PointHeadGoal()
00113 pgoal.target.header.frame_id = '/torso_lift_link'
00114 pgoal.target.point.x = 0.50
00115 pgoal.target.point.y = 0.0
00116 pgoal.target.point.z = -0.35
00117 pgoal.min_duration = rospy.Duration(0.6)
00118 pgoal.max_velocity = 1.0
00119 return pgoal
00120
00121 smach.StateMachine.add(
00122 'REPOINT_HEAD',
00123 SimpleActionState( '/head_traj_controller/point_head_action',
00124 PointHeadAction,
00125 goal_cb = repoint_goal_cb,
00126 input_keys = ['look_points']),
00127 remapping = {'look_points':'object_poses'},
00128 transitions = { 'succeeded' : 'PREP_UNFOLD' })
00129
00130
00131 smach.StateMachine.add(
00132 'PREP_UNFOLD',
00133 ServiceState( 'rfid_handoff/stow',
00134 HandoffSrv,
00135 request = HandoffSrvRequest()),
00136 transitions = { 'succeeded':'UNFOLD' })
00137
00138
00139 smach.StateMachine.add(
00140 'UNFOLD',
00141 ServiceState( 'traj_playback/unfold',
00142 TrajPlaybackSrv,
00143 request = TrajPlaybackSrvRequest( 0 )),
00144 transitions = { 'succeeded':'MANIPULATE' })
00145
00146
00147 sm_grasp = sm_overhead_grasp.sm_grasp()
00148 smach.StateMachine.add(
00149 'MANIPULATE',
00150 sm_grasp,
00151 transitions = {'succeeded':'BACKUP'})
00152
00153
00154 smach.StateMachine.add(
00155 'BACKUP',
00156 ServiceState( '/rotate_backup',
00157 RotateBackupSrv,
00158 request = RotateBackupSrvRequest(0.0, -0.50)),
00159 transitions = { 'succeeded':'PRE_STOW' })
00160
00161 smach.StateMachine.add(
00162 'PRE_STOW',
00163 ServiceState( 'rfid_handoff/stow_grasp',
00164 HandoffSrv,
00165 request = HandoffSrvRequest()),
00166 transitions = { 'succeeded':'succeeded' })
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184 return sm
00185
00186
00187
00188 if __name__ == '__main__':
00189 rospy.init_node('smach_sm_fetch')
00190
00191 sm = sm_fetch_object()
00192
00193 sis = IntrospectionServer('RFID_fetching', sm, '/SM_FETCHING')
00194 sis.start()
00195
00196
00197
00198 sm.userdata.tagid = 'SpectrMedBot'
00199 outcome = sm.execute()
00200
00201 sis.stop()
00202
00203
00204