def loader(filename=None): with open(filename) as f: f = [i.rstrip("\n") for i in f.readlines() if i != "\n"] l = len(f) - 1 b = int((len(f[1]) - 1) / 3) del f[0] f = f[::-1] plots = [] y = 0 while f: check = list(f.pop()) del check[0] cross_found = False x = 0 for i in check: if i == "x" or i == "■": cross_found = True if i == "|": if cross_found == True: plots.append((x,y)) x += 1 cross_found = False y += 1 print(GridMap(l,b,plots,True)) print(GridMap(l,b,plots)) return l,b,plots
def initGridMap(self, gridsize): self.gridsize = gridsize self.Nrows = self.rect.height / gridsize self.Ncols = self.rect.width / gridsize self.gridmap = GridMap(self.Nrows, self.Ncols) self.Astar = Astar(self) #,start_pos,goal_pos) self.path = None
def __init__(self, nrows, ncols, goal): self.map = GridMap(nrows, ncols) self.goal = goal # Path cache. For a coord, keeps the next coord to move # to in order to reach the goal. Invalidated when the # grid changes (with set_blocked) # self._path_cache = {}
def make_path_grid(self): # for our pathfinding, we're going to overlay a grid over the field with # squares that are sized by a constant in the config file origdim = (self.m_xmax_field, self.m_ymax_field) newdim = self.rescale_pt2path(origdim) self.m_pathgrid = GridMap(*self.rescale_pt2path((self.m_xmax_field, self.m_ymax_field))) self.m_pathfinder = PathFinder(self.m_pathgrid.successors, self.m_pathgrid.move_cost, self.m_pathgrid.estimate)
def _init_map(self): self.start_pos = 0, 0 self.goal_pos = 3, 8 nrows = self.field.height / self.grid_size ncols = self.field.width / self.grid_size self.map = GridMap(nrows, ncols) for b in [(1, 1), (1, 2), (0, 3), (1, 3), (2, 3), (2, 4), (2, 5), (2, 6)]: self.map.set_blocked(b) self._recompute_path()
def main(): grid_map = GridMap(1, (7, 7), 3, 4) env = Environment(grid_map) step_count = 0 while True: finished = True for p in grid_map.passengers: if p.status != 'dropped': finished = False clear_output() grid_map.visualize() print('-' * 10) env.step() step_count += 1 time.sleep(1) if finished: print('step cost:', step_count) break
def setup_grid_map(ox, oy, reso, sweep_direction, offset_grid=10): width = math.ceil((max(ox) - min(ox)) / reso) + offset_grid height = math.ceil((max(oy) - min(oy)) / reso) + offset_grid center_x = (np.max(ox) + np.min(ox)) / 2.0 center_y = (np.max(oy) + np.min(oy)) / 2.0 grid_map = GridMap(width, height, reso, center_x, center_y) grid_map.print_grid_map_info() grid_map.set_value_from_polygon(ox, oy, 1.0, inside=False) grid_map.expand_grid() x_inds_goal_y = [] goal_y = 0 if sweep_direction == SweepSearcher.SweepDirection.UP: x_inds_goal_y, goal_y = search_free_grid_index_at_edge_y( grid_map, from_upper=True) elif sweep_direction == SweepSearcher.SweepDirection.DOWN: x_inds_goal_y, goal_y = search_free_grid_index_at_edge_y( grid_map, from_upper=False) return grid_map, x_inds_goal_y, goal_y
def __str__(self): return 'N(%s) -> g: %s, f:%s' % (self.coord, self.g_cost, self.f_cost) def __repr__(self): return self.__str__() if __name__ == "__main__": from gridmap import GridMap start = 0, 0 goal = 1, 7 x = GridMap(8, 8) for a in n[(1, 1), (0, 2), (1, 2), (0, 3), (1, 3), (2, 3), (2, 5), (2, 5), (2, 7)]: x.set_blocked(a) x.printme() Path = PathFinder(x.adjacent_coords, x.move_cost, x.move_cost) import time time = time.clock() path = list(pf.compute_path(start, goal)) print("Elasped: %s" % (time.clock() - time)) print(path)
import time, os from matplotlib import pyplot from BlockSparseMatrix import BlockSparseMatrix from BresenhamAlgorithms import BresenhamLine, BresenhamTriangle, BresenhamPolygon from gridmap import GridMap, SonarSensor import numpy #set this true and have mencoder to create a video of the test makevideo = True #set up the map and scale scale = 100.0 groundtruth = ((1, 1, 1, 1, 1), (1, 0, 0, 0, 1), (1, 0, 1, 0, 1), (1, 0, 0, 0, 1), (1, 1, 1, 1, 1)) gridScale = 0.5 #set up the grid map on a 2cm scale (half the input resolution) estmap = GridMap(scale=gridScale) #this is the set of positions the rover moves between tour = ((150.0, 150.0, 0.0), (350.0, 150.0, 0.0), (350.0, 150.0, numpy.pi / 2.0), (350.0, 350.0, numpy.pi / 2.0), (350.0, 350.0, numpy.pi), (150.0, 350.0, numpy.pi), (150.0, 350.0, numpy.pi * 1.5), (150.0, 150.0, numpy.pi * 1.5), (150.0, 150.0, numpy.pi * 2)) #this is the number of steps along each part of the tour divs = 100 vals = [] for i in xrange(len(tour) - 1): for j in xrange(divs): position = numpy.array(tour[i]) * (1. - j / float(divs)) + numpy.array(
def create(filename,l,b): with open(filename, "w") as f: f.write(str(GridMap(l,b,lines=True)))
def simulation(n, steps, map_path, update_interval=1, fps=None, obedience=None, multi_sim=False): """ :param n: crowd size :param steps: how many times to simulate :param map_path: what map file to use :param update_interval: how fast to update being drawn :param fps: caps frames per seconds to have smooth video :Param obediance: 0 to 100 percent of following sign. If none then don't overide base case :return: """ winsize = 600 # goal = (winsize // 1.2, winsize // 1.2) # update_interval is visualization update interval # obtain map. NOTE: be careful of hidden spaces and enters in map file. g = GridMap(winsize, map_path) # create graphics # win, text = init_graphics(winsize) win = init_graphics(winsize) # create grid # Make grid to be square to be easier to work with g.init_grid(win) # create crowd crowd = g.make_particles(n, win) # Dijkstra's Algorithm dg = [] # values of policy ff = [] # direction vectors of policy for every_goal in g.goal: dg.append(g.DijkstraGrid(every_goal)) for every_dg in dg: ff.append(g.createFlowField(every_dg)) for every_sign_goal in g.sign_goals.values(): every_sign_goal.compute_dg_ff(g) dg.append(every_sign_goal.dg) ff.append(every_sign_goal.ff) # g.visualize_dg(win, dg[0]) # g.visualize_dg(win, dg) # TODO: try dynamic Fields later # g.visualize_flow(win, ff[0])d # g.visualize_flow(win, ff) # TODO: NATHAN add a list of SignGoals # TODO: NATHAN precompute dg and ff of sign goals if not multi_sim: # wait until you click on window to start simulation win.getMouse() # win.getKey() # simulate crowd start_t = time.time() # TODO: while simulation running, click to add obstacles step = 0 dt = 0 running = True # for step in range(steps): while running: running = False # decide which flow field to follow # should only do these if p is still in bounds for p in crowd: dt = p.dt # TODO: NATHAN change this so it actually does something with some random prob. distribution if not reached_goal(p, g.stepsize, dg): # if any agent hasn't reached any goal running = True # continue running if winsize >= p.x >= 0 and winsize >= p.y >= 0: # if im inside the building and not following a sign for key, sign_goal in g.sign_goals.items(): # TODO: Make the hazard a stand alone and not dependant on signs if g.hazard_encounter((p.x, p.y)): p.follow_sign(key) p.saw_hazard = 1 # TODO: if multiple hazard ID them else: # if sign_goal.within_influence(g.loc_to_state(p.x, p.y)) or p.can_be_influenced(): # basically, if we're close enough to the sign for it to do something to us if sign_goal.within_influence( (p.x, p.y), g.stepsize) and p.saw_hazard is None: # TODO: Follow the sign previously ignored # TODO: This should be going back to the sign last saw # TODO: this is for people changing their mind (i.e. i haven't seen an exit in a while maybe I should follow exit sign) if obedience is None: follow = choose_the_sign( p, g.stepsize, dg[0], sign_goal.dg ) # random.choices([True, False], _ODDS)[0] else: follow = choose_the_sign( p, g.stepsize, dg[0], sign_goal.dg, obedience) if follow: p.follow_sign(key) else: p.follow_sign(None) # p.ff_index = flowfieldchoose(p, g.stepsize, dg) # TODO: change flowfieldchoose to take in list of SignGoal objects if p.can_be_influenced( ) and p.saw_hazard is None: #if im stuck in a crowd, perhaps follow sign if choose_the_sign(p, g.stepsize, dg[0], sign_goal.dg, 50): p.follow_sign(key) # obtain force from map position, other particles, etc for p in crowd: if winsize >= p.x >= 0 and winsize >= p.y >= 0: if p.following_sign(): seek = 0.1 * flowfieldfollow( p, g.stepsize, g.sign_goals[p.flow_field_index].ff, basic=True) else: seek = 0.1 * flowfieldfollow( p, g.stepsize, ff[0], basic=True) # seek = seek_goal(p, np.array(g.goal[0]) * g.stepsize * 1.5) seperate = seperation(crowd, p) # attachment = cohesion(crowd, p) # alignment = align(crowd, p) # scale cohesion so that people in front aren't too affected by being pulled back w_cohesion = 0.05 # p.force = (seek[0] + seperate[0] + w_cohesion * attachment[0] + alignment[0], # seek[1] + seperate[1] + w_cohesion * attachment[1] + alignment[1]) p.force = (seek[0] + seperate[0], seek[1] + seperate[1]) # TODO: for each person in crowd apply force from other particles # TODO: instead of O^n search for each, try quadtree, cell binning, spatial index # move crowd for p in crowd: if winsize >= p.x >= 0 and winsize >= p.y >= 0: p.apply_force() # adjust particles when they're in collision with the world for p in crowd: if winsize >= p.x >= 0 and winsize >= p.y >= 0: p.collideWithWorld(g) # update visualization for p in crowd: p.move_graphic() if p.x > winsize or p.x < 0 or p.y > winsize or p.y < 0: p.graphic.undraw() if reached_goal(p, g.stepsize, dg): # if an agent reached any goal p.graphic.undraw() crowd.remove(p) if not multi_sim: # update_step(win, text, step, update_interval) update_step(win, step, update_interval, fps) else: update_step(win, step, 1) step = step + 1 # if step==800: # # after 500 steps, switch to other map, which has blocked goals # # update dg, ff according to new map # map_path2 = './map1_2.txt' # g = GridMap(winsize, map_path2) # # # create grid # g.init_grid(win) # # draw obstacles # for r in range(g.rows): # for c in range(g.cols): # if g.occupancy_grid[r, c]: # pos = g.map2_visgrid((r, c)) # g.drawEnv(win, pos, colors[7]) # # # put the particles back on top of the grid squares # for p in crowd: # p.graphic.undraw() # p.unit_graphics(win) # # # # Dijkstra's Algorithm # dg = [] # values of policy # ff = [] # direction vectors of policy # # for every_goal in g.goal: # dg.append(g.DijkstraGrid(every_goal)) # for every_dg in dg: # ff.append(g.createFlowField(every_dg)) # for every_sign_goal in g.sign_goals.values(): # every_sign_goal.compute_dg_ff(g) # dg.append(every_sign_goal.dg) # ff.append(every_sign_goal.ff) # for p in crowd: # p.flow_field_index = None # # g.visualize_dg(win, dg[0]) # g.visualize_flow(win, ff[0]) if not multi_sim: end_t = time.time() print('crowd simulation real time: {0} seconds'.format(end_t - start_t)) print('steps:', step) print('simulation time (sec)', step * dt) escapeKey = None while escapeKey != 'q': escapeKey = win.getKey() win.close() else: end_t = time.time() win.close() real_time = end_t - start_t # seconds sim_time = step * dt # seconds n_steps = step return real_time, sim_time, n_steps
from gridmap import GridMap plots = [ (0, 0), (1, 0), (2, 0), (3, 0), (4, 0), (5, 0), (6, 0), (7, 0), (8, 0), (9, 0), (0, 1), (1, 1), (2, 1), (3, 1), (6, 1), (7, 1), (8, 1), (9, 1), (0, 2), (1, 2), (2, 2), (7, 2), (8, 2), (9, 2), (0, 3), (1, 3), (8, 3), (9, 3), (0, 4), (1, 4), (8, 4), (9, 4), (0, 5), (9, 5), (4, 6), (5, 6), (3, 7), (4, 7), (5, 7), (6, 7), (2, 8), (3, 8), (4, 8), (5, 8), (6, 8), (7, 8), (1, 9), (2, 9), (3, 9), (4, 9), (5, 9), (6, 9), (7, 9), (8, 9), (4, 10), (5, 10), (4, 11), (5, 11), (4, 12), (5, 12), (4, 13), (5, 13), (4, 14), (5, 14), (1, 16), (3, 16), (5, 16), (6, 16), (7, 16), (1, 17), (3, 17), (5, 17), (7, 17), (1, 18), (3, 18), (5, 18), (6, 18), (7, 18), (1, 19), (2, 19), (3, 19), (5, 19), (9, 19) ] print(GridMap(20, 10, plots))
X = 0 Y = 1 TILE_SZ = (32, 32) SCREEN_SZ = (640, 480) if '__main__' == __name__: screen = pygame.display.set_mode( SCREEN_SZ ) running = True clock = pygame.time.Clock() tilesheet = pygame.image.load( 'Game/mapd01a.png' ) tilesheet_w = tilesheet.get_width() / TILE_SZ[X] gmap = GridMap( TestMap ) vx = SCREEN_SZ[X] / 2 #vy = SCREEN_SZ[Y] / 3 vy = 0 tilesheet.set_colorkey( (0, 0, 0) ) while running: for event in pygame.event.get(): if pygame.QUIT == event.type: running = False elif pygame.KEYDOWN == event.type: if pygame.K_ESCAPE == event.key: running = False
def main(): if len(sys.argv) < 2: usage() exit(0) printmap = False savefile = False debugmode = False f = None if '-p' in sys.argv: printmap = True if '-s' in sys.argv: savefile = True if '--debug' in sys.argv or '-d' in sys.argv: debugmode = True f = open('debug.log', 'w') ran_pick = make_random_pick(int(sys.argv[1]), int(sys.argv[2])) bfs = BestFirst(GridMap(int(sys.argv[1]), int(sys.argv[2]))) dijk = Dijkstra(GridMap(int(sys.argv[1]), int(sys.argv[2]))) astar = AStar(GridMap(int(sys.argv[1]), int(sys.argv[2]))) while len(ran_pick) < 2: ran_pick = make_random_pick(int(sys.argv[1]), int(sys.argv[2])) bfs.grid.put_multiple_obs(ran_pick[1:-2]) dijk.grid.put_multiple_obs(ran_pick[1:-2]) astar.grid.put_multiple_obs(ran_pick[1:-2]) bfs.grid.set_start(ran_pick[0][0], ran_pick[0][1]) dijk.grid.set_start(ran_pick[0][0], ran_pick[0][1]) astar.grid.set_start(ran_pick[0][0], ran_pick[0][1]) bfs.grid.set_goal(ran_pick[-1][0], ran_pick[-1][1]) dijk.grid.set_goal(ran_pick[-1][0], ran_pick[-1][1]) astar.grid.set_goal(ran_pick[-1][0], ran_pick[-1][1]) print 'Calculating by Best-First-Search...' bfs.calc_path(debugmode, f) print 'Finish!' print 'Calculating by Dijkstra algorithm...' dijk.calc_path(debugmode, f) print 'Finish!' print 'Calculating by A* algorithm...' astar.calc_path(debugmode, f) print 'Finish!' print print 'Result of Best-First-Search' bfs.print_result(printmap) print print 'Result of Dijkstra algorithm' dijk.print_result(printmap) print print 'Result of A* algorithm' astar.print_result(printmap) if debugmode: f.close()
from gridmap import GridMap from imu import IMU from lidar import Lidar if __name__ == "__main__": lidar_data = Lidar("lidar") imu_data = IMU("imu", "speed") map = GridMap() print(lidar_data) print(imu_data) print(map) # lidarData.show_all()
def __init__(self, nrows, ncols, goal): self.map = GridMap(nrows, ncols) self.goal = goal self.path_cache = {}
def clear(): if name == "nt": _ = system('cls') else: _ = system('clear') while True: ver = 10 hor = 10 p = [] step = 1 for x in range(hor): for y in range(ver): p.append((x, y)) for y in range(ver): for x in range(hor): p.append((x, y)) print(GridMap(ver, hor, p, True)) print(GridMap(ver, hor, p)) print(f"\n--> Point count: {len(p)}") sleep(0.08) clear() if step == 3: step = 0 p = [] step += 1 pp = [] stepp = 1 break
from util import Util from dqn import DQN class PairAlgorithm: def greedy_fcfs(self, grid_map): passengers = grid_map.passengers cars = grid_map.cars action = [0] * len(passengers) for i, p in enumerate(passengers): min_dist = math.inf assigned_car = None for j, c in enumerate(cars): dist = Util.cal_dist(p.pick_up_point, c.position) if dist < min_dist: min_dist = dist assigned_car = j action[i] = assigned_car return action if __name__ == '__main__': algorithm = PairAlgorithm() grid_map = GridMap(0, (5, 5), 3, 3) grid_map.init_map_cost() grid_map.visualize() print(grid_map) algorithm.greedy_fcfs(grid_map) print(grid_map)
def __str__(self): return 'N(%s) -> g: %s, f: %s' % (self.coord, self.g_cost, self.f_cost) def __repr__(self): return self.__str__() if __name__ == "__main__": from gridmap import GridMap start = 0, 0 goal = 1, 7 tm = GridMap(8, 8) for b in [(1, 1), (0, 2), (1, 2), (0, 3), (1, 3), (2, 3), (2, 5), (2, 5), (2, 5), (2, 7)]: tm.set_blocked(b) tm.printme() pf = PathFinder(tm.successors, tm.move_cost, tm.move_cost) import time t = time.clock() path = list(pf.compute_path(start, goal)) print "Elapsed: %s" % (time.clock() - t) print path
from dataset import DataSet from gridmap import GridMap from detection import Detection from tracking import Tracking import time # Choose scenario path = '/home/simonappel/KITTI/raw/' date = '2011_09_26' drive = '0001' frame_range = range(0, 100, 1) # Construct objects dataset = DataSet(path, date, drive, frame_range) grid = GridMap() detector = Detection() tracker = Tracking() # Loop through frames for frame in range(0, 2, 1): print "-----Frame " + str(frame) + "-----" point_cloud = dataset.get_point_cloud(frame) pose = dataset.get_pose(frame) timeframe = dataset.get_timeframe(frame) print "Pose " + str(pose) print "Timeframe " + str(timeframe) t0 = time.time() grid.fill_point_cloud_in_grid(point_cloud) t1 = time.time() print "Fill grid in " + str(t1 - t0) + "s" #grid.display_grid_map()
window_steps = total_loss[i:i+window_size] total_loss_smoothed[i] = np.average(window_steps) plt.title('Loss history') plt.xlabel('Episodes') plt.ylabel('Loss') plt.plot(self.loss_history) np.save("Loss_"+filename, total_loss_smoothed) plt.savefig("Loss_history_" + filename) if __name__ == '__main__': num_cars =20 num_passengers = 25 grid_map = GridMap(1, (100,100), num_cars, num_passengers) cars = grid_map.cars passengers = grid_map.passengers env = Environment(grid_map) input_size = 2*num_cars + 4*num_passengers # cars (px, py), passengers(pickup_x, pickup_y, dest_x, dest_y) output_size = num_cars * num_passengers # num_cars * (num_passengers + 1) hidden_size = 256 #load_file = "episode_49800_qmix_model_num_cars_10_num_passengers_10_num_episodes_50000_hidden_size_128.pth" # 3218 over 1000 episodes #load_file = "episode_41000_dqn_model_num_cars_20_num_passengers_25_num_episodes_100000_hidden_size_256.pth" # 3218 over 1000 episodes, 316.509, 16274 # greedy 3526, 348.731, 17251 # random 3386, 337.336, 17092 load_file = None #greedy, random, dqn, qmix agent = Agent(env, input_size, output_size, hidden_size, load_file = load_file, lr=0.001, mix_hidden = 64, batch_size=128, eps_decay = 20000, num_episodes=1000, mode = "dqn", training = False) # 50,000 episodes for full trains
plt.title('Loss history') plt.xlabel('Episodes') plt.ylabel('Loss') plt.plot(self.loss_history) np.save("Loss_" + filename, total_loss_smoothed) #plt.savefig("Loss_history_" + filename) if __name__ == '__main__': init_cars = 2 init_passengers = 7 max_cars = 20 max_passengers = 20 grid_map = GridMap(1, (500, 500), init_cars, init_passengers) cars = grid_map.cars passengers = grid_map.passengers env = Environment(grid_map) input_size = 3 * max_cars + 5 * max_passengers # cars (px, py), passengers(pickup_x, pickup_y, dest_x, dest_y) output_size = max_cars * max_passengers # num_cars * (num_passengers + 1) hidden_size = 100 # 512 #load_file = "episode_50000_dqn_model_num_cars_2_num_passengers_7_num_episodes_100000_hidden_size_512.pth" load_file = None #greedy, random, dqn, qmix agent = Agent(env, input_size, output_size, hidden_size, max_cars=max_cars,
def mood(pyg, screen_sz, zoom, use_color, use_patterns): gfx = Gfx(pyg, screen_sz, zoom) gmap = GridMap(DefaultMap, DefaultMapTiles) cam = GridCam(gmap, pos=(float(6), float(3)), facing=(float(-1), float(0)), plane=(float(0), float(0.66))) tiles = DefaultMapTiles mspeed = 0.5 running = True while (running): gfx.blank((0, 0, 0)) rspeed = 0.5 # Poll interaction events. for event in pygame.event.get(): if pygame.QUIT == event.type: running = False elif pygame.KEYDOWN == event.type: if pygame.K_ESCAPE == event.key: running = False keys = pygame.key.get_pressed() if keys[pygame.K_RIGHT]: cam.rotate(-1 * rspeed) if keys[pygame.K_LEFT]: cam.rotate(rspeed) if keys[pygame.K_UP]: cam.forward(mspeed) # Draw the walls. for x in range(0, screen_sz[X] - 1): ray = None try: ray = GridRay(gmap, x, cam.pos, cam.facing, cam.plane, screen_sz) except (ZeroDivisionError): continue walls = [] wall = None while None == wall or \ (ray.map_x < 23 and ray.map_y < 23 and \ ray.map_x > 0 and ray.map_y > 0): wall = ray.cast(x, cam.pos, screen_sz) if None == wall: continue walls.append(wall) walls = reversed(walls) last_wall_top = 0 for wall in walls: if GridWall.FACE_BACK != wall.face: if use_color: color = pick_wall_color(wall) else: color = WHITE gfx.line( color, x, wall.draw[START], wall.draw[END], \ pick_wall_pattern( wall, use_patterns ) ) if GridWall.FACE_BACK == wall.face: last_wall_top = wall.draw[START] elif 0 < last_wall_top: if use_color: color = WHITE gfx.line( color, x, last_wall_top, wall.draw[START], \ pick_top_pattern( wall, use_patterns ) ) last_wall_top = 0 # Draw the UI. gfx.text( '{},{}'.format( int( cam.pos[X] ), int( cam.pos[Y] ) ), \ WHITE, 0, 0, (0, 0, 0) ) gfx.flip() gfx.wait(10)