rfid_search.py
Go to the documentation of this file.
00001 #! /usr/bin/python
00002 
00003 import roslib; 
00004 roslib.load_manifest('rfid_people_following')
00005 roslib.load_manifest('std_srvs')
00006 roslib.load_manifest('explore_hrl')
00007 roslib.load_manifest('move_base_msgs')
00008 roslib.load_manifest('actionlib')
00009 roslib.load_manifest('geometry_msgs')
00010 roslib.load_manifest('tf')
00011 roslib.load_manifest('hrl_rfid')
00012 import rospy
00013 
00014 import tf
00015 import tf.transformations as tft
00016 import actionlib
00017 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
00018 import hrl_rfid.ros_M5e_client as rmc
00019 
00020 from rfid_people_following.srv import StringInt32_Int32
00021 from rfid_people_following.srv import String_Int32
00022 from rfid_people_following.srv import Int32_Int32
00023 from rfid_people_following.srv import String_StringArr
00024 from std_srvs.srv import Empty
00025 from geometry_msgs.msg import PointStamped
00026 import actionlib
00027 import explore_hrl.msg
00028 
00029 from geometry_msgs.msg import PoseStamped
00030 from geometry_msgs.msg import Quaternion
00031 
00032 import numpy as np, math
00033 import time
00034 from threading import Thread
00035 import os
00036 
00037 
00038 # A more general form of this should be folded back into ros_M5e_client!
00039 # It also appears in new_servo_node (rfid_people_following)
00040 class rfid_poller( Thread ):
00041     def __init__( self, tagid ):
00042         Thread.__init__( self )
00043         self.reader = rmc.ROS_M5e_Client('ears')
00044         self.listener = tf.TransformListener()
00045         self.listener.waitForTransform('/ear_antenna_left', '/map',
00046                                        rospy.Time(0), timeout = rospy.Duration(100) )
00047         self.listener.waitForTransform('/ear_antenna_right', '/map',
00048                                        rospy.Time(0), timeout = rospy.Duration(100) )
00049 
00050         self.should_run = True
00051         self.should_poll = False
00052         self.data = []
00053         self.tagid = tagid
00054         self.start()
00055 
00056     def transform( self, antname ):
00057         ps = PointStamped()
00058 
00059         ps2 = PointStamped()
00060         ps2.point.x = 0.1
00061 
00062         if antname == 'EleLeftEar':
00063             ps.header.frame_id = '/ear_antenna_left'
00064             ps.header.stamp = rospy.Time( 0 )
00065 
00066             ps2.header.frame_id = '/ear_antenna_left'
00067             ps2.header.stamp = rospy.Time( 0 )
00068         elif antname == 'EleRightEar':
00069             ps.header.frame_id = '/ear_antenna_right'
00070             ps.header.stamp = rospy.Time( 0 )
00071 
00072             ps2.header.frame_id = '/ear_antenna_right'
00073             ps2.header.stamp = rospy.Time( 0 )
00074         else:
00075             rospy.logout( 'Bad ear' )
00076             return False, 0.0, 0.0, 0.0
00077 
00078         try:
00079             ps_map = self.listener.transformPoint( '/map', ps )
00080             x = ps_map.point.x
00081             y = ps_map.point.y
00082             #rospy.logout( 'Done 1' )
00083 
00084             ps2_map = self.listener.transformPoint( '/map', ps2 )
00085             x2 = ps2_map.point.x
00086             y2 = ps2_map.point.y
00087             #rospy.logout( 'Done 1' )
00088             
00089             #rospy.logout( 'Transform Success ' + ps.header.frame_id )
00090             return True, x, y, np.arctan2( y2 - y, x2 - x )
00091         except:
00092             rospy.logout( 'Transform failed! ' + ps.header.frame_id )
00093             return False, 0.0, 0.0, 0.0
00094             
00095 
00096     def start_poller( self ):
00097         # Start appending into self.data
00098         #self.reader.track_mode( self.tagid )
00099         self.should_poll = True
00100 
00101     def stop_poller( self ):
00102         # Stop appending into self.data
00103         #self.reader.stop()
00104         self.should_poll = False
00105 
00106     def run( self ):
00107         rospy.logout( 'rfid_poller: Starting' )
00108         while self.should_run and not rospy.is_shutdown():
00109             if self.should_poll:
00110                 left = self.reader.read('EleLeftEar')[-1]
00111                 success, x, y, ang = self.transform( 'EleLeftEar' )
00112                 if success:
00113                     self.data.append( [left, [x,y,ang]] )
00114 
00115                 right = self.reader.read('EleRightEar')[-1]
00116                 success, x, y, ang = self.transform( 'EleRightEar' )
00117                 if success:
00118                     self.data.append( [right, [x,y,ang]] )
00119             else:
00120                 rospy.sleep( 0.050 )
00121             
00122         try:  # Shut it down to conserve power.  Something of a race condition (exception)
00123             self.reader.stop()
00124         except:
00125             pass
00126         rospy.logout( 'rfid_poller: Exiting' )
00127         
00128     def stop( self ):
00129         # Kill off the poller thread.
00130         self.should_run = False
00131         self.join(5)
00132         if (self.isAlive()):
00133             raise RuntimeError("rfid_poller: Unable to stop thread")
00134 
00135 
00136 # A more general form of this should be folded back into orient_node (rfid_people_following)!
00137 class Flapper( Thread ):
00138     def __init__( self, tagid = 'person      '):
00139         Thread.__init__( self )
00140         rospy.logout('Flapper: Initializing' )
00141         rospy.wait_for_service( '/rfid_orient/flap' )
00142         rospy.logout('Flapper: flap service ready.' )
00143 
00144         self._flap = rospy.ServiceProxy( '/rfid_orient/flap', String_StringArr )
00145         self.flap = lambda : self._flap( tagid )
00146 
00147         self.should_run = True
00148         self.should_flap = False
00149         self.start()
00150 
00151     def start_flapper( self ):
00152         # Start appending into self.data
00153         #self.reader.track_mode( self.tagid )
00154         self.should_flap = True
00155 
00156     def stop_flapper( self ):
00157         # Stop appending into self.data
00158         #self.reader.stop()
00159         self.should_flap = False
00160 
00161     def run( self ):
00162         rospy.logout( 'Flapper: Starting' )
00163         r = rospy.Rate( 10 )
00164         while self.should_run and not rospy.is_shutdown():
00165             if self.should_flap:
00166                 self.flap()
00167             else:
00168                 r.sleep()
00169             
00170         rospy.logout( 'Flapper: Exiting' )
00171         
00172     def stop( self ):
00173         # Kill off the poller thread.
00174         self.should_run = False
00175         self.join(15)
00176         if (self.isAlive()):
00177             raise RuntimeError("Flapper: Unable to stop thread")
00178 
00179 
00180 def navstack( x, y, ang ):
00181     try:
00182         rospy.logout( 'Requesting navstack move to <x,y,ang-deg> %3.3f %3.3f %3.3f.' % (x, y, math.degrees(ang)) )
00183 
00184         client = actionlib.SimpleActionClient( 'move_base', MoveBaseAction )
00185         client.wait_for_server()
00186 
00187         ps = PoseStamped()
00188         ps.header.frame_id = '/map'
00189         ps.header.stamp = rospy.Time(0)
00190         ps.pose.position.x = x
00191         ps.pose.position.y = y
00192         ps.pose.orientation = Quaternion( *tft.quaternion_from_euler( 0.0, 0.0, ang ))
00193 
00194         goal = MoveBaseGoal( ps )
00195         client.send_goal( goal )
00196         rospy.logout( 'Waiting for base to stop moving.' )
00197         client.wait_for_result()
00198         rospy.logout( 'Successfully navigated to desired position.' )
00199         return True
00200     except:
00201         rospy.logout( 'Navstack did not achieve desired position.' )
00202         return False
00203 
00204 
00205 class RFIDSearch():
00206     def __init__( self ):
00207         try:
00208             rospy.init_node( 'rfid_search' )
00209         except:
00210             pass
00211         
00212         rospy.logout( 'rfid_search: Initializing.' )
00213         rospy.wait_for_service( '/rfid_servo/servo' )
00214         rospy.wait_for_service( '/rfid_orient/orient' )
00215         rospy.wait_for_service( '/rfid_orient/flap' )
00216         rospy.wait_for_service( '/rfid_demo/demo' )
00217         #rospy.wait_for_service( '/rfid_gui/select' )
00218         self.explore_act = actionlib.SimpleActionClient('explore', explore_hrl.msg.ExploreAction)
00219         self.explore_act.wait_for_server()
00220         rospy.logout( 'rfid_search: Done Initializing.' )
00221 
00222         self._servo = rospy.ServiceProxy( '/rfid_servo/servo', StringInt32_Int32 )
00223         self.follow1 = lambda : self._servo( 'person      ', 1 ) # Stops at first obs
00224         self.follow = lambda : self._servo( 'person      ', 0 ) # Runs forever
00225 
00226         self._demo = rospy.ServiceProxy( '/rfid_demo/demo', Empty )
00227         self.demo = lambda : self._demo() 
00228 
00229         self._servo_stop = rospy.ServiceProxy( '/rfid_servo/stop_next_obs', Int32_Int32 )
00230         self.servo_toggle = lambda : self._servo_stop( 1 ) 
00231 
00232         self._orient = rospy.ServiceProxy( '/rfid_orient/orient', String_Int32 )
00233         self.orient = lambda tagid: self._orient( tagid )
00234 
00235         self.rp = rfid_poller('person      ')
00236         self.flapper = Flapper()
00237 
00238         rospy.logout( 'rfid_search: ready to go!' )
00239 
00240     def wait_for_finish( self, radius = 2.0 ):
00241         print 'Starting RFID tag scanning'
00242         self.rp.start_poller()
00243         self.flapper.start_flapper()
00244         rospy.sleep( 0.3 )
00245 
00246         print 'Starting Search'
00247         goal = explore_hrl.msg.ExploreGoal( radius = radius )
00248         self.explore_act.send_goal(goal)
00249         rospy.sleep( 0.5 )
00250         self.explore_act.wait_for_result()
00251         res = self.explore_act.get_result()
00252         print 'Search Complete'
00253 
00254         print 'Disabling RFID scanning'
00255         self.flapper.stop_flapper()
00256         self.rp.stop_poller()
00257 
00258         print 'Computing Best Position'
00259         readings =  self.rp.data
00260         print readings
00261         rr = list( self.rp.data ) # rfid_reads: [ [rssi,[x,y,ang]], ...]
00262         rssi = [ r for r,vec in rr ]
00263         max_rssi, max_pose = rr[ np.argmax( rssi ) ]
00264 
00265         print 'Moving to best Position: ', max_pose
00266         navstack( *max_pose )
00267 
00268         print 'Executing Remainder of Demo'
00269         if (os.environ.has_key('ROBOT') and os.environ['ROBOT'] == 'sim'):
00270             self.follow1() # Only do servoing in simulation
00271         else:
00272             try:
00273                 self.demo() # Run the full demo IRL
00274             except: # for some reason, NoneType throws exception...
00275                 pass
00276 
00277         print 'Shutting down threads'
00278         self.rp.stop()
00279         self.flapper.stop()
00280 
00281 
00282 if __name__ == '__main__':
00283     rs = RFIDSearch()
00284     time.sleep( 3 )
00285     rs.wait_for_finish( radius = 1.7 )
00286     #print rs.flap()
00287 
00288 #     while True:
00289 #         print 'DoneSearching: ', rs.doneSearching()


explore_hrl
Author(s): Travis Deyle
autogenerated on Wed Nov 27 2013 11:48:01