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 _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
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():
