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
00020 class DummyClass():
00021 def __init__(self, tagid):
00022 self.tagid = tagid
00023
00024 if __name__ == '__main__':
00025 import optparse
00026 p = optparse.OptionParser()
00027 p.add_option('--fname', action='store', type='string', dest='fname',
00028 help='File name. Should be without extension. [eg. \'trial\']', default='')
00029 p.add_option('--radius', action='store', type='float', dest='radius',
00030 help='Exploration radius in meters.', default=4.0)
00031
00032 opt, args = p.parse_args()
00033
00034 if opt.fname == '':
00035 print 'Fname required'
00036 exit()
00037 fname_base = '/u/travis/svn/robot1/src/projects/rfid_datacapture/src/rfid_datacapture/search_cap/search_aware_home/'
00038 fname = fname_base + opt.fname
00039
00040
00041
00042 sm = smach.StateMachine(outcomes=['succeeded','aborted','preempted'],
00043 input_keys = [ 'bagfile_name',
00044 'bagfile_topics',
00045 'tagid',
00046 'explore_radius' ])
00047 with sm:
00048 smach.StateMachine.add(
00049 'START_BAG_CAPTURE',
00050 ServiceState( '/bag_cap/capture',
00051 BagCapture,
00052 request_slots = ['topics','dest'] ),
00053 remapping = {'topics':'bagfile_topics',
00054 'dest':'bagfile_name'},
00055 transitions = {'succeeded':'SEARCH'})
00056
00057 sm_search = sm_rfid_explore.sm_search()
00058
00059 smach.StateMachine.add(
00060 'SEARCH',
00061 sm_search,
00062 transitions = {'succeeded':'STOP_BAG_CAPTURE'},
00063 remapping = {'tagid':'tagid',
00064 'explore_radius':'explore_radius'})
00065
00066 smach.StateMachine.add(
00067 'STOP_BAG_CAPTURE',
00068 ServiceState( '/bag_cap/capture',
00069 BagCapture,
00070 request = BagCaptureRequest('','') ),
00071 transitions = {'succeeded':'succeeded'})
00072
00073
00074
00075 rospy.init_node('smach_datacap_rfid_explore')
00076
00077 rec = recorder.Recorder( serv_name = 'temp_recorder', node_name = 'temp_recorder_py' )
00078 rec.process_service( None )
00079
00080
00081 sm.userdata.tagid = ''
00082 sm.userdata.explore_radius = opt.radius
00083 sm.userdata.bagfile_name = fname
00084 sm.userdata.bagfile_topics = '/tf /visarr /rfid/ears_reader /rfid/ears_reader_arr /map /robot_pose_ekf/odom_combined'
00085
00086 outcome = sm.execute()
00087
00088 rec.process_service( None )
00089
00090 print 'Saving recorder pickle data.'
00091 util.save_pickle( rec.recorder_data, fname + '_reads.pkl' )
00092
00093 print 'Saving best read locations.'
00094 tagids = ['OrangeMedBot','TravisTVremo','RedBottle ',
00095 'OnMetalKeys ','WhiteMedsBot','BlueMedsBox ',
00096 'TeddyBearToy','CordlessPhon','BlueHairBrus']
00097
00098 for t in tagids:
00099 print '\tTagid: \'%s\'' % t,
00100 tname = t.replace( ' ', '' )
00101
00102 try:
00103 pos = rec.bestvantage( DummyClass( t ))
00104 pos.header.stamp = rospy.Time(0)
00105 dat = pos.__str__() + '\n'
00106
00107 f = open( fname + '_tag_' + tname + '.yaml', 'w' )
00108 f.write( dat )
00109 f.close()
00110 print ' Done.'
00111 except:
00112 print ' NO READS. Done.'
00113 pass
00114