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