def __init__(self, tagid):
Thread.__init__(self)
self.reader = rmc.ROS_M5e_Client('ears')
self.reader.track_mode(tagid)
self.should_run = True
self.values = [75.0, 75.0]
self.start()
def __init__(self):
Thread.__init__(self)
self.should_run = True
rospy.logout('servo_node: Initializing')
rospy.init_node('servo_node')
# Unfold "ears" to servo positions
p_left = rr.ROS_Robotis_Client('left_pan')
t_left = rr.ROS_Robotis_Client('left_tilt')
p_left.move_angle(math.radians(-40), math.radians(10), blocking=False)
t_left.move_angle(0.0, math.radians(10), blocking=False)
p_right = rr.ROS_Robotis_Client('right_pan')
t_right = rr.ROS_Robotis_Client('right_tilt')
p_right.move_angle(math.radians(40), math.radians(10), blocking=False)
t_right.move_angle(0.0, math.radians(10), blocking=False)
self.reader = rmc.ROS_M5e_Client('ears')
self.reader.track_mode('person ')
# Use assisted teleop as pseudo-callback to see if valid movement:
self.check_pub = rospy.Publisher('rfid_cmd_vel_check', Twist)
self.sub = rospy.Subscriber("assisted_teleop_response", Twist,
self.callback)
self.command_pub = rospy.Publisher("rfid_cmd_vel", Twist)
self.moving = True
time.sleep(3.0) # give antennas time to settle
self.start()
def process_service( self, req ):
self.should_rec = not self.should_rec # toggle state. (bad way to do this...)
if self.should_rec == True:
self.data = []
self.rec = rmc.ROS_M5e_Client('ears', callbacks = [self.add_datum])
rospy.logout( 'RFID Recorder: Logging Reads.' )
rv = RecorderSrvResponse()
rv.rfid_reads = []
else:
rospy.logout( 'RFID Recorder: Halting recorder.' )
self.rec.unregister() # Stop processing new reads
rospy.sleep( 0.5 ) # Give it some time to settle
rv = RecorderSrvResponse()
rv.rfid_reads = list(self.data) # Save the data.
self.recorder_data = list( self.data )
return rv
def __init__( self, tag_id, sub_name = '/rfid/ears_reader', filt_len = 5 ):
rospy.logout( 'ZedCalc: Initializing' )
# We will process the reads ourself
self.values = { 'EleLeftEar': 75.0,
'EleRightEar': 75.0 }
self.lock = Lock()
self.buff_len = filt_len * 2 # two antennas' worth
self.zed_buff = deque( np.zeros( self.buff_len )) # Angle filter length
self.last_zed = 0.0 # last published command
self.new_zed = 0.0 # projected new command
self._sub = rospy.Subscriber( sub_name, RFIDread, self.callback)
self.reader = rmc.ROS_M5e_Client('ears')
self.reader.track_mode( tag_id )
rospy.logout( 'ZedCalc: Ready.' )
def __init__(self, tagid):
Thread.__init__(self)
self.reader = rmc.ROS_M5e_Client('ears')
self.listener = tf.TransformListener()
self.listener.waitForTransform('/ear_antenna_left',
'/map',
rospy.Time(0),
timeout=rospy.Duration(100))
self.listener.waitForTransform('/ear_antenna_right',
'/map',
rospy.Time(0),
timeout=rospy.Duration(100))
self.should_run = True
self.should_poll = False
self.data = []
self.tagid = tagid
self.start()
def __init__(self):
rospy.logout('orient_node: Initializing')
rospy.init_node('orient_node')
# After calling "flap ears", data will look something like this:
# { 'TagID1': [[ang,rssi], [ang,rssi], ...]
# 'TagID2': ... }
# * All angles are in /base_link and rssi's from both antennas
self.data = {}
# Will be transformed into base frame to determine best turn angle -- results in approximately 5-degrees (max) error for small angle assumption
self.tag_gt = {
'EleLeftEar': PointStamped(),
'EleRightEar': PointStamped()
}
self.tag_gt['EleLeftEar'].header.frame_id = '/ear_antenna_left'
self.tag_gt['EleLeftEar'].header.stamp = rospy.Time.now()
self.tag_gt['EleLeftEar'].point.x = 10.0
self.tag_gt['EleRightEar'].header.frame_id = '/ear_antenna_right'
self.tag_gt['EleRightEar'].header.stamp = rospy.Time.now()
self.tag_gt['EleRightEar'].point.x = 10.0
self.listener = tf.TransformListener()
self.listener.waitForTransform('/base_link',
'/ear_antenna_left',
rospy.Time(0),
timeout=rospy.Duration(100))
self.listener.waitForTransform('/base_link',
'/ear_antenna_right',
rospy.Time(0),
timeout=rospy.Duration(100))
rospy.logout('orient_node: Transforms ready')
# For movement...
self.rotate_backup_client = rb.RotateBackupClient()
# "Ears" Setup
self.p_left = rr.ROS_Robotis_Client('left_pan')
self.t_left = rr.ROS_Robotis_Client('left_tilt')
self.p_right = rr.ROS_Robotis_Client('right_pan')
self.t_right = rr.ROS_Robotis_Client('right_tilt')
self.EX_1 = 1.350
self.EX_2 = 0.920
self.p_left.move_angle(self.EX_1, math.radians(10), blocking=False)
self.p_right.move_angle(-1.0 * self.EX_1,
math.radians(10),
blocking=True)
self.t_left.move_angle(0.0, math.radians(10), blocking=False)
self.t_right.move_angle(0.0, math.radians(10), blocking=True)
while self.p_left.is_moving() or self.p_right.is_moving():
time.sleep(0.01)
self.bag_pid = None
self.r = rmc.ROS_M5e_Client('ears')
self.__service_flap = rospy.Service('/rfid_orient/flap', FlapEarsSrv,
self.flap_ears)
self.__service_bag = rospy.Service('/rfid_orient/bag', StringArr_Int32,
self.bag_cap)
self.__service_orient = rospy.Service('/rfid_orient/orient',
String_Int32, self.orient)
self.tag_arr_sub = rospy.Subscriber('/rfid/ears_reader_arr',
RFIDreadArr, self.add_tags)
rospy.logout('orient_node: Waiting for service calls.')
#! /usr/bin/python
import time
import roslib
roslib.load_manifest('hrl_rfid')
roslib.load_manifest('sound_play')
import rospy
import time
import numpy as np, math
import hrl_rfid.ros_M5e_client as rmc
import sound_play.libsoundplay as lsp
r = rmc.ROS_M5e_Client('ears')
r.query_mode()
speaker = lsp.SoundClient()
i = 0
reads_dict = {}
while not rospy.is_shutdown():
if i % 2 == 0:
read = r.read('EleLeftEar')
else:
read = r.read('EleRightEar')
i = i + 1
print 'read:', read
if read[-1] > 92:
if not reads_dict.has_key(read[-2]):