sm_fetch.py
Go to the documentation of this file.
00001 #! /usr/bin/python
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 # from hrl_object_fetching.srv import DetectTable
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 # Overhead grasping requres:
00029 #   run: hrl_pr2_gains/change_gains_grasp.py
00030 #   roslaunch pr2_overhead_grasping overhead_grasping_server.launch
00031 
00032 # Perception requires:
00033 #   roslaunch hrl_pr2_lib openni_kinect.launch
00034 #   roslaunch hrl_object_fetching tabletop_detect.launch
00035 
00036 
00037 def sm_fetch_object():
00038         # Create a SMACH state machine
00039     sm = smach.StateMachine(outcomes=['succeeded','aborted','preempted'],
00040                             input_keys = [ 'tagid' ])
00041 
00042     with sm:
00043         # Servo in towards table
00044         approach = sm_rfid_servo_approach.sm_rfid_servo_approach()
00045         smach.StateMachine.add(
00046             'SERVO_APPROACH', # outcomes: succeeded, aborted, preempted
00047             approach,
00048             remapping = { 'tagid' : 'tagid'}, #input
00049             transitions = { 'succeeded': 'POINT_HEAD' })
00050             
00051 
00052         # Point Head Down (eventaully roll this and perceive table into own sm?)
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         # Detect table and determine possible approach directions!
00066         # requires:
00067         #   roslaunch hrl_pr2_lib  openni_kinect.launch
00068         #   roslaunch hrl_object_fetching tabletop_detect.launch
00069         smach.StateMachine.add(
00070             'PERCEIVE_TABLE',
00071             ServiceState( '/table_detect_inst',
00072                           DetectTableInst,
00073                           request = DetectTableInstRequest( 1.0 ),
00074                           response_slots = ['grasp_points']), # PoseArray
00075             transitions = {'succeeded':'PERCEIVE_OBJECT'},
00076             remapping = {'grasp_points':'approach_poses'})
00077 
00078         # Setment objects
00079         smach.StateMachine.add(
00080             'PERCEIVE_OBJECT',
00081             ServiceState( '/obj_segment_inst',
00082                           DetectTableInst,
00083                           request = DetectTableInstRequest( 1.0 ),
00084                           response_slots = ['grasp_points']), # PoseArray
00085             transitions = {'succeeded':'APPROACH_TABLE'},
00086             remapping = {'grasp_points':'object_poses'}) #output
00087                           
00088 
00089         # Move to the desired approach vector
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', # input (PoseArray)
00095                          'movebase_pose_global':'approach_movebase_pose', # output (PoseStamped)
00096                          'table_edge_global':'table_edge_global'}, # output (PoseStamped)
00097             transitions = {'succeeded':'REPOINT_HEAD'})
00098 
00099         # Re-Point Head to table's edge.
00100         def repoint_goal_cb(userdata, goal):
00101             # Convert PoseStamped in userdata to PointHeadGoal
00102             # mobj = userdata.look_points.poses[0] # We'll have head point to first object.
00103 
00104             # pgoal = PointHeadGoal()
00105             # pgoal.target.header.frame_id = userdata.look_points.header.frame_id
00106             # pgoal.target.point.x = mobj.pose.position.x
00107             # pgoal.target.point.y = mobj.pose.position.y
00108             # pgoal.target.point.z = mobj.pose.position.z
00109             # pgoal.min_duration = rospy.Duration(0.6)
00110             # pgoal.max_velocity = 1.0
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'}, # input (PoseStamped)
00128             transitions = { 'succeeded' : 'PREP_UNFOLD' })
00129 
00130         # Prep unfold
00131         smach.StateMachine.add(
00132             'PREP_UNFOLD',
00133             ServiceState( 'rfid_handoff/stow',
00134                           HandoffSrv,
00135                           request = HandoffSrvRequest()), 
00136             transitions = { 'succeeded':'UNFOLD' })
00137 
00138         # Unfold
00139         smach.StateMachine.add(
00140             'UNFOLD',
00141             ServiceState( 'traj_playback/unfold',
00142                           TrajPlaybackSrv,
00143                           request = TrajPlaybackSrvRequest( 0 )), 
00144             transitions = { 'succeeded':'MANIPULATE' })
00145 
00146         # Manipulate
00147         sm_grasp = sm_overhead_grasp.sm_grasp()
00148         smach.StateMachine.add(
00149             'MANIPULATE',
00150             sm_grasp,
00151             transitions = {'succeeded':'BACKUP'})
00152 
00153         # Backup (60cm)
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         # # Setup robot for further navigation:
00169         # #   fold arms, position head, position ear antennas.
00170         # smach.StateMachine.add(
00171         #     'PRE_STOW',
00172         #     ServiceState( 'rfid_handoff/pre_stow',
00173         #                   HandoffSrv,
00174         #                   request = HandoffSrvRequest()), 
00175         #     transitions = { 'succeeded':'STOW' })
00176         # smach.StateMachine.add(
00177         #     'STOW',
00178         #     ServiceState( 'rfid_handoff/stow',
00179         #                   HandoffSrv,
00180         #                   request = HandoffSrvRequest()), 
00181         #     transitions = { 'succeeded':'succeeded' })
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     #sm.userdata.tagid = 'person      '
00197     #sm.userdata.tagid = 'OrangeMedBot'
00198     sm.userdata.tagid = 'SpectrMedBot'
00199     outcome = sm.execute()
00200     
00201     sis.stop()
00202 
00203     
00204 


rfid_demos
Author(s): Travis Deyle
autogenerated on Wed Nov 27 2013 11:50:51