orient_node.py
Go to the documentation of this file.
00001 #! /usr/bin/python
00002 import time
00003 import roslib
00004 roslib.load_manifest('rospy')
00005 roslib.load_manifest('tf')
00006 roslib.load_manifest('geometry_msgs')
00007 roslib.load_manifest('std_msgs')
00008 roslib.load_manifest('hrl_rfid')
00009 roslib.load_manifest('robotis')
00010 roslib.load_manifest('rfid_behaviors')
00011 import rospy
00012 
00013 import tf
00014 import tf.transformations as tft
00015 from geometry_msgs.msg import Twist
00016 from geometry_msgs.msg import PointStamped
00017 from geometry_msgs.msg import Point
00018 from geometry_msgs.msg import PoseStamped
00019 from geometry_msgs.msg import Quaternion
00020 from geometry_msgs.msg import PoseWithCovarianceStamped
00021 from std_msgs.msg import Float64
00022 
00023 import hrl_rfid.ros_M5e_client as rmc
00024 import robotis.ros_robotis as rr
00025 from hrl_rfid.msg import RFIDreadArr
00026 import rfid_behaviors.rotate_backup_node as rb
00027 from rfid_behaviors.cmd_process import CmdProcess
00028 from rfid_behaviors.srv import String_Int32
00029 from rfid_behaviors.srv import String_Int32Response
00030 from rfid_behaviors.srv import FlapEarsSrv
00031 from rfid_behaviors.srv import StringArr_Int32
00032 
00033 import numpy as np, math
00034 import time
00035 from threading import Thread
00036 from collections import deque
00037 from functools import reduce
00038 
00039 #PAN_RATE = 30.0
00040 PAN_RATE = 10.0  # Used for datacapture.
00041 #PAN_RATE = 3.0 
00042 
00043 def calculate_angle( pt1 ):
00044     return np.arctan2( pt1.point.y, pt1.point.x )
00045 
00046 def standard_rad(t):
00047     if t > 0:
00048         return ((t + np.pi) % (np.pi * 2))  - np.pi
00049     else:
00050         return ((t - np.pi) % (np.pi * -2)) + np.pi
00051 
00052 
00053 class OrientNode(  ):
00054     def __init__( self ):
00055         rospy.logout('orient_node: Initializing')
00056         rospy.init_node('orient_node')
00057 
00058         # After calling "flap ears", data will look something like this:
00059         # { 'TagID1': [[ang,rssi], [ang,rssi], ...]
00060         #   'TagID2': ... }
00061         #  * All angles are in /base_link and rssi's from both antennas
00062         self.data = {}
00063 
00064         # Will be transformed into base frame to determine best turn angle -- results in approximately 5-degrees (max) error for small angle assumption
00065         self.tag_gt = { 'EleLeftEar': PointStamped(), 'EleRightEar': PointStamped() }
00066         self.tag_gt[ 'EleLeftEar' ].header.frame_id = '/ear_antenna_left'
00067         self.tag_gt[ 'EleLeftEar' ].header.stamp = rospy.Time.now()
00068         self.tag_gt[ 'EleLeftEar' ].point.x = 10.0
00069         self.tag_gt[ 'EleRightEar' ].header.frame_id = '/ear_antenna_right'
00070         self.tag_gt[ 'EleRightEar' ].header.stamp = rospy.Time.now()
00071         self.tag_gt[ 'EleRightEar' ].point.x = 10.0
00072 
00073         self.listener = tf.TransformListener()
00074         self.listener.waitForTransform('/base_link', '/ear_antenna_left',
00075                                        rospy.Time(0), timeout = rospy.Duration(100) )
00076         self.listener.waitForTransform('/base_link', '/ear_antenna_right',
00077                                        rospy.Time(0), timeout = rospy.Duration(100) )
00078         rospy.logout('orient_node: Transforms ready')
00079 
00080         # For movement...
00081         self.rotate_backup_client = rb.RotateBackupClient()
00082         
00083         # "Ears" Setup
00084         self.p_left = rr.ROS_Robotis_Client( 'left_pan' )
00085         self.t_left = rr.ROS_Robotis_Client( 'left_tilt' )
00086         self.p_right = rr.ROS_Robotis_Client( 'right_pan' )
00087         self.t_right = rr.ROS_Robotis_Client( 'right_tilt' )
00088     
00089         self.EX_1 = 1.350
00090         self.EX_2 = 0.920 
00091 
00092         self.p_left.move_angle( self.EX_1, math.radians(10), blocking = False )
00093         self.p_right.move_angle( -1.0 * self.EX_1, math.radians(10), blocking = True )
00094         self.t_left.move_angle( 0.0, math.radians(10), blocking = False )
00095         self.t_right.move_angle( 0.0, math.radians(10), blocking = True )
00096         while self.p_left.is_moving() or self.p_right.is_moving():
00097             time.sleep( 0.01 )
00098 
00099         self.bag_pid = None
00100 
00101         self.r = rmc.ROS_M5e_Client('ears')
00102         self.__service_flap = rospy.Service( '/rfid_orient/flap',
00103                                              FlapEarsSrv,
00104                                              self.flap_ears )
00105         self.__service_bag = rospy.Service( '/rfid_orient/bag',
00106                                              StringArr_Int32,
00107                                              self.bag_cap )
00108         self.__service_orient = rospy.Service( '/rfid_orient/orient',
00109                                              String_Int32,
00110                                              self.orient )
00111         self.tag_arr_sub = rospy.Subscriber( '/rfid/ears_reader_arr', 
00112                                              RFIDreadArr, 
00113                                              self.add_tags )
00114         rospy.logout( 'orient_node: Waiting for service calls.' )
00115 
00116     def bag_cap( self, request ):
00117         # request.data => String array
00118         # sample args: ['rosbag', 'record', '/tf', '/rfid/ears_reader_arr', '-o', 'data/data']
00119         if (request.data == [] or request.data[0] == 'kill'):
00120             if self.bag_pid == None:
00121                 rospy.logout( 'orient_node: No open bag to kill.' )
00122             else:
00123                 rospy.logout( 'orient_node: Killing open bag.' )
00124                 self.bag_pid.kill()
00125                 self.bag_pid = None
00126             return int( True )
00127         
00128         s = reduce( lambda x,y: x+' '+y, request.data )
00129         rospy.logout( 'orient_node: Calling CmdProcess with args: %s' % s )
00130         self.bag_pid = CmdProcess( request.data )
00131         self.bag_pid.run()
00132         return int( True )
00133 
00134     def orient( self, request ):
00135         tagid = request.data
00136         if not self.data.has_key( tagid ):
00137             rospy.logout( 'Tag id \'%s\' not found during last scan.' % tagid )
00138             return String_Int32Response( int( False ))
00139         arr = np.array( self.data[ tagid ]).T
00140         arr = arr[:,np.argsort( arr[0] )]
00141         h, bins = np.histogram( arr[0], 36, ( -np.pi, np.pi ))
00142         ind = np.sum(arr[0][:, np.newaxis] > bins, axis = 1) - 1  # Gives indices for data into bins
00143         bin_centers = (bins[:-1] + bins[1:]) / 2.0
00144 
00145         best_dir = 0.0
00146         best_rssi = 0.0
00147         for i in np.unique( ind ):
00148             avg_rssi = np.mean(arr[1,np.argwhere( ind == i )])
00149             if  avg_rssi > best_rssi:
00150                 best_rssi = avg_rssi
00151                 best_dir = bin_centers[i]
00152 
00153         rospy.logout( 'orient_node: Best dir (deg): %2.2f with avg rssi: %2.1f' %
00154                       ( math.degrees(best_dir), best_rssi ))
00155 
00156         self.rotate_backup_client.rotate_backup( best_dir, 0.0 )
00157         return String_Int32Response( int( True ))
00158             
00159 
00160     def add_tags( self, msg ):
00161         for read in msg.arr:
00162             if read.rssi == -1:
00163                 return False
00164 
00165             self.tag_gt[ read.antenna_name ].header.stamp = rospy.Time(0)
00166             try:
00167                 pt = self.listener.transformPoint( '/base_link', 
00168                                                    self.tag_gt[ read.antenna_name ])
00169             except:
00170                 rospy.logout( 'orient_node: Transform failed' )
00171                 return False
00172 
00173             if not self.data.has_key( read.tagID ):
00174                 self.data[ read.tagID ] = []
00175             self.data[ read.tagID ].append([ calculate_angle( pt ), 1.0 * read.rssi ])
00176         return True
00177 
00178     def flap_ears( self, request ):
00179         if request.panrate == 0.0:
00180             rpan_rate = 30.0
00181         else:
00182             rpan_rate = request.panrate
00183         self.data = {}
00184         tagid = request.data
00185         if tagid == '':
00186             rospy.logout( 'orient_node: capture for tagid: \'\' requested. Using QueryEnv.' )
00187             self.r.query_mode( )
00188         else:
00189             rospy.logout( 'orient_node: capture for tagid: \'%s\' requested' % tagid )
00190             self.r.track_mode( tagid )
00191             
00192         forward = False
00193         tilt_angs = [ math.radians( 0.0 ),
00194                       math.radians( 0.0 ) ]
00195 
00196         for ta in tilt_angs:
00197             # Tilt
00198             self.t_left.move_angle( ta, math.radians( 30.0 ), blocking = False )
00199             self.t_right.move_angle( -1.0 * ta, math.radians( 30.0 ), blocking = False )
00200             while self.t_left.is_moving() or self.t_right.is_moving():
00201                 time.sleep(0.01)
00202 
00203             # Pan
00204             if forward:
00205                 self.p_left.move_angle( self.EX_1, math.radians( rpan_rate ), blocking = False )
00206                 self.p_right.move_angle( -1.0 * self.EX_1, math.radians( rpan_rate ), blocking = True )
00207                 forward = False
00208             else:
00209                 self.p_left.move_angle( -1.0 * self.EX_2, math.radians( rpan_rate ), blocking = False )
00210                 self.p_right.move_angle( self.EX_2, math.radians( rpan_rate ), blocking = True )
00211                 forward = True
00212 
00213             while self.p_left.is_moving() or self.p_right.is_moving():
00214                 time.sleep(0.01)
00215 
00216         time.sleep(0.1)
00217 
00218         self.r.stop()
00219         print self.data.keys()
00220 
00221         # Reset / Stow
00222         self.p_left.move_angle( self.EX_1, math.radians(10), blocking = False )
00223         self.t_left.move_angle( 0.0, math.radians(10), blocking = False )
00224     
00225         self.p_right.move_angle( -1.0 * self.EX_1, math.radians(10), blocking = False )
00226         self.t_right.move_angle( 0.0, math.radians(10), blocking = False )
00227 
00228         rospy.logout( 'orient_node: capture completed' )
00229         # print self.data
00230 
00231         return [self.data.keys()]
00232         
00233 
00234 
00235 if __name__ == '__main__':
00236     on = OrientNode()
00237     rospy.spin()


rfid_behaviors
Author(s): Travis Deyle
autogenerated on Wed Nov 27 2013 11:50:14