class GridPath(object): def __init__(self, nrows, ncols, goal): self.map = GridMap(nrows, ncols) self.goal = goal self.path_cache = {} def getNext(self, coord): if not (coord in self.path_cache): self.computePath(coord) if coord in self.path_cache: return self.path_cache[coord] else: return None def set_blocked(self, coord, blocked=True): self.map.set_blocked(coord, blocked) self.path_cache = {} def computePath(self, coord): pf = PathFinder(self.map.successors, self.map.move_cost, self.map.move_cost) path_list = list(pf.compute_path(coord, self.goal)) for i, path_coord in enumerate(path_list): next_i = i if i == len(path_list) - 1 else i + 1 self.path_cache[path_coord] = path_list[next_i]
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
class GridPath(object): def __init__(self,nrows,ncols,goal): self.map = GridMap(nrows,ncols) self.goal = goal self.path_cache = {} def getNext(self,coord): if not (coord in self.path_cache): self.computePath(coord) if coord in self.path_cache: return self.path_cache[coord] else: return None def set_blocked(self,coord,blocked=True): self.map.set_blocked(coord,blocked) self.path_cache = {} def computePath(self,coord): pf = PathFinder(self.map.successors, self.map.move_cost, self.map.move_cost) path_list = list(pf.compute_path(coord, self.goal) ) for i, path_coord in enumerate(path_list): next_i = i if i==len(path_list)-1 else i+1 self.path_cache[path_coord] = path_list[next_i]
class GridPath(object): """ Represents the game grid and answers questions about paths on this grid. After initialization, call set_blocked for changed information about the state of blocks on the grid, and get_next to get the next coordinate on the path to the goal from a given coordinate. """ 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 get_next(self, coord): """ Get the next coordinate to move to from 'coord' towards the goal. """ # If the next path for this coord is not cached, compute # it # if not (coord in self._path_cache): self._compute_path(coord) # _compute_path adds the path for the coord to the cache. # If it's still not cached after the computation, it means # that no path exists to the goal from this coord. # if coord in self._path_cache: return self._path_cache[coord] else: return None def set_blocked(self, coord, blocked=True): """ Set the 'blocked' state of a coord """ self.map.set_blocked(coord, blocked) # Invalidate cache, because the map has changed # self._path_cache = {} def _compute_path(self, coord): pf = PathFinder(self.map.successors, self.map.move_cost, self.map.move_cost) # Get the whole path from coord to the goal into a list, # and for each coord in the path write the next coord in # the path into the path cache # path_list = list(pf.compute_path(coord, self.goal)) for i, path_coord in enumerate(path_list): next_i = i if i == len(path_list) - 1 else i + 1 self._path_cache[path_coord] = path_list[next_i]
def main(ampm: bool = False): hr, mn, se = str(datetime.now()).split(' ')[1].split('.')[0].split(':') print( GridMap.str_to_gm("{0}:{1}:{2}".format(hr, mn, se)).grid_without_lines()) while True: _hr, _mn, _se = str( datetime.now()).split(' ')[1].split('.')[0].split(':') if (hr, mn, se) != (_hr, _mn, _se): if ampm: if int(_hr) < 12: if int(_hr) == 0: hr, mn, se = ("12", _mn, _se + " AM") else: hr, mn, se = (_hr, _mn, _se + " AM") else: hr, mn, se = (str(int(_hr) - 12), _mn, _se + " PM") else: hr, mn, se = (_hr, _mn, _se) clear() print( GridMap.str_to_gm("{0}:{1}:{2}".format( hr, mn, se)).grid_without_lines()) if ampm: hr, mn, se = (_hr, _mn, _se)
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 get_path(self, destination): gridmap = GridMap( len(entities.shop['object'].shop_grid[0]), len(entities.shop['object'].shop_grid) ) blocked_tiles = self.get_tiles('passable', False) for blocked_tile in blocked_tiles: gridmap.set_blocked(blocked_tile) self.pathfinder = PathFinder(gridmap.successors, gridmap.move_cost, gridmap.move_cost) self.path = self.pathfinder.compute_path( self.get_shop_grid_location(), destination )
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 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 self.path_grid = GridMap(self.scale2path(self.m_xmax_field), self.scale2path(self.m_ymax_field)) self.pathfinder = PathFinder(self.path_grid.successors, self.path_grid.move_cost, self.path_grid.estimate)
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 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 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 display(): globals()['var'] = tuple( str(datetime.now()).split(" ")[1].split(".")[0].split(":")) h = var[0] m = var[1] s = var[2] hms_grid_obj = gm.merge( gm.merge(gm.merge(gm.merge(grid_dict[h[0]], grid_dict[h[1]]), col), gm.merge(gm.merge(grid_dict[m[0]], grid_dict[m[1]]), col)), gm.merge(grid_dict[s[0]], grid_dict[s[1]])) print(hms_grid_obj.grid_without_lines())
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 create(filename,l,b): with open(filename, "w") as f: f.write(str(GridMap(l,b,lines=True)))
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)
def __init__(self,nrows,ncols,goal): self.map = GridMap(nrows,ncols) self.goal = goal self.path_cache = {}
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()
class MyField(Field): """An object representing the field. """ cellClass = MyCell connectorClass = MyConnector groupClass = MyGroup def __init__(self): self.m_xmin_field = XMIN_FIELD self.m_ymin_field = YMIN_FIELD self.m_xmax_field = XMAX_FIELD self.m_ymax_field = YMAX_FIELD self.m_xmin_vector = XMIN_VECTOR self.m_ymin_vector = YMIN_VECTOR self.m_xmax_vector = XMAX_VECTOR self.m_ymax_vector = YMAX_VECTOR self.m_xmin_screen = XMIN_SCREEN self.m_ymin_screen = YMIN_SCREEN self.m_xmax_screen = XMAX_SCREEN self.m_ymax_screen = YMAX_SCREEN self.m_path_unit = PATH_UNIT self.m_path_scale = 1.0/self.m_path_unit self.m_screen_scale = 1 self.m_vector_scale = 1 # our default margins, one will be overwriten below self.m_xmargin = int(self.m_xmax_screen*DEF_MARGIN) self.m_ymargin = int(self.m_ymax_screen*DEF_MARGIN) self.set_scaling() self.m_screen = object self.m_pathgrid = object self.m_pathfinder = object super(MyField, self).__init__() self.make_path_grid() # Screen Stuff def init_screen(self): # initialize window #(xmax_screen,ymax_screen) = self.screenMax() width = self.m_xmax_screen - self.m_xmin_screen height = self.m_ymax_screen - self.m_ymin_screen if dbug.LEV & dbug.FIELD: print "field:init_screen" self.m_screen = Window(self,width=width,height=height) # set window background color = r, g, b, alpha # each value goes from 0.0 to 1.0 # ... perform some additional initialisation # moved to window class #pyglet.gl.glClearColor(*DEF_BKGDCOLOR) #self.m_screen.clear() # register draw routing with pyglet # TESTED: These functions are being called correctly, and params are # being passed correctly self.m_screen.set_minimum_size(XMAX_SCREEN/4, YMAX_SCREEN/4) self.m_screen.set_visible() # Scaling def set_scaling(self,pmin_field=None,pmax_field=None,pmin_vector=None,pmax_vector=None, pmin_screen=None,pmax_screen=None,path_unit=None): """Set up scaling in the field. A word about graphics scaling: * The vision tracking system (our input data) measures in meters. * The laser DAC measures in uh, int16? -32,768 to 32,768 * Pyglet measures in pixels at the screen resolution of the window you create * The pathfinding units are each some ratio of the smallest expected radius So we will keep eveything internally in centemeters (so we can use ints instead of floats), and then convert it to the appropriate units before display depending on the output mode """ if pmin_field is not None: self.m_xmin_field = pmin_field[0] self.m_ymin_field = pmin_field[1] if pmax_field is not None: self.m_xmax_field = pmax_field[0] self.m_ymax_field = pmax_field[1] if pmin_vector is not None: self.m_xmin_vector = pmin_vector[0] self.m_ymin_vector = pmin_vector[1] if pmax_vector is not None: self.m_xmax_vector = pmax_vector[0] self.m_ymax_vector = pmax_vector[1] if pmin_screen is not None: self.m_xmin_screen = pmin_screen[0] self.m_ymin_screen = pmin_screen[1] if pmax_screen is not None: self.m_xmax_screen = pmax_screen[0] self.m_ymax_screen = pmax_screen[1] if path_unit is not None: self.m_path_unit = path_unit self.m_path_scale = 1.0/path_unit xmin_field = self.m_xmin_field ymin_field = self.m_ymin_field xmax_field = self.m_xmax_field ymax_field = self.m_ymax_field xmin_vector = self.m_xmin_vector ymin_vector = self.m_ymin_vector xmax_vector = self.m_xmax_vector ymax_vector = self.m_ymax_vector xmin_screen = self.m_xmin_screen ymin_screen = self.m_ymin_screen xmax_screen = self.m_xmax_screen ymax_screen = self.m_ymax_screen if dbug.LEV & dbug.MORE: print "Field dims:",(xmin_field,ymin_field),\ (xmax_field,ymax_field) # in order to find out how to display this, # 1) we find the aspect ratio (x/y) of the screen or vector (depending on the # mode). # 2) Then if the aspect ratio (x/y) of the reported field is greater, we # set the x axis to stretch to the edges of screen (or vector) and then use # that value to determine the scaling. # 3) But if the aspect ratio (x/y) of the reported field is less than, # we set the y axis to stretch to the top and bottom of screen (or # vector) and use that value to determine the scaling. # aspect ratios used only for comparison field_aspect = float(xmax_field-xmin_field)/(ymax_field-ymin_field) #if GRAPHMODES & GRAPHOPTS['osc']: #vector_aspect = float(xmax_vector-xmin_vector)/(ymax_vector-ymin_vector) if GRAPHMODES & GRAPHOPTS['screen']: screen_aspect = float(xmax_screen-xmin_screen)/(ymax_screen-ymin_screen) if field_aspect > screen_aspect: if dbug.LEV & dbug.MORE: print "Field:SetScaling:Longer in the x dimension" field_xlen=xmax_field-xmin_field if field_xlen: self.m_xmargin = int(xmax_screen*DEF_MARGIN) # scale = vector_width / field_width self.m_vector_scale = \ float(xmax_vector-xmin_vector)/field_xlen # scale = (screen_width - margin) / field_width self.m_screen_scale = \ float(xmax_screen-xmin_screen-(self.m_xmargin*2))/field_xlen self.m_ymargin = \ int(((ymax_screen-ymin_screen)- ((ymax_field-ymin_field)*self.m_screen_scale)) / 2) else: if dbug.LEV & dbug.FIELD: print "Field:SetScaling:Longer in the y dimension" field_ylen=ymax_field-ymin_field if field_ylen: self.m_ymargin = int(ymax_screen*DEF_MARGIN) self.m_vector_scale = \ float(ymax_vector-ymin_vector)/field_ylen self.m_screen_scale = \ float(ymax_screen-ymin_screen-(self.m_ymargin*2))/field_ylen self.m_xmargin = \ int(((xmax_screen-xmin_screen)- ((xmax_field-xmin_field)*self.m_screen_scale)) / 2) if dbug.LEV & dbug.MORE: print "Screen dims:",(xmin_screen,ymin_screen),\ (xmax_screen,ymax_screen) #print "Screen scale:",self.m_screen_scale #print "Screen margins:",(self.m_xmargin,self.m_ymargin) if dbug.LEV & dbug.MORE: print "Used screen space:",\ self.rescale_pt2screen((xmin_field,ymin_field)),\ self.rescale_pt2screen((xmax_field,ymax_field)) # Everything #CHANGE: incorporated into draw #def render_all(self): # """Render all the cells and connectors.""" # self.render_all_cells() # self.render_all_connectors() # self.render_all_groups() def draw_all(self): """Draw all the cells and connectors.""" self.m_screen.draw_guides() self.draw_all_cells() self.calc_all_paths() self.draw_all_connectors() self.draw_all_groups() #CHANGE: incorporated into draw #def render_cell(self,cell): # """Render a cell. # # We first check if the cell is good. # If not, we increment its suspect count # If yes, render it. # """ # if self.is_cell_good_to_go(cell.m_id): # cell.render() # #del self.m_suspect_cells[cell.m_id] #def render_all_cells(self): # # we don't call the Cell's render-er directly because we have some # # logic here at this level # for cell in self.m_cell_dict.values(): # self.render_cell(cell) def draw_cell(self,cell): if self.is_cell_good_to_go(cell.m_id): cell.draw() def draw_all_cells(self): # we don't call the Cell's draw-er directly because we may want # to introduce logic at this level for cell in self.m_cell_dict.values(): self.draw_cell(cell) # Connectors #CHANGE: incorporated into draw #def render_connector(self,connector): # """Render a connector. # # We first check if the connector's two cells are both good. # If not, we increment its suspect count # If yes, render it. # """ # if self.is_conx_good_to_go(connector.m_id): # connector.render() #CHANGE: incorporated into draw #def render_all_connectors(self): # # we don't call the Connector's render-er directly because we have some # # logic here at this level # for connector in self.m_conx_dict.values(): # self.render_connector(connector) def draw_connector(self,connector): if self.is_conx_good_to_go(connector.m_id): connector.draw() def draw_all_connectors(self): # we don't call the Connector's draw-er directly because we may want # to introduce logic at this level for connector in self.m_conx_dict.values(): connector.update() self.draw_connector(connector) # Groups #CHANGE: incorporated into draw #def render_group(self,group): # """Render a group. # # We first check if the group's is in the group list # If yes, render it. # """ # if self.is_group_good_to_go(group.m_id): # group.render() #CHANGE: incorporated into draw #def render_all_groups(self): # # we don't call the Connector's render-er directly because we have some # # logic here at this level # for group in self.m_group_dict.values(): # self.render_group(group) def draw_group(self,group): if self.is_group_good_to_go(group.m_id): group.draw() def draw_all_groups(self): # we don't call the Connector's draw-er directly because we may want # to introduce logic at this level for group in self.m_group_dict.values(): self.draw_group(group) # Distances - TODO: temporary -- this info will come from the conductor subsys #def dist_sqd(self,cell0,cell1): # moved to superclass #def calc_distances(self): # moved to superclass # Paths def calc_all_paths(self): self.reset_path_grid() self.set_path_blocks() self.calc_connector_paths() 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 reset_path_grid(self): self.m_pathgrid.reset_grid() # we store the results of all the paths, why? Not sure we need to anymore #self.allpaths = [] def set_path_blocks(self): #print "***Before path: ",self.m_cell_dict for cell in self.m_cell_dict.values(): if self.is_cell_good_to_go(cell.m_id): origpt = (cell.m_x, cell.m_y) newpt = self.rescale_pt2path(origpt) self.m_pathgrid.set_blocked( self.rescale_pt2path((cell.m_x, cell.m_y)), self.rescale_num2path(cell.m_diam/2), BLOCK_FUZZ) def calc_connector_paths(self): """ Find path for all the connectors. We sort the connectors by distance and do easy paths for the closest ones first. """ #conx_dict_rekeyed = self.m_conx_dict #for i in conx_dict_rekeyed.iterkeys(): conx_dict_rekeyed = {} for connector in self.m_conx_dict.values(): if self.is_conx_good_to_go(connector.m_id): # normally we'd take the sqrt to get the distance, but here this is # just used as a sort comparison, so we'll not take the hit for sqrt dist = sqrt((connector.m_cell0.m_x - connector.m_cell1.m_x)**2 + \ (connector.m_cell0.m_y - connector.m_cell1.m_y)**2) # here we save time by reindexing as we go through it connector.update(dist=dist) conx_dict_rekeyed[dist] = connector for i in sorted(conx_dict_rekeyed.iterkeys()): connector = conx_dict_rekeyed[i] #print "findpath--id:",connector.m_id,"dist:",i**0.5 path = self.find_path(connector) connector.add_path(path) #import pdb;pdb.set_trace() def find_path(self, connector): """ Find path in path_grid and then scale it appropriately.""" start = self.rescale_pt2path((connector.m_cell0.m_x, connector.m_cell0.m_y)) goal = self.rescale_pt2path((connector.m_cell1.m_x, connector.m_cell1.m_y)) # TODO: Either here or in compute_path we first try several simple/dumb # paths, reserving A* for the ones that are blocked and need more # smarts. We sort the connectors by distance and do easy paths for the # closest ones first. path = list(self.m_pathgrid.easy_path(start, goal)) #if not path: #path = list(self.m_pathfinder.compute_path(start, goal)) # take results of found paths and block them on the map self.m_pathgrid.set_block_line(path) #self.allpaths = self.allpaths + path rescaled_path = self.rescale_path2pt(path) #import pdb;pdb.set_trace() return rescaled_path def print_grid(self): self.m_pathgrid.printme() # Scaling conversions def _convert(self,obj,scale,min1,min2): """Recursively converts numbers in an object. This function accepts single integers, tuples, lists, or combinations. """ if isinstance(obj, (int, float)): #return(int(obj*scale) + min) if isinstance(min1, int) and isinstance(min2, int): return int((obj-min1)*scale) + min2 return (obj-min1)*scale + min2 elif isinstance(obj, list): mylist = [] for i in obj: mylist.append(self._convert(i,scale,min1,min2)) return mylist elif isinstance(obj, tuple): mylist = [] for i in obj: mylist.append(self._convert(i,scale,min1,min2)) return tuple(mylist) def scale2screen(self,n): """Convert internal unit (m) to units usable for screen. """ return self._convert(n,self.m_screen_scale,self.m_xmin_field,self.m_xmin_screen) def scale2vector(self,n): """Convert internal unit (m) to units usable for vector. """ return self._convert(n,self.m_vector_scale,self.m_xmin_field,self.m_xmin_vector) def scale2path(self,n): """Convert internal unit (m) to units usable for pathfinding. """ return self._convert(n,self.m_path_scale,self.m_xmin_field,0) def path2scale(self,n): """Convert pathfinding units to internal unit (cm). """ #print "m_path_scale",self.m_path_scale return self._convert(n,1/self.m_path_scale,0,self.m_xmin_field) def _rescale_pts(self,obj,scale,orig_pmin,new_pmin,type=None): """Recursively rescales points or lists of points. This function accepts single integers, tuples, lists, or combinations. """ # if this is a point, rescale it if isinstance(obj, tuple) and len(obj) == 2 and \ isinstance(obj[0], (int,float)) and \ isinstance(obj[1], (int,float)): # if we were given ints (pixel scaling), return ints if type == 'int': x = int((obj[0]-orig_pmin[0])*scale) + new_pmin[0] y = int((obj[1]-orig_pmin[1])*scale) + new_pmin[1] # otherwise (m scaling), return floats else: x = float(obj[0]-orig_pmin[0])*scale + new_pmin[0] y = float(obj[1]-orig_pmin[1])*scale + new_pmin[1] return x,y # if this is a list, examine each element, return list elif isinstance(obj, (list,tuple)): mylist = [] for i in obj: mylist.append(self._rescale_pts(i, scale, orig_pmin, new_pmin, type)) return mylist # if this is a tuple, examine each element, return tuple elif isinstance(obj, tuple): mylist = [] for i in obj: mylist.append(self._rescale_pts(i, scale, orig_pmin, new_pmin)) return tuple(mylist) # otherwise, we don't know what to do with it, return it # TODO: Consider throwing an exception else: print "ERROR: Can only rescale a point, not",obj return obj def rescale_pt2screen(self,p): """Convert coord in internal units (cm) to units usable for the vector or screen. """ orig_pmin = (self.m_xmin_field,self.m_ymin_field) scale = self.m_screen_scale new_pmin = (self.m_xmin_screen+self.m_xmargin,self.m_ymin_screen+self.m_ymargin) return self._rescale_pts(p,scale,orig_pmin,new_pmin, 'int') def rescale_pt2vector(self,p): """Convert coord in internal units (cm) to units usable for the vector or screen. """ orig_pmin = (self.m_xmin_field,self.m_ymin_field) scale = self.m_vector_scale new_pmin = (self.m_xmin_vector,self.m_ymin_vector) return self._rescale_pts(p,scale,orig_pmin,new_pmin, 'float') def rescale_pt2path(self,p): """Convert coord in internal units (cm) to units usable for the vector or screen. """ orig_pmin = (self.m_xmin_field,self.m_ymin_field) scale = self.m_path_scale new_pmin = (0,0) return self._rescale_pts(p,scale,orig_pmin,new_pmin, 'int') def rescale_path2pt(self,p): """Convert coord in internal units (cm) to units usable for the vector or screen. """ orig_pmin = (0.0,0.0) scale = 1.0/self.m_path_scale new_pmin = (self.m_xmin_field,self.m_ymin_field) return self._rescale_pts(p, scale, orig_pmin, new_pmin, 'float') def rescale_num2screen(self,n): """Convert num in internal units (cm) to units usable for screen. """ return int(n * self.m_screen_scale) def rescale_num2vector(self,n): """Convert num in internal units (cm) to units usable for vector. """ return float(n) * self.m_vector_scale def rescale_num2path(self,n): """Convert num in internal units (cm) to units usable for vector. """ return int(n * self.m_path_scale) def rescale_path2num(self,n): """Convert num in internal units (cm) to units usable for vector. """ return float(n) / self.m_path_scale
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()
class MyField(Field): """An object representing the field. """ cellClass = MyCell connectorClass = MyConnector def __init__(self): self.m_xmin_field = XMIN_FIELD self.m_ymin_field = YMIN_FIELD self.m_xmax_field = XMAX_FIELD self.m_ymax_field = YMAX_FIELD self.m_xmin_vector = XMIN_VECTOR self.m_ymin_vector = YMIN_VECTOR self.m_xmax_vector = XMAX_VECTOR self.m_ymax_vector = YMAX_VECTOR self.m_xmin_screen = XMIN_SCREEN self.m_ymin_screen = YMIN_SCREEN self.m_xmax_screen = XMAX_SCREEN self.m_ymax_screen = YMAX_SCREEN self.m_path_unit = PATH_UNIT self.m_output_mode = MODE_DEFAULT self.m_path_scale = 1 self.m_screen_scale = 1 self.m_vector_scale = 1 # our default margins, one will be overwriten below self.m_xmargin = int(self.m_xmax_screen*DEF_MARGIN) self.m_ymargin = int(self.m_ymax_screen*DEF_MARGIN) self.set_scaling() self.m_screen = object self.path_grid = object self.pathfinder = object super(MyField, self).__init__() self.make_path_grid() # Screen Stuff def init_screen(self): # initialize window #(xmax_screen,ymax_screen) = self.screenMax() #self.m_screen = pyglet.window.Window(width=xmax_screen,height=ymax_screen) width = self.m_xmax_screen - self.m_xmin_screen height = self.m_ymax_screen - self.m_ymin_screen if dbug.LEV & dbug.FIELD: print "field:init_screen" self.m_screen = Window(self,width=width,height=height) # set window background color = r, g, b, alpha # each value goes from 0.0 to 1.0 # ... perform some additional initialisation pyglet.gl.glClearColor(*DEF_BKGDCOLOR) self.m_screen.clear() # register draw routing with pyglet # TESTED: These functions are being called correctly, and params are # being passed correctly self.m_screen.set_minimum_size(XMAX_SCREEN/4, YMAX_SCREEN/4) self.m_screen.set_visible() # Scaling def set_scaling(self,pmin_field=None,pmax_field=None,pmin_vector=None,pmax_vector=None, pmin_screen=None,pmax_screen=None,path_unit=None,output_mode=None): """Set up scaling in the field. A word about graphics scaling: * The vision tracking system (our input data) measures in meters. * The laser DAC measures in uh, int16? -32,768 to 32,768 * Pyglet measures in pixels at the screen resolution of the window you create * The pathfinding units are each some ratio of the smallest expected radius So we will keep eveything internally in centemeters (so we can use ints instead of floats), and then convert it to the appropriate units before display depending on the output mode """ if pmin_field is not None: self.m_xmin_field = pmin_field[0] self.m_ymin_field = pmin_field[1] if pmax_field is not None: self.m_xmax_field = pmax_field[0] self.m_ymax_field = pmax_field[1] if pmin_vector is not None: self.m_xmin_vector = pmin_vector[0] self.m_ymin_vector = pmin_vector[1] if pmax_vector is not None: self.m_xmax_vector = pmax_vector[0] self.m_ymax_vector = pmax_vector[1] if pmin_screen is not None: self.m_xmin_screen = pmin_screen[0] self.m_ymin_screen = pmin_screen[1] if pmax_screen is not None: self.m_xmax_screen = pmax_screen[0] self.m_ymax_screen = pmax_screen[1] if path_unit is not None: self.m_path_unit = path_unit self.m_path_scale = float(1)/path_unit if output_mode is not None: self.m_output_mode = output_mode xmin_field = self.m_xmin_field ymin_field = self.m_ymin_field xmax_field = self.m_xmax_field ymax_field = self.m_ymax_field xmin_vector = self.m_xmin_vector ymin_vector = self.m_ymin_vector xmax_vector = self.m_xmax_vector ymax_vector = self.m_ymax_vector xmin_screen = self.m_xmin_screen ymin_screen = self.m_ymin_screen xmax_screen = self.m_xmax_screen ymax_screen = self.m_ymax_screen # in order to find out how to display this, # 1) we find the aspect ratio (x/y) of the screen or vector (depending on the # mode). # 2) Then if the aspect ratio (x/y) of the reported field is greater, we # set the x axis to stretch to the edges of screen (or vector) and then use # that value to determine the scaling. # 3) But if the aspect ratio (x/y) of the reported field is less than, # we set the y axis to stretch to the top and bottom of screen (or # vector) and use that value to determine the scaling. # aspect ratios used only for comparison field_aspect = float(xmax_field-xmin_field)/(ymax_field-ymin_field) if self.m_output_mode == MODE_SCREEN: display_aspect = float(xmax_screen-xmin_screen)/(ymax_screen-ymin_screen) else: display_aspect = float(xmax_vector-xmin_vector)/(ymax_vector-ymin_vector) if field_aspect > display_aspect: if dbug.LEV & dbug.FIELD: print "Field:SetScaling:Longer in the x dimension" field_xlen=xmax_field-xmin_field if field_xlen: self.m_xmargin = int(xmax_screen*DEF_MARGIN) # scale = vector_width / field_width self.m_vector_scale = \ float(xmax_vector-xmin_vector)/field_xlen # scale = (screen_width - margin) / field_width self.m_screen_scale = \ float(xmax_screen-xmin_screen-(self.m_xmargin*2))/field_xlen self.m_ymargin = \ int(((ymax_screen-ymin_screen)-((ymax_field-ymin_field)*self.m_screen_scale)) / 2) else: if dbug.LEV & dbug.FIELD: print "Field:SetScaling:Longer in the y dimension" field_ylen=ymax_field-ymin_field if field_ylen: self.m_ymargin = int(ymax_screen*DEF_MARGIN) self.m_vector_scale = \ float(ymax_vector-ymin_vector)/field_ylen self.m_screen_scale = \ float(ymax_screen-ymin_screen-(self.m_ymargin*2))/field_ylen self.m_xmargin = \ int(((xmax_screen-xmin_screen)-((xmax_field-xmin_field)*self.m_screen_scale)) / 2) if dbug.LEV & dbug.MORE: print "Field dims:",(xmin_field,ymin_field),(xmax_field,ymax_field) if dbug.LEV & dbug.MORE: print "Screen dims:",(xmin_screen,ymin_screen),(xmax_screen,ymax_screen) #print "Screen scale:",self.m_screen_scale #print "Screen margins:",(self.m_xmargin,self.m_ymargin) if dbug.LEV & dbug.MORE: print "Used screen space:",self.rescale_pt2out((xmin_field,ymin_field)),self.rescale_pt2out((xmax_field,ymax_field)) # Everything def render_all(self): """Render all the cells and connectors.""" self.render_all_cells() self.render_all_connectors() def draw_all(self): """Draw all the cells and connectors.""" self.draw_guides() self.draw_all_cells() self.draw_all_connectors() # Guides def draw_guides(self): # draw boundaries of field (if in screen mode) if self.m_output_mode == MODE_SCREEN: pyglet.gl.glColor3f(DEF_GUIDECOLOR[0],DEF_GUIDECOLOR[1],DEF_GUIDECOLOR[2]) points = [(self.m_xmin_field,self.m_ymin_field), (self.m_xmin_field,self.m_ymax_field), (self.m_xmax_field,self.m_ymax_field), (self.m_xmax_field,self.m_ymin_field)] if dbug.LEV & dbug.GRAPH: print "boundary points (field):",points index = [0,1,1,2,2,3,3,0] screen_pts = self.rescale_pt2out(points) if dbug.LEV & dbug.GRAPH: print "boundary points (screen):",screen_pts # boundary points (screen): [(72, 73), (72, 721), (1368, 721), (1368, 73)] if dbug.LEV & dbug.GRAPH: print "proc screen_pts:",tuple(chain(*screen_pts)) # proc screen_pts: (72, 73, 72, 721, 1368, 721, 1368, 73) if dbug.LEV & dbug.GRAPH: print "PYGLET:pyglet.graphics.draw_indexed(",len(screen_pts),", pyglet.gl.GL_LINES," if dbug.LEV & dbug.GRAPH: print " ",index if dbug.LEV & dbug.GRAPH: print " ('v2i',",tuple(chain(*screen_pts)),")," if dbug.LEV & dbug.GRAPH: print " )" pyglet.graphics.draw_indexed(len(screen_pts), pyglet.gl.GL_LINES, index, ('v2i',tuple(chain(*screen_pts))), ) #point = (self.m_xmin_field,self.m_ymin_field) #radius = self.rescale_num2out(DEF_RADIUS) #shape = Circle(self,point,radius,DEF_LINECOLOR,solid=False) #shape.render() #shape.draw() if dbug.LEV & dbug.MORE: print "Field:drawGuides" # Cells #def create_cell(self, id): # moved to superclass #def update_cell(self, id, p=None, r=None, effects=None, color=None): # moved to superclass #def is_cell_good_to_go(self, id): # moved to superclass #def del_cell(self, id): # moved to superclass #def check_people_count(self,reported_count): # moved to superclass #def hide_cell(self, id): # moved to superclass #def hide_all_cells(self): # moved to superclass def render_cell(self,cell): """Render a cell. We first check if the cell is good. If not, we increment its suspect count If yes, render it. """ if self.is_cell_good_to_go(cell.m_id): cell.render() #del self.m_suspect_cells[cell.m_id] else: if dbug.LEV & dbug.FIELD: print "Field:renderCell:Cell",cell.m_id,"is suspected lost for",\ self.m_suspect_cells[cell.m_id],"frames" if self.m_suspect_cells[cell.m_id] > MAX_LOST_PATIENCE: self.del_cell(cell.m_id) else: self.m_suspect_cells[cell.m_id] += 1 def render_all_cells(self): # we don't call the Cell's render-er directly because we have some # logic here at this level for cell in self.m_cell_dict.values(): self.render_cell(cell) def draw_cell(self,cell): cell.draw() def draw_all_cells(self): # we don't call the Cell's draw-er directly because we may want # to introduce logic at this level for cell in self.m_cell_dict.values(): self.draw_cell(cell) # Connectors #def create_connector(self, id, cell0, cell1): # moved to superclass #def del_connector(self,conxid): # moved to superclass def render_connector(self,connector): """Render a connector. We first check if the connector's two cells are both good. If not, we increment its suspect count If yes, render it. """ if self.is_cell_good_to_go(connector.m_cell0.m_id) and \ self.is_cell_good_to_go(connector.m_cell1.m_id): connector.render() if connector.m_id in self.m_suspect_conxs: del self.m_suspect_conxs[connector.m_id] else: if dbug.LEV & dbug.FIELD: print "Field:renderConnector:Conx",connector.m_id,"between",\ connector.m_cell0.m_id,"and",connector.m_cell1.m_id,"is suspected lost" if self.m_suspect_conxs[connector.m_id] > MAX_LOST_PATIENCE: self.del_connector(connector.m_id) else: self.m_suspect_conxs[connector.m_id] += 1 def render_all_connectors(self): # we don't call the Connector's render-er directly because we have some # logic here at this level for connector in self.m_connector_dict.values(): self.render_connector(connector) def draw_connector(self,connector): connector.draw() def draw_all_connectors(self): # we don't call the Connector's draw-er directly because we may want # to introduce logic at this level for connector in self.m_connector_dict.values(): self.draw_connector(connector) # Distances - TODO: temporary -- this info will come from the conduction subsys #def dist_sqd(self,cell0,cell1): # moved to superclass #def calc_distances(self): # moved to superclass # Paths # should the next two functions be in the gridmap module? No, because the GridMap # and Pathfinder classes have to be instantiated from somewhere. And if not # here they have to be called from the main loop. Better here. 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 self.path_grid = GridMap(self.scale2path(self.m_xmax_field), self.scale2path(self.m_ymax_field)) self.pathfinder = PathFinder(self.path_grid.successors, self.path_grid.move_cost, self.path_grid.estimate) def reset_path_grid(self): self.path_grid.reset_grid() # we store the results of all the paths, why? Not sure we need to anymore #self.allpaths = [] def path_score_cells(self): #print "***Before path: ",self.m_cell_dict for cell in self.m_cell_dict.values(): self.path_grid.set_blocked(self.scale2path(cell.m_location), self.scale2path(cell.m_radius),BLOCK_FUZZ) def path_find_connectors(self): """ Find path for all the connectors. We sort the connectors by distance and do easy paths for the closest ones first. """ #connector_dict_rekeyed = self.m_connector_dict #for i in connector_dict_rekeyed.iterkeys(): connector_dict_rekeyed = {} for connector in self.m_connector_dict.values(): p0 = connector.m_cell0.m_location p1 = connector.m_cell1.m_location # normally we'd take the sqrt to get the distance, but here this is # just used as a sort comparison, so we'll not take the hit for sqrt score = ((p0[0] - p1[0]) ** 2 + (p0[1] - p1[1]) ** 2) # here we save time by sorting as we go through it connector_dict_rekeyed[score] = connector for i in sorted(connector_dict_rekeyed.iterkeys()): connector = connector_dict_rekeyed[i] print "findpath--id:",connector.m_id,"dist:",i**0.5 connector.add_path(self.find_path(connector)) def find_path(self, connector): """ Find path in path_grid and then scale it appropriately.""" start = self.scale2path(connector.m_cell0.m_location) goal = self.scale2path(connector.m_cell1.m_location) # TODO: Either here or in compute_path we first try several simple/dumb # paths, reserving A* for the ones that are blocked and need more # smarts. We sort the connectors by distance and do easy paths for the # closest ones first. #path = list(self.path_grid.easy_path(start, goal)) #print "connector:id",connector.m_id,"path:",path #if not path: path = list(self.pathfinder.compute_path(start, goal)) # take results of found paths and block them on the map self.path_grid.set_block_line(path) #self.allpaths = self.allpaths + path return self.path2scale(path) def print_grid(self): self.path_grid.printme() # Scaling conversions def _convert(self,obj,scale,min1,min2): """Recursively converts numbers in an object. This function accepts single integers, tuples, lists, or combinations. """ if isinstance(obj, (int, float)): #return(int(obj*scale) + min) return int((obj-min1)*scale) + min2 elif isinstance(obj, list): mylist = [] for i in obj: mylist.append(self._convert(i,scale,min1,min2)) return mylist elif isinstance(obj, tuple): mylist = [] for i in obj: mylist.append(self._convert(i,scale,min1,min2)) return tuple(mylist) def scale2out(self,n): """Convert internal unit (cm) to units usable for the vector or screen. """ if self.m_output_mode == MODE_SCREEN: return self._convert(n,self.m_screen_scale,self.m_xmin_field,self.m_xmin_screen) return self._convert(n,self.m_vector_scale,self.m_xmin_field,self.m_xmin_vector) def scale2path(self,n): """Convert internal unit (cm) to units usable for pathfinding. """ return self._convert(n,self.m_path_scale,self.m_xmin_field,0) def path2scale(self,n): """Convert pathfinding units to internal unit (cm). """ #print "m_path_scale",self.m_path_scale return self._convert(n,1/self.m_path_scale,0,self.m_xmin_field) def _rescale_pts(self,obj,scale,orig_pmin,new_pmin): """Recursively rescales points or lists of points. This function accepts single integers, tuples, lists, or combinations. """ # if this is a point, rescale it if isinstance(obj, tuple) and len(obj) == 2 and \ isinstance(obj[0], (int,float)) and isinstance(obj[1], (int,float)): x = int((obj[0]-orig_pmin[0])*scale) + new_pmin[0] y = int((obj[1]-orig_pmin[1])*scale) + new_pmin[1] return x,y # if this is a list, examine each element, return list elif isinstance(obj, (list,tuple)): mylist = [] for i in obj: mylist.append(self._rescale_pts(i,scale,orig_pmin,new_pmin)) return mylist # if this is a tuple, examine each element, return tuple elif isinstance(obj, tuple): mylist = [] for i in obj: mylist.append(self._rescale_pts(i,scale,orig_pmin,new_pmin)) return tuple(mylist) # otherwise, we don't know what to do with it, return it # TODO: Consider throwing an exception else: print "ERROR: Can only rescale a point, not",obj return obj def rescale_pt2out(self,p): """Convert coord in internal units (cm) to units usable for the vector or screen. """ orig_pmin = (self.m_xmin_field,self.m_ymin_field) if self.m_output_mode == MODE_SCREEN: scale = self.m_screen_scale new_pmin = (self.m_xmin_screen+self.m_xmargin,self.m_ymin_screen+self.m_ymargin) else: scale = self.m_vector_scale new_pmin = (self.m_xmin_vector,self.m_ymin_vector) return self._rescale_pts(p,scale,orig_pmin,new_pmin) def rescale_num2out(self,n): """Convert num in internal units (cm) to units usable for the vector or screen. """ if self.m_output_mode == MODE_SCREEN: scale = self.m_screen_scale else: scale = self.m_vector_scale return n*scale
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
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(
class Visualizer(object): def __init__(self, screen, field, message_func): self.screen = screen self.field = field self.message_func = message_func self.grid_size = 15 self.field_color = Color('black') self.grid_color = Color('gray') self.start_pos_color = Color('red') self.goal_pos_color = Color('green') self.path_color = Color('violet') self.blocked_color = Color('gray') self._init_map() def draw(self): self._draw_grid(self.field) self._draw_map(self.field, self.blocked_list, self.start_pos, self.goal_pos, self.path) self.message_func(self.msg1, self.msg2) def user_event(self, event): if event.type == pygame.KEYDOWN: if event.key == pygame.K_F5: self._recompute_path() elif event.type == pygame.MOUSEBUTTONDOWN: self.path_valid = False self.msg1 = 'Please recompute path (F5)' self.msg2 = '' self._handle_mouse_click(event) ########################## PRIVATE ########################## 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 _handle_mouse_click(self, event): if not self.field.collidepoint(event.pos): return ncol = (event.pos[0] - self.field.left) / self.grid_size nrow = (event.pos[1] - self.field.top) / self.grid_size coord = (nrow, ncol) if event.button == 1: self.map.set_blocked(coord, not self.map.blocked[coord]) elif event.button == 2: self.start_pos = coord elif event.button == 3: self.goal_pos = coord def _recompute_path(self): self.blocked_list = self.map.blocked pf = PathFinder(self.map.successors, self.map.move_cost, self.map.move_cost) t = time.clock() self.path = list(pf.compute_path(self.start_pos, self.goal_pos)) dt = time.clock() - t if self.path == []: self.msg1 = "No path found" else: self.msg1 = "Found path (length %d)" % len(self.path) self.msg2 = "Elapsed: %s seconds" % dt self.path_valid = True def _draw_grid(self, field): """ Draw a grid on the given surface. """ self.screen.fill(self.field_color, field) nrows = field.height / self.grid_size ncols = field.width / self.grid_size for y in range(nrows + 1): pygame.draw.line( self.screen, self.grid_color, (field.left, field.top + y * self.grid_size - 1), (field.right - 1, field.top + y * self.grid_size - 1)) for x in range(ncols + 1): pygame.draw.line( self.screen, self.grid_color, (field.left + x * self.grid_size - 1, field.top), (field.left + x * self.grid_size - 1, field.bottom - 1)) def _draw_map(self, field, blocked, start, goal, path): def _fill_square((nrow, ncol), color): left = field.left + ncol * self.grid_size top = field.top + nrow * self.grid_size width = self.grid_size - 1 self.screen.fill(color, Rect(left, top, width, width)) def _fill_spot((nrow, ncol), color): pos_x = field.left + ncol * self.grid_size + self.grid_size / 2 pos_y = field.top + nrow * self.grid_size + self.grid_size / 2 radius = self.grid_size / 4 pygame.draw.circle(self.screen, color, (pos_x, pos_y), radius)
class Visualizer(object): def __init__(self, screen, field, message_func): self.screen = screen self.field = field self.message_func = message_func self.grid_size = 15 # Number of pixels per grid self.field_color = Color('black') self.grid_color = Color('gray') self.start_pos_color = Color('red') self.goal_pos_color = Color('green') self.path_color = Color('violet') self.blocked_color = Color('gray') self._init_map() def draw(self): self._draw_grid(self.field) self._draw_map(self.field, self.blocked_list, self.start_pos, self.goal_pos, self.path) self.message_func(self.msg1, self.msg2) def user_event(self, event): if event.type == pygame.KEYDOWN: if event.key == pygame.K_F5: self._recompute_path() elif event.type == pygame.MOUSEBUTTONDOWN: self.path_valid = False self.msg1 = 'Please recompute path (F5)' self.msg2 = '' self._handle_mouse_click(event) ########################## PRIVATE ########################## 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 _handle_mouse_click(self, event): if not self.field.collidepoint(event.pos): return ncol = (event.pos[0] - self.field.left) / self.grid_size nrow = (event.pos[1] - self.field.top) / self.grid_size coord = (nrow, ncol) if event.button == 1: self.map.set_blocked(coord, not self.map.blocked[coord]) elif event.button == 2: self.start_pos = coord elif event.button == 3: self.goal_pos = coord def _recompute_path(self): self.blocked_list = self.map.blocked pf = PathFinder(self.map.successors, self.map.move_cost, self.map.move_cost) t = time.clock() self.path = list(pf.compute_path(self.start_pos, self.goal_pos)) dt = time.clock() - t if self.path == []: self.msg1 = "No path found" else: self.msg1 = "Found path (length %d)" % len(self.path) self.msg2 = "Elapsed: %s seconds" % dt self.path_valid = True def _draw_grid(self, field): """ Draw a grid on the given surface. """ self.screen.fill(self.field_color, field) nrows = field.height / self.grid_size ncols = field.width / self.grid_size for y in range(nrows + 1): pygame.draw.line( self.screen, self.grid_color, (field.left, field.top + y * self.grid_size - 1), (field.right - 1, field.top + y * self.grid_size - 1)) for x in range(ncols + 1): pygame.draw.line( self.screen, self.grid_color, (field.left + x * self.grid_size - 1, field.top), (field.left + x * self.grid_size - 1, field.bottom - 1)) def _draw_map(self, field, blocked, start, goal, path): def _fill_square((nrow, ncol), color): left = field.left + ncol * self.grid_size top = field.top + nrow * self.grid_size width = self.grid_size - 1 self.screen.fill(color, Rect(left, top, width, width)) def _fill_spot((nrow, ncol), color): pos_x = field.left + ncol * self.grid_size + self.grid_size / 2 pos_y = field.top + nrow * self.grid_size + self.grid_size / 2 radius = self.grid_size / 4 pygame.draw.circle(self.screen, color, (pos_x, pos_y), radius)
class App: def __init__(self, gridsize=20): self.running = True self.initPygame() self.initGridMap(gridsize) def initPygame(self): pygame.init() resolution = 640, 480 self.screen = pygame.display.set_mode(resolution) self.screen.fill(bgcolor) self.rect = self.screen.get_rect() self.clock = pygame.time.Clock() 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 onEvent(self, event): if event.type == pygame.KEYDOWN: if event.key == pygame.K_F5: self.path = self.Astar.findPath() elif event.key == pygame.K_SPACE: self.Astar.step() elif event.key == pygame.K_RETURN: self.Astar.reset() self.path = None elif event.key == pygame.K_ESCAPE: self.running = False elif event.type == pygame.MOUSEBUTTONDOWN: self.onMouseClick(event) def onMouseClick(self, event): if not self.rect.collidepoint(event.pos): return row = (event.pos[1] - self.rect.top) / self.gridsize col = (event.pos[0] - self.rect.left) / self.gridsize coord = (row, col) if event.button == 1: self.gridmap.setStart(coord) elif event.button == 2: self.gridmap.setWall(coord) elif event.button == 3: self.gridmap.setGoal(coord) def getRect(self, coord): row, col = coord topleft = col * self.gridsize, row * self.gridsize rect = topleft, (self.gridsize, self.gridsize) return pygame.Rect(rect) def drawTile(self, coord, color=darkgray, thinkness=1): rect = self.getRect(coord) rect = rect.x + 1, rect.y + 1, rect.w - 1, rect.h - 1 pygame.draw.rect(self.screen, color, rect, thinkness) def drawCircle(self, coord, color=black): rect = self.getRect(coord) pygame.draw.circle(self.screen, color, rect.center, self.gridsize / 4) def drawStart(self, coord): self.drawCircle(coord, blue) def drawGoal(self, coord): self.drawCircle(coord, red) def drawWall(self, coord): rect = self.getRect(coord) pygame.draw.rect(self.screen, gray, rect) def drawExplored(self): #,explored): # print 'draw Explored => %s' %explored for node in self.Astar.explored: self.drawTile(node.coord, darkgreen, 1) def drawMap(self): for row in range(self.Nrows): for col in range(self.Ncols): coord = row, col self.drawTile(coord) val = self.gridmap.get(coord) if val == 'S': self.drawStart(coord) elif val == 'G': self.drawGoal(coord) elif val in [1, True]: self.drawWall(coord) def drawCurrent(self): current = self.Astar.current if current is None: return camefrom = current.camefrom if camefrom is None: return r, c = current.coord N = r - 1, c S = r + 1, c W = r + 0, c - 1 E = r + 0, c + 1 NE = r - 1, c + 1 NW = r - 1, c - 1 SW = r + 1, c - 1 NW = r - 1, c - 1 # if camefrom.coord == N: # from N print 'current, camefrom', current.coord, camefrom.coord pygame.draw.line(self.screen, red, current.coord, camefrom.coord, 1) def drawPath(self): if not self.path: return #print "============> found path <=============" for node in self.path: self.drawTile(node.coord, red, 1) def render(self, seconds): self.screen.fill(bgcolor) self.drawMap() self.drawExplored() self.drawPath() self.drawCurrent() pygame.display.flip() #self.gridmap.printme() def exit(self): pygame.quit() def mainloop(self): fps = 60 while self.running: seconds = self.clock.tick(fps) / 1000.0 for event in pygame.event.get(): self.onEvent(event) self.render(seconds) self.exit()
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
class App: def __init__(self,gridsize=20): self.running = True self.initPygame() self.initGridMap(gridsize) def initPygame(self): pygame.init() resolution = 640,480 self.screen = pygame.display.set_mode(resolution) self.screen.fill(bgcolor) self.rect = self.screen.get_rect() self.clock = pygame.time.Clock() 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 onEvent(self, event): if event.type == pygame.KEYDOWN: if event.key == pygame.K_F5: self.path = self.Astar.findPath() elif event.key == pygame.K_SPACE: self.Astar.step() elif event.key == pygame.K_RETURN: self.Astar.reset() self.path = None elif event.key == pygame.K_ESCAPE: self.running = False elif event.type == pygame.MOUSEBUTTONDOWN: self.onMouseClick(event) def onMouseClick(self,event): if not self.rect.collidepoint(event.pos): return row = (event.pos[1] - self.rect.top) /self.gridsize col = (event.pos[0] - self.rect.left)/self.gridsize coord = (row,col) if event.button == 1: self.gridmap.setStart(coord) elif event.button == 2: self.gridmap.setWall(coord) elif event.button == 3: self.gridmap.setGoal(coord) def getRect(self,coord): row,col = coord topleft = col*self.gridsize,row*self.gridsize rect = topleft,(self.gridsize,self.gridsize) return pygame.Rect(rect) def drawTile(self,coord,color=darkgray,thinkness=1): rect = self.getRect(coord) rect = rect.x + 1, rect.y + 1, rect.w - 1, rect.h - 1 pygame.draw.rect(self.screen,color,rect,thinkness) def drawCircle(self,coord,color=black): rect = self.getRect(coord) pygame.draw.circle(self.screen,color,rect.center,self.gridsize/4) def drawStart(self,coord): self.drawCircle(coord,blue) def drawGoal(self,coord): self.drawCircle(coord,red) def drawWall(self,coord): rect = self.getRect(coord) pygame.draw.rect(self.screen,gray,rect) def drawExplored(self): #,explored): # print 'draw Explored => %s' %explored for node in self.Astar.explored: self.drawTile(node.coord,darkgreen,1) def drawMap(self): for row in range(self.Nrows): for col in range(self.Ncols): coord = row,col self.drawTile(coord) val = self.gridmap.get(coord) if val == 'S': self.drawStart(coord) elif val == 'G': self.drawGoal(coord) elif val in [1,True]: self.drawWall(coord) def drawCurrent(self): current = self.Astar.current if current is None: return camefrom = current.camefrom if camefrom is None: return r,c = current.coord N = r-1, c S = r+1, c W = r+0, c-1 E = r+0, c+1 NE = r-1, c+1 NW = r-1, c-1 SW = r+1, c-1 NW = r-1, c-1 # if camefrom.coord == N: # from N print 'current, camefrom', current.coord,camefrom.coord pygame.draw.line(self.screen, red, current.coord, camefrom.coord,1) def drawPath(self): if not self.path: return #print "============> found path <=============" for node in self.path: self.drawTile(node.coord,red,1) def render(self,seconds): self.screen.fill(bgcolor) self.drawMap() self.drawExplored() self.drawPath() self.drawCurrent() pygame.display.flip() #self.gridmap.printme() def exit(self): pygame.quit() def mainloop(self): fps = 60 while self.running: seconds = self.clock.tick(fps)/1000.0 for event in pygame.event.get(): self.onEvent(event) self.render(seconds) self.exit()
def __init__(self, nrows, ncols, goal): self.map = GridMap(nrows, ncols) self.goal = goal self.path_cache = {}
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()
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 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)
return hash(self.coord) 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
class MyField(Field): """An object representing the field. """ cellClass = MyCell connectorClass = MyConnector groupClass = MyGroup def __init__(self): self.m_xmin_field = XMIN_FIELD self.m_ymin_field = YMIN_FIELD self.m_xmax_field = XMAX_FIELD self.m_ymax_field = YMAX_FIELD self.m_xmin_vector = XMIN_VECTOR self.m_ymin_vector = YMIN_VECTOR self.m_xmax_vector = XMAX_VECTOR self.m_ymax_vector = YMAX_VECTOR self.m_xmin_screen = XMIN_SCREEN self.m_ymin_screen = YMIN_SCREEN self.m_xmax_screen = XMAX_SCREEN self.m_ymax_screen = YMAX_SCREEN self.m_path_unit = PATH_UNIT self.m_path_scale = 1.0 / self.m_path_unit self.m_screen_scale = 1 self.m_vector_scale = 1 # our default margins, one will be overwriten below self.m_xmargin = int(self.m_xmax_screen * DEF_MARGIN) self.m_ymargin = int(self.m_ymax_screen * DEF_MARGIN) self.set_scaling() self.m_screen = object self.m_pathgrid = object self.m_pathfinder = object super(MyField, self).__init__() self.make_path_grid() # Screen Stuff def init_screen(self): # initialize window #(xmax_screen,ymax_screen) = self.screenMax() width = self.m_xmax_screen - self.m_xmin_screen height = self.m_ymax_screen - self.m_ymin_screen if dbug.LEV & dbug.FIELD: print "field:init_screen" self.m_screen = Window(self, width=width, height=height) # set window background color = r, g, b, alpha # each value goes from 0.0 to 1.0 # ... perform some additional initialisation # moved to window class #pyglet.gl.glClearColor(*DEF_BKGDCOLOR) #self.m_screen.clear() # register draw routing with pyglet # TESTED: These functions are being called correctly, and params are # being passed correctly self.m_screen.set_minimum_size(XMAX_SCREEN / 4, YMAX_SCREEN / 4) self.m_screen.set_visible() # Scaling def set_scaling(self, pmin_field=None, pmax_field=None, pmin_vector=None, pmax_vector=None, pmin_screen=None, pmax_screen=None, path_unit=None): """Set up scaling in the field. A word about graphics scaling: * The vision tracking system (our input data) measures in meters. * The laser DAC measures in uh, int16? -32,768 to 32,768 * Pyglet measures in pixels at the screen resolution of the window you create * The pathfinding units are each some ratio of the smallest expected radius So we will keep eveything internally in centemeters (so we can use ints instead of floats), and then convert it to the appropriate units before display depending on the output mode """ if pmin_field is not None: self.m_xmin_field = pmin_field[0] self.m_ymin_field = pmin_field[1] if pmax_field is not None: self.m_xmax_field = pmax_field[0] self.m_ymax_field = pmax_field[1] if pmin_vector is not None: self.m_xmin_vector = pmin_vector[0] self.m_ymin_vector = pmin_vector[1] if pmax_vector is not None: self.m_xmax_vector = pmax_vector[0] self.m_ymax_vector = pmax_vector[1] if pmin_screen is not None: self.m_xmin_screen = pmin_screen[0] self.m_ymin_screen = pmin_screen[1] if pmax_screen is not None: self.m_xmax_screen = pmax_screen[0] self.m_ymax_screen = pmax_screen[1] if path_unit is not None: self.m_path_unit = path_unit self.m_path_scale = 1.0 / path_unit xmin_field = self.m_xmin_field ymin_field = self.m_ymin_field xmax_field = self.m_xmax_field ymax_field = self.m_ymax_field xmin_vector = self.m_xmin_vector ymin_vector = self.m_ymin_vector xmax_vector = self.m_xmax_vector ymax_vector = self.m_ymax_vector xmin_screen = self.m_xmin_screen ymin_screen = self.m_ymin_screen xmax_screen = self.m_xmax_screen ymax_screen = self.m_ymax_screen if dbug.LEV & dbug.MORE: print "Field dims:",(xmin_field,ymin_field),\ (xmax_field,ymax_field) # in order to find out how to display this, # 1) we find the aspect ratio (x/y) of the screen or vector (depending on the # mode). # 2) Then if the aspect ratio (x/y) of the reported field is greater, we # set the x axis to stretch to the edges of screen (or vector) and then use # that value to determine the scaling. # 3) But if the aspect ratio (x/y) of the reported field is less than, # we set the y axis to stretch to the top and bottom of screen (or # vector) and use that value to determine the scaling. # aspect ratios used only for comparison field_aspect = float(xmax_field - xmin_field) / (ymax_field - ymin_field) #if GRAPHMODES & GRAPHOPTS['osc']: #vector_aspect = float(xmax_vector-xmin_vector)/(ymax_vector-ymin_vector) if GRAPHMODES & GRAPHOPTS['screen']: screen_aspect = float(xmax_screen - xmin_screen) / (ymax_screen - ymin_screen) if field_aspect > screen_aspect: if dbug.LEV & dbug.MORE: print "Field:SetScaling:Longer in the x dimension" field_xlen = xmax_field - xmin_field if field_xlen: self.m_xmargin = int(xmax_screen * DEF_MARGIN) # scale = vector_width / field_width self.m_vector_scale = \ float(xmax_vector-xmin_vector)/field_xlen # scale = (screen_width - margin) / field_width self.m_screen_scale = \ float(xmax_screen-xmin_screen-(self.m_xmargin*2))/field_xlen self.m_ymargin = \ int(((ymax_screen-ymin_screen)- ((ymax_field-ymin_field)*self.m_screen_scale)) / 2) else: if dbug.LEV & dbug.FIELD: print "Field:SetScaling:Longer in the y dimension" field_ylen = ymax_field - ymin_field if field_ylen: self.m_ymargin = int(ymax_screen * DEF_MARGIN) self.m_vector_scale = \ float(ymax_vector-ymin_vector)/field_ylen self.m_screen_scale = \ float(ymax_screen-ymin_screen-(self.m_ymargin*2))/field_ylen self.m_xmargin = \ int(((xmax_screen-xmin_screen)- ((xmax_field-xmin_field)*self.m_screen_scale)) / 2) if dbug.LEV & dbug.MORE: print "Screen dims:",(xmin_screen,ymin_screen),\ (xmax_screen,ymax_screen) #print "Screen scale:",self.m_screen_scale #print "Screen margins:",(self.m_xmargin,self.m_ymargin) if dbug.LEV & dbug.MORE: print "Used screen space:",\ self.rescale_pt2screen((xmin_field,ymin_field)),\ self.rescale_pt2screen((xmax_field,ymax_field)) # Everything #CHANGE: incorporated into draw #def render_all(self): # """Render all the cells and connectors.""" # self.render_all_cells() # self.render_all_connectors() # self.render_all_groups() def draw_all(self): """Draw all the cells and connectors.""" self.m_screen.draw_guides() self.draw_all_cells() self.calc_all_paths() self.draw_all_connectors() self.draw_all_groups() #CHANGE: incorporated into draw #def render_cell(self,cell): # """Render a cell. # # We first check if the cell is good. # If not, we increment its suspect count # If yes, render it. # """ # if self.is_cell_good_to_go(cell.m_id): # cell.render() # #del self.m_suspect_cells[cell.m_id] #def render_all_cells(self): # # we don't call the Cell's render-er directly because we have some # # logic here at this level # for cell in self.m_cell_dict.values(): # self.render_cell(cell) def draw_cell(self, cell): if self.is_cell_good_to_go(cell.m_id): cell.draw() def draw_all_cells(self): # we don't call the Cell's draw-er directly because we may want # to introduce logic at this level for cell in self.m_cell_dict.values(): self.draw_cell(cell) # Connectors #CHANGE: incorporated into draw #def render_connector(self,connector): # """Render a connector. # # We first check if the connector's two cells are both good. # If not, we increment its suspect count # If yes, render it. # """ # if self.is_conx_good_to_go(connector.m_id): # connector.render() #CHANGE: incorporated into draw #def render_all_connectors(self): # # we don't call the Connector's render-er directly because we have some # # logic here at this level # for connector in self.m_conx_dict.values(): # self.render_connector(connector) def draw_connector(self, connector): if self.is_conx_good_to_go(connector.m_id): connector.draw() def draw_all_connectors(self): # we don't call the Connector's draw-er directly because we may want # to introduce logic at this level for connector in self.m_conx_dict.values(): connector.update() self.draw_connector(connector) # Groups #CHANGE: incorporated into draw #def render_group(self,group): # """Render a group. # # We first check if the group's is in the group list # If yes, render it. # """ # if self.is_group_good_to_go(group.m_id): # group.render() #CHANGE: incorporated into draw #def render_all_groups(self): # # we don't call the Connector's render-er directly because we have some # # logic here at this level # for group in self.m_group_dict.values(): # self.render_group(group) def draw_group(self, group): if self.is_group_good_to_go(group.m_id): group.draw() def draw_all_groups(self): # we don't call the Connector's draw-er directly because we may want # to introduce logic at this level for group in self.m_group_dict.values(): self.draw_group(group) # Distances - TODO: temporary -- this info will come from the conductor subsys #def dist_sqd(self,cell0,cell1): # moved to superclass #def calc_distances(self): # moved to superclass # Paths def calc_all_paths(self): self.reset_path_grid() self.set_path_blocks() self.calc_connector_paths() 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 reset_path_grid(self): self.m_pathgrid.reset_grid() # we store the results of all the paths, why? Not sure we need to anymore #self.allpaths = [] def set_path_blocks(self): #print "***Before path: ",self.m_cell_dict for cell in self.m_cell_dict.values(): if self.is_cell_good_to_go(cell.m_id): origpt = (cell.m_x, cell.m_y) newpt = self.rescale_pt2path(origpt) self.m_pathgrid.set_blocked( self.rescale_pt2path((cell.m_x, cell.m_y)), self.rescale_num2path(cell.m_diam / 2), BLOCK_FUZZ) def calc_connector_paths(self): """ Find path for all the connectors. We sort the connectors by distance and do easy paths for the closest ones first. """ #conx_dict_rekeyed = self.m_conx_dict #for i in conx_dict_rekeyed.iterkeys(): conx_dict_rekeyed = {} for connector in self.m_conx_dict.values(): if self.is_conx_good_to_go(connector.m_id): # normally we'd take the sqrt to get the distance, but here this is # just used as a sort comparison, so we'll not take the hit for sqrt dist = sqrt((connector.m_cell0.m_x - connector.m_cell1.m_x)**2 + \ (connector.m_cell0.m_y - connector.m_cell1.m_y)**2) # here we save time by reindexing as we go through it connector.update(dist=dist) conx_dict_rekeyed[dist] = connector for i in sorted(conx_dict_rekeyed.iterkeys()): connector = conx_dict_rekeyed[i] #print "findpath--id:",connector.m_id,"dist:",i**0.5 path = self.find_path(connector) connector.add_path(path) #import pdb;pdb.set_trace() def find_path(self, connector): """ Find path in path_grid and then scale it appropriately.""" start = self.rescale_pt2path( (connector.m_cell0.m_x, connector.m_cell0.m_y)) goal = self.rescale_pt2path( (connector.m_cell1.m_x, connector.m_cell1.m_y)) # TODO: Either here or in compute_path we first try several simple/dumb # paths, reserving A* for the ones that are blocked and need more # smarts. We sort the connectors by distance and do easy paths for the # closest ones first. path = list(self.m_pathgrid.easy_path(start, goal)) #if not path: #path = list(self.m_pathfinder.compute_path(start, goal)) # take results of found paths and block them on the map self.m_pathgrid.set_block_line(path) #self.allpaths = self.allpaths + path rescaled_path = self.rescale_path2pt(path) #import pdb;pdb.set_trace() return rescaled_path def print_grid(self): self.m_pathgrid.printme() # Scaling conversions def _convert(self, obj, scale, min1, min2): """Recursively converts numbers in an object. This function accepts single integers, tuples, lists, or combinations. """ if isinstance(obj, (int, float)): #return(int(obj*scale) + min) if isinstance(min1, int) and isinstance(min2, int): return int((obj - min1) * scale) + min2 return (obj - min1) * scale + min2 elif isinstance(obj, list): mylist = [] for i in obj: mylist.append(self._convert(i, scale, min1, min2)) return mylist elif isinstance(obj, tuple): mylist = [] for i in obj: mylist.append(self._convert(i, scale, min1, min2)) return tuple(mylist) def scale2screen(self, n): """Convert internal unit (m) to units usable for screen. """ return self._convert(n, self.m_screen_scale, self.m_xmin_field, self.m_xmin_screen) def scale2vector(self, n): """Convert internal unit (m) to units usable for vector. """ return self._convert(n, self.m_vector_scale, self.m_xmin_field, self.m_xmin_vector) def scale2path(self, n): """Convert internal unit (m) to units usable for pathfinding. """ return self._convert(n, self.m_path_scale, self.m_xmin_field, 0) def path2scale(self, n): """Convert pathfinding units to internal unit (cm). """ #print "m_path_scale",self.m_path_scale return self._convert(n, 1 / self.m_path_scale, 0, self.m_xmin_field) def _rescale_pts(self, obj, scale, orig_pmin, new_pmin, type=None): """Recursively rescales points or lists of points. This function accepts single integers, tuples, lists, or combinations. """ # if this is a point, rescale it if isinstance(obj, tuple) and len(obj) == 2 and \ isinstance(obj[0], (int,float)) and \ isinstance(obj[1], (int,float)): # if we were given ints (pixel scaling), return ints if type == 'int': x = int((obj[0] - orig_pmin[0]) * scale) + new_pmin[0] y = int((obj[1] - orig_pmin[1]) * scale) + new_pmin[1] # otherwise (m scaling), return floats else: x = float(obj[0] - orig_pmin[0]) * scale + new_pmin[0] y = float(obj[1] - orig_pmin[1]) * scale + new_pmin[1] return x, y # if this is a list, examine each element, return list elif isinstance(obj, (list, tuple)): mylist = [] for i in obj: mylist.append( self._rescale_pts(i, scale, orig_pmin, new_pmin, type)) return mylist # if this is a tuple, examine each element, return tuple elif isinstance(obj, tuple): mylist = [] for i in obj: mylist.append(self._rescale_pts(i, scale, orig_pmin, new_pmin)) return tuple(mylist) # otherwise, we don't know what to do with it, return it # TODO: Consider throwing an exception else: print "ERROR: Can only rescale a point, not", obj return obj def rescale_pt2screen(self, p): """Convert coord in internal units (cm) to units usable for the vector or screen. """ orig_pmin = (self.m_xmin_field, self.m_ymin_field) scale = self.m_screen_scale new_pmin = (self.m_xmin_screen + self.m_xmargin, self.m_ymin_screen + self.m_ymargin) return self._rescale_pts(p, scale, orig_pmin, new_pmin, 'int') def rescale_pt2vector(self, p): """Convert coord in internal units (cm) to units usable for the vector or screen. """ orig_pmin = (self.m_xmin_field, self.m_ymin_field) scale = self.m_vector_scale new_pmin = (self.m_xmin_vector, self.m_ymin_vector) return self._rescale_pts(p, scale, orig_pmin, new_pmin, 'float') def rescale_pt2path(self, p): """Convert coord in internal units (cm) to units usable for the vector or screen. """ orig_pmin = (self.m_xmin_field, self.m_ymin_field) scale = self.m_path_scale new_pmin = (0, 0) return self._rescale_pts(p, scale, orig_pmin, new_pmin, 'int') def rescale_path2pt(self, p): """Convert coord in internal units (cm) to units usable for the vector or screen. """ orig_pmin = (0.0, 0.0) scale = 1.0 / self.m_path_scale new_pmin = (self.m_xmin_field, self.m_ymin_field) return self._rescale_pts(p, scale, orig_pmin, new_pmin, 'float') def rescale_num2screen(self, n): """Convert num in internal units (cm) to units usable for screen. """ return int(n * self.m_screen_scale) def rescale_num2vector(self, n): """Convert num in internal units (cm) to units usable for vector. """ return float(n) * self.m_vector_scale def rescale_num2path(self, n): """Convert num in internal units (cm) to units usable for vector. """ return int(n * self.m_path_scale) def rescale_path2num(self, n): """Convert num in internal units (cm) to units usable for vector. """ return float(n) / self.m_path_scale
f"{sysvar[0]}. is not a valid hour value - 0 <= hour < 24") m = int(sysvar[1]) if m > 59 or h < 0: raise ValueError( f"{sysvar[1]}. is not a valid minute value - 0 <= minute < 60") end_at = (h, m) current_time = tuple([ int(i) for i in tuple(str(datetime.now()).split(" ")[1].split(".")[0].split(":")) ]) if end_at == current_time[:2] and current_time[2] != 0: print("talarm: Time reached already!") exit() # GridMap objects zero = gm.zero() one = gm.one() two = gm.two() three = gm.three() four = gm.four() five = gm.five() six = gm.six() seven = gm.seven() eight = gm.eight() nine = gm.nine() # object mapping grid_dict = { "0": zero, "1": one, "2": two,
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))
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
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,
from time import sleep from os import system, name from gridmap import GridMap as g # merger method m = g.merge # create required gridmap objects zero = m(g.zero(), g.zero()) one = m(g.zero(), g.one()) two = m(g.zero(), g.two()) three = m(g.zero(), g.three()) four = m(g.zero(), g.four()) five = m(g.zero(), g.five()) six = m(g.zero(), g.six()) seven = m(g.zero(), g.seven()) eight = m(g.zero(), g.eight()) nine = m(g.zero(), g.nine()) ten = m(g.one(), g.zero()) grid_obj = [ten, nine, eight, seven, six, five, four, three, two, one, zero] def clear(): if name == "nt": _ = system('cls') else: _ = system('clear') def countdown():