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
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