def create_time_stamps(self, position):
hydrophone_locations = {
'hydro0': {
'x': self.hydro0[0],
'y': self.hydro0[1],
'z': self.hydro0[2]
},
'hydro1': {
'x': self.hydro1[0],
'y': self.hydro1[1],
'z': self.hydro1[2]
},
'hydro2': {
'x': self.hydro2[0],
'y': self.hydro2[1],
'z': self.hydro2[2]
},
'hydro3': {
'x': self.hydro3[0],
'y': self.hydro3[1],
'z': self.hydro3[2]
}
}
c = 1.484 # millimeters/microsecond
hydrophone_array = ReceiverArraySim(hydrophone_locations, c)
#sonar = Multilaterator(hydrophone_locations, c, 'LS')
pulse = Pulse(position[0], position[1], position[2], 0)
tstamps = hydrophone_array.listen(pulse)
tstamps = tstamps - tstamps[0]
return tstamps
def actual_time_stamps_service(self, data):
hydrophone_locations = {
'hydro0': {'x': self.hydro0[0], 'y': self.hydro0[1], 'z': self.hydro0[2]},
'hydro1': {'x': self.hydro1[0], 'y': self.hydro1[1], 'z': self.hydro1[2]},
'hydro2': {'x': self.hydro2[0], 'y': self.hydro2[1], 'z': self.hydro2[2]},
'hydro3': {'x': self.hydro3[0], 'y': self.hydro3[1], 'z': self.hydro3[2]}}
c = 1.484 # millimeters/microsecond
hydrophone_array = ReceiverArraySim(hydrophone_locations, c)
#sonar = Multilaterator(hydrophone_locations, c, 'LS')
pulse = Pulse(self.position[0], self.position[1], self.position[2], 0)
tstamps = hydrophone_array.listen(pulse)
tstamps = tstamps - tstamps[0]
#converts timestamps to Sec because create_time_stamps uses uSec
for i in range(0,4):
tstamps[i] = tstamps[i]*10**-6
self.tstamps=tstamps
microseconds = [1e6,1e6,1e6,1e6]
self.tstamps = [x * y for x, y in zip(self.tstamps,microseconds)]
actual_time_stamps = list(self.tstamps)
#print actual_time_stamps
return Actual_time_stamps_serviceResponse(actual_time_stamps)
def create_time_stamps(self, position):
hydrophone_locations = {
'hydro0': {'x': self.hydro0[0], 'y': self.hydro0[1], 'z': self.hydro0[2]},
'hydro1': {'x': self.hydro1[0], 'y': self.hydro1[1], 'z': self.hydro1[2]},
'hydro2': {'x': self.hydro2[0], 'y': self.hydro2[1], 'z': self.hydro2[2]},
'hydro3': {'x': self.hydro3[0], 'y': self.hydro3[1], 'z': self.hydro3[2]}}
c = 1.484 # millimeters/microsecond
hydrophone_array = ReceiverArraySim(hydrophone_locations, c)
#sonar = Multilaterator(hydrophone_locations, c, 'LS')
pulse = Pulse(position[0], position[1], position[2], 0)
tstamps = hydrophone_array.listen(pulse)
tstamps = tstamps - tstamps[0]
return tstamps
def actual_time_stamps_service(self, data):
hydro = rospy.ServiceProxy('hydrophones/hydrophone_position', Hydrophone_locations_service)
data = hydro()
self.hydro0 = data.hydro0_xyz
self.hydro1 = data.hydro1_xyz
self.hydro2 = data.hydro2_xyz
self.hydro3 = data.hydro3_xyz
hydrophone_locations = {
'hydro0': {'x': self.hydro0[0], 'y': self.hydro0[1], 'z': self.hydro0[2]},
'hydro1': {'x': self.hydro1[0], 'y': self.hydro1[1], 'z': self.hydro1[2]},
'hydro2': {'x': self.hydro2[0], 'y': self.hydro2[1], 'z': self.hydro2[2]},
'hydro3': {'x': self.hydro3[0], 'y': self.hydro3[1], 'z': self.hydro3[2]}}
c = 1.484 # millimeters/microsecond
hydrophone_array = ReceiverArraySim(hydrophone_locations, c)
#sonar = Multilaterator(hydrophone_locations, c, 'LS')
pos = rospy.ServiceProxy('hydrophones/actual_position', Actual_position)
pos = pos()
self.position = pos.actual_position
print self.position
pulse = Pulse(self.position[0], self.position[1], self.position[2], 0)
tstamps = hydrophone_array.listen(pulse)
tstamps = tstamps - tstamps[0]
#converts timestamps to Sec because create_time_stamps uses uSec
for i in range(0,4):
tstamps[i] = tstamps[i]*10**-6
self.tstamps=tstamps
microseconds = [1e6,1e6,1e6,1e6]
self.tstamps = [x * y for x, y in zip(self.tstamps,microseconds)]
actual_time_stamps = list(self.tstamps)
return Actual_time_stamps_serviceResponse(actual_time_stamps)
def actual_time_stamps_service(self, data):
hydro = rospy.ServiceProxy('hydrophones/hydrophone_position', Hydrophone_locations_service)
data = hydro()
self.hydro0 = data.hydro0_xyz
self.hydro1 = data.hydro1_xyz
self.hydro2 = data.hydro2_xyz
self.hydro3 = data.hydro3_xyz
hydrophone_locations = {
'hydro0': {'x': self.hydro0[0], 'y': self.hydro0[1], 'z': self.hydro0[2]},
'hydro1': {'x': self.hydro1[0], 'y': self.hydro1[1], 'z': self.hydro1[2]},
'hydro2': {'x': self.hydro2[0], 'y': self.hydro2[1], 'z': self.hydro2[2]},
'hydro3': {'x': self.hydro3[0], 'y': self.hydro3[1], 'z': self.hydro3[2]}}
c = 1.484 # millimeters/microsecond
hydrophone_array = ReceiverArraySim(hydrophone_locations, c)
#sonar = Multilaterator(hydrophone_locations, c, 'LS')
pos = rospy.ServiceProxy('hydrophones/actual_position', Actual_position)
pos = pos()
self.position = pos.actual_position
#print self.position
pulse = Pulse(self.position[0], self.position[1], self.position[2], 0)
tstamps = hydrophone_array.listen(pulse)
tstamps = tstamps - tstamps[0]
#converts timestamps to Sec because create_time_stamps uses uSec
for i in range(0,4):
tstamps[i] = tstamps[i]*10**-6
self.tstamps=tstamps
microseconds = [1e6,1e6,1e6,1e6]
self.tstamps = [x * y for x, y in zip(self.tstamps,microseconds)]
actual_time_stamps = list(self.tstamps)
return Actual_time_stamps_serviceResponse(actual_time_stamps)
# pulses will be generated with inside a cube with side-length $(pulse_range) (mm)
try:
for h in range(3,8):
# smallest cube will be a meter wide, largest will be 10 km wide
pulse_range = 10**h # in mm
rand_args = [-pulse_range, pulse_range + 1]
num_pulses = 10
print "\n\x1b[1mGenerating " + str(num_pulses) + " pulses within a " \
+ str(2*pulse_range/1000) + " meters wide cube\x1b[0m\n"
for i in range(num_pulses):
pulse = Pulse(random.randrange(*rand_args),
random.randrange(*rand_args),
random.randrange(*rand_args), 0)
tstamps = hydrophone_array.listen(pulse)
tstamps = tstamps - tstamps[0]
print_green(str(i).ljust(2) + str(pulse))
print "Perfect timestamps: (microseconds)\n\t", tstamps
res_msg = sonar.getPulseLocation(np.array(tstamps))
delete_last_lines(4) # more concise output
res = np.array([res_msg.x, res_msg.y, res_msg.z])
print "\t\x1b[33m".ljust(22) + error(res, pulse.position()) + "\x1b[0m"
print "Progressively adding noise to timestamps..."
for j in range(-5, 2):
sigma = 10**j
noisy_tstamps = [x + np.random.normal(0, sigma) for x in tstamps]
noisy_tstamps[0] = 0
print "Noisy timestamps:\n\t", noisy_tstamps
res_msg = sonar.getPulseLocation(np.array(noisy_tstamps))