def test_negative_index_on_partly_filled_buffer(self):
r = RingBuffer(10)
for i in range(5):
r.append(i)
self.assertEqual(
r[-1], 4, "Fails to return the most recent datum when "
"asked for index -1.")
class SmoothText(object):
def __init__(self, queue, height, new_message_time, scroll_time):
self.queue = queue
self.y_step = textAscent() + textDescent()
self.y = 0
self.ring = RingBuffer(height / self.y_step)
self.last_time = 0
self.new_message_time = float(new_message_time)
self.scroll_time = float(scroll_time)
self.animation_start_time = 0
def _update_start(self, millis):
pass
def _update_end(self, millis):
pass
def _wait_for_message(self, millis):
if millis - self.last_time > self.new_message_time:
try:
new_item = self.queue.get_nowait()
self.ring.append(new_item)
new_item.color_fader.start(millis)
new_item.timestamp = millis
self.animation_start_time = millis
self._update = self._animate
except Queue.Empty:
pass
def _animate(self, millis):
time = (millis - self.animation_start_time) / self.scroll_time
self.y = lerp(self.y-self.y_step, self.y, time)
if time > 1:
self._update = self._wait_for_message
_update = _wait_for_message
def update(self, millis):
self._update_start(millis)
self._update(millis)
self._update_end(millis)
def append(self, message):
self.ring.append(message)
def draw(self, millis):
background(51)
items = self.ring.get()
(x,self.y) = (0, self.y_step * len(items))
self.update(millis)
for item in items:
try:
if item is not None:
fill(item.color)
text(item.text, x, self.y)
else:
text("Something bad happened", x, self.y)
except UnicodeDecodeError:
text("Unsupported Language Tweeted", x, self.y)
self.y-= self.y_step
def test_wraps(self):
r = RingBuffer(10)
for i in range(20):
r.append(i)
self.assertEqual(
r, [10, 11, 12, 13, 14, 15, 16, 17, 18, 19],
"Fails to wrap buffer when more data is added "
"than the max buffer length.")
def test_size(self):
buffer = RingBuffer(10)
self.assertEqual(buffer.size, 0)
buffer.append(42)
self.assertEqual(buffer.size, 1)
for i in range(11):
buffer.append(i)
self.assertEqual(buffer.size, 10)
def test_string_shows_buffer_and_length(self):
r = RingBuffer(10)
for i in range(20):
r.append(i)
self.assertEqual(
str(r), "<RingBuffer of length 10 and "
"[10, 11, 12, 13, 14, 15, 16, 17, 18, 19]>",
"Fails to print RingBuffer to spec.")
def test_two_similar_ringbuffers_are_equal(self):
r = RingBuffer(10)
s = RingBuffer(10)
for i in range(20):
r.append(i)
s.append(i)
self.assertEqual(
r, s, "Fails to say that two identically filled "
"RingBuffers are equal.")
class CausalIntegralFilter():
def __init__(self, initial_value, initial_time):
self._y = initial_value
self._t_buffer = RingBuffer(2)
self._t_buffer.append(initial_time)
def append_integral_value(self, y):
self._y = y
def append(self, dydt, t):
self._t_buffer.append(t)
self._y += dydt * (self._t_buffer[-1] - self._t_buffer[-2])
def get_datum(self):
return self._y
class GPTuner():
def __init__(self, dim, stepper, kernel, obs_var, history_len=100):
self.dim = dim
self.stepper = stepper
self.k = kernel
self.history_x = RingBuffer([dim], history_len)
self.history_y = RingBuffer([1], history_len)
self.obs_var_p = obs_var
self.eps = 0.00
def obs_var(self):
return F.softplus(self.obs_var_p)
def predict(self, x):
mean, var = GP.predict(self.history_x.all(), self.history_y.all(),
self.obs_var().expand(len(self.history_y)), x,
mean_zero, self.k)
var += self.eps * torch.eye(var.shape[0])
return mean, var
def thompson_step(self, grid):
mean, var = self.predict(grid)
choice = torch.argmax(sample(mean, var))
print(grid[choice])
result = self.stepper(grid[choice])
self.history_x.append(grid[choice])
self.history_y.append(result)
return result
def log_prob(self):
locs = self.history_x.all()
var = self.k(locs, locs) + self.obs_var() * torch.eye(len(locs))
return D.MultivariateNormal(torch.zeros(len(locs)),
var).log_prob(self.history_y.all())
def sample_argmax(self, init_grid, samples=1024):
mean, var = self.predict(init_grid)
choices = torch.argmax(sample(mean, var, [samples]), dim=1)
return init_grid[choices]
def test_sort(self):
buffer = RingBuffer(10)
buffer.append(3)
buffer.append(1)
buffer.append('zoo')
buffer.append(100)
buffer.append("X")
expected = [1, 3, 100, 'X', 'zoo']
actual = buffer.sort()
self.assertEqual(expected, actual)
class AnalogSensor(threading.Thread):
def __init__(self, sleep, records):
threading.Thread.__init__(self)
self.sleep = sleep
self.records = records
def run(self):
self.A0 = RingBuffer(
self.records
) # creates a buffer with maximum numbers equal to the number of records /reading
self.A1 = RingBuffer(self.records)
self.A2 = RingBuffer(self.records)
self.A3 = RingBuffer(self.records)
if self.sleep > 0.01 * 4:
self.sleep = self.sleep - 0.01 * 4 # each reading of the ADC takes about 0.01s. this time is compensated during sleep
while True:
# Adds measures and keep track of num of measurements
self.A0.append(adc.read_adc(
0, gain=GAIN, data_rate=DATA_RATE)) # pin gain and data_rate
self.A1.append(adc.read_adc(
1, gain=GAIN, data_rate=DATA_RATE)) # pin gain and data_rate
self.A2.append(adc.read_adc(
2, gain=GAIN, data_rate=DATA_RATE)) # pin gain and data_rate
self.A3.append(adc.read_adc(
3, gain=GAIN, data_rate=DATA_RATE)) # pin gain and data_rate
time.sleep(self.sleep)
def read_analog(self):
"""reads the channels as a rounded mean"""
# Compute the mean
analog = [0] * 4
#for channel in range(4):
analog[0] = mean(self.A0.get())
analog[1] = mean(self.A1.get())
analog[2] = mean(self.A2.get())
analog[3] = mean(self.A3.get())
return analog
def test_distinct(self):
buffer = RingBuffer(10)
d = buffer.distinct()
self.assertEqual(len(d), 0)
buffer.append(42)
self.assertEqual(len(buffer.distinct()), 1)
self.assertEqual(buffer.distinct()[0], 42)
buffer.append(42)
self.assertEqual(len(buffer.distinct()), 1)
self.assertEqual(buffer.distinct()[0], 42)
buffer.append(21)
self.assertEqual(len(buffer.distinct()), 2)
self.assertEqual(buffer.distinct()[0], 42)
self.assertEqual(buffer.distinct()[1], 21)
def test_get(self):
buffer = RingBuffer(10)
self.assertEqual(buffer.get(0), None)
try:
buffer.get(11)
self.assertTrue(False, "Should throw exception")
except:
self.assertTrue(True, "Should throw exception")
buffer.append(42)
self.assertEqual(buffer.get(0), 42)
buffer.append('21')
self.assertEqual(buffer.get(0), '21')
self.assertEqual(buffer.get(1), 42)
self.assertEqual(buffer[1], 42)
buffer.append('33')
self.assertEqual(buffer[1:3], ['21', 42])
def test_append(self):
buffer = RingBuffer(10)
buffer.append(42)
self.assertEqual(buffer.last(), 42)
buffer.append(21)
self.assertEqual(buffer.last(), 21)
class HUDChatBox(ChatBox, EventListener):
HISTORY_ON = 1
HISTORY_OFF = 0
def __init__(self):
ChatBox.__init__(self, [
"msg_public", "msg_team", "msg_private", "msg_squad", "msg_clan",
"msg_server", "_HCBinternal"
])
self.buffer.setFadeTop(True)
#history messages
self.historyBuffer = MessageBuffer([
"msg_public", "msg_team", "msg_private", "msg_squad", "msg_clan",
"msg_server", "_HCBinternal"
])
self.historyBuffer.setEditable(False)
scroll = glass.GlassScrollArea(self.historyBuffer)
scroll.setScrollPolicy(glass.GlassScrollArea.SHOW_NEVER,
glass.GlassScrollArea.SHOW_ALWAYS)
scroll.setAutoscroll(True)
self.add(scroll)
self.historyBuffer.parentScroll = scroll
scroll.setVisible(False)
for i in range(40):
self.historyBuffer.addRow(" ")
self.scroll.setVerticalScrollPolicy(glass.GlassScrollArea.SHOW_NEVER)
self.input.setVisible(False)
self.inputType = glass.GlassLabel("(squad)")
self.inputType.setAlignment(glass.Graphics.RIGHT)
self.add(self.inputType)
self.history = RingBuffer(16)
self.historyIndex = 0
self.chatType = "all"
self.replyTarget = ""
self.oldHeight = 0
self.typing = False
self.fade = ActionSequence(savage.WaitAction(5000),
savage.CheckAction(self.typing, False),
FadeOutAction(self))
self.showHistory(False)
def resize(self):
self.scroll.setSize(
self.getWidth(),
int(self.getHeight() * 0.5 - 0.4 * inputLineHeight))
self.buffer.setSize(self.scroll.getWidth(), self.scroll.getHeight())
self.historyBuffer.parentScroll.setSize(
self.getWidth(), int(self.getHeight() - 0.4 * inputLineHeight))
self.historyBuffer.setSize(self.historyBuffer.parentScroll.getWidth(),
self.historyBuffer.parentScroll.getHeight())
self.input.setSize(self.getWidth() - 25, inputLineHeight)
self.scroll.setPosition(0, int(self.getHeight() * 0.5))
self.input.setPosition(0,
self.getHeight() - inputLineHeight)
def makeVisible(self, forever=False):
self.fade.stop()
self.setAlpha(255)
self.setVisible(True)
if not forever and not self.typing:
self.fade = ActionSequence(savage.WaitAction(5000),
savage.CheckAction(self.typing, False),
FadeOutAction(self))
def onEvent(self, e):
self.makeVisible()
if e.scope == "msg_private":
self.replyTarget = e.fromstr
def onKeyPress(self, e):
self.typing = True
if e.key == glass.Key.ESCAPE:
self.deactivate()
elif e.key == glass.Key.UP:
histdata = self.history.get()
if len(histdata) == 0: return
self.historyIndex = (self.historyIndex - 1) % len(histdata)
self.input.setText(histdata[self.historyIndex])
self.input.setCaretPosition(len(self.input.getText()))
elif e.key == glass.Key.DOWN:
histdata = self.history.get()
if len(histdata) == 0: return
self.input.setText(histdata[self.historyIndex])
self.input.setCaretPosition(len(self.input.getText()))
self.historyIndex = (self.historyIndex + 1) % len(histdata)
elif e.key == glass.Key.TAB:
return
"""
#tab complete
#1. from the current position, move backwards until you find a space, or the start of the input text
#2. call the string between the space/start and the current position STRING
#3. create an empty list LIST
#3. loop over each player
#a. see if their name .startsWith STRING
#i. if it does, append this name to LIST
#4. if LIST is empty, don't do anything, just make sure a tab character isn't added
#5. sort the elements of LIST alphabetically
#6. replace the name in the input field
# cache the startswith STRING to save searching through each player
# cache the index of the filtered names so you can cycle through them
"""
def onKeyReleased(self, e):
if e.key == glass.Key.ENTER or e.key == 10: #e.key == glass.Key.RETURN #Big: hacky fix because I forgot where the defines are...
content = self.input.getText().rstrip()
if content == "":
self.deactivate()
return
sh = self.history.get()
if len(sh) == 0 or (len(sh) != 0 and sh[-1] != content):
self.history.append(content)
if content.startswith("/") and not content.startswith("//"):
#use one / for chat commands
data = content.split()
cmdname = data[0][1:]
if hasattr(hudchatcommands, cmdname):
args = data[1:]
try:
cmdreturn = getattr(hudchatcommands, cmdname)(self,
args)
except Exception as ex:
con_println("^rError: User tried to exec ^w/" +
cmdname + " " + " ".join(args) + "\n")
con_println("^rException: " + str(ex) + "\n")
self.deactivate()
return
if cmdreturn != None:
cmdreturn = str(cmdreturn)
gblEventHandler.notifyEvent("_HCBinternal", "",
cmdreturn)
self.deactivate()
return
if content.startswith("/"):
#// will directly execute python.
#/ will execute python as long as no matching command is found.
content = content.lstrip("/")
try:
exec(content)
except Exception as ex:
con_println("^rError: User tried to exec ^w" + content +
"\n")
con_println("^rException: " + str(ex) + "\n")
finally:
self.deactivate()
return
#if we haven't returned yet, then we're just chatting!
content = content.replace('\\', '\\\\')
content = content.replace('"', '\\"')
CL_SendChat(content, self.chatType)
if self.chatType == "comm":
gblEventHandler.notifyEvent(
"_HCBinternal", "", commColorCode + "-> Comm " + content)
Sound_PlaySound("/sound/gui/msg_send.ogg")
self.deactivate()
def onMouseWheelMovedUp(self, e):
if self._historyShown:
self.historyBuffer.parentScroll.mouseWheelMovedUp()
#is this even the right method to call?
def onMouseWheelMovedDown(self, e):
if self._historyShown:
self.historyBuffer.parentScroll.mouseWheelMovedDown()
def activate(self, chatType="all"):
self.typing = True
self.makeVisible(True)
self.chatType = chatType
#"all" "team" "comm" "squad" "private" "clan"
#don't use "private" here, use /w, /msg, /r and /re for that
self.input.setText("")
self.input.setVisible(True)
self.input.requestFocus()
self.inputType.setCaption("^777(" + chatType + ")")
self.inputType.setPosition(
0,
self.input.getY() +
(self.input.getHeight() - self.inputType.getHeight()) // 2)
self.input.setX(self.inputType.getWidth() + 2)
self.input.setWidth(self.getWidth() - self.inputType.getWidth())
def deactivate(self):
self.makeVisible()
self.inputType.setCaption("")
self.historyIndex = 0
self.input.setText("")
self.typing = False
if self.bShowInput == 0:
self.input.setVisible(False)
#this also REMOVES the focus
def showHistory(self, status):
vis = 0
if self.historyBuffer.parentScroll.isVisible():
vis = 1
if vis != status:
if status == self.HISTORY_ON:
self.makeVisible(True)
self.buffer.setVisible(False)
self.historyBuffer.parentScroll.setVisible(True)
self.historyBuffer.parentScroll.requestFocus()
else:
self.makeVisible()
self.buffer.setVisible(True)
self.historyBuffer.parentScroll.setVisible(False)
class Lab2Program:
def __init__(self):
self.initialize_vrep_client()
self.initilize_vrep_api()
self.define_constants()
self.killer = GracefulKiller()
self.idash = IDash(framerate=0.05)
def initialize_vrep_client(self):
#Initialisation for Python to connect to VREP
print 'Python program started'
count = 0
num_tries = 10
while count < num_tries:
vrep.simxFinish(-1) # just in case, close all opened connections
self.clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5) #Timeout=5000ms, Threadcycle=5ms
if self.clientID!=-1:
print 'Connected to V-REP'
break
else:
"Trying again in a few moments..."
time.sleep(3)
count += 1
if count >= num_tries:
print 'Failed connecting to V-REP'
vrep.simxFinish(self.clientID)
def initilize_vrep_api(self):
# initialize ePuck handles and variables
_, self.bodyelements=vrep.simxGetObjectHandle(
self.clientID, 'ePuck_bodyElements', vrep.simx_opmode_oneshot_wait)
_, self.leftMotor=vrep.simxGetObjectHandle(
self.clientID, 'ePuck_leftJoint', vrep.simx_opmode_oneshot_wait)
_, self.rightMotor=vrep.simxGetObjectHandle(
self.clientID, 'ePuck_rightJoint', vrep.simx_opmode_oneshot_wait)
_, self.ePuck=vrep.simxGetObjectHandle(
self.clientID, 'ePuck', vrep.simx_opmode_oneshot_wait)
_, self.ePuckBase=vrep.simxGetObjectHandle(
self.clientID, 'ePuck_base', vrep.simx_opmode_oneshot_wait)
# proxSens = prox_sens_initialize(self.clientID)
# initialize odom of ePuck
_, self.xyz = vrep.simxGetObjectPosition(
self.clientID, self.ePuck, -1, vrep.simx_opmode_streaming)
_, self.eulerAngles = vrep.simxGetObjectOrientation(
self.clientID, self.ePuck, -1, vrep.simx_opmode_streaming)
# initialize overhead cam
_, self.overheadCam=vrep.simxGetObjectHandle(
self.clientID, 'Global_Vision', vrep.simx_opmode_oneshot_wait)
_, self.resolution, self.image = vrep.simxGetVisionSensorImage(
self.clientID,self.overheadCam,0,vrep.simx_opmode_oneshot_wait)
# initialize goal handle + odom
_, self.goalHandle=vrep.simxGetObjectHandle(
self.clientID, 'Goal_Position', vrep.simx_opmode_oneshot_wait)
_, self.goalPose = vrep.simxGetObjectPosition(
self.clientID, self.goalHandle, -1, vrep.simx_opmode_streaming)
# STOP Motor Velocities. Not sure if this is necessary
_ = vrep.simxSetJointTargetVelocity(
self.clientID,self.leftMotor,0,vrep.simx_opmode_oneshot_wait)
_ = vrep.simxSetJointTargetVelocity(
self.clientID,self.rightMotor,0,vrep.simx_opmode_oneshot_wait)
def define_constants(self):
self.map_side_length = 2.55
# FIMXE: hard coded goals
# self.GOALS = [(40+2,6), (40, 6+2), (40,21), (35, 19), (30,22), (29,10), (27,5), (20,8), (20,33), (20, 48), (5,55)]
self.GOALS = None
self.worldNorthTheta = None
self.maxVelocity = 2.0
self.history_length = 5
self.theta_history = RingBuffer(self.history_length)
self.e_theta_h = RingBuffer(self.history_length)
self.blurred_paths = None
self.path_skip = 8
def global_map_preprocess(self, resolution, image):
im = format_vrep_image(resolution, image) # original image
im = image_vert_flip(im)
resize_length = int(im.shape[0]/2)
im = skimage.transform.resize(im, (resize_length, resize_length, 3))
return im
def global_map_process(self, im):
walls = im[:,:,0] > 0.25
paths = walls < 0.15
if self.blurred_paths is None:
print "Computed"
blurred_map = skimage.filters.gaussian_filter(walls, sigma=2)
blurred_paths = blurred_map < 0.15
# np.save('binary_grid.npy', blurred_paths)
return paths, blurred_paths
else:
return paths, self.blurred_paths
def robot_pose_get(self):
_, xyz = vrep.simxGetObjectPosition(
self.clientID, self.ePuck, -1, vrep.simx_opmode_buffer)
_, eulerAngles = vrep.simxGetObjectOrientation(
self.clientID, self.ePuck, -1, vrep.simx_opmode_buffer)
x, y, z = xyz
theta = eulerAngles[2]
self.theta_history.append(theta)
return (x, y, theta)
def lidar_scan_get(self, window_size=21):
""" gets lidar scan range values """
fr = (window_size - 1) / 2 # fire range
lidarValues = pseudoLidarSensor(self.paths, self.pose[0], self.pose[1], fr, original_four=False)
return lidarValues
def repulsion_vectors_compute(self, lidarValues, k_repulse=10.0):
numLidarValues = len(lidarValues)
lidarAngles = [np.pi / numLidarValues * index for index in range(numLidarValues)]
repulsionVectors = [np.array(pol2cart(force_repulsion(k_repulse, np.sqrt(val), 2), angle)) for val, angle in zip(lidarValues, lidarAngles)]
return repulsionVectors
def attraction_vector_compute(self, k_attract=100.0):
self.attractionVal, self.attractionAngle = cart2pol(
self.goal_pose_pixel[1] - self.pose_pixel[1], # cols counts same to normal horz axes
self.pose_pixel[0] - self.goal_pose_pixel[0] # rows counts opposite to normal vert axes
)
attractionVector = np.array(pol2cart(k_attract*self.attractionVal, self.attractionAngle))
return attractionVector
def robot_code(self):
t = time.time()
self.curr_goal = 0
while (time.time() - t) < 200:
# global map
_,resolution,image = vrep.simxGetVisionSensorImage(self.clientID,self.overheadCam,0,vrep.simx_opmode_oneshot_wait) # Get image
im = self.global_map_preprocess(resolution, image)
self.pose = self.robot_pose_get()
x, y, theta = self.pose
# theta = self.theta_history.weighted_average(scheme='last', mathfunc=lambda x: np.exp(x))
# initialize worldNorthTheta for the first time
if self.worldNorthTheta is None:
self.worldNorthTheta = self.pose[2]
_ = [self.theta_history.append(self.pose[2]) for _ in range(self.history_length)]
self.pose_pixel = np.array(odom2pixelmap(x, y, self.map_side_length, im.shape[0]))
m, n = self.pose_pixel
self.paths, self.blurred_paths = self.global_map_process(im)
# calculate intermediate goals once
if self.GOALS is None:
raw_input("")
# acquire pixel location of goal
_, finishPose = vrep.simxGetObjectPosition(
self.clientID, self.goalHandle, -1, vrep.simx_opmode_buffer)
self.finish_pixel = odom2pixelmap(finishPose[0], finishPose[1], self.map_side_length, im.shape[0])
contiguousPath = AStarBinaryGrid(self.blurred_paths).calculate_path(self.pose_pixel, self.finish_pixel)
self.GOALS = contiguousPath[::self.path_skip]
# SKIP THIS FIRST LOOP AND CONTINUE
continue
else:
self.goal_pose_pixel = np.array(self.GOALS[self.curr_goal])
goal_m, goal_n = self.goal_pose_pixel
lidarValues = self.lidar_scan_get(window_size=21)
#############################
# Potential Field Algorithm #
#############################
repulsionVectors = self.repulsion_vectors_compute(lidarValues, k_repulse=10.0)
attractionVector = self.attraction_vector_compute(k_attract=100.0)
finalVector = np.sum(np.vstack((repulsionVectors, attractionVector)), axis=0)
finalUnitVector = finalVector / np.linalg.norm(finalVector)
print "finalUnitVector: ", finalUnitVector
# TODO: do we use the unit vector (for direction) only, or the finalVector?
finalValue, finalAngle = cart2pol(finalUnitVector[0], finalUnitVector[1])
error_theta = angle_diff(mapTheta2worldTheta(finalAngle, self.worldNorthTheta), theta)
self.e_theta_h.append(error_theta)
k_angular_p = 2.0 * self.maxVelocity
k_angular_D = 0.25
k_angular_S = 1.0
k_angular_I = 2.5
omega = k_angular_p * (
error_theta
+ k_angular_D / k_angular_S * (self.e_theta_h[-1] - self.e_theta_h[-2])
+ k_angular_S / k_angular_I * sum(self.e_theta_h)
)
print "Omega: ", round(omega,1)
#############################
# StateController Algorithm #
#############################
# if desired heading is not directly in front
if np.abs(error_theta) > np.pi / 3:
# turn in place
forward_vel = self.maxVelocity
# omega *= 0.25
else:
# Direct yourself to the goal
goal_distance = np.linalg.norm(self.goal_pose_pixel - self.pose_pixel)
print "distance_from_goal: ", goal_distance
if goal_distance <= 4:
# Achieved Goal!
forward_vel = self.maxVelocity * 0.25 # Slow down to prepare for the next one
self.curr_goal += 1
else:
forward_vel = self.maxVelocity
# control the motors
ctrl_sig_left, ctrl_sig_right = vomega2bytecodes(forward_vel, omega, g=1)
_ = vrep.simxSetJointTargetVelocity(
self.clientID,self.leftMotor,ctrl_sig_left,vrep.simx_opmode_oneshot_wait) # set left wheel velocity
_ = vrep.simxSetJointTargetVelocity(
self.clientID,self.rightMotor,ctrl_sig_right,vrep.simx_opmode_oneshot_wait) # set right wheel velocity
self.idash.add(lambda: self.plot_maze(self.blurred_paths*1.0, m, n, goal_m, goal_n))
self.idash.add(lambda: self.plot_maze(im, m, n, goal_m, goal_n))
def plot_current_and_desired_heading():
self.plot_unit_quiver(finalUnitVector, 'r')
self.plot_unit_quiver(pol2cart(1, worldTheta2mapTheta(theta, self.worldNorthTheta)), 'k')
plt.title("Error Theta: %f" % error_theta)
self.idash.add(plot_current_and_desired_heading)
self.idash.add(self.plot_theta_history)
self.idash.plotframe()
if self.killer.kill_now:
self.clean_exit()
def plot_maze(self, im, m, n, goal_m, goal_n):
""" plots the maze, with robot pose and goal pose visualized """
if len(im.shape) == 3:
goal_pixel_values = np.array((1.0, 1.0, 1.0))
robot_pixel_values = np.array((255.0/255.0,192/255.0,203/255.0))
elif len(im.shape) == 2:
goal_pixel_values = 0.5
robot_pixel_values = 0.5
im[goal_m,goal_n] = goal_pixel_values
im[m,n] = robot_pixel_values
plt.imshow(im)
def plot_unit_quiver(self, vector, color):
X, Y = (0, 0)
U, V = (vector[0], vector[1])
plt.quiver(X,Y,U,V,angles='xy',scale_units='xy',scale=1,color=color)
plt.xlim([-1.1,1.1])
plt.ylim([-1.1,1.1])
def plot_theta_history(self, expansion=5):
plt.plot([theta for theta in self.theta_history])
if len(self.theta_history) < expansion:
plt.xlim([0, expansion])
else:
plt.xlim([0, len(self.theta_history)])
ylim = np.pi + 0.5
plt.ylim([-ylim, ylim])
def plot_all_goals(self, im):
# display all goals
for goal_idx in range(len(self.GOALS)):
im[self.GOALS[goal_idx][0], self.GOALS[goal_idx][1]] = np.array((1.0, 1.0, 1.0))
def clean_exit(self):
_ = vrep.simxStopSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)
vrep.simxFinish(self.clientID)
print 'Program ended'
sys.exit(0)
def run(self):
if self.clientID!=-1:
_ = vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)
self.robot_code()
self.clean_exit()
def run(in_filename, out_filename):
print(in_filename, out_filename)
width, height, rate, display_aspect_ratio, field_order = get_video_size(in_filename)
print(width, height, rate, display_aspect_ratio, field_order)
process1 = start_ffmpeg_process1(in_filename, rate)
process2 = start_ffmpeg_process2(out_filename, width, height, rate, display_aspect_ratio)
i = 0
N = 5
y_shifts_a = []
y_shifts_b = []
x_shifts_a = []
x_shifts_b = []
shape = (height, width, 3)
frame_buffer = RingBuffer(N, shape, np.uint8)
while True:
in_frame = read_frame(process1, width, height)
if in_frame is None:
logger.info('End of input stream')
break
frame_buffer.append(in_frame)
logger.debug('Processing frame')
i+=1
#then the read is filled
if i > N-1:
# get mean frame
frame_mean = np.mean(frame_buffer, axis = 0)
frame_central = frame_buffer.get()
frame_central_a, frame_central_b = fields(frame_central)
frame_mean_a, frame_mean_b = fields(frame_mean)
y_shift_a = cross_corr(y_average(frame_mean_a), y_average(frame_central_a))
y_shift_b = cross_corr(y_average(frame_mean_b), y_average(frame_central_b))
x_shift_a = cross_corr(x_average(frame_mean_a), x_average(frame_central_a))
x_shift_b = cross_corr(x_average(frame_mean_b), x_average(frame_central_b))
# y_shifts_a.append( y_shift_a )
# y_shifts_b.append( y_shift_b )
# x_shifts_a.append( x_shift_a )
# x_shifts_b.append( x_shift_b )
out_a = np.roll(frame_central_a, (y_shift_a, x_shift_a), axis=(0,1))
out_b = np.roll(frame_central_b, (y_shift_b, x_shift_b), axis=(0,1))
# out_a = shift(frame_central_a, (y_shift_a, x_shift_a, 0), mode="nearest", order=0)
# out_b = shift(frame_central_b, (y_shift_b, x_shift_b, 0), mode="nearest", order=0)
# write output
out_frame = frame(out_a , out_b)
write_frame(process2, out_frame)
# if i == 5500:
# break
# flush end of buffer in the end
for out_frame in frame_buffer.flush_frames():
write_frame(process2, out_frame)
# plt.plot(y_shifts_a)
# plt.plot(y_shifts_b)
# plt.plot(x_shifts_a)
# plt.plot(x_shifts_b)
# plt.show()
print(frame_buffer)
logger.info('Waiting for ffmpeg process1')
process1.wait()
logger.info('Waiting for ffmpeg process2')
process2.stdin.close()
process2.wait()
def test_iter(self):
buffer = RingBuffer(10)
for i in range(5):
buffer.append(i)
for i in buffer:
self.assertTrue(buffer[i] is not None)
class BasicMemory(ReplayMemory):
"""Memory to record and sample past experience.
Uses a single ring buffer to record all previous
observations, actions, rewards, and terminals
where each index stores a tuple.
"""
def __init__(self, max_size, window_length):
self.max_size = max_size
self.window_length = window_length
self.memory = RingBuffer(max_size)
def append(self, state, action, reward, is_terminal):
obs = Observation(state, action, reward, is_terminal)
self.memory.append(obs)
def sample(self, batch_size, indexes=None):
# Get random indexes if not given
if indexes is None:
try:
all_indexes = xrange(self.memory.size)
except:
all_indexes = range(self.memory.size)
indexes = random.sample(all_indexes, batch_size)
assert (self.memory.size > self.window_length)
assert (batch_size == len(indexes))
state_shape = np.shape(self.memory.buffer[0].state)
samples = []
for index in indexes:
index = index % self.memory.size
state_shape = np.shape(self.memory.buffer[0].state)
# Sample index again if index is at the end of an episode
# or index is at the ring buffer boundary
while self.is_episode_end(index) or index == self.memory.index - 1:
try:
all_indexes = xrange(self.memory.size)
except:
all_indexes = range(self.memory.size)
index = random.sample(all_indexes, 1)[0]
states = np.zeros(state_shape + (self.window_length, ))
next_states = np.zeros(state_shape + (self.window_length, ))
# Get the most recent frames without crossing episode boundary
for frame in range(self.window_length):
frame_index = (index - frame) % self.memory.size
if self.invalid_frame_index(frame_index, index, frame):
break
state = self.memory.buffer[frame_index].state
states[..., self.window_length - frame - 1] = state
# Get the next frames
next_state = self.memory.buffer[(index + 1) %
self.memory.size].state
next_states[..., :-1] = states[..., 1:]
next_states[..., -1] = next_state
action = self.memory.buffer[index].action
reward = self.memory.buffer[index].reward
is_terminal = self.memory.buffer[index].is_terminal
sample = Sample(states, action, reward, next_states, is_terminal)
samples.append(sample)
return samples
# Checks if index is at the end of an episode or index is at the ring buffer boundary
def invalid_frame_index(self, frame_index, index, frame):
return (self.memory.size == self.max_size and frame_index == self.memory.index) \
or (self.memory.size < self.max_size and frame_index == self.memory.index - 1) \
or (self.memory.buffer[frame_index % self.memory.size].is_terminal and frame > 0)
# Checks if index is at the end of an episode
def is_episode_end(self, index):
return self.memory.buffer[(index - 1) % self.memory.size].is_terminal \
and self.memory.buffer[index % self.memory.size].is_terminal
def clear(self):
self.memory = RingBuffer(self.max_size)
def test_fills(self):
r = RingBuffer(10)
for i in range(10):
r.append(i)
self.assertEqual(r, [0, 1, 2, 3, 4, 5, 6, 7, 8, 9],
"Fails to fill buffer.")
def test_partly_filled_buffer(self):
r = RingBuffer(10)
for i in range(5):
r.append(i)
self.assertEqual(r, [0, 1, 2, 3, 4], "Fails to partly fill a buffer.")
class HUDChatBox( ChatBox, EventListener ):
HISTORY_ON = 1
HISTORY_OFF = 0
def __init__( self ):
ChatBox.__init__(self, ["msg_public", "msg_team", "msg_private", "msg_squad", "msg_clan", "_HCBinternal"]);
self.buffer.showTime(1);
self.buffer.setFadeTop(1);
self.scroll.setVerticalScrollPolicy( glass.GlassScrollArea.SHOW_NEVER );
self.input.setVisible(False);
self.inputType = glass.GlassLabel("(squad)");
self.inputType.setAlignment( glass.Graphics.RIGHT );
self.add(self.inputType)
self.history = RingBuffer(16);
self.historyIndex = 0;
self.chatType = "all";
self.replyTarget = "";
self.oldHeight = 0;
self._historyShown = 0;
self.showHistory(0);
def onEvent (self, e):
if e.scope == "msg_private":
self.replyTarget = e.fromstr;
def onKeyPress( self, e):
if e.key == glass.Key.ESCAPE:
self.deactivate();
elif e.key == glass.Key.UP:
histdata = self.history.get();
if len(histdata) == 0: return;
self.historyIndex = (self.historyIndex - 1) % len(histdata);
self.input.setText( histdata[self.historyIndex] );
self.input.setCaretPosition( len( self.input.getText() ) );
elif e.key == glass.Key.DOWN:
histdata = self.history.get();
if len(histdata) == 0: return;
self.input.setText( histdata[self.historyIndex] );
self.input.setCaretPosition( len( self.input.getText() ) );
self.historyIndex = (self.historyIndex + 1) % len(histdata);
elif e.key == glass.Key.TAB:
return;
"""
#tab complete
#1. from the current position, move backwards until you find a space, or the start of the input text
#2. call the string between the space/start and the current position STRING
#3. create an empty list LIST
#3. loop over each player
#a. see if their name .startsWith STRING
#i. if it does, append this name to LIST
#4. if LIST is empty, don't do anything, just make sure a tab character isn't added
#5. sort the elements of LIST alphabetically
#6. replace the name in the input field
# cache the startswith STRING to save searching through each player
# cache the index of the filtered names so you can cycle through them
"""
def onKeyReleased(self, e):
if e.key == glass.Key.ENTER:
content = self.input.getText().rstrip();
if content == "":
self.deactivate();
return;
sh = self.history.get();
if len(sh)==0 or (len(sh) != 0 and sh[-1] != content):
self.history.append( content );
if content.startswith("/") and not content.startswith("//"):
#use one / for chat commands
data = content.split();
cmdname = data[0][1:];
if hasattr( hudchatcommands, cmdname ):
args = data[1:];
try:
cmdreturn = getattr( hudchatcommands, cmdname )( self, args);
except Exception, ex:
con_println("^rError: User tried to exec ^w/"+cmdname+" "+" ".join(args)+"\n");
con_println("^rException: "+str(ex)+"\n");
self.deactivate();
return;
if cmdreturn != None:
cmdreturn = str(cmdreturn);
gblEventHandler.notifyEvent( "_HCBinternal", "", cmdreturn );
self.deactivate();
return
if content.startswith("/"):
#// will directly execute python.
#/ will execute python as long as no matching command is found.
content = content.lstrip("/")
try:
exec content;
except Exception, ex:
con_println("^rError: User tried to exec ^w"+content+"\n");
con_println("^rException: "+str(ex)+"\n");
finally:
class Lab2Program:
def __init__(self):
self.initialize_vrep_client()
self.initilize_vrep_api()
self.define_constants()
self.killer = GracefulKiller()
self.idash = IDash(framerate=0.05)
def initialize_vrep_client(self):
#Initialisation for Python to connect to VREP
print 'Python program started'
count = 0
num_tries = 10
while count < num_tries:
vrep.simxFinish(-1) # just in case, close all opened connections
self.clientID = vrep.simxStart('127.0.0.1', 19999, True, True,
5000,
5) #Timeout=5000ms, Threadcycle=5ms
if self.clientID != -1:
print 'Connected to V-REP'
break
else:
"Trying again in a few moments..."
time.sleep(3)
count += 1
if count >= num_tries:
print 'Failed connecting to V-REP'
vrep.simxFinish(self.clientID)
def initilize_vrep_api(self):
# initialize ePuck handles and variables
_, self.bodyelements = vrep.simxGetObjectHandle(
self.clientID, 'ePuck_bodyElements', vrep.simx_opmode_oneshot_wait)
_, self.leftMotor = vrep.simxGetObjectHandle(
self.clientID, 'ePuck_leftJoint', vrep.simx_opmode_oneshot_wait)
_, self.rightMotor = vrep.simxGetObjectHandle(
self.clientID, 'ePuck_rightJoint', vrep.simx_opmode_oneshot_wait)
_, self.ePuck = vrep.simxGetObjectHandle(self.clientID, 'ePuck',
vrep.simx_opmode_oneshot_wait)
_, self.ePuckBase = vrep.simxGetObjectHandle(
self.clientID, 'ePuck_base', vrep.simx_opmode_oneshot_wait)
# proxSens = prox_sens_initialize(self.clientID)
# initialize odom of ePuck
_, self.xyz = vrep.simxGetObjectPosition(self.clientID, self.ePuck, -1,
vrep.simx_opmode_streaming)
_, self.eulerAngles = vrep.simxGetObjectOrientation(
self.clientID, self.ePuck, -1, vrep.simx_opmode_streaming)
# initialize overhead cam
_, self.overheadCam = vrep.simxGetObjectHandle(
self.clientID, 'Global_Vision', vrep.simx_opmode_oneshot_wait)
_, self.resolution, self.image = vrep.simxGetVisionSensorImage(
self.clientID, self.overheadCam, 0, vrep.simx_opmode_oneshot_wait)
# initialize goal handle + odom
_, self.goalHandle = vrep.simxGetObjectHandle(
self.clientID, 'Goal_Position', vrep.simx_opmode_oneshot_wait)
_, self.goalPose = vrep.simxGetObjectPosition(
self.clientID, self.goalHandle, -1, vrep.simx_opmode_streaming)
# STOP Motor Velocities. Not sure if this is necessary
_ = vrep.simxSetJointTargetVelocity(self.clientID, self.leftMotor, 0,
vrep.simx_opmode_oneshot_wait)
_ = vrep.simxSetJointTargetVelocity(self.clientID, self.rightMotor, 0,
vrep.simx_opmode_oneshot_wait)
def define_constants(self):
self.map_side_length = 2.55
# FIMXE: hard coded goals
# self.GOALS = [(40+2,6), (40, 6+2), (40,21), (35, 19), (30,22), (29,10), (27,5), (20,8), (20,33), (20, 48), (5,55)]
self.GOALS = None
self.worldNorthTheta = None
self.maxVelocity = 2.0
self.history_length = 5
self.theta_history = RingBuffer(self.history_length)
self.e_theta_h = RingBuffer(self.history_length)
self.blurred_paths = None
self.path_skip = 8
def global_map_preprocess(self, resolution, image):
im = format_vrep_image(resolution, image) # original image
im = image_vert_flip(im)
resize_length = int(im.shape[0] / 2)
im = skimage.transform.resize(im, (resize_length, resize_length, 3))
return im
def global_map_process(self, im):
walls = im[:, :, 0] > 0.25
paths = walls < 0.15
if self.blurred_paths is None:
print "Computed"
blurred_map = skimage.filters.gaussian_filter(walls, sigma=2)
blurred_paths = blurred_map < 0.15
# np.save('binary_grid.npy', blurred_paths)
return paths, blurred_paths
else:
return paths, self.blurred_paths
def robot_pose_get(self):
_, xyz = vrep.simxGetObjectPosition(self.clientID, self.ePuck, -1,
vrep.simx_opmode_buffer)
_, eulerAngles = vrep.simxGetObjectOrientation(self.clientID,
self.ePuck, -1,
vrep.simx_opmode_buffer)
x, y, z = xyz
theta = eulerAngles[2]
self.theta_history.append(theta)
return (x, y, theta)
def lidar_scan_get(self, window_size=21):
""" gets lidar scan range values """
fr = (window_size - 1) / 2 # fire range
lidarValues = pseudoLidarSensor(self.paths,
self.pose[0],
self.pose[1],
fr,
original_four=False)
return lidarValues
def repulsion_vectors_compute(self, lidarValues, k_repulse=10.0):
numLidarValues = len(lidarValues)
lidarAngles = [
np.pi / numLidarValues * index for index in range(numLidarValues)
]
repulsionVectors = [
np.array(
pol2cart(force_repulsion(k_repulse, np.sqrt(val), 2), angle))
for val, angle in zip(lidarValues, lidarAngles)
]
return repulsionVectors
def attraction_vector_compute(self, k_attract=100.0):
self.attractionVal, self.attractionAngle = cart2pol(
self.goal_pose_pixel[1] -
self.pose_pixel[1], # cols counts same to normal horz axes
self.pose_pixel[0] -
self.goal_pose_pixel[0] # rows counts opposite to normal vert axes
)
attractionVector = np.array(
pol2cart(k_attract * self.attractionVal, self.attractionAngle))
return attractionVector
def robot_code(self):
t = time.time()
self.curr_goal = 0
while (time.time() - t) < 200:
# global map
_, resolution, image = vrep.simxGetVisionSensorImage(
self.clientID, self.overheadCam, 0,
vrep.simx_opmode_oneshot_wait) # Get image
im = self.global_map_preprocess(resolution, image)
self.pose = self.robot_pose_get()
x, y, theta = self.pose
# theta = self.theta_history.weighted_average(scheme='last', mathfunc=lambda x: np.exp(x))
# initialize worldNorthTheta for the first time
if self.worldNorthTheta is None:
self.worldNorthTheta = self.pose[2]
_ = [
self.theta_history.append(self.pose[2])
for _ in range(self.history_length)
]
self.pose_pixel = np.array(
odom2pixelmap(x, y, self.map_side_length, im.shape[0]))
m, n = self.pose_pixel
self.paths, self.blurred_paths = self.global_map_process(im)
# calculate intermediate goals once
if self.GOALS is None:
raw_input("")
# acquire pixel location of goal
_, finishPose = vrep.simxGetObjectPosition(
self.clientID, self.goalHandle, -1,
vrep.simx_opmode_buffer)
self.finish_pixel = odom2pixelmap(finishPose[0], finishPose[1],
self.map_side_length,
im.shape[0])
contiguousPath = AStarBinaryGrid(
self.blurred_paths).calculate_path(self.pose_pixel,
self.finish_pixel)
self.GOALS = contiguousPath[::self.path_skip]
# SKIP THIS FIRST LOOP AND CONTINUE
continue
else:
self.goal_pose_pixel = np.array(self.GOALS[self.curr_goal])
goal_m, goal_n = self.goal_pose_pixel
lidarValues = self.lidar_scan_get(window_size=21)
#############################
# Potential Field Algorithm #
#############################
repulsionVectors = self.repulsion_vectors_compute(lidarValues,
k_repulse=10.0)
attractionVector = self.attraction_vector_compute(k_attract=100.0)
finalVector = np.sum(np.vstack(
(repulsionVectors, attractionVector)),
axis=0)
finalUnitVector = finalVector / np.linalg.norm(finalVector)
print "finalUnitVector: ", finalUnitVector
# TODO: do we use the unit vector (for direction) only, or the finalVector?
finalValue, finalAngle = cart2pol(finalUnitVector[0],
finalUnitVector[1])
error_theta = angle_diff(
mapTheta2worldTheta(finalAngle, self.worldNorthTheta), theta)
self.e_theta_h.append(error_theta)
k_angular_p = 2.0 * self.maxVelocity
k_angular_D = 0.25
k_angular_S = 1.0
k_angular_I = 2.5
omega = k_angular_p * (
error_theta + k_angular_D / k_angular_S *
(self.e_theta_h[-1] - self.e_theta_h[-2]) +
k_angular_S / k_angular_I * sum(self.e_theta_h))
print "Omega: ", round(omega, 1)
#############################
# StateController Algorithm #
#############################
# if desired heading is not directly in front
if np.abs(error_theta) > np.pi / 3:
# turn in place
forward_vel = self.maxVelocity
# omega *= 0.25
else:
# Direct yourself to the goal
goal_distance = np.linalg.norm(self.goal_pose_pixel -
self.pose_pixel)
print "distance_from_goal: ", goal_distance
if goal_distance <= 4:
# Achieved Goal!
forward_vel = self.maxVelocity * 0.25 # Slow down to prepare for the next one
self.curr_goal += 1
else:
forward_vel = self.maxVelocity
# control the motors
ctrl_sig_left, ctrl_sig_right = vomega2bytecodes(forward_vel,
omega,
g=1)
_ = vrep.simxSetJointTargetVelocity(
self.clientID, self.leftMotor, ctrl_sig_left,
vrep.simx_opmode_oneshot_wait) # set left wheel velocity
_ = vrep.simxSetJointTargetVelocity(
self.clientID, self.rightMotor, ctrl_sig_right,
vrep.simx_opmode_oneshot_wait) # set right wheel velocity
self.idash.add(lambda: self.plot_maze(self.blurred_paths * 1.0, m,
n, goal_m, goal_n))
self.idash.add(lambda: self.plot_maze(im, m, n, goal_m, goal_n))
def plot_current_and_desired_heading():
self.plot_unit_quiver(finalUnitVector, 'r')
self.plot_unit_quiver(
pol2cart(1, worldTheta2mapTheta(theta,
self.worldNorthTheta)),
'k')
plt.title("Error Theta: %f" % error_theta)
self.idash.add(plot_current_and_desired_heading)
self.idash.add(self.plot_theta_history)
self.idash.plotframe()
if self.killer.kill_now:
self.clean_exit()
def plot_maze(self, im, m, n, goal_m, goal_n):
""" plots the maze, with robot pose and goal pose visualized """
if len(im.shape) == 3:
goal_pixel_values = np.array((1.0, 1.0, 1.0))
robot_pixel_values = np.array(
(255.0 / 255.0, 192 / 255.0, 203 / 255.0))
elif len(im.shape) == 2:
goal_pixel_values = 0.5
robot_pixel_values = 0.5
im[goal_m, goal_n] = goal_pixel_values
im[m, n] = robot_pixel_values
plt.imshow(im)
def plot_unit_quiver(self, vector, color):
X, Y = (0, 0)
U, V = (vector[0], vector[1])
plt.quiver(X,
Y,
U,
V,
angles='xy',
scale_units='xy',
scale=1,
color=color)
plt.xlim([-1.1, 1.1])
plt.ylim([-1.1, 1.1])
def plot_theta_history(self, expansion=5):
plt.plot([theta for theta in self.theta_history])
if len(self.theta_history) < expansion:
plt.xlim([0, expansion])
else:
plt.xlim([0, len(self.theta_history)])
ylim = np.pi + 0.5
plt.ylim([-ylim, ylim])
def plot_all_goals(self, im):
# display all goals
for goal_idx in range(len(self.GOALS)):
im[self.GOALS[goal_idx][0], self.GOALS[goal_idx][1]] = np.array(
(1.0, 1.0, 1.0))
def clean_exit(self):
_ = vrep.simxStopSimulation(self.clientID,
vrep.simx_opmode_oneshot_wait)
vrep.simxFinish(self.clientID)
print 'Program ended'
sys.exit(0)
def run(self):
if self.clientID != -1:
_ = vrep.simxStartSimulation(self.clientID,
vrep.simx_opmode_oneshot_wait)
self.robot_code()
self.clean_exit()
class What(Popen):
"""
Wrapper around subprocess.Popen that has additional methods for checking
process output and return code.
Inspired by the solution from J.F. Sebastian.
Source: http://stackoverflow.com/a/4896288/242451
"""
BUFFER_SIZE = 100
def __init__(self, *args, **kwargs):
"""
Both args and kwargs will be passed to Popen constructor.
"""
kwargs['stdout'] = PIPE
kwargs['stderr'] = STDOUT
kwargs['bufsize'] = 0
kwargs['close_fds'] = True
super(What, self).__init__(args, **kwargs)
self.timeout = 10
self.queue = Queue(self.BUFFER_SIZE)
self.lines = RingBuffer(self.BUFFER_SIZE)
self.reader = Thread(target=self._enqueue_output)
self.reader.daemon = True
self.reader.start()
def expect(self, string, timeout=None):
"""
Expect a string in output. If timeout is given and expected string
is not found before timeout Timeout will be raised. If end of the file
is reached while waiting for string EOF will be raised.
If timeout is None, self.timeout is going to be used as a value.
"""
if timeout is None:
timeout = self.timeout
start = time()
try:
while 1:
passed = time() - start
get_timeout = timeout - passed
if get_timeout < 0:
raise Timeout(self, string)
line = self.queue.get(timeout=get_timeout)
if line is EOF:
self.wait()
raise EOF(self, string)
if string in line:
return line
except Empty:
raise Timeout(self, string)
def expect_exit(self, exit_code=None, timeout=None):
"""
Expect process to exit with specified exit code.
If timeout is None, self.timeout is going to be used as a value.
"""
if timeout is None:
timeout = self.timeout
start = time()
while 1:
returncode = self.poll()
if returncode is not None:
break
passed = time() - start
if passed > timeout:
raise Timeout(self, exit_code)
if exit_code is not None and returncode != exit_code:
raise UnexpectedExit(self, exit_code)
self.wait()
def get_output(self):
"""Return lines read in the read buffer."""
return '\n'.join(self.lines)
def _enqueue_output(self):
"""Thread target of self.reader."""
for line in iter(self.stdout.readline, b''):
line = line.rstrip('\n')
self.queue.put(line)
self.lines.append(line)
self.queue.put(EOF)
self.stdout.close()
class PIP():
"""A filter that takes a pressure waveform and gives the
Peak Inspiratory Pressure (PIP).
"""
def __init__(self,
sampling_period,
minimum_pip=MINIMUM_PIP,
window_length=5,
polynomial_order=3,
buffer_length=10):
"""
Parameters
----------
sampling_period : float
The amount of time between data samples.
minimum_pip=tetra_constants.MINIMUM_PIP : float
The maximum pressure considered to be PIP.
window_length=5 : int
The number of points sampled for a low-pass filter of the
data. It must be odd.
polynomial_order=3 : int
The order of the Savitsky-Golay filter used to smooth the
data. It must be less than window_length.
buffer_length=10 : int
The number of data points considered when smoothing.
"""
if (window_length > buffer_length):
raise ValueError("window_length must be <= buffer_length")
if (window_length % 2 != 1):
raise ValueError("window_length must be odd")
if (buffer_length <= window_length):
raise ValueError("buffer_length must be greater in length "
"than window_length")
self._sampling_period = sampling_period
self._minimum_pip = minimum_pip
self._currently_in_pip_range = False
self._current_pip = minimum_pip
self._window_length = window_length
self._polynomial_order = polynomial_order
self._buffer_length = buffer_length
self._data_buffer = RingBuffer(buffer_length,
initial_state=[minimum_pip] *
buffer_length)
def append(self, datum):
"""Adds a datum to the data buffer."""
if datum > self._minimum_pip:
self._currently_in_pip_range = True
if datum > self._data_buffer[-1]:
self._update_pip(datum)
else:
self._update_pip(self._current_pip)
else:
self._currently_in_pip_range = False
def get_datum(self):
"""Returns the current PIP value."""
return self._current_pip
def _update_pip(self, datum):
self._data_buffer.append(datum)
self._current_pip = (signal.savgol_filter(self._data_buffer,
self._window_length,
self._polynomial_order,
mode="interp")[-1])
