Go to the documentation of this file.00001
00002 import roslib
00003 roslib.load_manifest('smach_ros')
00004 roslib.load_manifest('actionlib')
00005 roslib.load_manifest('rfid_datacapture')
00006 roslib.load_manifest('rfid_demos')
00007 roslib.load_manifest('rfid_behaviors')
00008 roslib.load_manifest('hrl_lib')
00009 import rospy
00010
00011 import smach
00012 import actionlib
00013
00014 from smach_ros import SimpleActionState, ServiceState, IntrospectionServer
00015 from rfid_demos import sm_rfid_explore
00016 from rfid_behaviors import recorder
00017 from hrl_lib import util
00018 from rfid_datacapture.srv import BagCapture, BagCaptureRequest
00019 from rfid_demos import sm_rfid_servo_approach
00020
00021 if __name__ == '__main__':
00022 import optparse
00023 p = optparse.OptionParser()
00024 p.add_option('--fname', action='store', type='string', dest='fname',
00025 help='File name. Should be without extension. [eg. \'trial\']', default='')
00026
00027 p.add_option('--tag', action='store', type='string', dest='tagid',
00028 help='Tagid to approach', default='person ')
00029 opt, args = p.parse_args()
00030
00031 if opt.fname == '':
00032 print 'Fname required'
00033 exit()
00034 fname_base = '/u/travis/svn/robot1/src/projects/rfid_datacapture/src/rfid_datacapture/search_cap/search_aware_home/'
00035 fname = fname_base + opt.fname
00036
00037 print 'SERVO APPROACH to ID: \'%s\'' % (opt.tagid)
00038
00039 rospy.init_node('smach_servo_datacapture')
00040
00041 sm = smach.StateMachine(outcomes=['succeeded','aborted','preempted'],
00042 input_keys = [ 'bagfile_name',
00043 'bagfile_topics',
00044 'tagid'])
00045 with sm:
00046 smach.StateMachine.add(
00047 'START_BAG_CAPTURE',
00048 ServiceState( '/bag_cap/capture',
00049 BagCapture,
00050 request_slots = ['topics','dest'] ),
00051 remapping = {'topics':'bagfile_topics',
00052 'dest':'bagfile_name'},
00053 transitions = {'succeeded':'SERVO'})
00054
00055 sm_servo = sm_rfid_servo_approach.sm_rfid_servo_approach()
00056
00057 smach.StateMachine.add(
00058 'SERVO',
00059 sm_servo,
00060 transitions = {'succeeded':'STOP_BAG_CAPTURE'},
00061 remapping = {'tagid':'tagid'})
00062
00063 smach.StateMachine.add(
00064 'STOP_BAG_CAPTURE',
00065 ServiceState( '/bag_cap/capture',
00066 BagCapture,
00067 request = BagCaptureRequest('','') ),
00068 transitions = {'succeeded':'succeeded'})
00069
00070 sm.userdata.tagid = opt.tagid
00071 sm.userdata.bagfile_name = fname + '_servo'
00072 sm.userdata.bagfile_topics = '/tf /rfid/ears_reader /rfid/ears_reader_arr /map /robot_pose_ekf/odom_combined /navigation/cmd_vel'
00073
00074 outcome = sm.execute()
00075