00001
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_people_following')
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_people_following.rotate_backup_node as rb
00027 from rfid_people_following.cmd_process import CmdProcess
00028 from rfid_people_following.srv import String_Int32
00029 from rfid_people_following.srv import String_Int32Response
00030 from rfid_people_following.srv import String_StringArr
00031 from rfid_people_following.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
00041
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
00059
00060
00061
00062 self.data = {}
00063
00064
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
00081 self.rotate_backup_client = rb.RotateBackupClient()
00082
00083
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 String_StringArr,
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
00118
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
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 self.data = {}
00180 tagid = request.data
00181 if tagid == '':
00182 rospy.logout( 'orient_node: capture for tagid: \'\' requested. Using QueryEnv.' )
00183 self.r.query_mode( )
00184 else:
00185 rospy.logout( 'orient_node: capture for tagid: \'%s\' requested' % tagid )
00186 self.r.track_mode( tagid )
00187
00188 forward = False
00189 tilt_angs = [ math.radians( 0.0 ),
00190 math.radians( 0.0 ) ]
00191
00192 for ta in tilt_angs:
00193
00194 self.t_left.move_angle( ta, math.radians( 30.0 ), blocking = False )
00195 self.t_right.move_angle( -1.0 * ta, math.radians( 30.0 ), blocking = False )
00196 while self.t_left.is_moving() or self.t_right.is_moving():
00197 time.sleep(0.01)
00198
00199
00200 if forward:
00201 self.p_left.move_angle( self.EX_1, math.radians( PAN_RATE ), blocking = False )
00202 self.p_right.move_angle( -1.0 * self.EX_1, math.radians( PAN_RATE ), blocking = True )
00203 forward = False
00204 else:
00205 self.p_left.move_angle( -1.0 * self.EX_2, math.radians( PAN_RATE ), blocking = False )
00206 self.p_right.move_angle( self.EX_2, math.radians( PAN_RATE ), blocking = True )
00207 forward = True
00208
00209 while self.p_left.is_moving() or self.p_right.is_moving():
00210 time.sleep(0.01)
00211
00212 time.sleep(0.1)
00213
00214 self.r.stop()
00215 print self.data.keys()
00216
00217
00218 self.p_left.move_angle( self.EX_1, math.radians(10), blocking = False )
00219 self.t_left.move_angle( 0.0, math.radians(10), blocking = False )
00220
00221 self.p_right.move_angle( -1.0 * self.EX_1, math.radians(10), blocking = False )
00222 self.t_right.move_angle( 0.0, math.radians(10), blocking = False )
00223
00224 rospy.logout( 'orient_node: capture completed' )
00225
00226
00227 return [self.data.keys()]
00228
00229
00230
00231
00232 if __name__ == '__main__':
00233 on = OrientNode()
00234 rospy.spin()