def test_negative_index_on_partly_filled_buffer(self): r = RingBuffer(10) for i in range(5): r.append(i) self.assertEqual( r[-1], 4, "Fails to return the most recent datum when " "asked for index -1.")
class AudioBuffer(): def __init__(self, logger): self.ringbuffer = RingBuffer(logger) self.newpoints = 0 self.delay_samples = 0 def data(self, length): return self.ringbuffer.data(length) def data_old(self, length, delay_samples): return self.ringbuffer.data_old(length, delay_samples) def newdata(self): return self.data(self.newpoints) def set_newdata(self, newpoints): self.newpoints = newpoints def data_delayed(self, length): undelayed = self.data(length) delayed = self.data_old(length, self.delay_samples) data = delayed data[1, :] = undelayed[1, :] return data def set_delay_ms(self, delay_ms): self.delay_samples = delay_ms * 1e-3 * FS def data_indexed(self, start, length): return self.ringbuffer.data_indexed(start, length)
class WaterLevel: def __init__(self, port): self.port = port self.uart = UART(port, 9600) self.uart.init(9600, timeout=10) # 9600, 1byte about 1ms, wait for 10ms self.buffer = RingBuffer(10) # return # False: no change # True: change, need read value def Check(self): count = self.uart.any() if count < 3: return False # At lease 3 bytes in buffer (For example:0mm) value_change = None while 1: # maybe too many data in UART RX buffer data = self.uart.readline() if data: number_string = re.search(b'^\d+', data) if number_string: number = int(number_string.group(0)) value_change = self.buffer.InsertData(number, True) else: break return value_change def GetValue(self): return self.buffer.GetAverage()
def __init__( self, size, path, triggered=False, saving=False, channel_folders=None, filename_format="recording_{0}.wav", min_size=None, sampling_rate=44100, parent=None ): """ Parameters ---------- size : int Size of each file to be saved in samples """ super(SoundSaver, self).__init__(parent) self._buffer = RingBuffer() self._save_buffer = RingBuffer() self._idx = 0 self.path = path self.saving = saving self.channel_folders = channel_folders self.triggered = triggered self.min_size = min_size self.sampling_rate = sampling_rate self.filename_format = filename_format self._file_idx = collections.defaultdict(int) self.size = size self._recording = False self._trigger_timer = None
class SmoothText(object): def __init__(self, queue, height, new_message_time, scroll_time): self.queue = queue self.y_step = textAscent() + textDescent() self.y = 0 self.ring = RingBuffer(height / self.y_step) self.last_time = 0 self.new_message_time = float(new_message_time) self.scroll_time = float(scroll_time) self.animation_start_time = 0 def _update_start(self, millis): pass def _update_end(self, millis): pass def _wait_for_message(self, millis): if millis - self.last_time > self.new_message_time: try: new_item = self.queue.get_nowait() self.ring.append(new_item) new_item.color_fader.start(millis) new_item.timestamp = millis self.animation_start_time = millis self._update = self._animate except Queue.Empty: pass def _animate(self, millis): time = (millis - self.animation_start_time) / self.scroll_time self.y = lerp(self.y-self.y_step, self.y, time) if time > 1: self._update = self._wait_for_message _update = _wait_for_message def update(self, millis): self._update_start(millis) self._update(millis) self._update_end(millis) def append(self, message): self.ring.append(message) def draw(self, millis): background(51) items = self.ring.get() (x,self.y) = (0, self.y_step * len(items)) self.update(millis) for item in items: try: if item is not None: fill(item.color) text(item.text, x, self.y) else: text("Something bad happened", x, self.y) except UnicodeDecodeError: text("Unsupported Language Tweeted", x, self.y) self.y-= self.y_step
def test_string_shows_buffer_and_length(self): r = RingBuffer(10) for i in range(20): r.append(i) self.assertEqual( str(r), "<RingBuffer of length 10 and " "[10, 11, 12, 13, 14, 15, 16, 17, 18, 19]>", "Fails to print RingBuffer to spec.")
def test_size(self): buffer = RingBuffer(10) self.assertEqual(buffer.size, 0) buffer.append(42) self.assertEqual(buffer.size, 1) for i in range(11): buffer.append(i) self.assertEqual(buffer.size, 10)
def test_wraps(self): r = RingBuffer(10) for i in range(20): r.append(i) self.assertEqual( r, [10, 11, 12, 13, 14, 15, 16, 17, 18, 19], "Fails to wrap buffer when more data is added " "than the max buffer length.")
def __init__(self, dim, stepper, kernel, obs_var, history_len=100): self.dim = dim self.stepper = stepper self.k = kernel self.history_x = RingBuffer([dim], history_len) self.history_y = RingBuffer([1], history_len) self.obs_var_p = obs_var self.eps = 0.00
def test_write_once(self): """signle write should return number of elements written to buffer""" buffer = RingBuffer(5) items_written = buffer.write([1, 2, 3]) self.assertEqual(items_written, 3, msg="3 items should have been written")
def set_triggered(self, triggered): self.triggered = triggered self._buffer.clear() if self.triggered: self._buffer = RingBuffer( maxlen=int(Settings.DETECTION_BUFFER * self.sampling_rate) ) else: self._buffer = RingBuffer()
def test_write_with_overflow(self): """ if we want to write more items than free space in Buffer, we get only subset of items written in buffer """ buffer = RingBuffer(5) num_items = buffer.write([1, 2, 3]) self.assertEqual(num_items, 3) num_items = buffer.write([4, 5, 6, 7]) self.assertEqual(num_items, 2)
def __init__(self, pin): self.check_ticks_ms = 0 self.ds = DS18X20(OneWire(Pin(pin))) self.rom = None self.buffer = RingBuffer(6) self.state = DS_IDEL roms = self.ds.scan() if roms: self.rom = roms[0] UI.LogOut(str(self.rom))
def define_constants(self): self.map_side_length = 2.55 # FIMXE: hard coded goals # self.GOALS = [(40+2,6), (40, 6+2), (40,21), (35, 19), (30,22), (29,10), (27,5), (20,8), (20,33), (20, 48), (5,55)] self.GOALS = None self.worldNorthTheta = None self.maxVelocity = 2.0 self.history_length = 5 self.theta_history = RingBuffer(self.history_length) self.e_theta_h = RingBuffer(self.history_length) self.blurred_paths = None self.path_skip = 8
class AudiostreamSource(Thread): def __init__(self,sample_rate=16000,channels=1,audio_length=80): Thread.__init__(self) self.running = False self.input_device = 'default' self.bytes_per_sample=2 self.sample_rate = sample_rate self.channels = channels self.audio_length = audio_length self.blocksize = int(((self.sample_rate) * (self.audio_length) / 1000) * self.channels * self.bytes_per_sample) self._cmd = [ 'arecord', '-q', '-t', 'raw', '-D', self.input_device, '-c', str(self.channels), '-f', 's16', '-r', str(self.sample_rate), ] self._arecord = None self.audio_buffer = RingBuffer() def print_info(self): print("Blocksize: " + str(self.blocksize)) print("Sample Rate: " + str(self.sample_rate)) print("Channels: " + str(self.channels)) # Get len number of samples # Blocks until samples is available def read(self,chunk_size,advance): return self.audio_buffer.read(chunk_size,advance) def stop(self): self.running = False def run(self): self._arecord = subprocess.Popen(self._cmd, stdout=subprocess.PIPE) self.running = True while self._arecord and self.running: input_data = self._arecord.stdout.read(self.blocksize) if input_data: self.audio_buffer.write(input_data) #Shutdown record command if(self._arecord): self._arecord.kill() self._arecord = None
class TempSensor: def __init__(self, pin): self.check_ticks_ms = 0 self.ds = DS18X20(OneWire(Pin(pin))) self.rom = None self.buffer = RingBuffer(6) self.state = DS_IDEL roms = self.ds.scan() if roms: self.rom = roms[0] UI.LogOut(str(self.rom)) def Start(self): # Wait for 1000ms(1s) to start if self.state != DS_IDEL: return if time.ticks_diff(time.ticks_ms(), self.check_ticks_ms) < 1000: return if self.rom: self.ds.convert_temp() self.state = DS_START self.check_ticks_ms = time.ticks_ms() def Check(self): if self.rom is None: return None # Need wait for 750ms(800ms) after start if self.state != DS_START: return False if time.ticks_diff(time.ticks_ms(), self.check_ticks_ms) < 800: return False try: value = int(self.ds.read_temp(self.rom) * 100) except: UI.LogOut("Read DS18B20 failed!") return False else: value_change = self.buffer.InsertData(value, True) self.state = DS_IDEL return value_change def GetValue(self): return self.buffer.GetAverage()
class AudiostreamSource(Thread): def __init__(self,sample_rate=16000,channels=1,audio_length=80): Thread.__init__(self) self.p = pyaudio.PyAudio() self.running = False self.input_device = 'default' self.bytes_per_sample=2 self.sample_rate = sample_rate self.channels = channels self.audio_length = audio_length self.blocksize = int(((self.sample_rate) * (self.audio_length) / 1000) * self.channels * self.bytes_per_sample) self.audio_buffer = RingBuffer() def print_info(self): print("Blocksize: " + str(self.blocksize)) print("Sample Rate: " + str(self.sample_rate)) print("Channels: " + str(self.channels)) # Get len number of samples # Blocks until samples is available def read(self,chunk_size,advance): return self.audio_buffer.read(chunk_size,advance) def stop(self): self.running = False def run(self): self.mic_stream = self.p.open(format=pyaudio.paInt16, channels=self.channels, rate=self.sample_rate, input=True, frames_per_buffer=self.blocksize) self.running = True while self.mic_stream and self.running: input_data = self.mic_stream.read(self.blocksize) if input_data: self.audio_buffer.write(input_data) #Shutdown record command if(self.mic_stream): self.mic_stream.close() self.mic_stream = None
class CausalIntegralFilter(): def __init__(self, initial_value, initial_time): self._y = initial_value self._t_buffer = RingBuffer(2) self._t_buffer.append(initial_time) def append_integral_value(self, y): self._y = y def append(self, dydt, t): self._t_buffer.append(t) self._y += dydt * (self._t_buffer[-1] - self._t_buffer[-2]) def get_datum(self): return self._y
def __init__(self,sample_rate=16000,channels=1,audio_length=80): Thread.__init__(self) self.p = pyaudio.PyAudio() self.running = False self.input_device = 'default' self.bytes_per_sample=2 self.sample_rate = sample_rate self.channels = channels self.audio_length = audio_length self.blocksize = int(((self.sample_rate) * (self.audio_length) / 1000) * self.channels * self.bytes_per_sample) self.audio_buffer = RingBuffer()
class Stream(threading.Thread): """class to store the stream in a fifo (as daemon)""" queue = RingBuffer() def __init__(self, fifo): threading.Thread.__init__(self) self.fifo = fifo def run(self): """write all segments from queue to fifo""" try: # todo better directory with open('/tmp/loading.mov', 'r') as fd: loading = fd.read() except: loading = '' while self.queue.is_empty(): time.sleep(1) try: os.mkfifo(self.fifo) except: pass with open(self.fifo, 'a+') as fifo: while True: if self.queue.is_empty(): segment = loading else: segment = self.queue.pop() fifo.write(segment)
def test_two_similar_ringbuffers_are_equal(self): r = RingBuffer(10) s = RingBuffer(10) for i in range(20): r.append(i) s.append(i) self.assertEqual( r, s, "Fails to say that two identically filled " "RingBuffers are equal.")
class Game: def __init__(self): # The number of equations the user is shown at the start self.difficulty = 2 # The maximum number that can appear in an equation self.max_int = 10 # Count of correct answers self.correct = 0 # Total number of equations completed self.total = 0 # Number of equations for the user to answer self.max_equations = 10 # Ring buffer of equations self.rb = RingBuffer(self.difficulty) def nextEquation(self): ''' Creates the next equation Returns a string representation of the equation for the user to see. The string includes the order number corresponding to the equation ''' # Keep track of the number of equations self.total += 1 # Create the equation eq = Equation(10) # Add it to the buffer self.rb.add(eq) # And display it return '#%s) %s' % (self.total, eq) def validateAnswer(self, answer): ''' Validates the answer and increments the count of correct answers if the answer is correct. ''' if(self.rb.getOldest().validate(answer)): self.correct +=1 return True else: return False
def test_sort(self): buffer = RingBuffer(10) buffer.append(3) buffer.append(1) buffer.append('zoo') buffer.append(100) buffer.append("X") expected = [1, 3, 100, 'X', 'zoo'] actual = buffer.sort() self.assertEqual(expected, actual)
def __init__(self, queue, height, new_message_time, scroll_time): self.queue = queue self.y_step = textAscent() + textDescent() self.y = 0 self.ring = RingBuffer(height / self.y_step) self.last_time = 0 self.new_message_time = float(new_message_time) self.scroll_time = float(scroll_time) self.animation_start_time = 0
def test_ringbuffer_cannot_append_when_full(): """pushing to a full RingBuffer fails""" rblength = 16 rb = RingBuffer(rblength) for i in range(rblength): rb.append(i) with pytest.raises(IndexError): rb.append(42)
def __init__(self): ChatBox.__init__(self, [ "msg_public", "msg_team", "msg_private", "msg_squad", "msg_clan", "msg_server", "_HCBinternal" ]) self.buffer.setFadeTop(True) #history messages self.historyBuffer = MessageBuffer([ "msg_public", "msg_team", "msg_private", "msg_squad", "msg_clan", "msg_server", "_HCBinternal" ]) self.historyBuffer.setEditable(False) scroll = glass.GlassScrollArea(self.historyBuffer) scroll.setScrollPolicy(glass.GlassScrollArea.SHOW_NEVER, glass.GlassScrollArea.SHOW_ALWAYS) scroll.setAutoscroll(True) self.add(scroll) self.historyBuffer.parentScroll = scroll scroll.setVisible(False) for i in range(40): self.historyBuffer.addRow(" ") self.scroll.setVerticalScrollPolicy(glass.GlassScrollArea.SHOW_NEVER) self.input.setVisible(False) self.inputType = glass.GlassLabel("(squad)") self.inputType.setAlignment(glass.Graphics.RIGHT) self.add(self.inputType) self.history = RingBuffer(16) self.historyIndex = 0 self.chatType = "all" self.replyTarget = "" self.oldHeight = 0 self.typing = False self.fade = ActionSequence(savage.WaitAction(5000), savage.CheckAction(self.typing, False), FadeOutAction(self)) self.showHistory(False)
def __init__(self, host, port, tamanhoJanela): self.host = host self.port = port self.tamanhoJanela = tamanhoJanela self.ultimoEnviado = -1 self.ultimoRecebido = -1 self.reenviados = 0 self.buffer = RingBuffer(self.tamanhoJanela) self.socketCliente = iniciaSocketCliente() self.condition = Condition() self.enviando = True
def test_ringbuffer_clear_empties_buffer(): """clearing a RingBuffer leaves it empty: len(rb)==0""" rblength = 16 rb = RingBuffer(rblength) # fill buffer with random number of elements, but at least 1 n = random.randint(1,rblength) for i in range(n): rb.append(n) rb.clear() assert len(rb) == 0
def test_add(self): '''An non-full ring buffer should contain everything inserted into it ''' rb = RingBuffer(3) rb.add(1) self.assertListEqual(rb.toList(), [1]) rb.add(2) rb.add(3) self.assertListEqual(rb.toList(), [1,2,3])
def test_ringbuffer_popleft_decreases_length(): """popping from a RingBuffer reduces its length by 1""" rblength = 16 rb = RingBuffer(rblength) # fill buffer with random number of elements, but at least 1 n = random.randint(1,rblength) for i in range(n): rb.append(n) rb.popleft() assert len(rb) == n-1
def test_ringbuffer_append_increases_length(): """pushing to a RingBuffer increases its length by 1""" rblength = 16 rb = RingBuffer(rblength) # fill buffer with random number of elements, but at most length-1 n = random.randint(1,rblength-1) for i in range(n): rb.append(n) rb.append(42) assert len(rb) == n+1
def __init__( self ): ChatBox.__init__(self, ["msg_public", "msg_team", "msg_private", "msg_squad", "msg_clan", "_HCBinternal"]); self.buffer.showTime(1); self.buffer.setFadeTop(1); self.scroll.setVerticalScrollPolicy( glass.GlassScrollArea.SHOW_NEVER ); self.input.setVisible(False); self.inputType = glass.GlassLabel("(squad)"); self.inputType.setAlignment( glass.Graphics.RIGHT ); self.add(self.inputType) self.history = RingBuffer(16); self.historyIndex = 0; self.chatType = "all"; self.replyTarget = ""; self.oldHeight = 0; self._historyShown = 0; self.showHistory(0);
def __init__(self, sampling_period, minimum_pip=MINIMUM_PIP, window_length=5, polynomial_order=3, buffer_length=10): """ Parameters ---------- sampling_period : float The amount of time between data samples. minimum_pip=tetra_constants.MINIMUM_PIP : float The maximum pressure considered to be PIP. window_length=5 : int The number of points sampled for a low-pass filter of the data. It must be odd. polynomial_order=3 : int The order of the Savitsky-Golay filter used to smooth the data. It must be less than window_length. buffer_length=10 : int The number of data points considered when smoothing. """ if (window_length > buffer_length): raise ValueError("window_length must be <= buffer_length") if (window_length % 2 != 1): raise ValueError("window_length must be odd") if (buffer_length <= window_length): raise ValueError("buffer_length must be greater in length " "than window_length") self._sampling_period = sampling_period self._minimum_pip = minimum_pip self._currently_in_pip_range = False self._current_pip = minimum_pip self._window_length = window_length self._polynomial_order = polynomial_order self._buffer_length = buffer_length self._data_buffer = RingBuffer(buffer_length, initial_state=[minimum_pip] * buffer_length)
def __init__(self,sample_rate=16000,channels=1,audio_length=80): Thread.__init__(self) self.running = False self.input_device = 'default' self.bytes_per_sample=2 self.sample_rate = sample_rate self.channels = channels self.audio_length = audio_length self.blocksize = int(((self.sample_rate) * (self.audio_length) / 1000) * self.channels * self.bytes_per_sample) self._cmd = [ 'arecord', '-q', '-t', 'raw', '-D', self.input_device, '-c', str(self.channels), '-f', 's16', '-r', str(self.sample_rate), ] self._arecord = None self.audio_buffer = RingBuffer()
def test_add_full(self): '''A full ring buffer should remove the oldest element after a new add ''' rb = RingBuffer(3) rb.add(1) rb.add(2) rb.add(3) rb.add(4) self.assertListEqual(rb.toList(),[2,3,4])
def __init__(self, *args, **kwargs): """ Both args and kwargs will be passed to Popen constructor. """ kwargs['stdout'] = PIPE kwargs['stderr'] = STDOUT kwargs['bufsize'] = 0 kwargs['close_fds'] = True super(What, self).__init__(args, **kwargs) self.timeout = 10 self.queue = Queue(self.BUFFER_SIZE) self.lines = RingBuffer(self.BUFFER_SIZE) self.reader = Thread(target=self._enqueue_output) self.reader.daemon = True self.reader.start()
class SoundDetector(MicrophoneListener): """Detects sound by looking at number of times the signal crosses a given threshold value """ def __init__(self, size, parent=None): super(SoundDetector, self).__init__(parent) self._buffer = RingBuffer(maxlen=size) self._channels = None self.thresholds = {} def reset(self): self._buffer.clear() self._channels = None @pyqtSlot(int) def set_sampling_rate(self, sampling_rate): self._buffer.clear() self._buffer = RingBuffer( maxlen=int(Settings.DETECTION_BUFFER * sampling_rate) ) def set_threshold(self, ch, threshold): self.thresholds[ch] = threshold @pyqtSlot(object) def receive_data(self, data): if self._channels is None: self._channels = data.shape[1] if data.shape[1] != self._channels: return self._buffer.extend(data) dat = np.array(self._buffer) if not len(dat): return for ch_idx in range(dat.shape[1]): if ch_idx not in self.thresholds: self.thresholds[ch_idx] = Settings.DEFAULT_POWER_THRESHOLD threshold_crossings = np.nonzero( np.diff(np.abs(dat[:, ch_idx]) > self.thresholds[ch_idx]) )[0] ratio = int(threshold_crossings.size) / Settings.DETECTION_CROSSINGS_PER_CHUNK if ratio > 1: self.OUT.emit() break
def __init__(self): # The number of equations the user is shown at the start self.difficulty = 2 # The maximum number that can appear in an equation self.max_int = 10 # Count of correct answers self.correct = 0 # Total number of equations completed self.total = 0 # Number of equations for the user to answer self.max_equations = 10 # Ring buffer of equations self.rb = RingBuffer(self.difficulty)
class GPTuner(): def __init__(self, dim, stepper, kernel, obs_var, history_len=100): self.dim = dim self.stepper = stepper self.k = kernel self.history_x = RingBuffer([dim], history_len) self.history_y = RingBuffer([1], history_len) self.obs_var_p = obs_var self.eps = 0.00 def obs_var(self): return F.softplus(self.obs_var_p) def predict(self, x): mean, var = GP.predict(self.history_x.all(), self.history_y.all(), self.obs_var().expand(len(self.history_y)), x, mean_zero, self.k) var += self.eps * torch.eye(var.shape[0]) return mean, var def thompson_step(self, grid): mean, var = self.predict(grid) choice = torch.argmax(sample(mean, var)) print(grid[choice]) result = self.stepper(grid[choice]) self.history_x.append(grid[choice]) self.history_y.append(result) return result def log_prob(self): locs = self.history_x.all() var = self.k(locs, locs) + self.obs_var() * torch.eye(len(locs)) return D.MultivariateNormal(torch.zeros(len(locs)), var).log_prob(self.history_y.all()) def sample_argmax(self, init_grid, samples=1024): mean, var = self.predict(init_grid) choices = torch.argmax(sample(mean, var, [samples]), dim=1) return init_grid[choices]
def test_getOldest(self): '''You can access the oldest element in the ring buffer with getOldest When the list is empty, return None ''' rb = RingBuffer(2) self.assertTrue(rb.getOldest() == None) rb.add(1) self.assertTrue(rb.getOldest() == 1) rb.add(2) self.assertTrue(rb.getOldest() == 1) rb.add(3) self.assertTrue(rb.getOldest() == 2)
class What(Popen): """ Wrapper around subprocess.Popen that has additional methods for checking process output and return code. Inspired by the solution from J.F. Sebastian. Source: http://stackoverflow.com/a/4896288/242451 """ BUFFER_SIZE = 100 def __init__(self, *args, **kwargs): """ Both args and kwargs will be passed to Popen constructor. """ kwargs['stdout'] = PIPE kwargs['stderr'] = STDOUT kwargs['bufsize'] = 0 kwargs['close_fds'] = True super(What, self).__init__(args, **kwargs) self.timeout = 10 self.queue = Queue(self.BUFFER_SIZE) self.lines = RingBuffer(self.BUFFER_SIZE) self.reader = Thread(target=self._enqueue_output) self.reader.daemon = True self.reader.start() def expect(self, string, timeout=None): """ Expect a string in output. If timeout is given and expected string is not found before timeout Timeout will be raised. If end of the file is reached while waiting for string EOF will be raised. If timeout is None, self.timeout is going to be used as a value. """ if timeout is None: timeout = self.timeout start = time() try: while 1: passed = time() - start get_timeout = timeout - passed if get_timeout < 0: raise Timeout(self, string) line = self.queue.get(timeout=get_timeout) if line is EOF: self.wait() raise EOF(self, string) if string in line: return line except Empty: raise Timeout(self, string) def expect_exit(self, exit_code=None, timeout=None): """ Expect process to exit with specified exit code. If timeout is None, self.timeout is going to be used as a value. """ if timeout is None: timeout = self.timeout start = time() while 1: returncode = self.poll() if returncode is not None: break passed = time() - start if passed > timeout: raise Timeout(self, exit_code) if exit_code is not None and returncode != exit_code: raise UnexpectedExit(self, exit_code) self.wait() def get_output(self): """Return lines read in the read buffer.""" return '\n'.join(self.lines) def _enqueue_output(self): """Thread target of self.reader.""" for line in iter(self.stdout.readline, b''): line = line.rstrip('\n') self.queue.put(line) self.lines.append(line) self.queue.put(EOF) self.stdout.close()
class Lab2Program: def __init__(self): self.initialize_vrep_client() self.initilize_vrep_api() self.define_constants() self.killer = GracefulKiller() self.idash = IDash(framerate=0.05) def initialize_vrep_client(self): #Initialisation for Python to connect to VREP print 'Python program started' count = 0 num_tries = 10 while count < num_tries: vrep.simxFinish(-1) # just in case, close all opened connections self.clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5) #Timeout=5000ms, Threadcycle=5ms if self.clientID!=-1: print 'Connected to V-REP' break else: "Trying again in a few moments..." time.sleep(3) count += 1 if count >= num_tries: print 'Failed connecting to V-REP' vrep.simxFinish(self.clientID) def initilize_vrep_api(self): # initialize ePuck handles and variables _, self.bodyelements=vrep.simxGetObjectHandle( self.clientID, 'ePuck_bodyElements', vrep.simx_opmode_oneshot_wait) _, self.leftMotor=vrep.simxGetObjectHandle( self.clientID, 'ePuck_leftJoint', vrep.simx_opmode_oneshot_wait) _, self.rightMotor=vrep.simxGetObjectHandle( self.clientID, 'ePuck_rightJoint', vrep.simx_opmode_oneshot_wait) _, self.ePuck=vrep.simxGetObjectHandle( self.clientID, 'ePuck', vrep.simx_opmode_oneshot_wait) _, self.ePuckBase=vrep.simxGetObjectHandle( self.clientID, 'ePuck_base', vrep.simx_opmode_oneshot_wait) # proxSens = prox_sens_initialize(self.clientID) # initialize odom of ePuck _, self.xyz = vrep.simxGetObjectPosition( self.clientID, self.ePuck, -1, vrep.simx_opmode_streaming) _, self.eulerAngles = vrep.simxGetObjectOrientation( self.clientID, self.ePuck, -1, vrep.simx_opmode_streaming) # initialize overhead cam _, self.overheadCam=vrep.simxGetObjectHandle( self.clientID, 'Global_Vision', vrep.simx_opmode_oneshot_wait) _, self.resolution, self.image = vrep.simxGetVisionSensorImage( self.clientID,self.overheadCam,0,vrep.simx_opmode_oneshot_wait) # initialize goal handle + odom _, self.goalHandle=vrep.simxGetObjectHandle( self.clientID, 'Goal_Position', vrep.simx_opmode_oneshot_wait) _, self.goalPose = vrep.simxGetObjectPosition( self.clientID, self.goalHandle, -1, vrep.simx_opmode_streaming) # STOP Motor Velocities. Not sure if this is necessary _ = vrep.simxSetJointTargetVelocity( self.clientID,self.leftMotor,0,vrep.simx_opmode_oneshot_wait) _ = vrep.simxSetJointTargetVelocity( self.clientID,self.rightMotor,0,vrep.simx_opmode_oneshot_wait) def define_constants(self): self.map_side_length = 2.55 # FIMXE: hard coded goals # self.GOALS = [(40+2,6), (40, 6+2), (40,21), (35, 19), (30,22), (29,10), (27,5), (20,8), (20,33), (20, 48), (5,55)] self.GOALS = None self.worldNorthTheta = None self.maxVelocity = 2.0 self.history_length = 5 self.theta_history = RingBuffer(self.history_length) self.e_theta_h = RingBuffer(self.history_length) self.blurred_paths = None self.path_skip = 8 def global_map_preprocess(self, resolution, image): im = format_vrep_image(resolution, image) # original image im = image_vert_flip(im) resize_length = int(im.shape[0]/2) im = skimage.transform.resize(im, (resize_length, resize_length, 3)) return im def global_map_process(self, im): walls = im[:,:,0] > 0.25 paths = walls < 0.15 if self.blurred_paths is None: print "Computed" blurred_map = skimage.filters.gaussian_filter(walls, sigma=2) blurred_paths = blurred_map < 0.15 # np.save('binary_grid.npy', blurred_paths) return paths, blurred_paths else: return paths, self.blurred_paths def robot_pose_get(self): _, xyz = vrep.simxGetObjectPosition( self.clientID, self.ePuck, -1, vrep.simx_opmode_buffer) _, eulerAngles = vrep.simxGetObjectOrientation( self.clientID, self.ePuck, -1, vrep.simx_opmode_buffer) x, y, z = xyz theta = eulerAngles[2] self.theta_history.append(theta) return (x, y, theta) def lidar_scan_get(self, window_size=21): """ gets lidar scan range values """ fr = (window_size - 1) / 2 # fire range lidarValues = pseudoLidarSensor(self.paths, self.pose[0], self.pose[1], fr, original_four=False) return lidarValues def repulsion_vectors_compute(self, lidarValues, k_repulse=10.0): numLidarValues = len(lidarValues) lidarAngles = [np.pi / numLidarValues * index for index in range(numLidarValues)] repulsionVectors = [np.array(pol2cart(force_repulsion(k_repulse, np.sqrt(val), 2), angle)) for val, angle in zip(lidarValues, lidarAngles)] return repulsionVectors def attraction_vector_compute(self, k_attract=100.0): self.attractionVal, self.attractionAngle = cart2pol( self.goal_pose_pixel[1] - self.pose_pixel[1], # cols counts same to normal horz axes self.pose_pixel[0] - self.goal_pose_pixel[0] # rows counts opposite to normal vert axes ) attractionVector = np.array(pol2cart(k_attract*self.attractionVal, self.attractionAngle)) return attractionVector def robot_code(self): t = time.time() self.curr_goal = 0 while (time.time() - t) < 200: # global map _,resolution,image = vrep.simxGetVisionSensorImage(self.clientID,self.overheadCam,0,vrep.simx_opmode_oneshot_wait) # Get image im = self.global_map_preprocess(resolution, image) self.pose = self.robot_pose_get() x, y, theta = self.pose # theta = self.theta_history.weighted_average(scheme='last', mathfunc=lambda x: np.exp(x)) # initialize worldNorthTheta for the first time if self.worldNorthTheta is None: self.worldNorthTheta = self.pose[2] _ = [self.theta_history.append(self.pose[2]) for _ in range(self.history_length)] self.pose_pixel = np.array(odom2pixelmap(x, y, self.map_side_length, im.shape[0])) m, n = self.pose_pixel self.paths, self.blurred_paths = self.global_map_process(im) # calculate intermediate goals once if self.GOALS is None: raw_input("") # acquire pixel location of goal _, finishPose = vrep.simxGetObjectPosition( self.clientID, self.goalHandle, -1, vrep.simx_opmode_buffer) self.finish_pixel = odom2pixelmap(finishPose[0], finishPose[1], self.map_side_length, im.shape[0]) contiguousPath = AStarBinaryGrid(self.blurred_paths).calculate_path(self.pose_pixel, self.finish_pixel) self.GOALS = contiguousPath[::self.path_skip] # SKIP THIS FIRST LOOP AND CONTINUE continue else: self.goal_pose_pixel = np.array(self.GOALS[self.curr_goal]) goal_m, goal_n = self.goal_pose_pixel lidarValues = self.lidar_scan_get(window_size=21) ############################# # Potential Field Algorithm # ############################# repulsionVectors = self.repulsion_vectors_compute(lidarValues, k_repulse=10.0) attractionVector = self.attraction_vector_compute(k_attract=100.0) finalVector = np.sum(np.vstack((repulsionVectors, attractionVector)), axis=0) finalUnitVector = finalVector / np.linalg.norm(finalVector) print "finalUnitVector: ", finalUnitVector # TODO: do we use the unit vector (for direction) only, or the finalVector? finalValue, finalAngle = cart2pol(finalUnitVector[0], finalUnitVector[1]) error_theta = angle_diff(mapTheta2worldTheta(finalAngle, self.worldNorthTheta), theta) self.e_theta_h.append(error_theta) k_angular_p = 2.0 * self.maxVelocity k_angular_D = 0.25 k_angular_S = 1.0 k_angular_I = 2.5 omega = k_angular_p * ( error_theta + k_angular_D / k_angular_S * (self.e_theta_h[-1] - self.e_theta_h[-2]) + k_angular_S / k_angular_I * sum(self.e_theta_h) ) print "Omega: ", round(omega,1) ############################# # StateController Algorithm # ############################# # if desired heading is not directly in front if np.abs(error_theta) > np.pi / 3: # turn in place forward_vel = self.maxVelocity # omega *= 0.25 else: # Direct yourself to the goal goal_distance = np.linalg.norm(self.goal_pose_pixel - self.pose_pixel) print "distance_from_goal: ", goal_distance if goal_distance <= 4: # Achieved Goal! forward_vel = self.maxVelocity * 0.25 # Slow down to prepare for the next one self.curr_goal += 1 else: forward_vel = self.maxVelocity # control the motors ctrl_sig_left, ctrl_sig_right = vomega2bytecodes(forward_vel, omega, g=1) _ = vrep.simxSetJointTargetVelocity( self.clientID,self.leftMotor,ctrl_sig_left,vrep.simx_opmode_oneshot_wait) # set left wheel velocity _ = vrep.simxSetJointTargetVelocity( self.clientID,self.rightMotor,ctrl_sig_right,vrep.simx_opmode_oneshot_wait) # set right wheel velocity self.idash.add(lambda: self.plot_maze(self.blurred_paths*1.0, m, n, goal_m, goal_n)) self.idash.add(lambda: self.plot_maze(im, m, n, goal_m, goal_n)) def plot_current_and_desired_heading(): self.plot_unit_quiver(finalUnitVector, 'r') self.plot_unit_quiver(pol2cart(1, worldTheta2mapTheta(theta, self.worldNorthTheta)), 'k') plt.title("Error Theta: %f" % error_theta) self.idash.add(plot_current_and_desired_heading) self.idash.add(self.plot_theta_history) self.idash.plotframe() if self.killer.kill_now: self.clean_exit() def plot_maze(self, im, m, n, goal_m, goal_n): """ plots the maze, with robot pose and goal pose visualized """ if len(im.shape) == 3: goal_pixel_values = np.array((1.0, 1.0, 1.0)) robot_pixel_values = np.array((255.0/255.0,192/255.0,203/255.0)) elif len(im.shape) == 2: goal_pixel_values = 0.5 robot_pixel_values = 0.5 im[goal_m,goal_n] = goal_pixel_values im[m,n] = robot_pixel_values plt.imshow(im) def plot_unit_quiver(self, vector, color): X, Y = (0, 0) U, V = (vector[0], vector[1]) plt.quiver(X,Y,U,V,angles='xy',scale_units='xy',scale=1,color=color) plt.xlim([-1.1,1.1]) plt.ylim([-1.1,1.1]) def plot_theta_history(self, expansion=5): plt.plot([theta for theta in self.theta_history]) if len(self.theta_history) < expansion: plt.xlim([0, expansion]) else: plt.xlim([0, len(self.theta_history)]) ylim = np.pi + 0.5 plt.ylim([-ylim, ylim]) def plot_all_goals(self, im): # display all goals for goal_idx in range(len(self.GOALS)): im[self.GOALS[goal_idx][0], self.GOALS[goal_idx][1]] = np.array((1.0, 1.0, 1.0)) def clean_exit(self): _ = vrep.simxStopSimulation(self.clientID,vrep.simx_opmode_oneshot_wait) vrep.simxFinish(self.clientID) print 'Program ended' sys.exit(0) def run(self): if self.clientID!=-1: _ = vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot_wait) self.robot_code() self.clean_exit()
def test_distinct(self): buffer = RingBuffer(10) d = buffer.distinct() self.assertEqual(len(d), 0) buffer.append(42) self.assertEqual(len(buffer.distinct()), 1) self.assertEqual(buffer.distinct()[0], 42) buffer.append(42) self.assertEqual(len(buffer.distinct()), 1) self.assertEqual(buffer.distinct()[0], 42) buffer.append(21) self.assertEqual(len(buffer.distinct()), 2) self.assertEqual(buffer.distinct()[0], 42) self.assertEqual(buffer.distinct()[1], 21)
class Delay_Estimator_Widget(QtGui.QWidget): def __init__(self, parent = None, logger = None): QtGui.QWidget.__init__(self, parent) self.audiobuffer = None # store the logger instance if logger is None: self.logger = parent.parent.logger else: self.logger = logger self.previous_delay_message = "" self.previous_correlation_message = "" self.previous_polarity_message = "" self.previous_channelInfo_message = "" self.setObjectName("Delay_Estimator_Widget") self.layout = QtGui.QFormLayout(self) self.layout.setObjectName("layout") font = QtGui.QFont() font.setPointSize(14) font.setWeight(75) font.setBold(True) self.delay_label = QtGui.QLabel(self) self.delay_label.setFont(font) self.delay_label.setObjectName("delay_label") self.delayText_label = QtGui.QLabel(self) self.delayText_label.setObjectName("delayText_label") self.delayText_label.setText("Delay:") self.correlation_label = QtGui.QLabel(self) self.correlation_label.setObjectName("Correlation_label") self.correlationText_label = QtGui.QLabel(self) self.correlationText_label.setObjectName("correlationText_label") self.correlationText_label.setText("Confidence:") self.polarity_label = QtGui.QLabel(self) self.polarity_label.setFont(font) self.polarity_label.setObjectName("polarity_label") self.polarityText_label = QtGui.QLabel(self) self.polarityText_label.setObjectName("polarityText_label") self.polarityText_label.setText("Polarity:") self.channelInfo_label = QtGui.QLabel(self) self.channelInfo_label.setObjectName("channelInfo_label") self.layout.addRow(self.delayText_label, self.delay_label) self.layout.addRow(self.correlationText_label, self.correlation_label) self.layout.addRow(self.polarityText_label, self.polarity_label) self.layout.addRow(None, self.channelInfo_label) self.settings_dialog = Delay_Estimator_Settings_Dialog(self, self.logger) self.i = 0 # We will decimate several times # no decimation => 1/fs = 23 µs resolution # 1 ms resolution => fs = 1000 Hz is enough => can divide the sampling rate by 44 ! # if I decimate 2 times (2**2 = 4 => 0.092 ms (3 cm) resolution)! # if I decimate 3 times (2**3 = 8 => 0.184 ms (6 cm) resolution)! # if I decimate 4 times (2**4 = 16 => 0.368 ms (12 cm) resolution)! # if I decimate 5 times (2**5 = 32 => 0.7 ms (24 cm) resolution)! # (actually, I could fit a gaussian on the cross-correlation peak to get # higher resolution even at low sample rates) self.Ndec = 2 self.subsampled_sampling_rate = SAMPLING_RATE/2**(self.Ndec) [self.bdec, self.adec] = generated_filters.params['dec'] self.zfs0 = subsampler_filtic(self.Ndec, self.bdec, self.adec) self.zfs1 = subsampler_filtic(self.Ndec, self.bdec, self.adec) # ringbuffers for the subsampled data self.ringbuffer0 = RingBuffer() self.ringbuffer1 = RingBuffer() self.delayrange_s = DEFAULT_DELAYRANGE # confidence range self.old_Xcorr = None # method def set_buffer(self, buffer): self.audiobuffer = buffer # method def update(self): if not self.isVisible(): return # temporary buffer just to check the data shape floatdata = self.audiobuffer.data(2) if floatdata.shape[0] == 1: message = """Delay estimator only works with two channels. Select two-channels mode in the setup window.""" if message <> self.previous_channelInfo_message: self.previous_delay_message = "N/A ms\n(N/A m)" self.delay_label.setText(self.previous_delay_message) self.previous_correlation_message = "N/A %" self.correlation_label.setText(self.previous_correlation_message) self.previous_polarity_message = "N/A" self.polarity_label.setText(self.previous_polarity_message) self.channelInfo_label.setText(message) self.previous_channelInfo_message = message else: #get the fresh data floatdata = self.audiobuffer.newdata() # separate the channels x0 = floatdata[0,:] x1 = floatdata[1,:] #subsample them x0_dec, self.zfs0 = subsampler(self.Ndec, self.bdec, self.adec, x0, self.zfs0) x1_dec, self.zfs1 = subsampler(self.Ndec, self.bdec, self.adec, x1, self.zfs1) # push to a 1-second ring buffer x0_dec.shape = (1, x0_dec.size) x1_dec.shape = (1, x1_dec.size) self.ringbuffer0.push(x0_dec) self.ringbuffer1.push(x1_dec) if (self.i==5): self.i = 0 # retrieve last one-second of data time = 2*self.delayrange_s length = time*self.subsampled_sampling_rate d0 = self.ringbuffer0.data(length) d1 = self.ringbuffer1.data(length) d0.shape = (d0.size) d1.shape = (d1.size) std0 = numpy.std(d0) std1 = numpy.std(d1) if d0.size>0 and std0>0. and std1>0.: # substract the means # (in order to get a normalized cross-correlation at the end) d0 -= d0.mean() d1 -= d1.mean() # compute the cross-correlation D0 = rfft(d0) D1 = rfft(d1) D0r = D0.conjugate() G = D0r*D1 G = (G==0.)*1e-30 + (G<>0.)*G #W = 1. # frequency unweighted W = 1./numpy.abs(G) # "PHAT" #D1r = D1.conjugate(); G0 = D0r*D0; G1 = D1r*D1; W = numpy.abs(G)/(G0*G1) # HB weighted Xcorr = irfft(W*G) #Xcorr_unweighted = irfft(G) #numpy.save("d0.npy", d0) #numpy.save("d1.npy", d1) #numpy.save("Xcorr.npy", Xcorr) if self.old_Xcorr != None and self.old_Xcorr.shape == Xcorr.shape: # smoothing alpha = 0.15 Xcorr = alpha*Xcorr + (1. - alpha)*self.old_Xcorr absXcorr = numpy.abs(Xcorr) i = argmax(absXcorr) # normalize #Xcorr_max_norm = Xcorr_unweighted[i]/(d0.size*std0*std1) Xcorr_extremum = Xcorr[i] Xcorr_max_norm = abs(Xcorr[i])/(3*numpy.std(Xcorr)) delay_ms = 1e3*float(i)/self.subsampled_sampling_rate # store for smoothing self.old_Xcorr = Xcorr else: delay_ms = 0. Xcorr_max_norm = 0. Xcorr_extremum = 0. # debug wrong phase detection #if Xcorr[i] < 0.: # numpy.save("Xcorr.npy", Xcorr) c = 340. # speed of sound, in meters per second (approximate) distance_m = delay_ms*1e-3*c # home-made measure of the significance slope = 0.12 p = 3 x = (Xcorr_max_norm>1.)*(Xcorr_max_norm-1.) x = (slope*x)**p correlation = int((x/(1. + x))*100) delay_message = "%.1f ms\n= %.2f m" %(delay_ms, distance_m) correlation_message = "%d%%" %(correlation) if Xcorr_extremum >= 0: polarity_message = "In-phase" else: polarity_message = "Reversed phase" channelInfo_message = "" if delay_message <> self.previous_delay_message: self.delay_label.setText(delay_message) self.previous_delay_message = delay_message if correlation_message <> self.previous_correlation_message: self.correlation_label.setText(correlation_message) self.previous_correlation_message = correlation_message if polarity_message <> self.previous_polarity_message: self.polarity_label.setText(polarity_message) self.previous_polarity_message = polarity_message if channelInfo_message <> self.previous_channelInfo_message: self.channelInfo_label.setText(channelInfo_message) self.previous_channelInfo_message = channelInfo_message self.i += 1 def set_delayrange(self, delay_s): self.delayrange_s = delay_s # slot def settings_called(self, checked): self.settings_dialog.show() # method def saveState(self, settings): self.settings_dialog.saveState(settings) # method def restoreState(self, settings): self.settings_dialog.restoreState(settings)
def test_iter(self): buffer = RingBuffer(10) for i in range(5): buffer.append(i) for i in buffer: self.assertTrue(buffer[i] is not None)
def test_get(self): buffer = RingBuffer(10) self.assertEqual(buffer.get(0), None) try: buffer.get(11) self.assertTrue(False, "Should throw exception") except: self.assertTrue(True, "Should throw exception") buffer.append(42) self.assertEqual(buffer.get(0), 42) buffer.append('21') self.assertEqual(buffer.get(0), '21') self.assertEqual(buffer.get(1), 42) self.assertEqual(buffer[1], 42) buffer.append('33') self.assertEqual(buffer[1:3], ['21', 42])
def test_append(self): buffer = RingBuffer(10) buffer.append(42) self.assertEqual(buffer.last(), 42) buffer.append(21) self.assertEqual(buffer.last(), 21)
def __init__(self, logger): self.ringbuffer = RingBuffer(logger) self.newpoints = 0 self.delay_samples = 0
def main(): """ image processing occurs here """ intrusion_detected = False alarm_hold_time = time.time() + 5 frame_rate = 60 refresh_time = int((1 / frame_rate) * 1000) ########################################################################### # VIDEO CAPTURE: open camera. user -1 for default. 1 for c920 or external # camera. ########################################################################### #vc = cv2.VideoCapture(-1) #vc = cv2.VideoCapture("Video 3.wmv") vc = cv2.VideoCapture(0) ########################################################################### # create processing windows ########################################################################### cv2.namedWindow("src", flags=cv2.WINDOW_NORMAL) cv2.namedWindow("gray", flags=cv2.WINDOW_NORMAL) cv2.namedWindow("Vish Movement Detector", flags=cv2.WINDOW_NORMAL) cv2.namedWindow("image_delta_open", flags=cv2.WINDOW_NORMAL) cv2.namedWindow("noise removal", flags=cv2.WINDOW_NORMAL) ########################################################################### # create window with trackbars for runtime adjusting ########################################################################### create_trackbars() ########################################################################### # flush camera buffer. Don't quite understand why ########################################################################### for i in range(10): cv2.waitKey(refresh_time) _, src = vc.read() ########################################################################### # create a ring buffer to handle background history ########################################################################### prev_history_length = get_trackbar_pos("HistoryLength") rb = RingBuffer(prev_history_length) ########################################################################### # process image ########################################################################### print ("start processing image") while True: # Clear os.system('cls' if os.name == 'nt' else 'clear') ####################################################################### # start measuring time ####################################################################### start_time = time.time() ####################################################################### # read data directly from the camera or video ####################################################################### _, src = vc.read() ####################################################################### # convert to hsv and gray frames ####################################################################### gray = cv2.cvtColor(src, cv2.COLOR_BGR2GRAY) cv2.imshow("gray", gray) ####################################################################### # remove noise from image ####################################################################### #gray = cv2.GaussianBlur(gray, (5, 5), 0) gray = cv2.blur(gray, (3, 3)) #gray = cv2.medianBlur(gray, 5) cv2.imshow("noise removal", gray) ####################################################################### # update background history, and background average ####################################################################### history_length = get_trackbar_pos("HistoryLenght") history_length = 1 if history_length == 9 else history_length if prev_history_length != history_length: rb = RingBuffer(history_length) prev_history_length = history_length rb.push(gray.astype(np.float)) history = rb.buffer gray_background = (sum(history) / len(history)).astype(np.uint8) cv2.imshow("background", gray_background) ####################################################################### # do background substraction ####################################################################### image_delta = abs(gray.astype(np.int8) - gray_background.astype(np.int8)) image_delta = image_delta.astype(np.uint8) delta_threshold = get_trackbar_pos("DeltaThreshold") _, image_delta = cv2.threshold(image_delta, delta_threshold, 255, cv2.THRESH_BINARY) cv2.imshow("Vishnus Detection", image_delta) ####################################################################### # open image ####################################################################### size = get_trackbar_pos("MorphOpsSize") size = 1 if size == 0 else size kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (size, size)) iterations = get_trackbar_pos("Iterations") iterations = 1 if size == 0 else iterations image_delta = cv2.morphologyEx(image_delta, cv2.MORPH_OPEN, kernel, iterations=iterations) ####################################################################### # dilate image ####################################################################### element = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (5, 5)) image_delta = cv2.dilate(image_delta, element) cv2.imshow("image_delta_open", image_delta) ####################################################################### # Detect if Motion Alarm is to be triggered # hold off for 5 seconds until image stabilized ####################################################################### # import pdb; pdb.set_trace() if alarm_hold_time < time.time(): delta_percentage = 100 * np.count_nonzero(image_delta) / image_delta.size alarm_threshold = get_trackbar_pos("AlarmThreshold") if delta_percentage > alarm_threshold and not intrusion_detected: t = threading.Thread(target=run_alarm) t.start() intrusion_detected = True #import pdb;pdb.set_trace(100) ####################################################################### # find and draw contours ####################################################################### src,contours,hierarchy = cv2.findContours(image_delta, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) print ("num countours found = %s" % (len(contours))) cv2.drawContours(src, contours, -1, (0, 255, 0), -1) ########################################################################### # blob detection: detect blobs in image ########################################################################### pass ####################################################################### # image info ####################################################################### # import pdb; pdb.set_trace() if alarm_hold_time < time.time(): pos = (5, 100) info = "motion_rate = %0.2f, threshold = %0.2f" % (delta_percentage, alarm_threshold) cv2.putText(src, info, pos, cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), thickness=6, lineType=cv2.LINE_AA) ####################################################################### # display video processing output ####################################################################### cv2.imshow("src", src) ####################################################################### # calculate elapsed time ####################################################################### elapsed_time = time.time() - start_time print ("processing time = %.2sms, frame rate = %dfps" % (elapsed_time * 1000, refresh_time)) ####################################################################### # cv2.waitkey expects a value that corresponds to the msecs to wait # which finally defines the frame rate of video. ####################################################################### key = cv2.waitKey(refresh_time) ####################################################################### # stop video processing if user press ESC key ####################################################################### if key == 27: vc.release() cv2.destroyAllWindows() break
class Delay_Estimator_Widget(QtGui.QWidget): def __init__(self, parent = None, logger = None): QtGui.QWidget.__init__(self, parent) self.audiobuffer = None # store the logger instance if logger is None: self.logger = parent.parent.logger else: self.logger = logger self.previous_delay_message = "" self.previous_correlation_message = "" self.previous_polarity_message = "" self.previous_channelInfo_message = "" self.setObjectName("Delay_Estimator_Widget") self.layout = QtGui.QFormLayout(self) self.layout.setObjectName("layout") font = QtGui.QFont() font.setPointSize(14) font.setWeight(75) font.setBold(True) self.delay_label = QtGui.QLabel(self) self.delay_label.setFont(font) self.delay_label.setObjectName("delay_label") self.delayText_label = QtGui.QLabel(self) self.delayText_label.setObjectName("delayText_label") self.delayText_label.setText("Delay:") self.correlation_label = QtGui.QLabel(self) self.correlation_label.setObjectName("Correlation_label") self.correlationText_label = QtGui.QLabel(self) self.correlationText_label.setObjectName("correlationText_label") self.correlationText_label.setText("Confidence:") self.polarity_label = QtGui.QLabel(self) self.polarity_label.setFont(font) self.polarity_label.setObjectName("polarity_label") self.polarityText_label = QtGui.QLabel(self) self.polarityText_label.setObjectName("polarityText_label") self.polarityText_label.setText("Polarity:") self.channelInfo_label = QtGui.QLabel(self) self.channelInfo_label.setObjectName("channelInfo_label") self.layout.addRow(self.delayText_label, self.delay_label) self.layout.addRow(self.correlationText_label, self.correlation_label) self.layout.addRow(self.polarityText_label, self.polarity_label) self.layout.addRow(None, self.channelInfo_label) self.settings_dialog = Delay_Estimator_Settings_Dialog(self, self.logger) # We will decimate several times # no decimation => 1/fs = 23 µs resolution # 1 ms resolution => fs = 1000 Hz is enough => can divide the sampling rate by 44 ! # if I decimate 2 times (2**2 = 4 => 0.092 ms (3 cm) resolution)! # if I decimate 3 times (2**3 = 8 => 0.184 ms (6 cm) resolution)! # if I decimate 4 times (2**4 = 16 => 0.368 ms (12 cm) resolution)! # if I decimate 5 times (2**5 = 32 => 0.7 ms (24 cm) resolution)! # (actually, I could fit a gaussian on the cross-correlation peak to get # higher resolution even at low sample rates) self.Ndec = 2 self.subsampled_sampling_rate = SAMPLING_RATE/2**(self.Ndec) [self.bdec, self.adec] = generated_filters.params['dec'] self.zfs0 = subsampler_filtic(self.Ndec, self.bdec, self.adec) self.zfs1 = subsampler_filtic(self.Ndec, self.bdec, self.adec) # ringbuffers for the subsampled data self.ringbuffer0 = RingBuffer(self.logger) self.ringbuffer1 = RingBuffer(self.logger) self.delayrange_s = DEFAULT_DELAYRANGE # confidence range self.old_Xcorr = None self.old_index = 0 # method def set_buffer(self, buffer): self.audiobuffer = buffer # method def update(self): if not self.isVisible(): return # temporary buffer just to check the data shape floatdata = self.audiobuffer.data(2) if floatdata.shape[0] == 1: message = """Delay estimator only works with two channels. Select two-channels mode in the setup window.""" if message <> self.previous_channelInfo_message: self.previous_delay_message = "N/A ms\n(N/A m)" self.delay_label.setText(self.previous_delay_message) self.previous_correlation_message = "N/A %" self.correlation_label.setText(self.previous_correlation_message) self.previous_polarity_message = "N/A" self.polarity_label.setText(self.previous_polarity_message) self.channelInfo_label.setText(message) self.previous_channelInfo_message = message else: #get the fresh data floatdata = self.audiobuffer.newdata() # separate the channels x0 = floatdata[0,:] x1 = floatdata[1,:] #subsample them x0_dec, self.zfs0 = subsampler(self.Ndec, self.bdec, self.adec, x0, self.zfs0) x1_dec, self.zfs1 = subsampler(self.Ndec, self.bdec, self.adec, x1, self.zfs1) # push to a 1-second ring buffer x0_dec.shape = (1, x0_dec.size) x1_dec.shape = (1, x1_dec.size) self.ringbuffer0.push(x0_dec) self.ringbuffer1.push(x1_dec) # we need to maintain an index of where we are in the buffer index = self.ringbuffer0.offset available = index - self.old_index if available < 0: #ringbuffer must have grown or something... available = 0 self.old_index = index time = 2*self.delayrange_s length = time*self.subsampled_sampling_rate overlap = 0.5 needed = int(overlap*length) realizable = int(available/needed) #print available, needed, realizable for i in range(realizable): self.old_index += int(needed) # retrieve data d0 = self.ringbuffer0.data_indexed(self.old_index, length) d1 = self.ringbuffer1.data_indexed(self.old_index, length) d0.shape = (d0.size) d1.shape = (d1.size) std0 = numpy.std(d0) std1 = numpy.std(d1) if std0>0. and std1>0.: Xcorr = generalized_cross_correlation(d0, d1) if self.old_Xcorr != None and self.old_Xcorr.shape == Xcorr.shape: # smoothing alpha = 0.3 smoothed_Xcorr = alpha*Xcorr + (1. - alpha)*self.old_Xcorr else: smoothed_Xcorr = Xcorr absXcorr = numpy.abs(smoothed_Xcorr) i = argmax(absXcorr) # normalize #Xcorr_max_norm = Xcorr_unweighted[i]/(d0.size*std0*std1) Xcorr_extremum = smoothed_Xcorr[i] Xcorr_max_norm = abs(smoothed_Xcorr[i])/(3*numpy.std(smoothed_Xcorr)) delay_ms = 1e3*float(i)/self.subsampled_sampling_rate # delays larger than the half of the window most likely are actually negative if delay_ms > 1e3*time/2.: delay_ms -= 1e3*time #numpy.save("Xcorr_%d_%.1f.npy" %(i,delay_ms), Xcorr) #numpy.save("smoothed_Xcorr%d_%.1f.npy" %(i,delay_ms), smoothed_Xcorr) # store for smoothing self.old_Xcorr = smoothed_Xcorr else: delay_ms = 0. Xcorr_max_norm = 0. Xcorr_extremum = 0. # debug wrong phase detection #if Xcorr[i] < 0.: # numpy.save("Xcorr.npy", Xcorr) c = 340. # speed of sound, in meters per second (approximate) distance_m = delay_ms*1e-3*c # home-made measure of the significance slope = 0.12 p = 3 x = (Xcorr_max_norm>1.)*(Xcorr_max_norm-1.) x = (slope*x)**p correlation = int((x/(1. + x))*100) delay_message = "%.1f ms\n= %.2f m" %(delay_ms, distance_m) correlation_message = "%d%%" %(correlation) if Xcorr_extremum >= 0: polarity_message = "In-phase" else: polarity_message = "Reversed phase" channelInfo_message = "" if delay_message <> self.previous_delay_message: self.delay_label.setText(delay_message) self.previous_delay_message = delay_message if correlation_message <> self.previous_correlation_message: self.correlation_label.setText(correlation_message) self.previous_correlation_message = correlation_message if polarity_message <> self.previous_polarity_message: self.polarity_label.setText(polarity_message) self.previous_polarity_message = polarity_message if channelInfo_message <> self.previous_channelInfo_message: self.channelInfo_label.setText(channelInfo_message) self.previous_channelInfo_message = channelInfo_message def set_delayrange(self, delay_s): self.delayrange_s = delay_s # slot def settings_called(self, checked): self.settings_dialog.show() # method def saveState(self, settings): self.settings_dialog.saveState(settings) # method def restoreState(self, settings): self.settings_dialog.restoreState(settings)
def __init__(self, parent = None, logger = None): QtGui.QWidget.__init__(self, parent) self.audiobuffer = None # store the logger instance if logger is None: self.logger = parent.parent.logger else: self.logger = logger self.previous_delay_message = "" self.previous_correlation_message = "" self.previous_polarity_message = "" self.previous_channelInfo_message = "" self.setObjectName("Delay_Estimator_Widget") self.layout = QtGui.QFormLayout(self) self.layout.setObjectName("layout") font = QtGui.QFont() font.setPointSize(14) font.setWeight(75) font.setBold(True) self.delay_label = QtGui.QLabel(self) self.delay_label.setFont(font) self.delay_label.setObjectName("delay_label") self.delayText_label = QtGui.QLabel(self) self.delayText_label.setObjectName("delayText_label") self.delayText_label.setText("Delay:") self.correlation_label = QtGui.QLabel(self) self.correlation_label.setObjectName("Correlation_label") self.correlationText_label = QtGui.QLabel(self) self.correlationText_label.setObjectName("correlationText_label") self.correlationText_label.setText("Confidence:") self.polarity_label = QtGui.QLabel(self) self.polarity_label.setFont(font) self.polarity_label.setObjectName("polarity_label") self.polarityText_label = QtGui.QLabel(self) self.polarityText_label.setObjectName("polarityText_label") self.polarityText_label.setText("Polarity:") self.channelInfo_label = QtGui.QLabel(self) self.channelInfo_label.setObjectName("channelInfo_label") self.layout.addRow(self.delayText_label, self.delay_label) self.layout.addRow(self.correlationText_label, self.correlation_label) self.layout.addRow(self.polarityText_label, self.polarity_label) self.layout.addRow(None, self.channelInfo_label) self.settings_dialog = Delay_Estimator_Settings_Dialog(self, self.logger) self.i = 0 # We will decimate several times # no decimation => 1/fs = 23 µs resolution # 1 ms resolution => fs = 1000 Hz is enough => can divide the sampling rate by 44 ! # if I decimate 2 times (2**2 = 4 => 0.092 ms (3 cm) resolution)! # if I decimate 3 times (2**3 = 8 => 0.184 ms (6 cm) resolution)! # if I decimate 4 times (2**4 = 16 => 0.368 ms (12 cm) resolution)! # if I decimate 5 times (2**5 = 32 => 0.7 ms (24 cm) resolution)! # (actually, I could fit a gaussian on the cross-correlation peak to get # higher resolution even at low sample rates) self.Ndec = 2 self.subsampled_sampling_rate = SAMPLING_RATE/2**(self.Ndec) [self.bdec, self.adec] = generated_filters.params['dec'] self.zfs0 = subsampler_filtic(self.Ndec, self.bdec, self.adec) self.zfs1 = subsampler_filtic(self.Ndec, self.bdec, self.adec) # ringbuffers for the subsampled data self.ringbuffer0 = RingBuffer() self.ringbuffer1 = RingBuffer() self.delayrange_s = DEFAULT_DELAYRANGE # confidence range self.old_Xcorr = None
def test_size_one(self): '''The ring buffer behaves correctly when the max is 1 ''' rb = RingBuffer(1) self.assertTrue(rb.getOldest() == None) rb.add(1) self.assertTrue(rb.getOldest() == 1) rb.add(2) self.assertTrue(rb.getOldest() == 2) rb.add(3) self.assertTrue(rb.getOldest() == 3)