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