def getPulseLocation(self, timestamps):
'''
Returns a ros message with the location and time of emission of a pinger pulse.
'''
assert timestamps.size == self.hydrophone_locations.shape[0]
self.timestamps = timestamps
print self.timestamps
init_guess = np.array([0, 0, 0])
for idx in range(1,4):
if(timestamps[idx] > 0):
init_guess = init_guess - self.hydrophone_locations[idx]
else:
init_guess = init_guess + self.hydrophone_locations[idx]
print "Init guess:", init_guess
opt = {'disp': True}
opt_method = 'Nelder-Mead'
result = optimize.minimize(self._getCost, init_guess, method=opt_method, options=opt)
pulse_location = result.x[:3]
if(result.success):
resp_data = SonarResponse()
resp_data.x = result.x[0]
resp_data.y = result.x[1]
resp_data.z = result.x[2]
resp_data.t = -np.sqrt(np.sum(np.square(result.x))) / self.wave_propagation_speed
return resp_data
else:
self.locate_pulse_error_alarm.raise_alarm(
problem_description=("SciPy optimize, using method '" + opt_method
+ "', failed to converge on a pinger pulse location."),
parameters={
'fault_info': {'data': result.message}
}
)
return None
def getPulseLocation(self, timestamps, method=None):
'''
Returns a ros message with the location and time of emission of a pinger pulse.
'''
try:
if method == None:
method = self.method
# print "\x1b[32mMultilateration algorithm:", method, "\x1b[0m"
assert len(self.hydrophone_locations) == len(timestamps)
source = None
if method == 'bancroft':
source = self.estimate_pos_bancroft(timestamps)
elif method == 'LS':
source = self.estimate_pos_LS(timestamps)
else:
print method, "is not an available multilateration algorithm"
return
response = SonarResponse()
response.x = source[0]
response.y = source[1]
response.z = source[2]
print "Reconstructed Pulse:\n\t" + "x: " + str(response.x) + " y: " + str(response.y) \
+ " z: " + str(response.z) + " (mm)"
return response
except KeyboardInterrupt:
print "Source localization interupted, returning all zeroes."
response = SonarResponse()
response.x, response.y, response.z = (0, 0, 0)
def getPulseLocation(self, timestamps, method=None):
'''
Returns a ros message with the location and time of emission of a pinger pulse.
'''
try:
if method == None:
method = self.method
# print "\x1b[32mMultilateration algorithm:", method, "\x1b[0m"
assert len(self.hydrophone_locations) == len(timestamps)
source = None
if method == 'bancroft':
source = self.estimate_pos_bancroft(timestamps)
elif method == 'LS':
source = self.estimate_pos_LS(timestamps)
else:
print method, "is not an available multilateration algorithm"
return
response = SonarResponse()
response.x = source[0]
response.y = source[1]
response.z = source[2]
print "Reconstructed Pulse:\n\t" + "x: " + str(response.x) + " y: " + str(response.y) \
+ " z: " + str(response.z) + " (mm)"
return response
except KeyboardInterrupt:
print "Source localization interupted, returning all zeroes."
response = SonarResponse()
response.x, response.y, response.z = (0, 0, 0)
def getPulseLocation(self, timestamps):
'''
Returns a ros message with the location and time of emission of a pinger pulse.
'''
assert timestamps.size == self.hydrophone_locations.shape[0]
self.timestamps = timestamps
print self.timestamps
init_guess = np.array([0, 0, 0])
for idx in range(1, 4):
if (timestamps[idx] > 0):
init_guess = init_guess - self.hydrophone_locations[idx]
else:
init_guess = init_guess + self.hydrophone_locations[idx]
print "Init guess:", init_guess
opt = {'disp': True}
opt_method = 'Nelder-Mead'
result = optimize.minimize(self._getCost,
init_guess,
method=opt_method,
options=opt)
pulse_location = result.x[:3]
if (result.success):
resp_data = SonarResponse()
resp_data.x = result.x[0]
resp_data.y = result.x[1]
resp_data.z = result.x[2]
resp_data.t = -np.sqrt(np.sum(np.square(
result.x))) / self.wave_propagation_speed
return resp_data
else:
self.locate_pulse_error_alarm.raise_alarm(
problem_description=(
"SciPy optimize, using method '" + opt_method +
"', failed to converge on a pinger pulse location."),
parameters={'fault_info': {
'data': result.message
}})
return None
