def __init__(self, source_level, source_bw, array_size, interval, min_snr, angular=False, noise_mean=50, noise_std=5, **kwds) :
Sensor.__init__(self, **kwds)
self._source_level = source_level # dB
self._source_bw = source_bw
self._angles = 360 / array_size
self._interval = interval
self._min_snr = min_snr
self._angular = angular
self._noise_mean = noise_mean
self._noise_std = noise_std
self._elevation = Elevation()
class SonarSensor(Sensor) :
def __init__(self, source_level, source_bw, array_size, interval, min_snr, angular=False, noise_mean=50, noise_std=5, **kwds) :
Sensor.__init__(self, **kwds)
self._source_level = source_level # dB
self._source_bw = source_bw
self._angles = 360 / array_size
self._interval = interval
self._min_snr = min_snr
self._angular = angular
self._noise_mean = noise_mean
self._noise_std = noise_std
self._elevation = Elevation()
def _get_edges(self) :
edges = []
lat, lon, agl = self.get_owner().get_position()
angle = 0
for angle in range(0, 360) :
e_lat, e_lon, e_dist, e_height = self._elevation.get_above(lat, lon, angle)
edges.append([e_lat, e_lon, self._get_snr(e_dist * 1000, 0)])
return edges
def _get_noise(self) :
return random.gauss(self._noise_mean, self._noise_std)
def _get_sound_speed(self, depth) :
# Based on http://en.wikipedia.org/wiki/Speed_of_sound#Seawater.
# T, S, and z will vary by region and shouldn't be hardcoded
t = 25
s = 35
d = abs(depth)
return 1448.96+4.591*t-(5.304e-2)*t**2+(2.374e-4)*t**3+1.340*(s-35)+(1.630e-2)*d+(1.675e-7)*d**2-(1.025e-2)*t*(s-35)-(7.139e-13)*t*d**3
def _get_prop_time(self, src, target) :
x, y = a
distance = math.sqrt((a[0] - b[0])**2 + (a[1] - b[1])**2)
n = distance
theta = math.atan2(b[1] - a[1], b[0] - a[0])
total = (distance / n) / self._get_sound_speed(a[1])
total += (distance / n) / self._get_sound_speed(b[1])
for k in xrange(1, int(n)) :
if a[0] != b[0] :
x += math.cos(theta) * distance / n
if a[1] != b[1] :
y += math.sin(theta) * distance / n
total += 2 * (distance / n) / self._get_sound_speed(y)
return total * (distance / (2*n))
def _get_snr(self, distance, target_strength) :
loss = 10 * math.log(distance, 10) # cylindrical spreading
#print '>', target_strength, distance, loss
snr = self._source_level - 2*loss + target_strength - self._get_noise() - 10 * math.log(self._source_bw, 10)
return snr
def run(self) :
self._edges = self._get_edges()
while True :
detects = self._edges[:]
for entity in self.get_world().get_entities() :
if entity.get_uid() == self.get_owner().get_uid() :
continue
lat, lon, agl = self.get_owner().get_position()
e_lat, e_lon, e_agl = entity.get_position()
if e_agl <= 0 :
#TODO: use prop time
distance = haver_distance(lat, lon, e_lat, e_lon)#math.sqrt(haver_distance(lat, lon, e_lat, e_lon)**2 + (agl - e_agl)**2) * 1000
snr = self._get_snr(distance, entity.get_parameter('sonar_level', 0))
bearing = int(bearing_from_pts(lat, lon, e_lat, e_lon))
# print entity.get_uid(), snr, self._min_snr, distance / 1000.0 < detects[bearing][0], distance / 1000.0, detects[bearing][0]
if distance/1000.0 < detects[bearing][0] and snr >= self._min_snr and distance < 3.0 :
#tlat, tlon = loc_from_bearing_dist(lat, lon, bearing, distance/1000)
# TODO: This removes all error
tlat, tlon = e_lat, e_lon
detects[bearing] = [tlat, tlon, snr]
merged_detects = {}
for detector_start in range(0, 360, self._angles) :
self._publish_data(SonarEvent(self.get_owner().get_uid(), detector_start, detects[detector_start:detector_start + self._angles]))
time.sleep(self._interval)
