def TrackSingleTag(self, tagID, timeout=50):
rospy.sleep(timeout * 0.001)
# 6DOF params -- Calculate these using tf
ant_zp = PointStamped()
ant_zp.header.stamp = rospy.Time(0)
tag_zp = PointStamped()
tag_zp.header.stamp = rospy.Time(0)
tag_zp.header.frame_id = '/tag_gt'
if self.TXport == 1 and self.RXport == 1: # 'EleLeftEar'
ear_frame = '/ear_antenna_left'
else: # 'EleRightEar'
ear_frame = '/ear_antenna_right'
ant_zp.header.frame_id = ear_frame
# should probably catch exceptions and throw big warnings...
tag_in_ant = self.listener.transformPoint(ear_frame, tag_zp)
ant_in_tag = self.listener.transformPoint('/tag_gt', ant_zp)
tag_in_ant_sphere = np.array(
friis.CartToSphere2(tag_in_ant.point.x, tag_in_ant.point.y,
tag_in_ant.point.z))
ant_in_tag_sphere = np.array(
friis.CartToSphere2(ant_in_tag.point.x, ant_in_tag.point.y,
ant_in_tag.point.z))
# Determine Friis Powers
Ptag = friis.pwr_inc_tag(
tag_in_ant_sphere[0], # radius
friis.patch.G,
tag_in_ant_sphere[1],
tag_in_ant_sphere[2],
friis.dipole.G,
ant_in_tag_sphere[1],
ant_in_tag_sphere[2])
Prdr = friis.pwr_inc_rdr(
tag_in_ant_sphere[0], # radius
friis.patch.G,
tag_in_ant_sphere[1],
tag_in_ant_sphere[2],
friis.dipole.G,
ant_in_tag_sphere[1],
ant_in_tag_sphere[2])
Prdr_dBm = friis.WattsToDBm(Prdr)
# Calculate expectation (read/no-read and RSSI if applicable)
sim_read = Ptag > friis.DBmToWatts(-18.0)
sim_rssi_ind = np.sum(Prdr_dBm > d['bins'][:-1]) - 1
sim_rssi_ind = np.clip(
sim_rssi_ind, 0,
sim_rssi_ind) # if less than 0 (power lower than in previous data)
sim_rssi = d['mean_rssi'][sim_rssi_ind]
if sim_read:
return int(sim_rssi) # be a little more accurate by returning int
else:
return -1
def TrackSingleTag(self, tagID, timeout=50):
rospy.sleep( timeout * 0.001 )
# 6DOF params -- Calculate these using tf
ant_zp = PointStamped()
ant_zp.header.stamp = rospy.Time(0)
tag_zp = PointStamped()
tag_zp.header.stamp = rospy.Time(0)
tag_zp.header.frame_id = '/tag_gt'
if self.TXport == 1 and self.RXport == 1: # 'EleLeftEar'
ear_frame = '/ear_antenna_left'
else: # 'EleRightEar'
ear_frame = '/ear_antenna_right'
ant_zp.header.frame_id = ear_frame
# should probably catch exceptions and throw big warnings...
tag_in_ant = self.listener.transformPoint( ear_frame, tag_zp )
ant_in_tag = self.listener.transformPoint( '/tag_gt', ant_zp )
tag_in_ant_sphere = np.array( friis.CartToSphere2( tag_in_ant.point.x,
tag_in_ant.point.y,
tag_in_ant.point.z ))
ant_in_tag_sphere = np.array( friis.CartToSphere2( ant_in_tag.point.x,
ant_in_tag.point.y,
ant_in_tag.point.z ))
# Determine Friis Powers
Ptag = friis.pwr_inc_tag( tag_in_ant_sphere[0], # radius
friis.patch.G, tag_in_ant_sphere[1], tag_in_ant_sphere[2],
friis.dipole.G, ant_in_tag_sphere[1], ant_in_tag_sphere[2] )
Prdr = friis.pwr_inc_rdr( tag_in_ant_sphere[0], # radius
friis.patch.G, tag_in_ant_sphere[1], tag_in_ant_sphere[2],
friis.dipole.G, ant_in_tag_sphere[1], ant_in_tag_sphere[2] )
Prdr_dBm = friis.WattsToDBm( Prdr )
# Calculate expectation (read/no-read and RSSI if applicable)
sim_read = Ptag > friis.DBmToWatts( -18.0 )
sim_rssi_ind = np.sum( Prdr_dBm > d['bins'][:-1] ) - 1
sim_rssi_ind = np.clip( sim_rssi_ind, 0, sim_rssi_ind ) # if less than 0 (power lower than in previous data)
sim_rssi = d['mean_rssi'][sim_rssi_ind]
if sim_read:
return int(sim_rssi) # be a little more accurate by returning int
else:
return -1
def friis_pwr_rdr( reading ):
# Model estimate of P^inc_rdr.
# r_rdr, theta_rdr, phi_rdr, r_tag, theta_tag, phi_tag, rssi, antname, tagid = reading
d_rdr = reading[0]
d_tag = reading[1]
read = reading[2]
d_rot = reading[3]
ps = reading[4]
r_rdr, theta_rdr, phi_rdr = d_rdr
r_tag, theta_tag, phi_tag = d_tag
watts = friis.pwr_inc_rdr( r_rdr,
friis.patch.G, theta_rdr, phi_rdr,
friis.dipole.G, theta_tag, phi_tag )
return friis.WattsToDBm( watts )
