class JsonOutput(object):
def __init__(self, file, logLimit):
self.jsonOutputFile = file
self.jsonOutput = {}
self.jsonOutputCoins = {}
self.jsonOutput["raw_data"] = self.jsonOutputCoins
self.jsonOutputLog = RingBuffer(logLimit)
def status(self, status, time):
self.jsonOutput["last_update"] = time
self.jsonOutput["last_status"] = status
def printline(self, line):
self.jsonOutputLog.append(line)
def writeJsonFile(self):
with io.open(self.jsonOutputFile, 'w', encoding='utf-8') as f:
self.jsonOutput["log"] = self.jsonOutputLog.get()
f.write(
unicode(
json.dumps(self.jsonOutput,
ensure_ascii=False,
sort_keys=True)))
f.close()
def statusValue(self, coin, key, value):
if (coin not in self.jsonOutputCoins):
self.jsonOutputCoins[coin] = {}
self.jsonOutputCoins[coin][key] = str(value)
def clearStatusValues(self):
self.jsonOutputCoins = {}
def __init__(self, calib, dt=0.01):
super(INS, self).__init__()
# self.imuCalibration = ImuCalibration('/home/xaedes/bags/mit_kamera/magcalib/2014-08-20-17-33-13.bag')
self.imuCalibration = calib
self.states = ['speed', 'orientation', 'yaw_rate', 'pos_x', 'pos_y']
self.states = dict(map((lambda x: (x, 0)), self.states))
self.dt = dt
self.gyro_integrated = Integrator()
self.accel_integrated = Integrator()
self.orientation_error = WienerKalman(
self.dt, self.imuCalibration.gyro_z_variance,
self.imuCalibration.mag_theta_variance)
self.velocity_error = WienerKalman(
self.dt, self.imuCalibration.accel_x_variance,
self.imuCalibration.odometer_variance)
self.vx_integrated = Integrator()
self.vy_integrated = Integrator()
self.mag_offset = None
self.o_error_last = None
self.velocity_carthesian_history = RingBuffer(100, 2)
class JsonOutput(object):
def __init__(self, file, logLimit):
self.jsonOutputFile = file
self.jsonOutput = {}
self.jsonOutputCoins = {}
self.jsonOutput["raw_data"] = self.jsonOutputCoins
self.jsonOutputLog = RingBuffer(logLimit);
def status(self, status, time):
self.jsonOutput["last_update"] = time
self.jsonOutput["last_status"] = status
def printline(self, line):
self.jsonOutputLog.append(line)
def writeJsonFile(self):
with io.open(self.jsonOutputFile, 'w', encoding='utf-8') as f:
self.jsonOutput["log"] = self.jsonOutputLog.get()
f.write(unicode(json.dumps(self.jsonOutput, ensure_ascii=False, sort_keys=True)))
f.close()
def statusValue(self, coin, key, value):
if(coin not in self.jsonOutputCoins):
self.jsonOutputCoins[coin] = {}
self.jsonOutputCoins[coin][key] = str(value)
def clearStatusValues(self):
self.jsonOutputCoins = {}
class JsonOutput(object):
def __init__(self, file, logLimit, exchange=''):
self.jsonOutputFile = file
self.jsonOutput = {}
self.clearStatusValues()
self.jsonOutputLog = RingBuffer(logLimit)
self.jsonOutput['exchange'] = exchange
def status(self, status, time, days_remaining_msg):
self.jsonOutput["last_update"] = time + days_remaining_msg
self.jsonOutput["last_status"] = status
def printline(self, line):
line = line.replace("\n", ' | ')
self.jsonOutputLog.append(line)
def writeJsonFile(self):
with io.open(self.jsonOutputFile, 'w', encoding='utf-8') as f:
self.jsonOutput["log"] = self.jsonOutputLog.get()
f.write(unicode(json.dumps(self.jsonOutput, ensure_ascii=False, sort_keys=True)))
f.close()
def statusValue(self, coin, key, value):
if coin not in self.jsonOutputCoins:
self.jsonOutputCoins[coin] = {}
self.jsonOutputCoins[coin][key] = str(value)
def clearStatusValues(self):
self.jsonOutputCoins = {}
self.jsonOutput["raw_data"] = self.jsonOutputCoins
self.jsonOutputCurrency = {}
self.jsonOutput["outputCurrency"] = self.jsonOutputCurrency
def outputCurrency(self, key, value):
self.jsonOutputCurrency[key] = str(value)
def __init__(self, file, logLimit, exchange=''):
self.jsonOutputFile = file
self.jsonOutput = {}
self.clearStatusValues()
self.jsonOutputLog = RingBuffer(logLimit)
self.jsonOutput['exchange'] = exchange
self.jsonOutput['label'] = Config.get("BOT", "label", "Lending Bot")
class DistanceData:
def __init__(self, nodeid, buffersize=10):
self.nodeid = nodeid
self.buffersize = buffersize
self.buffer = RingBuffer(buffersize)
def add_observation(self, observation):
self.buffer.append(observation)
def get_filtered_value(self):
#TODO implement filtering
curr_vals = self.buffer.get()
curr_vals.sort()
return curr_vals[len(curr_vals) / 2]
class DistanceData:
def __init__(self, nodeid, buffersize=10):
self.nodeid = nodeid
self.buffersize = buffersize
self.buffer = RingBuffer(buffersize)
def add_observation(self, observation):
self.buffer.append(observation)
def get_filtered_value(self):
#TODO implement filtering
curr_vals = self.buffer.get()
curr_vals.sort()
return curr_vals[len(curr_vals)/2]
def run(self):
rb = RingBuffer(56)
while self.measure:
rb.append(self.ser.read(1)[0])
if rb[0] == ord("B") and rb[1] == ord("E") and rb[54] == ord("E") and rb[55] == ord("N"):
timestamp = int.from_bytes(bytes(rb[2:6]), byteorder="little")
timestamp = timestamp & 0xffffffff
data = np.fromstring(bytes(rb[6:54]), dtype="<f")
if self.start_time is None:
self.start_time = timestamp
self.newData.emit([timestamp-self.start_time, data])
self.ser.close()
def run(self):
rb = RingBuffer(56)
while self.measure:
rb.append(self.ser.read(1)[0])
if rb[0] == ord("B") and rb[1] == ord("E") and rb[54] == ord(
"E") and rb[55] == ord("N"):
timestamp = int.from_bytes(bytes(rb[2:6]), byteorder="little")
timestamp = timestamp & 0xffffffff
data = np.fromstring(bytes(rb[6:54]), dtype="<f")
if self.start_time is None:
self.start_time = timestamp
self.newData.emit([timestamp - self.start_time, data])
self.ser.close()
def main():
# e0 = Element(0)
# e1 = Element(1)
# e2 = Element(2)
# e3 = Element(3)
rb = RingBuffer(4)
rb.append(0)
rb.append(1)
rb.append(2)
rb.append(3)
rb.append(4)
rb.append(5)
print(rb.delete())
print(rb.delete())
print(rb.get_data())
def __init__(self, file, logLimit, exchange=''):
self.jsonOutputFile = file
self.jsonOutput = {}
self.clearStatusValues()
self.jsonOutputLog = RingBuffer(logLimit)
self.jsonOutput['exchange'] = exchange
self.jsonOutput['label'] = Config.get("BOT", "label", "Lending Bot")
def __init__(self, process_config_obj=None):
if process_config_obj != None and type(process_config_obj) == type(
ProcessConfig()):
self.name = process_config_obj.name
self.startcmds = process_config_obj.startcmds
self.wdir = process_config_obj.wdir
self.outlines = process_config_obj.outlines
self.outbuffer = RingBuffer(self.outlines)
self.stdin_pipe = process_config_obj.stdin_pipe
self.envdict = process_config_obj.envdict
self.startuplvl = process_config_obj.startuplvl
###
self.retcode = None
self.subproc = None
self.started = False
self.tmpfpath = None
self.stdout_queue = None
self.stdout_thread = None
class JsonOutput(object):
def __init__(self, file, logLimit):
self.jsonOutputFile = file
self.jsonOutput = {}
self.jsonOutputLog = RingBuffer(logLimit);
def status(self, status, time):
self.jsonOutput["last_update"] = time
self.jsonOutput["last_status"] = status
self.writeJsonFile()
def printline(self, line):
self.jsonOutputLog.append(line)
def writeJsonFile(self):
with io.open(self.jsonOutputFile, 'w', encoding='utf-8') as f:
self.jsonOutput["log"] = self.jsonOutputLog.get()
f.write(unicode(json.dumps(self.jsonOutput, ensure_ascii=False, sort_keys=True)))
f.close()
class JsonOutput(object):
def __init__(self, file, logLimit, exchange=''):
self.jsonOutputFile = file
self.jsonOutput = {}
self.clearStatusValues()
self.jsonOutputLog = RingBuffer(logLimit)
self.jsonOutput['exchange'] = exchange
self.jsonOutput['label'] = Config.get("BOT", "label", "Lending Bot")
def status(self, status, time, days_remaining_msg):
self.jsonOutput["last_update"] = time + days_remaining_msg
self.jsonOutput["last_status"] = status
def printline(self, line):
line = line.replace("\n", ' | ')
self.jsonOutputLog.append(line)
def writeJsonFile(self):
with io.open(self.jsonOutputFile, 'w', encoding='utf-8') as f:
self.jsonOutput["log"] = self.jsonOutputLog.get()
f.write(unicode(json.dumps(self.jsonOutput, ensure_ascii=True, sort_keys=True), errors='replace'))
f.close()
def addSectionLog(self, section, key, value):
if section not in self.jsonOutput:
self.jsonOutput[section] = {}
if key not in self.jsonOutput[section]:
self.jsonOutput[section][key] = {}
self.jsonOutput[section][key] = value
def statusValue(self, coin, key, value):
if coin not in self.jsonOutputCoins:
self.jsonOutputCoins[coin] = {}
self.jsonOutputCoins[coin][key] = str(value)
def clearStatusValues(self):
self.jsonOutputCoins = {}
self.jsonOutput["raw_data"] = self.jsonOutputCoins
self.jsonOutputCurrency = {}
self.jsonOutput["outputCurrency"] = self.jsonOutputCurrency
def outputCurrency(self, key, value):
self.jsonOutputCurrency[key] = str(value)
def run(self):
# Queue
if self.name == 'pack_data_user1':
queue_packets = hold_data.queuePacketsUser1
elif self.name == 'pack_data_user2':
queue_packets = hold_data.queuePacketsUser2
else:
queue_packets = []
logging.error('(Thread) BlockPackData (name : ' + self.name +
'): problem selecting queue, queue_packets is empty')
# Objects
if self.name == 'pack_data_user1':
ring_buffer = RingBuffer(hold_data.bufferBinFilesDataUser1)
elif self.name == 'pack_data_user2':
ring_buffer = RingBuffer(hold_data.bufferBinFilesDataUser2)
else:
ring_buffer = []
logging.error(
'(Thread) BlockPackData (name : ' + self.name +
'): problem selecting ring_buffer, ring_buffer is empty')
while True:
# checks if the queue is full
# size of queue is defined in the HoldData files
if not queue_packets.full():
# --------------------------- Get payload --------------------------
# Get a payload that need to be packed
payload = ring_buffer.get_payload_from_buffer()
# ---------------------------- Add Packet --------------------------
# Make the packet
packet = util.create_packet(payload)
# Add the packet in the queue
queue_packets.put(packet)
# ----------------------------- logger -----------------------------
# Shows who is getting or setting data to or from the buffer/queue
# and shows the size of the queue
# logging.debug('Add data: ' + str(packet)
# + ' : ' + str(queue_packets.qsize()) + ' items in queue')
return
def __init__(self, source):
##
self.source = source
self.mAppSink = None
self.mBus = None
self.mPipeline = None
self.isStreaming = False
self.mBufferRGB = RingBuffer()
self.mWaitEvent = Event()
print("INFO -- Creating Gst Source")
self.start()
print("INFO -- Created Gst Source")
class JsonOutput(object):
def __init__(self, file, logLimit):
self.jsonOutputFile = file
self.jsonOutput = {}
self.jsonOutputLog = RingBuffer(logLimit)
def status(self, status, time):
self.jsonOutput["last_update"] = time
self.jsonOutput["last_status"] = status
self.writeJsonFile()
def printline(self, line):
self.jsonOutputLog.append(line)
def writeJsonFile(self):
with io.open(self.jsonOutputFile, 'w', encoding='utf-8') as f:
self.jsonOutput["log"] = self.jsonOutputLog.get()
f.write(
unicode(
json.dumps(self.jsonOutput,
ensure_ascii=False,
sort_keys=True)))
f.close()
def __init__(self, file, logLimit):
self.jsonOutputFile = file
self.jsonOutput = {}
self.jsonOutputCoins = {}
self.jsonOutput["raw_data"] = self.jsonOutputCoins
self.jsonOutputLog = RingBuffer(logLimit);
def __init__(self):
super(App, self).__init__()
# load background
self.background = Background(filename="background.png")
# get array copy of background image
self.background.arr = pygame.surfarray.array3d(self.background.img)
# create bw from image
_, self.background.arr_bw = cv2.threshold(self.background.arr[:, :, 0],
128, 1, cv2.THRESH_BINARY)
# print self.background.arr_bw.shape, self.background.arr_bw.dtype
# self.background.arr_dist = cv2.distanceTransform(self.background.arr_bw, cv.CV_DIST_L1, 3)
# get nearest (zero) pixel labels with corresponding distances
self.background.arr_dist, self.labels = cv2.distanceTransformWithLabels(
self.background.arr_bw,
cv.CV_DIST_L1,
3,
labelType=cv2.DIST_LABEL_PIXEL)
self.distances = self.background.arr_dist
### get x,y coordinates for each label
# get positions of zero points
zero_points = Utils.zero_points(self.background.arr_bw)
# get labels for zero points
zero_labels = self.labels[zero_points[:, 0], zero_points[:, 1]]
# create dictionary mapping labels to zero point positions
self.label_positions = dict(
zip(zero_labels, zip(zero_points[:, 0], zero_points[:, 1])))
# create hilbert curve lookup table
self.hilbert = Hilbert.hilbert_lookup(*self.background.arr.shape[:2])
# provide a rgb variant of dist for display
self.background.arr_dist_rgb = self.background.arr.copy()
self.background.arr_dist_rgb[:, :, 0] = self.background.arr_dist
self.background.arr_dist_rgb[:, :, 1] = self.background.arr_dist
self.background.arr_dist_rgb[:, :, 2] = self.background.arr_dist
# print a.shape
self.setup_pygame()
self.events = Events()
self.lane = Lane(self.events)
self.lane.load("parkour.sv")
# self.lane.add_support_point(100,100)
# self.lane.add_support_point(200,100)
# self.lane.add_support_point(200,200)
# self.lane.add_support_point(100,200)
self.optimize = Optimize(self.lane)
self.cars = []
# for k in range(1):
# self.cars.append(Car(x=150+k*5,y=100,theta=np.random.randint(0,360),speed=np.random.randint(45,180)))
self.cars.append(Car(x=50, y=250, theta=90, speed=1 * 1.5 * 90))
self.cars.append(Car(x=50, y=250, theta=90, speed=1 * 90)) # [1] human
self.cars.append(Car(x=50, y=250, theta=90,
speed=1 * 90)) # [2] ghost of ins estimating [0]
self.action = None
self.human = HumanController()
self.heuristic = Heuristic(self.lane)
Node.heuristic = self.heuristic
self.onestep = OneStepLookaheadController(self.cars, self.lane,
self.heuristic)
self.nstep = NStepLookaheadController(self.cars, self.lane,
self.heuristic, 2)
self.bestfirst = BestFirstController(self.cars, self.lane,
self.heuristic)
self.controller = self.bestfirst
self.cars[0].camview = CamView(self.cars[0], self.background.arr)
self.cars[0].camview.register_events(self.events)
self.cars[0].controller = self.controller
self.cars[0].collision = False
self.cars[0].imu = IMU(self.cars[0])
self.cars[0].ins = INS(self.cars[0].imu.calibration_noise)
self.cars[0].ins.update_pose(self.cars[0].x,
self.cars[0].y,
self.cars[0].theta / Utils.d2r,
gain=1)
self.insghost = INSGhostController(self.cars[0].ins)
self.cars[1].controller = self.human
self.cars[2].controller = self.insghost
self.cars[2].collision = False
self.cars[2].size *= 1.25
self.cars[2].camview = CamView(self.cars[2],
self.background.arr_dist_rgb,
width=275,
height=275,
offset=(0, 75),
angle_offset=-25)
self.cars[2].camview.register_events(self.events)
self.cars[0].name = "actual"
self.cars[1].name = "human"
self.cars[2].name = "estimate"
# this causes the controller of cars[0] to use the information from cars[0].ghost but act on cars[0]
# self.cars[0].ghost = self.cars[2]
# self.window = Window(self.screen, self.events, 300, 200, "caption")
self.grid = Grid(50, 50, *self.size)
self.last_distance_grid_update = time() - 10
self.update_distance_grid()
self.done = False
for car in self.cars:
# save original speed
if not hasattr(car, "speed_on"):
car.speed_on = car.speed
# toggle speed
car.speed = car.speed_on - car.speed
# car.pause = not car.pause
self.plot_window_size = 100
self.xyt_corr_ring_buffer = RingBuffer(self.plot_window_size,
channels=3)
self.xyt_corr_plot = RingBufferPlot(self.xyt_corr_ring_buffer)
# self.normal_test_p_value_plot = RingBufferPlot(RingBuffer(self.plot_window_size,channels=self.xyt_corr_ring_buffer.channels))
self.std_plot = RingBufferPlot(
RingBuffer(self.plot_window_size,
channels=self.xyt_corr_ring_buffer.channels))
self.velocity_carthesian_history_plot = RingBufferPlot(
self.cars[0].ins.velocity_carthesian_history)
# self.hist_plot = HistogramPlot(10)
self.register_events()
self.spin()
'''
https://blog.francoismaillet.com/epic-celebration/
'''
import pyaudio
import librosa
import numpy as np
import requests
from RingBuffer import RingBuffer
import sounddevice as sd
from time import sleep
import time
import serial
# ring buffer will keep the last 2 seconds worth of audio
ringBuffer = RingBuffer(5 * 22050)
arduinodata=serial.Serial('com4',9600,timeout=(1))
tempo=0
def callback(in_data, frame_count, time_info, flag):
audio_data = np.frombuffer(in_data, dtype=np.float32)
# we trained on audio with a sample rate of 22050 so we need to convert it
audio_data = librosa.resample(audio_data, 44100, 22050)
ringBuffer.extend(audio_data)
# analysis beat
#print(audio_data)
onset_env = librosa.onset.onset_strength(ringBuffer.get(), sr=22050, aggregate=np.median)
tempo, beat_times = librosa.beat.beat_track(onset_envelope=onset_env, sr=22050)
#tempo = librosa.beat.tempo(ringBuffer.get(), sr=22050, start_bpm=60)
print("tempo: ", tempo)
arduinodata.reset_input_buffer()
arduinodata.reset_output_buffer()
if tempo<90:
def __init__(self, file, logLimit):
self.jsonOutputFile = file
self.jsonOutput = {}
self.jsonOutputCoins = {}
self.jsonOutput["raw_data"] = self.jsonOutputCoins
self.jsonOutputLog = RingBuffer(logLimit)
class Process:
""" Represents a Process that can be launched, monitored and killed """
def __init__(self, process_config_obj=None):
if process_config_obj != None and type(process_config_obj) == type(
ProcessConfig()):
self.name = process_config_obj.name
self.startcmds = process_config_obj.startcmds
self.wdir = process_config_obj.wdir
self.outlines = process_config_obj.outlines
self.outbuffer = RingBuffer(self.outlines)
self.stdin_pipe = process_config_obj.stdin_pipe
self.envdict = process_config_obj.envdict
self.startuplvl = process_config_obj.startuplvl
###
self.retcode = None
self.subproc = None
self.started = False
self.tmpfpath = None
self.stdout_queue = None
self.stdout_thread = None
def start(self):
if self.started:
return
tmpfhandle, self.tmpfpath = tempfile.mkstemp()
tmpf = os.fdopen(tmpfhandle, 'w')
tmpf.write('#!{shellcmd}\n'.format(shellcmd=SHELL_CMD))
tmpf.write('trap "exit" INT TERM\n')
tmpf.write('trap "kill 0" EXIT\n')
tmpf.write('cd "{path}"\n'.format(path=self.wdir))
if type(self.startcmds) == type([]):
for l in self.startcmds:
if not l.endswith(' $'):
l = l + ' $'
tmpf.write(l + "\n")
elif type(self.startcmds) == type(""):
bgmode = ' $' if not self.startcmds.endswith(' $') else ''
tmpf.write(self.startcmds + bgmode + "\n")
tmpf.write('sleep infinity\n')
tmpf.close()
#tmpf = open(self.tmpfpath, "r")
#for l in tmpf:
# print("{l}".format(l=l))
#tmpf.close()
stdin_arg = subprocess.PIPE if self.stdin_pipe else None
env_arg = dict(os.environ)
env_arg.update(self.envdict)
try:
self.subproc = subprocess.Popen([SHELL_CMD, self.tmpfpath],
bufsize=0,
preexec_fn=os.setpgrp,
stdin=stdin_arg,
stdout=subprocess.PIPE,
stderr=subprocess.STDOUT,
cwd=self.wdir,
env=env_arg)
except OSError:
print("Process.start(): subprocess.Popen(...) raised OSError")
return
self.stdout_queue = Queue()
self.stdout_thread = Thread(target=self._enqueue_stdout,
args=(self.subproc.stdout,
self.stdout_queue))
self.stdout_thread.daemon = True
self.stdout_thread.start()
self.started = True
def _update_out_buffer(self):
if not self.started:
return
pollresult = self.subproc.poll()
if pollresult != None:
self.retcode = pollresult
while True:
try:
line = self.stdout_queue.get_nowait()
except Empty:
break
if (len(line) > 0 and line[-1] == '\n'):
line = line[:-1]
self.outbuffer.append(line)
def terminate(self):
if not self.started:
return
os.killpg(os.getpgid(self.subproc.pid), signal.SIGTERM)
self.started = False
def kill(self):
if not self.started:
return
os.killpg(os.getpgid(self.subproc.pid), signal.SIGKILL)
self.started = False
def terminated(self):
return self.retcode != None
def running(self):
return self.started and not self.terminated()
def return_code(self):
return self.retcode
def get_output(self):
if not self.started:
return
self._update_out_buffer()
return self.outbuffer.get()
def send(self, input_data):
if not self.started:
return
stdoutdata, stderrdata = self.subproc.communicate(input_data)
pollresult = self.subproc.poll()
if pollresult != None:
self.retcode = pollresult
return
lines = stdoutdata.split('\n')
for l in lines:
self.outbuffer.append(l)
def _enqueue_stdout(self, out, queue):
for line in iter(out.readline, b''):
queue.put(line.decode("utf-8"))
out.close()
class INS(object):
# gyro and orientation in rad/s
sensors = ['accel', 'odometer', 'gyro', 'mag_x', 'mag_y']
def __init__(self, calib, dt=0.01):
super(INS, self).__init__()
# self.imuCalibration = ImuCalibration('/home/xaedes/bags/mit_kamera/magcalib/2014-08-20-17-33-13.bag')
self.imuCalibration = calib
self.states = ['speed', 'orientation', 'yaw_rate', 'pos_x', 'pos_y']
self.states = dict(map((lambda x: (x, 0)), self.states))
self.dt = dt
self.gyro_integrated = Integrator()
self.accel_integrated = Integrator()
self.orientation_error = WienerKalman(
self.dt, self.imuCalibration.gyro_z_variance,
self.imuCalibration.mag_theta_variance)
self.velocity_error = WienerKalman(
self.dt, self.imuCalibration.accel_x_variance,
self.imuCalibration.odometer_variance)
self.vx_integrated = Integrator()
self.vy_integrated = Integrator()
self.mag_offset = None
self.o_error_last = None
self.velocity_carthesian_history = RingBuffer(100, 2)
def calibrate_sensors(self, Z):
Z = Z.copy()
Z[0] = (Z[0] - self.imuCalibration.accel_x_bias
) * self.imuCalibration.accel_scale
Z[2] = (Z[2] - self.imuCalibration.gyro_z_bias
) * self.imuCalibration.gyro_scale
Z[[3, 4]] -= self.imuCalibration.mag_offset
Z[[3, 4]] /= self.imuCalibration.mag_scale
return Z
def update_pose(self, dpos_x, dpos_y, dorientation, gain=0.95):
self.vx_integrated.sum = gain * (self.vx_integrated.sum + dpos_x) + (
1 - gain) * self.vx_integrated.sum
self.vy_integrated.sum = gain * (self.vy_integrated.sum + dpos_y) + (
1 - gain) * self.vy_integrated.sum
# self.states['orientation'] = gain*orientation + (1-gain)*self.states['orientation']
self.gyro_integrated.sum = gain * (
self.gyro_integrated.sum +
dorientation) + (1 - gain) * self.gyro_integrated.sum
# self.orientation_error.update(np.matrix([0]))
# self.orientation_error.update(np.matrix([0]))
# self.orientation_error.update(np.matrix([0]))
# self.orientation_error.update(np.matrix([0]))
def update(self, Z, dt):
# Z contains data for ['accel','odometer','gyro','mag_x', 'mag_y']
# 0 1 2 3 4
# calibrate sensors
Z = self.calibrate_sensors(Z)
sensor_accel = Z[0]
sensor_odometer = Z[1]
sensor_gyro = Z[2]
sensor_mag_x = Z[3]
sensor_mag_y = Z[4]
self.orientation_error.update_dt(dt)
self.velocity_error.update_dt(dt)
# integrate gyro to get orientation estimate
self.gyro_integrated.add(sensor_gyro, dt)
# calculate orientation from magnetometer
mag = np.arctan2(sensor_mag_y, sensor_mag_x)
# first orientation from magnetometer should be zero
# if self.mag_offset is None:
# self.mag_offset = mag
# mag -= self.mag_offset
# fix orientation 2pi jumps
mag_diff = mag - self.states['orientation']
mag_diff -= np.round(mag_diff / (2 * math.pi)) * (2 * math.pi)
mag = self.states['orientation'] + mag_diff
self.mag = mag
# update orientation error
o_error = self.gyro_integrated.sum - mag
self.orientation_error.last = o_error
self.orientation_error.update(np.matrix([o_error]))
self.orientation_error.predict()
# output corrected orientation estimate
self.states[
'orientation'] = self.gyro_integrated.sum - self.orientation_error.x[
0, 0]
# integrate acceleration to get speed estimate
self.accel_integrated.add(sensor_accel, dt)
# update speed error
vel_error = self.accel_integrated.sum - sensor_odometer
self.velocity_error.update(np.matrix([vel_error]))
self.velocity_error.predict()
# output corrected speed estimate
self.states[
'speed'] = self.accel_integrated.sum - self.velocity_error.x[0, 0]
# resolution of velocity vector
velocity = Utils.rotate_points([(self.states['speed'], 0)],
self.states['orientation'] /
Utils.d2r)[0]
self.velocity_carthesian_history.add(velocity)
# integrate velocity vector
self.vx_integrated.add(velocity[0], dt)
self.vy_integrated.add(velocity[1], dt)
# feed through gyro
self.states['yaw_rate'] = sensor_gyro
# print self.states['yaw_rate'] / Utils.d2r
# output positions
self.states['pos_x'] = self.vx_integrated.sum
self.states['pos_y'] = self.vy_integrated.sum
def get_state(self, name):
# name can be one of ['speed', 'orientation', 'yaw_rate', 'pos_x', 'pos_y']
return self.states[name]
def __init__(self, logger):
"""
Connects to LoRaWAN using OTAA and sets up a ring buffer for storing messages.
:param logger: status logger
:type logger: LoggerFactory object
"""
self.logger = logger
self.message_limit = int(
float(config.get_config("fair_access")) /
(float(config.get_config("air_time")) / 1000))
self.transmission_date = config.get_config(
"transmission_date") # last date when lora was transmitting
today = time.gmtime()
date = str(today[0]) + str(today[1]) + str(today[2])
if self.transmission_date == date: # if device was last transmitting today
self.message_count = config.get_config(
"message_count") # get number of messages sent today
else:
self.message_count = 0 # if device was last transmitting a day or more ago, reset message_count for the day
self.transmission_date = date
config.save_config({
"message_count": self.message_count,
"transmission_date": date
})
regions = {
"Europe": LoRa.EU868,
"Asia": LoRa.AS923,
"Australia": LoRa.AU915,
"United States": LoRa.US915
}
region = regions[config.get_config("region")]
self.lora = LoRa(mode=LoRa.LORAWAN, region=region, adr=True)
# create an OTAA authentication parameters
app_eui = ubinascii.unhexlify(config.get_config("application_eui"))
app_key = ubinascii.unhexlify(config.get_config("app_key"))
# join a network using OTAA (Over the Air Activation)
self.lora.join(activation=LoRa.OTAA,
auth=(app_eui, app_key),
timeout=0)
# create a LoRa socket
self.lora_socket = socket.socket(socket.AF_LORA, socket.SOCK_RAW)
# request acknowledgment of data sent
# self.lora_socket.setsockopt(socket.SOL_LORA, socket.SO_CONFIRMED, True)
# do not request acknowledgment of data sent
self.lora_socket.setsockopt(socket.SOL_LORA, socket.SO_CONFIRMED,
False)
# sets timeout for sending data
self.lora_socket.settimeout(
int(config.get_config("lora_timeout")) * 1000)
# set up callback for receiving downlink messages
self.lora.callback(trigger=LoRa.RX_PACKET_EVENT,
handler=self.lora_recv)
# initialises circular lora stack to back up data up to about 22.5 days depending on the length of the month
self.lora_buffer = RingBuffer(self.logger, s.processing_path,
s.lora_file_name,
31 * self.message_limit, 100)
try: # this fails if the buffer is empty
self.check_date() # remove messages that are over a month old
except Exception as e:
pass
class LoRaWAN:
def __init__(self, logger):
"""
Connects to LoRaWAN using OTAA and sets up a ring buffer for storing messages.
:param logger: status logger
:type logger: LoggerFactory object
"""
self.logger = logger
self.message_limit = int(
float(config.get_config("fair_access")) /
(float(config.get_config("air_time")) / 1000))
self.transmission_date = config.get_config(
"transmission_date") # last date when lora was transmitting
today = time.gmtime()
date = str(today[0]) + str(today[1]) + str(today[2])
if self.transmission_date == date: # if device was last transmitting today
self.message_count = config.get_config(
"message_count") # get number of messages sent today
else:
self.message_count = 0 # if device was last transmitting a day or more ago, reset message_count for the day
self.transmission_date = date
config.save_config({
"message_count": self.message_count,
"transmission_date": date
})
regions = {
"Europe": LoRa.EU868,
"Asia": LoRa.AS923,
"Australia": LoRa.AU915,
"United States": LoRa.US915
}
region = regions[config.get_config("region")]
self.lora = LoRa(mode=LoRa.LORAWAN, region=region, adr=True)
# create an OTAA authentication parameters
app_eui = ubinascii.unhexlify(config.get_config("application_eui"))
app_key = ubinascii.unhexlify(config.get_config("app_key"))
# join a network using OTAA (Over the Air Activation)
self.lora.join(activation=LoRa.OTAA,
auth=(app_eui, app_key),
timeout=0)
# create a LoRa socket
self.lora_socket = socket.socket(socket.AF_LORA, socket.SOCK_RAW)
# request acknowledgment of data sent
# self.lora_socket.setsockopt(socket.SOL_LORA, socket.SO_CONFIRMED, True)
# do not request acknowledgment of data sent
self.lora_socket.setsockopt(socket.SOL_LORA, socket.SO_CONFIRMED,
False)
# sets timeout for sending data
self.lora_socket.settimeout(
int(config.get_config("lora_timeout")) * 1000)
# set up callback for receiving downlink messages
self.lora.callback(trigger=LoRa.RX_PACKET_EVENT,
handler=self.lora_recv)
# initialises circular lora stack to back up data up to about 22.5 days depending on the length of the month
self.lora_buffer = RingBuffer(self.logger, s.processing_path,
s.lora_file_name,
31 * self.message_limit, 100)
try: # this fails if the buffer is empty
self.check_date() # remove messages that are over a month old
except Exception as e:
pass
def lora_recv(self, arg):
"""
Callback for receiving packets through LoRaWAN. Decodes messages to commands for updating over WiFi.
Requires a dummy argument.
"""
payload = self.lora_socket.recv(600) # receive bytes message
self.logger.info("Lora message received")
msg = payload.decode() # convert to string
try:
if msg == "0": # reboot device
self.logger.info("Reset triggered over LoRa")
self.logger.info("Rebooting...")
machine.reset()
elif msg == "1": # start software update
self.logger.info("Software update triggered over LoRa")
config.save_config({"update": True})
machine.reset()
else:
split_msg = msg.split(":")
if split_msg[0] == "2": # update wifi credentials
self.logger.info("WiFi credentials updated over LoRa")
config.save_config({
"SSID": split_msg[1],
"wifi_password": split_msg[2]
})
elif split_msg[
0] == "3": # update wifi credentials and start software update
self.logger.info("WiFi credentials updated over LoRa")
config.save_config({
"SSID": split_msg[1],
"wifi_password": split_msg[2]
})
self.logger.info("Software update triggered over LoRa")
config.save_config({"update": True})
machine.reset()
else:
self.logger.error("Unknown command received over LoRa")
except Exception as e:
self.logger.exception(
"Failed to interpret message received over LoRa")
def lora_send(self):
"""Lora send method to run as a thread. Checks if messages are up to date in the lora buffer, pops the one on
top of the stack, encodes it to a message and sends it to the right port.
Takes two dummy arguments required by the threading library"""
if lora_lock.locked():
self.logger.debug("Waiting for other lora thread to finish")
with lora_lock:
self.logger.debug("LoRa thread started")
try:
self.check_date() # remove messages that are over a month old
if self.lora.has_joined():
self.logger.debug("LoRa connected")
else:
raise Exception("LoRa is not connected")
if s.lora_file_name not in os.listdir(s.root_path +
s.processing):
raise Exception('LoRa - File: {} does not exist'.format(
s.lora_file_name))
else:
port, payload = self.get_sending_details()
self.lora_socket.bind(
port) # bind to port to decode at backend
self.lora_socket.send(
payload) # send payload to the connected socket
self.logger.debug("LoRa - sent payload")
self.message_count += 1 # increment number of files sent over LoRa today
config.save_config({"message_count": self.message_count
}) # save number of messages today
# remove message sent
self.lora_buffer.remove_head()
except Exception:
self.logger.exception("Sending payload over LoRaWAN failed")
blink_led((0x550000, 0.4, True))
def get_sending_details(self):
"""
Gets message sitting on top of the lora stack, and constructs payload according to its format
:return: port, payload
:rtype: int, bytes
"""
buffer_line = self.lora_buffer.read()
buffer_lst = buffer_line.split(
',') # convert string to a list of strings
# get structure and port from format
fmt = buffer_lst[2] # format is third item in the list
fmt_dict = {
"TPP": s.TPP,
"TP": s.TP,
"PP": s.PP,
"P": s.P,
"T": s.T,
"G": s.G
}
port_struct_dict = fmt_dict[
fmt] # get dictionary corresponding to the format
port = port_struct_dict["port"] # get port corresponding to the format
structure = port_struct_dict[
"structure"] # get structure corresponding to the format
# cast message according to format to form valid payload
lst_message = buffer_lst[3:] # chop year, month and format off
cast_lst_message = []
for i in range(
(len(structure) -
1)): # iterate for length of structure having '<' stripped
if structure[i +
1] == 'f': # iterate through structure ignoring '<'
cast_lst_message.append(float(
lst_message[i])) # cast to float if structure is 'f'
else:
cast_lst_message.append(int(
lst_message[i])) # cast to int otherwise
# pack payload
self.logger.debug("Sending over LoRa: " + str(cast_lst_message))
payload = struct.pack(
structure, *cast_lst_message
) # define payload with given structure and list of averages
return port, payload
# removes messages from end of lora stack until they are all within a month
def check_date(self):
"""
Checks recursively if the message on the bottom of the stack is within a month
"""
buffer_line = self.lora_buffer.read(
read_tail=True) # read the tail of the lora_buffer
buffer_lst = buffer_line.split(
',') # convert string to a list of strings
time_now = time.gmtime() # get current date
# get year, month and minutes of the month now
year_now, month_now, minutes_now = time_now[0], time_now[
1], minutes_of_the_month()
# get year, month and minutes of the month of the last message in the lora_buffer
year_then, month_then, minutes_then = int(buffer_lst[0]) + 2000, int(
buffer_lst[1]), int(buffer_lst[4])
# logic to decide if message is older than a month
if year_then < year_now or month_then < month_now:
if (month_then + 1
== month_now) or (month_then == 12 and month_now == 1
and year_then + 1 == year_now):
if minutes_then < minutes_now + 24 * 60:
self.lora_buffer.remove_tail() # remove message
self.check_date() # call recursively
else:
self.lora_buffer.remove_tail() # remove message
self.check_date() # call recursively
def process_subtitles(self, srtpath):
'''
Process ``scenario.txt``,
translate «date-based» subtitles to time-bases SRT-subtitles.
'''
lf = open(self.scenariopath, "r")
subtitles = lf.read().decode("utf-8")
lf.close()
subtitles = re.sub(r"(?m)^#[^\n]*\n?", "", subtitles)
scenario = []
for line in subtitles.splitlines():
if len(line.split(" ", 1))>1:
datas, text = line.split(" ", 1)
vcstime = None
datas = datas.replace(u'\ufeff','')
for format in ["%d.%m.%Y", "%Y-%m-%d"]:
try:
vcstime = time.strptime(datas.encode("latin-1"), format)
break
except ValueError:
pass
if not vcstime:
print "Warning: can not understand date <%s>" % datas
subtitletimestart = (time.mktime(vcstime) -
time.mktime(self.startdate)) * self.movielength / self.historylength
if -1 < subtitletimestart < 0:
subtitletimestart = 0
if subtitletimestart < 0:
print "Date <%s> before start of work history" % datas
else:
scenario.append( (subtitletimestart, text) )
if len(scenario) == 0:
return
scenario.sort()
messages = RingBuffer(3)
class SRTText:
"""
Subtitles in SRT format
"""
def __init__(self):
self.srt = ""
self.index = 0
self.stime = float(max([scenario[0][0]-1, 0]))
self.etime = 0.0
def append(self, messages):
"""
Append subtitle from list of messages
"""
ml = messages.get()
msg = " * ".join(ml)
subtitletimestart_str = time.strftime("%H:%M:%S", time.gmtime(self.stime))
subtitletimeend_str = time.strftime("%H:%M:%S", time.gmtime(self.etime))
self.srt += """
%d
%s,000 --> %s,000
%s
""" % (self.index, subtitletimestart_str, subtitletimeend_str, msg)
self.stime = self.etime
self.index += 1
default_subtitle_time = 3
srt_text = SRTText()
for s in scenario:
newtime = float(s[0])
if newtime > srt_text.stime + default_subtitle_time:
srt_text.etime = srt_text.stime + default_subtitle_time
srt_text.append(messages)
messages = RingBuffer(3)
srt_text.stime = newtime
else:
srt_text.etime = newtime
srt_text.append(messages)
messages.append(s[1])
srt_text.etime = min(srt_text.etime + 3, self.movielength)
srt_text.append(messages)
lf = open(srtpath, "w")
lf.write(srt_text.srt.encode("utf-8"))
lf.close()
def __init__(self, file, logLimit):
self.jsonOutputFile = file
self.jsonOutput = {}
self.jsonOutputLog = RingBuffer(logLimit)
class App(object):
"""docstring for App"""
def __init__(self):
super(App, self).__init__()
# load background
self.background = Background(filename="background.png")
# get array copy of background image
self.background.arr = pygame.surfarray.array3d(self.background.img)
# create bw from image
_, self.background.arr_bw = cv2.threshold(self.background.arr[:, :, 0],
128, 1, cv2.THRESH_BINARY)
# print self.background.arr_bw.shape, self.background.arr_bw.dtype
# self.background.arr_dist = cv2.distanceTransform(self.background.arr_bw, cv.CV_DIST_L1, 3)
# get nearest (zero) pixel labels with corresponding distances
self.background.arr_dist, self.labels = cv2.distanceTransformWithLabels(
self.background.arr_bw,
cv.CV_DIST_L1,
3,
labelType=cv2.DIST_LABEL_PIXEL)
self.distances = self.background.arr_dist
### get x,y coordinates for each label
# get positions of zero points
zero_points = Utils.zero_points(self.background.arr_bw)
# get labels for zero points
zero_labels = self.labels[zero_points[:, 0], zero_points[:, 1]]
# create dictionary mapping labels to zero point positions
self.label_positions = dict(
zip(zero_labels, zip(zero_points[:, 0], zero_points[:, 1])))
# create hilbert curve lookup table
self.hilbert = Hilbert.hilbert_lookup(*self.background.arr.shape[:2])
# provide a rgb variant of dist for display
self.background.arr_dist_rgb = self.background.arr.copy()
self.background.arr_dist_rgb[:, :, 0] = self.background.arr_dist
self.background.arr_dist_rgb[:, :, 1] = self.background.arr_dist
self.background.arr_dist_rgb[:, :, 2] = self.background.arr_dist
# print a.shape
self.setup_pygame()
self.events = Events()
self.lane = Lane(self.events)
self.lane.load("parkour.sv")
# self.lane.add_support_point(100,100)
# self.lane.add_support_point(200,100)
# self.lane.add_support_point(200,200)
# self.lane.add_support_point(100,200)
self.optimize = Optimize(self.lane)
self.cars = []
# for k in range(1):
# self.cars.append(Car(x=150+k*5,y=100,theta=np.random.randint(0,360),speed=np.random.randint(45,180)))
self.cars.append(Car(x=50, y=250, theta=90, speed=1 * 1.5 * 90))
self.cars.append(Car(x=50, y=250, theta=90, speed=1 * 90)) # [1] human
self.cars.append(Car(x=50, y=250, theta=90,
speed=1 * 90)) # [2] ghost of ins estimating [0]
self.action = None
self.human = HumanController()
self.heuristic = Heuristic(self.lane)
Node.heuristic = self.heuristic
self.onestep = OneStepLookaheadController(self.cars, self.lane,
self.heuristic)
self.nstep = NStepLookaheadController(self.cars, self.lane,
self.heuristic, 2)
self.bestfirst = BestFirstController(self.cars, self.lane,
self.heuristic)
self.controller = self.bestfirst
self.cars[0].camview = CamView(self.cars[0], self.background.arr)
self.cars[0].camview.register_events(self.events)
self.cars[0].controller = self.controller
self.cars[0].collision = False
self.cars[0].imu = IMU(self.cars[0])
self.cars[0].ins = INS(self.cars[0].imu.calibration_noise)
self.cars[0].ins.update_pose(self.cars[0].x,
self.cars[0].y,
self.cars[0].theta / Utils.d2r,
gain=1)
self.insghost = INSGhostController(self.cars[0].ins)
self.cars[1].controller = self.human
self.cars[2].controller = self.insghost
self.cars[2].collision = False
self.cars[2].size *= 1.25
self.cars[2].camview = CamView(self.cars[2],
self.background.arr_dist_rgb,
width=275,
height=275,
offset=(0, 75),
angle_offset=-25)
self.cars[2].camview.register_events(self.events)
self.cars[0].name = "actual"
self.cars[1].name = "human"
self.cars[2].name = "estimate"
# this causes the controller of cars[0] to use the information from cars[0].ghost but act on cars[0]
# self.cars[0].ghost = self.cars[2]
# self.window = Window(self.screen, self.events, 300, 200, "caption")
self.grid = Grid(50, 50, *self.size)
self.last_distance_grid_update = time() - 10
self.update_distance_grid()
self.done = False
for car in self.cars:
# save original speed
if not hasattr(car, "speed_on"):
car.speed_on = car.speed
# toggle speed
car.speed = car.speed_on - car.speed
# car.pause = not car.pause
self.plot_window_size = 100
self.xyt_corr_ring_buffer = RingBuffer(self.plot_window_size,
channels=3)
self.xyt_corr_plot = RingBufferPlot(self.xyt_corr_ring_buffer)
# self.normal_test_p_value_plot = RingBufferPlot(RingBuffer(self.plot_window_size,channels=self.xyt_corr_ring_buffer.channels))
self.std_plot = RingBufferPlot(
RingBuffer(self.plot_window_size,
channels=self.xyt_corr_ring_buffer.channels))
self.velocity_carthesian_history_plot = RingBufferPlot(
self.cars[0].ins.velocity_carthesian_history)
# self.hist_plot = HistogramPlot(10)
self.register_events()
self.spin()
def setup_pygame(self):
pygame.init()
# Set the width and height of the screen [width, height]
self.size = (self.background.rect.width, self.background.rect.height)
self.screen = pygame.display.set_mode(self.size)
self.font = pygame.font.SysFont("arial", 10)
Draw.font = self.font
pygame.display.set_caption("My Game")
def draw_string(self, string, x, y, color=Draw.BLACK):
Draw.draw_string(self.screen, self.font, string, x, y, color)
def draw(self):
self.background.draw(self.screen)
# self.grid.draw(self.screen)
self.lane.draw(self.screen)
# self.camview.draw(self.screen)
# blende tatsächlichen view in view von ins estimated ein
# actual_view = self.cars[0].camview.view
# ins_view = self.cars[2].camview.view
# if actual_view is not None and ins_view is not None:
# actual_view = actual_view.copy()
# ins_view = ins_view.copy()
# # horizontal center alignment of actual view and ins view
# low_x = math.floor(actual_view.shape[0] / 2)
# hgh_x = low_x + math.ceil((actual_view.shape[0] / 2) - low_x)
# x1 = self.cars[2].camview.offset[0]+math.floor(ins_view.shape[0]/2)-low_x
# x2 = self.cars[2].camview.offset[0]+math.floor(ins_view.shape[0]/2)+hgh_x
# # vertical placement
# y1 = math.floor(self.cars[2].camview.offset[1])
# y2 = math.floor(self.cars[2].camview.offset[1])+actual_view.shape[1]
# # draw edges of actual_view with white in ins_view
# np.maximum( # only draw if brighter
# 255-actual_view[:,:,:], #
# ins_view[y1:y2,x1:x2,:], #
# ins_view[y1:y2,x1:x2,:]) # dst, in-place
# # draw edges of actual_view with black in ins_view
# # np.minimum( # only draw if darker
# # actual_view[:,:,:],
# # ins_view[y1:y2,x1:x2,:],
# # ins_view[y1:y2,x1:x2,:]) # dst, in-place
# # show image
# cv2.imshow("0 in 2",ins_view)
# # # # bw
# bw = actual_view[:,:,0]
# # extract edge pixel positions from actual_view
# xx,yy = np.meshgrid(*map(np.arange,bw.shape))
# xx,yy = xx[bw == 0], yy[bw == 0] # select black pixel positions
# # edge = np.array(zip(yy,xx),dtype="int32")
# skip = 20
# xx,yy = xx[::skip],yy[::skip]
# xx,yy = yy,xx
# if xx.shape[0] > 0:
# # transform edge positions into car coordinate system, with car position on (0,0) and y-axis pointing to driving direction
# # reverses camview offset and angle offset
# # yy -= self.cars[0].camview.offset[0]
# xx = self.cars[0].camview.width - (xx)
# xx -= self.cars[0].camview.offset[0]
# yy -= self.cars[0].camview.offset[1]
# # a second rotation to account for the car theta can be integrated into the camview.angle_offset rotation
# xxyy = np.array(Utils.rotate_points(zip(xx,yy),self.cars[0].camview.angle_offset + self.cars[2].theta))
# xx = xxyy[:,0]
# yy = xxyy[:,1]
# # add car offset
# xx += self.cars[2].x
# yy += self.cars[2].y
# # to use as index
# xx = np.round(xx).astype("int32")
# yy = np.round(yy).astype("int32")
# # transform edge positions into global card using ins estimate
# # show edge on distance transformation of bg
# tmp = (self.background.arr_dist/self.background.arr_dist.max()).copy()
# in_bounds = np.logical_and(np.logical_and(xx>=0,yy>=0),np.logical_and(xx<tmp.shape[0],yy<tmp.shape[1]))
# tmp[xx[in_bounds],yy[in_bounds]] = tmp.max()
# cv2.imshow("tmp",tmp)
# # show distance transformation of bg
# cv2.imshow("bg dist",self.background.arr_dist/self.background.arr_dist.max())
# Draw car
for car in self.cars:
if self.controller is not None:
if hasattr(self.controller, "action"):
car.draw(self.screen, self.controller.action)
self.controller.draw(self.screen, car)
else:
car.draw(self.screen)
if hasattr(car, "camview"):
car.camview.draw(self.screen)
def on_keyup(self, event):
if event.key == pygame.K_SPACE:
for car in self.cars:
# save original speed
if not hasattr(car, "speed_on"):
car.speed_on = car.speed
# toggle speed
car.speed = car.speed_on - car.speed
# car.pause = not car.pause
elif self.lane.selected is not None \
and event.key == pygame.K_DELETE:
self.lane.remove_support_point(self.lane.selected)
self.lane.selected = None
self.update_distance_grid()
elif event.key == pygame.K_RETURN:
self.controller = self.human if self.controller != self.human else self.onestep
def input(self):
# get mouse info
cursor = pygame.mouse.get_pos()
(left_button, middle_button, right_button) = pygame.mouse.get_pressed()
keys = pygame.key.get_pressed()
if keys[pygame.K_HOME]:
self.cars[0].ins.update_pose(
self.cars[0].x - self.cars[0].ins.get_state("pos_x"),
self.cars[0].y - self.cars[0].ins.get_state("pos_y"),
self.cars[0].theta * Utils.d2r -
self.cars[0].ins.get_state("orientation"),
gain=0.5)
# if self.lane.selected is not None:
# if keys[pygame.K_DELETE]:
# self.lane.remove_support_point(self.lane.selected)
# self.lane.selected = None
# self.update_distance_grid()
# if keys[pygame.K_SPACE]:
# for car in self.cars:
# # save original speed
# if not hasattr(car,"speed_on"):
# car.speed_on = car.speed
# # toggle speed
# car.speed = car.speed_on - car.speed
# car.pause = True
# if keys[pygame.K_RETURN]:
# self.controller = self.human if self.controller != self.human else self.onestep
def update_distance_grid(self):
# return
if time() - self.last_distance_grid_update > 1 / 5:
self.last_distance_grid_update = time()
for i in range(self.grid.width):
for j in range(self.grid.height):
x, y = self.grid.xs[i], self.grid.ys[j]
closest_idx = self.lane.closest_sampled_idx(x, y)
distance = Utils.distance_between(
(self.lane.sampled_x[closest_idx],
self.lane.sampled_y[closest_idx]), (x, y))
# diff = np.array([self.lane.sampled_x[closest_idx]-x,self.lane.sampled_y[closest_idx]-y])
# distance = math.sqrt(np.sum(np.square(diff)))
self.grid.data[i, j] = distance * distance
def register_events(self):
self.events.register_callback("quit", self.on_quit)
self.events.register_callback("laneupdate", self.on_laneupdate)
self.events.register_callback("keyup", self.on_keyup)
def on_quit(self, args):
self.done = True
def on_laneupdate(self, lane):
if lane == self.lane:
if self.lane.selected is None:
self.update_distance_grid()
# pass
def update_ins(self, car, dt):
# print "--"
# print car.speed
# print car.theta
# print car.gyro
# car.ins.update_pose(car.x, car.y, (car.theta) * Utils.d2r,gain=0.05)
actual_view = self.cars[0].camview.view
if actual_view is not None:
# bw
bw = actual_view[:, :, 0]
edge_points = Utils.zero_points(bw)
theta_corr = 0
(x_corr,
y_corr) = self.optimize.correct_xy_nearest_edge_multi_pass(
edge_points=edge_points,
x0=car.ins.get_state("pos_x"),
y0=car.ins.get_state("pos_y"),
theta0=car.ins.get_state("orientation") / Utils.d2r,
labels=self.labels,
label_positions=self.label_positions,
camview=self.cars[0].camview,
skip=20,
tol=1e-1,
maxiter=1)
# correct estimate
theta_corr = self.optimize.correct_theta_parable(
edge_points=edge_points,
x=car.ins.get_state("pos_x") + x_corr,
y=car.ins.get_state("pos_y") + y_corr,
theta0=car.ins.get_state("orientation") / Utils.d2r,
camview=self.cars[0].camview,
distances=self.distances)
# theta_corr = self.optimize.correct_theta_nearest_edge(
# edge_points = edge_points,
# x = car.ins.get_state("pos_x") + x_corr,
# y = car.ins.get_state("pos_y") + y_corr,
# theta0 = car.ins.get_state("orientation") / Utils.d2r,
# labels = self.labels,
# label_positions = self.label_positions,
# hilbert = self.hilbert,
# camview = self.cars[0].camview,
# skip = 40,
# cutoff = 20
# )
error = self.optimize.distance_mean(
xytheta=(x_corr, y_corr, theta_corr),
edge_points=edge_points,
x0=car.ins.get_state("pos_x"),
y0=car.ins.get_state("pos_y"),
theta0=car.ins.get_state("orientation") / Utils.d2r,
camview=self.cars[0].camview,
distances=self.distances)
if error is None:
x_corr, y_corr, theta_corr, error = 0, 0, 0, 0
else:
error += 0.001 * (np.array([x_corr, y_corr, theta_corr])**
2).sum()
# (x_corr, y_corr, theta_corr), error = self.optimize_correction(
# (x_corr, y_corr, theta_corr), error = self.optimize.optimize_correction(
# edge_points = Utils.zero_points(bw),
# distances = self.distances,
# camview = self.cars[0].camview,
# x0 = car.ins.get_state("pos_x"),
# y0 = car.ins.get_state("pos_y"),
# theta0 = car.ins.get_state("orientation") / Utils.d2r,
# skip = 5,
# maxiter = 5,
# k = 1
# )
print x_corr, y_corr, theta_corr, error
self.xyt_corr_ring_buffer.add([x_corr, y_corr, theta_corr])
_, p_values = scipy.stats.normaltest(
self.xyt_corr_ring_buffer.buffer[-50:, :])
# self.normal_test_p_value_plot.ring_buffer.add(p_values)
self.std_plot.ring_buffer.add(
self.xyt_corr_ring_buffer.buffer[-50:, :].std(axis=0))
car.ins.update_pose(
x_corr,
y_corr,
theta_corr * Utils.d2r,
# gain = 1)
gain=min(1, 2.5 * 1 / error if error != 0 else 1))
car.ins.update(car.imu.get_sensor_array(), dt)
if not hasattr(
self,
"last_plot_update") or time() - self.last_plot_update > 5:
self.last_plot_update = time()
self.xyt_corr_plot.draw()
# self.hist_plot.draw(np.sqrt(np.abs(self.xyt_corr_ring_buffer.buffer))*np.sign(self.xyt_corr_ring_buffer.buffer))
self.std_plot.draw()
self.velocity_carthesian_history_plot.draw()
# self.normal_test_p_value_plot.draw()
def spin(self):
# Loop until the user clicks the close button.
self.done = False
# Used to manage how fast the screen updates
clock = pygame.time.Clock()
self.last_time = time()
# -------- Main Program Loop -----------
while not self.done:
dt = time() - self.last_time
self.last_time = time()
# --- Main event loop
for event in pygame.event.get(): # User did something
if event.type in self.events.pygame_mappings:
self.events.fire_callbacks(
self.events.pygame_mappings[event.type], event)
# if event.type == pygame.QUIT: # If user clicked close
# done = True # Flag that we are done so we exit this loop
self.input()
# --- Game logic should go here
# apply controllers
for car in self.cars:
if not car.pause:
# eventually set default controller
if car.controller is None and self.controller is not None:
car.controller = self.controller
# apply controller
if car.controller is not None:
car.controller.update(car, dt)
# update ins
if hasattr(car, "ins"):
self.update_ins(car, dt)
# --- Drawing code should go here
# First, clear the screen to white. Don't put other drawing commands
# above this, or they will be erased with this command.
self.screen.fill(Draw.WHITE)
self.draw()
# --- Go ahead and update the screen with what we've drawn.
pygame.display.flip()
# --- Limit to 60 frames per second
clock.tick(60)
# Close the window and quit.
# If you forget this line, the program will 'hang'
# on exit if running from IDLE.
pygame.quit()
def __init__(self, file, logLimit):
self.jsonOutputFile = file
self.jsonOutput = {}
self.clearStatusValues()
self.jsonOutputLog = RingBuffer(logLimit)
def __init__(self, nodeid, buffersize=10):
self.nodeid = nodeid
self.buffersize = buffersize
self.buffer = RingBuffer(buffersize)
def __init__(self, file, logLimit):
self.jsonOutputFile = file
self.jsonOutput = {}
self.clearStatusValues()
self.jsonOutputLog = RingBuffer(logLimit)
def __init__(self, nodeid, buffersize=10):
self.nodeid = nodeid
self.buffersize = buffersize
self.buffer = RingBuffer(buffersize)
def __init__(self, api_key, secret):
self.APIKey = api_key
self.Secret = secret
self.req_per_sec = 6
self.req_time_log = RingBuffer(self.req_per_sec)
self.lock = threading.RLock()
def __init__(self, file, logLimit, exchange=''):
self.jsonOutputFile = file
self.jsonOutput = {}
self.clearStatusValues()
self.jsonOutputLog = RingBuffer(logLimit)
self.jsonOutput['exchange'] = exchange
def __init__(self, file, logLimit):
self.jsonOutputFile = file
self.jsonOutput = {}
self.jsonOutputLog = RingBuffer(logLimit);
def run(self):
"""
This function is the main loop of the Process and is where driver
data is received. The camera driver is created here as the
__init__() function is not run in this Process. This loop waits for
data from the camera driver and either compute or pass frames to the
shared buffer to be used by another process.
"""
if(self.openni_version==True):
device = OpenNi2Driver(fps_window=self.driver_fps_window)
print('OpenNi2Driver')
else:
device = OpenNiDriver(fps_window=self.driver_fps_window)
print('OpenNiDriver')
self.height = device.height
self.width = device.width
self.ring_buffer = RingBuffer(self.height, self.width,
self.buffer_size)
frame_number = 0
chacha = device.wait_and_get_depth_frame()
background_frame = ones_like(chacha) * 65535 ####### 65535 is maximum of uint16
replace_value = int((summ(chacha) / summ(chacha) != 0)) *66 ######### TO BE CHECKED!
print("[Driver] Replacing bad values with {}".format(replace_value))
print("[Driver] Estimating background from {} " \
"frames".format(self.nb_background_frame))
mean = zeros(chacha.shape)
stdev = zeros(chacha.shape)
try:
while not self.cancel.value:
# We wait for the consumer thread to handle the frame before
# writing a new one
while self.frame_available.is_set():
pass
else:
frame = device.wait_and_get_depth_frame()
# self.ring_buffer.extend(frame) ################################# ONLY NECESSARY IF RECORDING (1)
if self.background_ready:
# Synchronized write access to shared array
# with self.shared_array.get_lock():
arr = frombuffer(self.shared_array.get_obj(),
dtype='I').reshape(self.height,
self.width)
arr[:] = frame
self.frame_available.set()
else:
frame_number = frame_number + 1
# print(frame_number)
if frame_number <= self.nb_background_frame:
mean = mean + frame
for j in range(frame.shape[0]):
for k in range(frame.shape[1]):
if frame[j][k] < background_frame[j][k] and \
frame[j][k] != 0:
background_frame[j][k] = frame[j][k]
elif frame[j][k] == 0:
background_frame[j][k] = replace_value
if (frame_number == self.nb_background_frame):
self.background = background_frame
mean = mean / self.nb_background_frame
if (frame_number >= self.nb_background_frame) and (frame_number-1 < (self.nb_background_frame*2)):
# Compute standard deviation frame
stdev += (frame - mean) ** 2
if (frame_number == (self.nb_background_frame*2)):
stdev = stdev / self.nb_background_frame
stdev = roundd(sqrtt(stdev), 0)
# Extract values for parameters estimation
self.middle_pixel_value = mean[int(self.height / 2)][
int(self.width / 2)]
self.percentile_stdev = percentile(stdev, 97)
self.compute_parameters()
arr = frombuffer(self.shared_array.get_obj(),
dtype='I').reshape(self.height,
self.width)
arr[:] = self.background
self.background_ready = True
print("[Driver] Background ready")
self.frame_available.set()
# del frame_list ####################### adicionado por mi
# if self.recording_trigger.is_set(): ################################# ONLY NECESSARY IF RECORDING (4)
# self.async_buffer_save() ################################# ONLY NECESSARY IF RECORDING (2)
# self.recording_trigger.clear() ################################# ONLY NECESSARY IF RECORDING (3)
print("[Driver] Process closing")
device.shutdown()
except Exception:
print("[Driver] Exception in process")
device.shutdown()
raise
class Poloniex:
def __init__(self, api_key, secret):
self.APIKey = api_key
self.Secret = secret
self.req_per_sec = 6
self.req_time_log = RingBuffer(self.req_per_sec)
self.lock = threading.RLock()
# socket.setdefaulttimeout(int(Config.get("BOT", "timeout", 30, 1, 180)))
@synchronized
def limit_request_rate(self):
now = time.time()
# start checking only when request time log is full
if len(self.req_time_log) == self.req_per_sec:
time_since_oldest_req = now - self.req_time_log[0]
# check if oldest request is more than 1sec ago
if time_since_oldest_req < 1:
# print self.req_time_log.get()
# uncomment to debug
# print "Waiting %s sec to keep api request rate" % str(1 - time_since_oldest_req)
# print "Req: %d 6th Req: %d Diff: %f sec" %(now, self.req_time_log[0], time_since_oldest_req)
self.req_time_log.append(now + 1 - time_since_oldest_req)
time.sleep(1 - time_since_oldest_req)
return
# uncomment to debug
# else:
# print self.req_time_log.get()
# print "Req: %d 6th Req: %d Diff: %f sec" % (now, self.req_time_log[0], time_since_oldest_req)
# append current request time to the log, pushing out the 6th request time before it
self.req_time_log.append(now)
@synchronized
def api_query(self, command, req=None):
# keep the 6 request per sec limit
self.limit_request_rate()
if req is None:
req = {}
def _read_response(resp):
data = json.loads(resp.read())
if 'error' in data:
raise PoloniexApiError(data['error'])
return data
try:
if command == "returnTicker" or command == "return24hVolume":
ret = urllib2.urlopen(
urllib2.Request('https://poloniex.com/public?command=' +
command))
return _read_response(ret)
elif command == "returnOrderBook":
ret = urllib2.urlopen(
urllib2.Request('https://poloniex.com/public?command=' +
command + '¤cyPair=' +
str(req['currencyPair'])))
return _read_response(ret)
elif command == "returnMarketTradeHistory":
ret = urllib2.urlopen(
urllib2.Request('https://poloniex.com/public?command=' +
"returnTradeHistory" + '¤cyPair=' +
str(req['currencyPair']) + '&start=' +
str(req['start']) + '&stop=' +
str(req['stop'])))
return _read_response(ret)
elif command == "returnChartData":
ret = urllib2.urlopen(
urllib2.Request('https://poloniex.com/public?command=' +
"returnChartData" + '¤cyPair=' +
str(req['currencyPair']) + '&start=' +
str(req['start']) + '&stop=' +
str(req['stop']) + '&period=' +
str(req['period'])))
return _read_response(ret)
elif command == "returnLoanOrders":
req_url = 'https://poloniex.com/public?command=' + "returnLoanOrders" + '¤cy=' + str(
req['currency'])
if req['limit'] != '':
req_url += '&limit=' + str(req['limit'])
ret = urllib2.urlopen(urllib2.Request(req_url))
return _read_response(ret)
else:
req['command'] = command
req['nonce'] = int(time.time() * 1000)
post_data = urllib.urlencode(req)
sign = hmac.new(self.Secret, post_data,
hashlib.sha512).hexdigest()
headers = {'Sign': sign, 'Key': self.APIKey}
print(post_data)
ret = urllib2.urlopen(
urllib2.Request('https://poloniex.com/tradingApi',
post_data, headers))
json_ret = _read_response(ret)
return post_process(json_ret)
except urllib2.HTTPError as ex:
raw_polo_response = ex.read()
try:
data = json.loads(raw_polo_response)
polo_error_msg = data['error']
except:
if ex.code == 502: # 502 Bad Gateway so response is likely HTML from Cloudflare
polo_error_msg = ''
else:
polo_error_msg = raw_polo_response
ex.message = ex.message if ex.message else str(ex)
ex.message = "{0} Requesting {1}. Poloniex reports: '{2}'".format(
ex.message, command, polo_error_msg)
raise ex
except Exception as ex:
ex.message = ex.message if ex.message else str(ex)
ex.message = "{0} Requesting {1}".format(ex.message, command)
raise
def return_ticker(self):
return self.api_query("returnTicker")
def return24h_volume(self):
return self.api_query("return24hVolume")
def return_order_book(self, currency_pair):
return self.api_query("returnOrderBook",
{'currencyPair': currency_pair})
# def return_market_trade_history(self, currency_pair):
# return self.api_query("returnMarketTradeHistory", {'currencyPair': currency_pair})
def return_chart_data(self, currency_pair, start, stop, period):
return self.api_query(
"returnChartData", {
"currencyPair": currency_pair,
'start': start,
'stop': stop,
'period': period
})
def return_market_trade_history(self, currency_pair, start, stop):
return self.api_query("returnMarketTradeHistory", {
"currencyPair": currency_pair,
'start': start,
'stop': stop
})
def transfer_balance(self, currency, amount, from_account, to_account):
return self.api_query(
"transferBalance", {
'currency': currency,
'amount': amount,
'fromAccount': from_account,
'toAccount': to_account
})
# Returns all of your balances.
# Outputs:
# {"BTC":"0.59098578","LTC":"3.31117268", ... }
def return_balances(self):
return self.api_query('returnBalances')
def return_available_account_balances(self, account):
balances = self.api_query('returnAvailableAccountBalances',
{"account": account})
if isinstance(
balances, list
): # silly api wrapper, empty dict returns a list, which breaks the code later.
balances = {}
return balances
# Returns your open orders for a given market, specified by the "currencyPair" POST parameter, e.g. "BTC_XCP"
# Inputs:
# currencyPair The currency pair e.g. "BTC_XCP"
# Outputs:
# orderNumber The order number
# type sell or buy
# rate Price the order is selling or buying at
# Amount Quantity of order
# total Total value of order (price * quantity)
def return_open_orders(self, currency_pair):
return self.api_query('returnOpenOrders',
{"currencyPair": currency_pair})
def return_open_loan_offers(self):
loan_offers = self.api_query('returnOpenLoanOffers')
if isinstance(
loan_offers, list
): # silly api wrapper, empty dict returns a list, which breaks the code later.
loan_offers = {}
return loan_offers
def return_active_loans(self):
return self.api_query('returnActiveLoans')
def return_lending_history(self, start, stop, limit=500):
return self.api_query('returnLendingHistory', {
'start': start,
'end': stop,
'limit': limit
})
# Returns your trade history for a given market, specified by the "currencyPair" POST parameter
# Inputs:
# currencyPair The currency pair e.g. "BTC_XCP"
# Outputs:
# date Date in the form: "2014-02-19 03:44:59"
# rate Price the order is selling or buying at
# amount Quantity of order
# total Total value of order (price * quantity)
# type sell or buy
# def return_trade_history(self, currency_pair):
# return self.api_query('returnTradeHistory', {"currencyPair": currency_pair})
def return_trade_history(self, currency_pair, start, stop):
return self.api_query('returnTradeHistory', {
"currencyPair": currency_pair,
'start': start,
'stop': stop
})
# Places a buy order in a given market. Required POST parameters are "currencyPair", "rate", and "amount".
# If successful, the method will return the order number.
# Inputs:
# currencyPair The curreny pair
# rate price the order is buying at
# amount Amount of coins to buy
# Outputs:
# orderNumber The order number
def buy(self, currency_pair, rate, amount):
return self.api_query('buy', {
"currencyPair": currency_pair,
"rate": rate,
"amount": amount
})
# Places a sell order in a given market. Required POST parameters are "currencyPair", "rate", and "amount".
# If successful, the method will return the order number.
# Inputs:
# currencyPair The curreny pair
# rate price the order is selling at
# amount Amount of coins to sell
# Outputs:
# orderNumber The order number
def sell(self, currency_pair, rate, amount):
return self.api_query('sell', {
"currencyPair": currency_pair,
"rate": rate,
"amount": amount
})
def create_loan_offer(self, currency, amount, duration, auto_renew,
lending_rate):
return self.api_query(
'createLoanOffer', {
"currency": currency,
"amount": amount,
"duration": duration,
"autoRenew": auto_renew,
"lendingRate": lending_rate,
})
# Cancels an order you have placed in a given market. Required POST parameters are "currencyPair" and "orderNumber".
# Inputs:
# currencyPair The curreny pair
# orderNumber The order number to cancel
# Outputs:
# succes 1 or 0
def cancel(self, currency_pair, order_number):
return self.api_query('cancelOrder', {
"currencyPair": currency_pair,
"orderNumber": order_number
})
def cancel_loan_offer(self, currency, order_number):
return self.api_query('cancelLoanOffer', {
"currency": currency,
"orderNumber": order_number
})
# Immediately places a withdrawal for a given currency, with no email confirmation.
# In order to use this method, the withdrawal privilege must be enabled for your API key.
# Required POST parameters are "currency", "amount", and "address". Sample output: {"response":"Withdrew 2398 NXT."}
# Inputs:
# currency The currency to withdraw
# amount The amount of this coin to withdraw
# address The withdrawal address
# Outputs:
# response Text containing message about the withdrawal
def withdraw(self, currency, amount, address):
return self.api_query('withdraw', {
"currency": currency,
"amount": amount,
"address": address
})
def return_loan_orders(self, currency, limit=''):
return self.api_query('returnLoanOrders', {
"currency": currency,
"limit": limit
})
# Toggles the auto renew setting for the specified orderNumber
def toggle_auto_renew(self, order_number):
return self.api_query('toggleAutoRenew', {"orderNumber": order_number})
class RingBufferTestCase(unittest.TestCase):
def setUp(self):
self.ringbuffer = RingBuffer(2)
def test_create_ring_buffer(self):
self.assertIsNotNone(self.ringbuffer)
self.assertEquals(2, self.ringbuffer.size)
def test_addOneItem_SameItemReturns(self):
self.ringbuffer.add(1)
self.assertEquals(1, self.ringbuffer.get())
def test_addFullBuffer_sameOrderReturns(self):
self.ringbuffer.add(1)
self.ringbuffer.add(2)
self.assertEquals(1, self.ringbuffer.get())
self.assertEquals(2, self.ringbuffer.get())
def test_addOneMoreItemThanSize_FirstItemOverwritten(self):
self.ringbuffer.add(1)
self.ringbuffer.add(2)
self.ringbuffer.add(3)
self.assertEquals(2, self.ringbuffer.get())
self.assertEquals(3, self.ringbuffer.get())
self.assertEquals(None, self.ringbuffer.get())
def test_firstAddAndReadThenAddFullBuffer_sameOrderAsPutInAfterRead(self):
self.ringbuffer.add(1)
self.assertEquals(1, self.ringbuffer.get())
self.ringbuffer.add(2)
self.ringbuffer.add(3)
self.assertEquals(2, self.ringbuffer.get())
self.assertEquals(3, self.ringbuffer.get())
self.assertEquals(None, self.ringbuffer.get())
def test_firstAddAndReadThenAddOneMoreItemThanSize_SecondItemOverwritten(self):
self.ringbuffer.add(1)
self.assertEquals(1, self.ringbuffer.get())
self.ringbuffer.add(2)
self.ringbuffer.add(3)
self.ringbuffer.add(4)
self.assertEquals(3, self.ringbuffer.get())
self.assertEquals(4, self.ringbuffer.get())
self.assertEquals(None, self.ringbuffer.get())
def setUp(self):
self.ringbuffer = RingBuffer(2)
def __init__(self):
self.__frame_id = 0
self.__id_of_last_anim_end = 0 # id of the last frame of the saved animation
self.__frame_cache = RingBuffer(self.max_gif_length_f)
self.__frame_thumbs_cache = RingBuffer(self.max_gif_length_f)
self.__stats = {'frames_saved_as_anim': 0, 'frames_dist': []}
class RingBufferTests(unittest.TestCase):
def assert_arrays(self,a1,a2,msg="Array's not equal"):
self.assertTrue(a1.size==a2.size,"{0}: {1} != {2}".format(msg,a1,a2))
self.assertTrue(np.array_equal(a1,a1),"{0}: {1} != {2}".format(msg,a1,a2))
# @property
# def log(self):
# return self._log
def setUp(self):
# self._log = logging.getLogger("RingBuffer.Tests")
self.sample_data = np.arange(10.0)
self.ring_buffer = RingBuffer(10)
def tearDown(self):
del self.ring_buffer
def test_size_total(self):
self.assertEqual(self.ring_buffer.size_total,10,'total size incorrect')
def test_size_used(self):
d = [0,1,3]
self.ring_buffer.add(d)
self.assertEqual(self.ring_buffer.size_used,len(d),'used size incorrect')
self.ring_buffer.get(2)
self.assertEqual(self.ring_buffer.size_used,1,'used size incorrect')
def test_size_used(self):
d = [0,1,3]
self.ring_buffer.add(d)
self.assertEqual(self.ring_buffer.size_remaining,self.ring_buffer.size_total-len(d),'remaining size incorrect')
self.ring_buffer.get(2)
self.assertEqual(self.ring_buffer.size_remaining,9,'remaining size incorrect')
def test_basic_get(self):
self.ring_buffer.add(self.sample_data[0:5])
data = self.ring_buffer.get(2)
self.assert_arrays(data,self.sample_data[0:2])
def test_second_get(self):
self.ring_buffer.add(self.sample_data[0:5])
data = self.ring_buffer.get(2)
data = self.ring_buffer.get(2)
self.assert_arrays(data,self.sample_data[3:5])
def test_read_all_data_when_number_is_larger_than_buffer(self):
d = self.sample_data[0:2]
self.ring_buffer.add(d)
g = self.ring_buffer.get(4)
self.assert_arrays(d,g)
self.assertEqual(self.ring_buffer.size_used,0)
def test_throw_on_read_from_empty_buffer(self):
self.ring_buffer.add(self.sample_data[0:2])
self.ring_buffer.get(2)
# Should throw exception if reading beyond buffer
with self.assertRaises(RingBuffer.Empty):
data = self.ring_buffer.get(2)
def test_throw_on_write_to_full_buffer(self):
# Should throw if buffer is full
self.ring_buffer.add(self.sample_data) # Fill buffer
with self.assertRaises(RingBuffer.Full):
self.ring_buffer.add([0,1])
# once we get some items, we should be able to add
self.ring_buffer.get(2)
self.ring_buffer.add([0,1])
# adding anything else should throw again
with self.assertRaises(RingBuffer.Full):
self.ring_buffer.add([4])
def test_should_be_thread_safe(self):
def write_thread(ring,w_buf):
#print "Writing to ring buffer"
for j in range(0,w_buf.size,2):
ring.add(w_buf[j:j+2])
#print "Write:Sleep for .1 sec"
time.sleep(.1)
def read_thread(ring,buf):
#print "Reading from RingBuffer"
for i in range(buf.size):
buf[i] = ring.get(1)
#print "Read:Sleep for .06 sec"
time.sleep(0.06)
write_buf = np.arange(10.0,20.0)
# Write thread
wt = threading.Thread(target=write_thread,args=(self.ring_buffer,write_buf))
data = np.zeros(shape=10)
rt = threading.Thread(target=read_thread,args=(self.ring_buffer,data))
wt.start()
time.sleep(0.01)
rt.start()
wt.join()
rt.join()
self.assert_arrays(write_buf,data)
