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