def connect(lanes, matrix, length, i0, j0, i1, j1):
if i0 == i1: # horizontal
lanes.append(Lane(length, matrix[i0][j0], Direction.EAST,
matrix[i1][j1], Direction.WEST))
lanes.append(Lane(length, matrix[i1][j1], Direction.WEST,
matrix[i0][j0], Direction.EAST))
else: # vertical
lanes.append(Lane(length, matrix[i0][j0], Direction.SOUTH,
matrix[i1][j1], Direction.NORTH))
lanes.append(Lane(length, matrix[i1][j1], Direction.NORTH,
matrix[i0][j0], Direction.SOUTH))
def get_lanes_list_coordinates_zero():
try:
laneList = Lane()
except RuntimeError as exc:
mensaje, codigo = exc.args
response = {codigo: mensaje}
return Response(json.dumps(response),
mimetype='application/json',
status=codigo)
return Response(json.dumps(laneList.get_list_coordinates_zero()),
mimetype='application/json',
status=200)
def get_lane_count():
try:
laneList = Lane()
except RuntimeError as exc:
mensaje, codigo = exc.args
response = {codigo: mensaje}
return Response(json.dumps(response),
mimetype='application/json',
status=codigo)
return Response(json.dumps({'count': laneList.count()}),
mimetype='application/json',
status=200)
def DisplayTestMap(self):
Lane.ResetLaneStartingIndex()
self.Map.append(Lane.GenerateSafetyLane())
self.Map.append(Lane.GenerateEasyLane())
self.Map.append(Lane.GenerateEasyLane())
self.Map.append(Lane.GenerateEasyLane())
self.Map.append(Lane.GenerateSafetyLane())
self.Map.append(Lane.GenerateFinalLane())
self.Map.append(
Lane.GenerateFinalLane(lilyPadPattern=Config.lilypadPatternBO3))
self.Map.append(
Lane.GenerateFinalLane(lilyPadPattern=Config.lilypadPatternBO5V2))
self.Map.append(Lane.GenerateFinalLane(randomPatternBO5=True))
def __init__(self, config=None):
super().__init__()
self.data = config
self.frame_num = 0
#self.debug_frame = config['debug_frame']
self.image = None
# Camera calibration
calibration = Calibrate()
# Load calibration data
calibration.load()
pt1 = PerspectiveTransform()
pt2 = PerspectiveTransform(pt1)
sw = SlidingWindow()
tls = TargettedLineSearch()
lane = Lane()
b_gradient = Filter()
self.steps = [calibration, b_gradient, pt1, sw, tls, lane, pt2]
self.debug = config['debug']
print('Pipeline object created ...')
def LevelPassed(self):
#fja koja se poziva kad su svih 5 Lilypada popunjena, prosledjuje se u konstruktoru, prvo finalLejnu pa samim objektima
self.Level += 1
Lane.ResetLaneStartingIndex()
self.GameOverBrojac = 0
self.DeleteMap()
if self.player1 != None and self.player2 != None:
Config.p1Score = self.player1.score
Config.p2Score = self.player2.score
Config.p1Lives = self.player1.lives
Config.p2Lives = self.player2.lives
self.DisplayMap(TwoPlayers=True)
self.CreatePlayers(TwoPlayers=True)
elif self.player1 != None:
Config.p1Score = self.player1.score
Config.p1Lives = self.player1.lives
self.DisplayMap()
self.CreatePlayers()
elif self.player2 != None:
Config.p2Score = self.player2.score
Config.p2Lives = self.player2.lives
self.DisplayMap()
self.CreatePlayers(TwoPlayers=True)
self.player1.RemoveFromScreen()
self.player1 = None
if self.isHost and self.player2 != None:
self.SendClientToReplicateObjects()
def __init__(self, original_image_size, mask_vertices, anchor_points, tranformed_image_size,
cali_mtx, cali_dist, convert_x, convert_y,
window_width=50, window_height=80, margin=100,
left_lane_bound=None, right_lane_bound=None,
left_lane_pixel_thres = 600, right_lane_pixel_thres=200,
smooth_window=5):
self.original_image_size = original_image_size
self.lane = Lane(tranformed_image_size, convert_x, convert_y, smooth_window)
self.height = tranformed_image_size[0]
self.width = tranformed_image_size[1]
self.window_width = window_width
self.window_height = window_height
self.margin = margin
self.level_num = (int)(tranformed_image_size[0]/window_height)
self.window = np.ones(window_width) # Define window template
self.channels = []
[self.idx,self.idy] = np.meshgrid(range(tranformed_image_size[1]),
range(tranformed_image_size[0]))
self.calibration_matrix = cali_mtx
self.calibration_dist = cali_dist
src = np.array(anchor_points, dtype = "float32")
h_margin = 200
top_margin = 200
bottom_margin = 50
dst = np.array([[h_margin, top_margin],
[tranformed_image_size[0] - h_margin, top_margin],
[tranformed_image_size[0] - h_margin, tranformed_image_size[1] - bottom_margin],
[h_margin, tranformed_image_size[1] - bottom_margin]], dtype = "float32")
self.M = cv2.getPerspectiveTransform(src, dst)
self.M_inv = np.linalg.inv(self.M)
roi = np.zeros(original_image_size,dtype=np.uint8)
cv2.fillPoly(roi, np.array([mask_vertices], dtype=np.int32),1)
roi = cv2.undistort(roi, cali_mtx, cali_dist, None, cali_mtx)
self.mask = cv2.warpPerspective(roi, self.M, tranformed_image_size, flags=cv2.INTER_LINEAR)
if left_lane_bound is None:
left_lane_bound = (int(self.width/8),int(self.width*3/8),int(self.height*3/4), self.height)
if right_lane_bound is None:
right_lane_bound = (int(self.width*5/8),int(self.width*7/8),int(self.height*1/8), self.height)
self.set_lane_initial_detect_range(left_lane_bound, right_lane_bound)
self.left_lane_pixel_thres = left_lane_pixel_thres
self.right_lane_pixel_thres = right_lane_pixel_thres
self.texts = [] # store center offset and curvature, as well as other info for debuging
def DisplayMap(self, TwoPlayers=False):
self.Map.append(Lane.GenerateSafetyLane()) #prvi je uvek sejf lejn
self.GenerateLanes('Road') #fja da generise lejnove za put
#self.Map.append(Lane.GenerateSafetyLane())
##ovaj ce da bude uvek tu da bi se moglo duze igrati
##lejn sa zivotom
self.Map.append(Lane.GenerateSafetyLaneWithDeus())
self.GenerateLanes('Water') #fja da generise lejnove za reku
if TwoPlayers:
self.Map.append(Lane.GenerateFinalLane(
self.LevelPassed)) # zadnji je uvek finalLane
else:
self.Map.append(
Lane.GenerateFinalLane(
self.LevelPassed,
lilyPadPattern=Config.lilypadPatternBO5Standard)
) # zadnji je uvek finalLane
def get_lanes_description(id):
try:
laneList = Lane()
except RuntimeError as exc:
mensaje, codigo = exc.args
response = {codigo: mensaje}
return Response(json.dumps(response),
mimetype='application/json',
status=codigo)
try:
lane = laneList.get_description(id)
except ValueError:
response = {'404': 'Bici lane ogc-fid {} not found'.format(id)}
return Response(json.dumps(response),
mimetype='application/json',
status=404)
return Response(json.dumps(lane), mimetype='application/json', status=200)
def find_lanes_by_id(id):
try:
laneList = Lane()
except RuntimeError as exc:
mensaje, codigo = exc.args
response = {codigo: mensaje}
return Response(json.dumps(response),
mimetype='application/json',
status=codigo)
try:
lane = laneList.find_by_id(id)
except ValueError:
response = {'404': 'Bici lane id \'{}\' not found'.format(id)}
return Response(json.dumps(response),
mimetype='application/json',
status=404)
return Response(json.dumps(lane), mimetype='application/json', status=200)
def get_lanes(self):
lanes = []
for lane in self.raw_lanes:
for cell in lane:
raw_position = cell['position']
lanes.append(
Lane(Position(raw_position['y'], raw_position['x']),
BlockObject(cell['surfaceObject']),
cell['occupiedByPlayerId']))
return lanes
def __init__(self):
super(App, self).__init__()
self.setup_pygame()
self.events = Events()
self.lane = Lane(self.events)
self.lane.add_support_point(100, 100)
self.lane.add_support_point(200, 100)
self.lane.add_support_point(200, 200)
self.lane.add_support_point(100, 200)
self.cars = []
for k in range(4):
self.cars.append(
Car(x=150 + k * 5,
y=100,
theta=np.random.randint(0, 360),
speed=np.random.randint(45, 180)))
# self.cars.append(Car(x=250,y=100,theta=-45,speed=2*90))
self.action = None
self.human = HumanController()
self.heuristic = Heuristic(self.lane)
Node.heuristic = self.heuristic
self.onestep = OneStepLookaheadController(self.cars, self.lane,
self.heuristic)
self.nstep = NStepLookaheadController(self.cars, self.lane,
self.heuristic, 2)
self.bestfirst = BestFirstController(self.cars, self.lane,
self.heuristic)
self.controller = self.bestfirst
# self.window = Window(self.screen, self.events, 300, 200, "caption")
self.grid = Grid(50, 50, *self.size)
self.last_distance_grid_update = time() - 10
self.update_distance_grid()
self.done = False
self.register_events()
self.spin()
def __init__(self,
calibration_file_path='calibration.p',
sobel_kernel=3,
h_threshold=(15, 25),
y_threshold=(200, 255),
sobel_window=(50, 80, 100),
direction_threshold=(30 * np.pi / 180, 80 * np.pi / 180),
frames=8):
assert not sobel_window[0] % 2 == 0, "Error: Window size should be odd."
self._kernel = sobel_kernel
self._h_thresh = h_threshold
self._y_thresh = y_threshold
self._dir_thresh = direction_threshold
self._center_shift = 40
self._vertical_shift = 85
self._x_scaller = 3.7 / 580
self._y_scaller = 8.5 * 3 / 720
self._window_width = sobel_window[0]
self._window_height = sobel_window[1]
self._search_margin = sobel_window[2]
self._cc = CC(calibration_file_path)
self._left_lane = Lane("_left_lane", frames=frames)
self._right_lane = Lane("_right_lane")
def GameOver(self):
# javimo klijentu da bi se vratio na meni
if self.isHost:
self.host.SendToClient(Config.network_ConnectionError)
#fja koja se poziva kad su svi igraci igraci ostali bez zivota
self.stopThreadForUpdatingObjects()
self.DeleteMap(deleteP1=True, deleteP2=True)
self.Menu.ShowMainMenu()
Lane.ResetLaneStartingIndex()
self.scoreboard.HideScores()
self.Menu.kisa.hide()
self.Menu.sneg.hide()
self.scoreboard.CreateGreenLives(0)
self.scoreboard.CreatePinkLives(0)
self.InitFlagsAndVariables()
def setup(self):
# setup products & product_stats
for grp_id in range(Constants.PRODUCT_COUNT):
p = Product(grp_id)
p.setup()
sp = SmartProduct(p)
self.smart_products.append(sp)
# setup employees
self.employee_manager.create_employees(Constants.NUM_EMPLOYEES)
# setup lanes
for i in range(Constants.MAX_LANES):
ln = Lane()
self.lanes.append(ln)
today = datetime(self.clock.year, self.clock.month, self.clock.day)
for sp in self.smart_products:
sp.setup_starter_inventory(today)
def __init__(self):
super(App, self).__init__()
self.setup_pygame()
self.events = Events()
self.lane = Lane(self.events)
self.lane.add_support_point(100,100)
self.lane.add_support_point(200,100)
self.lane.add_support_point(200,200)
self.lane.add_support_point(100,200)
self.cars = []
for k in range(4):
self.cars.append(Car(x=150+k*5,y=100,theta=np.random.randint(0,360),speed=np.random.randint(45,180)))
# self.cars.append(Car(x=250,y=100,theta=-45,speed=2*90))
self.action = None
self.human = HumanController()
self.heuristic = Heuristic(self.lane)
Node.heuristic = self.heuristic
self.onestep = OneStepLookaheadController(self.cars,self.lane,self.heuristic)
self.nstep = NStepLookaheadController(self.cars,self.lane, self.heuristic, 2)
self.bestfirst = BestFirstController(self.cars,self.lane, self.heuristic)
self.controller = self.bestfirst
# self.window = Window(self.screen, self.events, 300, 200, "caption")
self.grid = Grid(50,50,*self.size)
self.last_distance_grid_update = time() - 10
self.update_distance_grid()
self.done = False
self.register_events()
self.spin()
def find_by_ogc_fid(id):
laneList = Lane()
return laneList.find_by_ogc_fid(id)
def get_count():
laneList = Lane()
return laneList.count()
def get_list_id_names():
laneList = Lane()
return laneList.get_list_id_names()
########################################################
table = PrettyTable(
["Step", "from_east", "from_west", "from_north", "from_south"])
lane_table = PrettyTable(["Lane", "Q length", "Awt"])
lanevar_table = PrettyTable(["Lane", "Tin", "Tout"])
data = {
"east": 0,
"west": 0,
"north": 0,
"south": 0,
}
#initiate lanes' objects
ln, ls, le, lw = Lane("north"), Lane("south"), Lane("east"), Lane("west")
lanes = [le, lw, ln, ls]
lanes = {
"east": le,
"west": lw,
"north": ln,
"south": ls,
}
#initiate variables
lane_data = {
"east": 0,
"west": 0,
"north": 0,
"south": 0,
}
inter = Intersection("Intersection 1")
def get_lanes():
laneList = Lane()
return laneList.get_lanes()
def __init__(self):
super(App, self).__init__()
# load background
self.background = Background(filename="background.png")
# get array copy of background image
self.background.arr = pygame.surfarray.array3d(self.background.img)
# create bw from image
_, self.background.arr_bw = cv2.threshold(self.background.arr[:, :, 0],
128, 1, cv2.THRESH_BINARY)
# print self.background.arr_bw.shape, self.background.arr_bw.dtype
# self.background.arr_dist = cv2.distanceTransform(self.background.arr_bw, cv.CV_DIST_L1, 3)
# get nearest (zero) pixel labels with corresponding distances
self.background.arr_dist, self.labels = cv2.distanceTransformWithLabels(
self.background.arr_bw,
cv.CV_DIST_L1,
3,
labelType=cv2.DIST_LABEL_PIXEL)
self.distances = self.background.arr_dist
### get x,y coordinates for each label
# get positions of zero points
zero_points = Utils.zero_points(self.background.arr_bw)
# get labels for zero points
zero_labels = self.labels[zero_points[:, 0], zero_points[:, 1]]
# create dictionary mapping labels to zero point positions
self.label_positions = dict(
zip(zero_labels, zip(zero_points[:, 0], zero_points[:, 1])))
# create hilbert curve lookup table
self.hilbert = Hilbert.hilbert_lookup(*self.background.arr.shape[:2])
# provide a rgb variant of dist for display
self.background.arr_dist_rgb = self.background.arr.copy()
self.background.arr_dist_rgb[:, :, 0] = self.background.arr_dist
self.background.arr_dist_rgb[:, :, 1] = self.background.arr_dist
self.background.arr_dist_rgb[:, :, 2] = self.background.arr_dist
# print a.shape
self.setup_pygame()
self.events = Events()
self.lane = Lane(self.events)
self.lane.load("parkour.sv")
# self.lane.add_support_point(100,100)
# self.lane.add_support_point(200,100)
# self.lane.add_support_point(200,200)
# self.lane.add_support_point(100,200)
self.optimize = Optimize(self.lane)
self.cars = []
# for k in range(1):
# self.cars.append(Car(x=150+k*5,y=100,theta=np.random.randint(0,360),speed=np.random.randint(45,180)))
self.cars.append(Car(x=50, y=250, theta=90, speed=1 * 1.5 * 90))
self.cars.append(Car(x=50, y=250, theta=90, speed=1 * 90)) # [1] human
self.cars.append(Car(x=50, y=250, theta=90,
speed=1 * 90)) # [2] ghost of ins estimating [0]
self.action = None
self.human = HumanController()
self.heuristic = Heuristic(self.lane)
Node.heuristic = self.heuristic
self.onestep = OneStepLookaheadController(self.cars, self.lane,
self.heuristic)
self.nstep = NStepLookaheadController(self.cars, self.lane,
self.heuristic, 2)
self.bestfirst = BestFirstController(self.cars, self.lane,
self.heuristic)
self.controller = self.bestfirst
self.cars[0].camview = CamView(self.cars[0], self.background.arr)
self.cars[0].camview.register_events(self.events)
self.cars[0].controller = self.controller
self.cars[0].collision = False
self.cars[0].imu = IMU(self.cars[0])
self.cars[0].ins = INS(self.cars[0].imu.calibration_noise)
self.cars[0].ins.update_pose(self.cars[0].x,
self.cars[0].y,
self.cars[0].theta / Utils.d2r,
gain=1)
self.insghost = INSGhostController(self.cars[0].ins)
self.cars[1].controller = self.human
self.cars[2].controller = self.insghost
self.cars[2].collision = False
self.cars[2].size *= 1.25
self.cars[2].camview = CamView(self.cars[2],
self.background.arr_dist_rgb,
width=275,
height=275,
offset=(0, 75),
angle_offset=-25)
self.cars[2].camview.register_events(self.events)
self.cars[0].name = "actual"
self.cars[1].name = "human"
self.cars[2].name = "estimate"
# this causes the controller of cars[0] to use the information from cars[0].ghost but act on cars[0]
# self.cars[0].ghost = self.cars[2]
# self.window = Window(self.screen, self.events, 300, 200, "caption")
self.grid = Grid(50, 50, *self.size)
self.last_distance_grid_update = time() - 10
self.update_distance_grid()
self.done = False
for car in self.cars:
# save original speed
if not hasattr(car, "speed_on"):
car.speed_on = car.speed
# toggle speed
car.speed = car.speed_on - car.speed
# car.pause = not car.pause
self.plot_window_size = 100
self.xyt_corr_ring_buffer = RingBuffer(self.plot_window_size,
channels=3)
self.xyt_corr_plot = RingBufferPlot(self.xyt_corr_ring_buffer)
# self.normal_test_p_value_plot = RingBufferPlot(RingBuffer(self.plot_window_size,channels=self.xyt_corr_ring_buffer.channels))
self.std_plot = RingBufferPlot(
RingBuffer(self.plot_window_size,
channels=self.xyt_corr_ring_buffer.channels))
self.velocity_carthesian_history_plot = RingBufferPlot(
self.cars[0].ins.velocity_carthesian_history)
# self.hist_plot = HistogramPlot(10)
self.register_events()
self.spin()
def createLanes(self, niz, type):
#funkcija koja generise lejnove u zavisnosti od prosledjenog niza i karaktera u njemu
if type == 'Road':
for letter in niz:
if letter == 'e':
self.Map.append(Lane.GenerateEasyLane())
elif letter == 'm':
self.Map.append(Lane.GenerateMediumLane())
elif letter == 'h':
self.Map.append(Lane.GenerateHardLane())
elif type == 'Water':
for letter in niz:
if letter == 'e':
self.Map.append(Lane.GenerateEasyWaterLane())
elif letter == 'm':
self.Map.append(Lane.GenerateMediumWaterLane())
elif letter == 'h':
self.Map.append(Lane.GenerateHardWaterLane())
elif type == 'Random':
for letter in niz:
if letter == 's': # safe lane
self.Map.append(Lane.GenerateSafetyLane())
elif letter == 'e': # easy lane, if generated -1 -> easyWaterLane if 1 easyTrafficLane, isto vazi za ostale samo se menja tezina s,m,h...
if [-1, 1][randrange(2)] == -1:
self.Map.append(Lane.GenerateEasyWaterLane())
else:
self.Map.append(Lane.GenerateEasyLane())
elif letter == 'm':
if [-1, 1][randrange(2)] == -1:
self.Map.append(Lane.GenerateMediumWaterLane())
else:
self.Map.append(Lane.GenerateMediumLane())
elif letter == 'h':
if [-1, 1][randrange(2)] == -1:
self.Map.append(Lane.GenerateHardWaterLane())
else:
self.Map.append(Lane.GenerateHardLane())
def createLanes(road):
laneNames = chargeLaneNames()
for i in range(0, laneQuantity):
lane = Lane(laneNames[i])
road.lanes.append(lane)
def main():
#CHANGE THESE FLAGS TO CONTROL THE EXPERIMENT
VIZ = False # Set to True for visualization
USE_ROUNDABOUT = True # Set to True to use roundabout
PAUSE = 0.1 # time (in sec) between each update, if VIZ is True
MAX_TIME = 1000000
SEED = 0
SPAWN_PROB = 0.1
row, col = 2, 2
stat = Stats()
# grid of intersection/roundabout
if USE_ROUNDABOUT:
matrix = [[Roundabout(1) for j in range(col)] for i in range(row)]
else:
matrix = [[Intersection() for j in range(col)] for i in range(row)]
despawns = [CarDespawner(stat) for i in range(row * 4)]
spawns = [CarSpawner(stat, SPAWN_PROB, despawns) for i in range(col * 4)]
# connect lanes
baseLaneLen = 10
lanes = []
# spawn/despawn lanes first
lanes.append(
Lane(baseLaneLen, spawns[0], None, matrix[0][0], Direction.NORTH))
lanes.append(
Lane(baseLaneLen, spawns[1], None, matrix[0][1], Direction.NORTH))
lanes.append(
Lane(baseLaneLen, spawns[2], None, matrix[0][1], Direction.EAST))
lanes.append(
Lane(baseLaneLen, spawns[3], None, matrix[1][1], Direction.EAST))
lanes.append(
Lane(baseLaneLen, spawns[4], None, matrix[1][1], Direction.SOUTH))
lanes.append(
Lane(baseLaneLen, spawns[5], None, matrix[1][0], Direction.SOUTH))
lanes.append(
Lane(baseLaneLen, spawns[6], None, matrix[1][0], Direction.WEST))
lanes.append(
Lane(baseLaneLen, spawns[7], None, matrix[0][0], Direction.WEST))
lanes.append(
Lane(baseLaneLen, matrix[0][0], Direction.NORTH, despawns[0], None))
lanes.append(
Lane(baseLaneLen, matrix[0][1], Direction.NORTH, despawns[1], None))
lanes.append(
Lane(baseLaneLen, matrix[0][1], Direction.EAST, despawns[2], None))
lanes.append(
Lane(baseLaneLen, matrix[1][1], Direction.EAST, despawns[3], None))
lanes.append(
Lane(baseLaneLen, matrix[1][1], Direction.SOUTH, despawns[4], None))
lanes.append(
Lane(baseLaneLen, matrix[1][0], Direction.SOUTH, despawns[5], None))
lanes.append(
Lane(baseLaneLen, matrix[1][0], Direction.WEST, despawns[6], None))
lanes.append(
Lane(baseLaneLen, matrix[0][0], Direction.WEST, despawns[7], None))
# always order endpoints as northern node, southern node OR
# western node, eastern node
connect(lanes, matrix, baseLaneLen, 0, 0, 0, 1)
connect(lanes, matrix, baseLaneLen, 0, 0, 1, 0)
connect(lanes, matrix, baseLaneLen, 0, 1, 1, 1)
connect(lanes, matrix, baseLaneLen, 1, 0, 1, 1)
if VIZ:
root = Tk()
viz = Viz(matrix, lanes, baseLaneLen, root)
random.seed(SEED)
for x in range(MAX_TIME):
for i in range(len(matrix)):
for j in range(len(matrix[0])):
if matrix[i][j]:
matrix[i][j].update()
for lane in lanes:
lane.update()
for spawn in spawns:
spawn.update()
stat.cur_time += 1
# Then update viz
if VIZ:
viz.update()
time.sleep(PAUSE)
print(len(stat.car_times))
print(sum(stat.car_times) / len(stat.car_times))
print(stat.n_rejected)
# Close window to terminate
if VIZ:
root.mainloop()
class App(object):
"""docstring for App"""
def __init__(self):
super(App, self).__init__()
# load background
self.background = Background(filename="background.png")
# get array copy of background image
self.background.arr = pygame.surfarray.array3d(self.background.img)
# create bw from image
_, self.background.arr_bw = cv2.threshold(self.background.arr[:, :, 0],
128, 1, cv2.THRESH_BINARY)
# print self.background.arr_bw.shape, self.background.arr_bw.dtype
# self.background.arr_dist = cv2.distanceTransform(self.background.arr_bw, cv.CV_DIST_L1, 3)
# get nearest (zero) pixel labels with corresponding distances
self.background.arr_dist, self.labels = cv2.distanceTransformWithLabels(
self.background.arr_bw,
cv.CV_DIST_L1,
3,
labelType=cv2.DIST_LABEL_PIXEL)
self.distances = self.background.arr_dist
### get x,y coordinates for each label
# get positions of zero points
zero_points = Utils.zero_points(self.background.arr_bw)
# get labels for zero points
zero_labels = self.labels[zero_points[:, 0], zero_points[:, 1]]
# create dictionary mapping labels to zero point positions
self.label_positions = dict(
zip(zero_labels, zip(zero_points[:, 0], zero_points[:, 1])))
# create hilbert curve lookup table
self.hilbert = Hilbert.hilbert_lookup(*self.background.arr.shape[:2])
# provide a rgb variant of dist for display
self.background.arr_dist_rgb = self.background.arr.copy()
self.background.arr_dist_rgb[:, :, 0] = self.background.arr_dist
self.background.arr_dist_rgb[:, :, 1] = self.background.arr_dist
self.background.arr_dist_rgb[:, :, 2] = self.background.arr_dist
# print a.shape
self.setup_pygame()
self.events = Events()
self.plt = Plot()
self.lane = Lane(self.events)
self.lane.load("parkour.sv")
# self.lane.add_support_point(100,100)
# self.lane.add_support_point(200,100)
# self.lane.add_support_point(200,200)
# self.lane.add_support_point(100,200)
self.optimize = Optimize(self.lane)
self.cars = []
self.cars.append(Car(x=100, y=100, theta=-45,
speed=0)) # representation for actual car
self.cars.append(Car(x=100, y=100, theta=-45 + 15,
speed=0)) # representation for ins estimate
self.cars.append(
Car(x=100, y=100, theta=-45, speed=0)
) # representation for ins estimate xy optimization single pass
self.cars.append(
Car(x=100, y=100, theta=-45, speed=0)
) # representation for ins estimate xy optimization multi pass
self.cars.append(
Car(x=100, y=100, theta=-45,
speed=0)) # representation for ins estimate theta optimization
for car in self.cars:
car.color = Draw.WHITE
self.cars[2].color = Draw.YELLOW
self.cars[3].color = Draw.RED
self.cars[4].color = Draw.GREEN
self.action = None
self.dragdrop = DragAndDropController(self.events)
self.controller = self.dragdrop
# self.cars[0].camview = CamView(self.cars[0],self.background.arr)
# self.cars[0].camview.register_events(self.events)
self.cars[0].name = "actual"
self.cars[1].name = "estimate"
self.cars[2].name = "opt"
self.cars[3].name = "opt*"
self.cars[4].name = "theta"
self.cars[0].controller = self.controller
self.cars[1].controller = self.controller
self.cars[0].camview = CamView(self.cars[0], self.background.arr)
self.cars[2].controller = OptimizeNearestEdgeXYSinglePass(
optimize=self.optimize,
labels=self.labels,
label_positions=self.label_positions,
camview=self.cars[0].camview,
estimate_car=self.cars[1])
self.cars[3].controller = OptimizeNearestEdgeXYMultiPass(
optimize=self.optimize,
labels=self.labels,
label_positions=self.label_positions,
camview=self.cars[0].camview,
estimate_car=self.cars[1])
self.cars[4].controller = OptimizeThetaParable(
optimize=self.optimize,
distances=self.distances,
camview=self.cars[0].camview,
estimate_car=self.cars[3])
# self.cars[4].controller = OptimizeNearestEdgeTheta(
# optimize = self.optimize,
# labels = self.labels,
# label_positions = self.label_positions,
# hilbert = self.hilbert,
# camview = self.cars[0].camview,
# estimate_car = self.cars[1])
# self.window = Window(self.screen, self.events, 300, 200, "caption")
self.done = False
self.cursor = (0, 0)
self.register_events()
self.spin()
def setup_pygame(self):
pygame.init()
# Set the width and height of the screen [width, height]
self.size = (self.background.rect.width, self.background.rect.height)
self.screen = pygame.display.set_mode(self.size)
self.font = pygame.font.SysFont("arial", 10)
Draw.font = self.font
pygame.display.set_caption("My Game")
def draw_string(self, string, x, y, color=Draw.BLACK):
Draw.draw_string(self.screen, self.font, string, x, y, color)
def draw(self):
self.background.draw(self.screen)
# Draw.draw_nparr(self.screen, self.background.arr_dist_rgb)
camview = self.cars[0].camview
actual_view = camview.view
edge_points_in_car_xy = None
if actual_view is not None:
# bw
bw = actual_view[:, :, 0]
skip = 5
edge_points = Utils.zero_points(bw)[::skip, :]
edge_points_in_car_xy = camview.transform_camview_to_car_xy(
edge_points, flip_x=True, flip_y=False, flip_xy=False)
transformed = camview.transform_car_xy_to_global(
edge_points_in_car_xy,
angle=self.cars[1].theta + camview.angle_offset,
global_x=self.cars[1].x,
global_y=self.cars[1].y)
xx, yy = transformed[:, 0], transformed[:, 1]
## show edge on distance transformation of bg
# get arr_dist copy scaled to 0..1
# tmp = (self.background.arr_dist/self.background.arr_dist.max()).copy()
tmp = 1 - (self.background.arr_bw /
self.background.arr_bw.max()).copy()
# select points that in bound of the drawing area
in_bounds = np.logical_and(
np.logical_and(xx >= 0, yy >= 0),
np.logical_and(xx < tmp.shape[0], yy < tmp.shape[1]))
# set points that are in bounds to maximum value, i.e. white
tmp[xx[in_bounds], yy[in_bounds]] = tmp.max()
# get nearest points on bg edge of all transformed points in bounds
nearest_edge = np.array([
self.label_positions[label]
for label in self.labels[xx[in_bounds], yy[in_bounds]]
])
# set nearest points to white
tmp[nearest_edge[:, 0], nearest_edge[:, 1]] = tmp.max()
# show cv2 image in extra window
# cv2.imshow("tmp",tmp)
# blit what we draw so far
Draw.draw_nparr(self.screen, 255 * tmp)
# draw lines from transformed points to corresponding nearest bg edge point
for i in range(nearest_edge.shape[0]):
pygame.draw.aaline(self.screen, Draw.GRAY,
(xx[in_bounds][i], yy[in_bounds][i]),
(nearest_edge[i, 0], nearest_edge[i, 1]))
# self.cars[2].x = self.cars[1].x + (nearest_edge[:,0] - xx[in_bounds]).mean()
# self.cars[2].y = self.cars[1].y + (nearest_edge[:,1] - yy[in_bounds]).mean()
# self.cars[2].theta = self.cars[1].theta
self.lane.draw(self.screen)
# draw cursor position
pygame.draw.circle(self.screen, Draw.WHITE,
(self.cursor[0], self.cursor[1]), 2, 1)
label = self.labels[self.cursor[0], self.cursor[1]]
x, y = self.label_positions[label]
pygame.draw.circle(self.screen, Draw.WHITE,
(int(x + 0.5), int(y + 0.5)), 2, 1)
# print self.label_positions[label]
# Draw car
for car in self.cars:
if self.controller is not None:
if hasattr(self.controller, "action"):
car.draw(self.screen, self.controller.action)
else:
car.draw(self.screen)
self.controller.draw(self.screen, car)
else:
car.draw(self.screen)
if hasattr(car, "camview"):
car.camview.draw(self.screen)
# # plot error_over_theta
# if edge_points_in_car_xy is not None:
# self.plt.begin()
# thetas = np.linspace(-20,20,20)
# error_over_theta = [self.optimize.distance_mean_in_car_xy(
# xytheta = (0,0,theta),
# edge_points_in_car_xy = edge_points_in_car_xy,
# x0 = self.cars[3].x,
# y0 = self.cars[3].y,
# theta0 = self.cars[3].theta,
# camview = self.cars[0].camview,
# distances = self.distances,
# k = 2
# ) for theta in thetas]
# self.plt.fig.plot(thetas, error_over_theta)
# self.plt.end()
def on_keyup(self, event):
if self.lane.selected is not None \
and event.key == pygame.K_DELETE:
self.lane.remove_support_point(self.lane.selected)
self.lane.selected = None
elif event.key == pygame.K_RETURN:
self.controller = self.human if self.controller != self.human else self.onestep
def on_mousemotion(self, event):
self.cursor = event.pos
def input(self):
# get mouse info
cursor = pygame.mouse.get_pos()
(left_button, middle_button, right_button) = pygame.mouse.get_pressed()
keys = pygame.key.get_pressed()
if keys[pygame.K_UP]:
self.cars[1].theta += 5
if keys[pygame.K_DOWN]:
self.cars[1].theta -= 5
def register_events(self):
self.events.register_callback("quit", self.on_quit)
self.events.register_callback("laneupdate", self.on_laneupdate)
self.events.register_callback("keyup", self.on_keyup)
self.events.register_callback("mousemotion", self.on_mousemotion)
def on_quit(self, args):
self.done = True
def on_laneupdate(self, lane):
pass
def spin(self):
# Loop until the user clicks the close button.
self.done = False
# Used to manage how fast the screen updates
clock = pygame.time.Clock()
self.last_time = time()
# -------- Main Program Loop -----------
while not self.done:
dt = time() - self.last_time
self.last_time = time()
# --- Main event loop
for event in pygame.event.get(): # User did something
if event.type in self.events.pygame_mappings:
self.events.fire_callbacks(
self.events.pygame_mappings[event.type], event)
# if event.type == pygame.QUIT: # If user clicked close
# done = True # Flag that we are done so we exit this loop
self.input()
# --- Game logic should go here
# apply controllers
for car in self.cars:
if not car.pause:
# eventually set default controller
if car.controller is None and self.controller is not None:
car.controller = self.controller
# apply controller
if car.controller is not None:
car.controller.update(car, dt)
# --- Drawing code should go here
# First, clear the screen to white. Don't put other drawing commands
# above this, or they will be erased with this command.
self.screen.fill(Draw.WHITE)
self.draw()
# --- Go ahead and update the screen with what we've drawn.
pygame.display.flip()
# --- Limit to 60 frames per second
clock.tick(60)
# Close the window and quit.
# If you forget this line, the program will 'hang'
# on exit if running from IDLE.
pygame.quit()
def __init__(self):
super(App, self).__init__()
# load background
self.background = Background(filename="background.png")
# get array copy of background image
self.background.arr = pygame.surfarray.array3d(self.background.img)
# create bw from image
_, self.background.arr_bw = cv2.threshold(self.background.arr[:, :, 0],
128, 1, cv2.THRESH_BINARY)
# print self.background.arr_bw.shape, self.background.arr_bw.dtype
# self.background.arr_dist = cv2.distanceTransform(self.background.arr_bw, cv.CV_DIST_L1, 3)
# get nearest (zero) pixel labels with corresponding distances
self.background.arr_dist, self.labels = cv2.distanceTransformWithLabels(
self.background.arr_bw,
cv.CV_DIST_L1,
3,
labelType=cv2.DIST_LABEL_PIXEL)
self.distances = self.background.arr_dist
### get x,y coordinates for each label
# get positions of zero points
zero_points = Utils.zero_points(self.background.arr_bw)
# get labels for zero points
zero_labels = self.labels[zero_points[:, 0], zero_points[:, 1]]
# create dictionary mapping labels to zero point positions
self.label_positions = dict(
zip(zero_labels, zip(zero_points[:, 0], zero_points[:, 1])))
# create hilbert curve lookup table
self.hilbert = Hilbert.hilbert_lookup(*self.background.arr.shape[:2])
# provide a rgb variant of dist for display
self.background.arr_dist_rgb = self.background.arr.copy()
self.background.arr_dist_rgb[:, :, 0] = self.background.arr_dist
self.background.arr_dist_rgb[:, :, 1] = self.background.arr_dist
self.background.arr_dist_rgb[:, :, 2] = self.background.arr_dist
# print a.shape
self.setup_pygame()
self.events = Events()
self.plt = Plot()
self.lane = Lane(self.events)
self.lane.load("parkour.sv")
# self.lane.add_support_point(100,100)
# self.lane.add_support_point(200,100)
# self.lane.add_support_point(200,200)
# self.lane.add_support_point(100,200)
self.optimize = Optimize(self.lane)
self.cars = []
self.cars.append(Car(x=100, y=100, theta=-45,
speed=0)) # representation for actual car
self.cars.append(Car(x=100, y=100, theta=-45 + 15,
speed=0)) # representation for ins estimate
self.cars.append(
Car(x=100, y=100, theta=-45, speed=0)
) # representation for ins estimate xy optimization single pass
self.cars.append(
Car(x=100, y=100, theta=-45, speed=0)
) # representation for ins estimate xy optimization multi pass
self.cars.append(
Car(x=100, y=100, theta=-45,
speed=0)) # representation for ins estimate theta optimization
for car in self.cars:
car.color = Draw.WHITE
self.cars[2].color = Draw.YELLOW
self.cars[3].color = Draw.RED
self.cars[4].color = Draw.GREEN
self.action = None
self.dragdrop = DragAndDropController(self.events)
self.controller = self.dragdrop
# self.cars[0].camview = CamView(self.cars[0],self.background.arr)
# self.cars[0].camview.register_events(self.events)
self.cars[0].name = "actual"
self.cars[1].name = "estimate"
self.cars[2].name = "opt"
self.cars[3].name = "opt*"
self.cars[4].name = "theta"
self.cars[0].controller = self.controller
self.cars[1].controller = self.controller
self.cars[0].camview = CamView(self.cars[0], self.background.arr)
self.cars[2].controller = OptimizeNearestEdgeXYSinglePass(
optimize=self.optimize,
labels=self.labels,
label_positions=self.label_positions,
camview=self.cars[0].camview,
estimate_car=self.cars[1])
self.cars[3].controller = OptimizeNearestEdgeXYMultiPass(
optimize=self.optimize,
labels=self.labels,
label_positions=self.label_positions,
camview=self.cars[0].camview,
estimate_car=self.cars[1])
self.cars[4].controller = OptimizeThetaParable(
optimize=self.optimize,
distances=self.distances,
camview=self.cars[0].camview,
estimate_car=self.cars[3])
# self.cars[4].controller = OptimizeNearestEdgeTheta(
# optimize = self.optimize,
# labels = self.labels,
# label_positions = self.label_positions,
# hilbert = self.hilbert,
# camview = self.cars[0].camview,
# estimate_car = self.cars[1])
# self.window = Window(self.screen, self.events, 300, 200, "caption")
self.done = False
self.cursor = (0, 0)
self.register_events()
self.spin()
class App(object):
"""docstring for App"""
def __init__(self):
super(App, self).__init__()
self.setup_pygame()
self.events = Events()
self.lane = Lane(self.events)
self.lane.add_support_point(100,100)
self.lane.add_support_point(200,100)
self.lane.add_support_point(200,200)
self.lane.add_support_point(100,200)
self.cars = []
for k in range(4):
self.cars.append(Car(x=150+k*5,y=100,theta=np.random.randint(0,360),speed=np.random.randint(45,180)))
# self.cars.append(Car(x=250,y=100,theta=-45,speed=2*90))
self.action = None
self.human = HumanController()
self.heuristic = Heuristic(self.lane)
Node.heuristic = self.heuristic
self.onestep = OneStepLookaheadController(self.cars,self.lane,self.heuristic)
self.nstep = NStepLookaheadController(self.cars,self.lane, self.heuristic, 2)
self.bestfirst = BestFirstController(self.cars,self.lane, self.heuristic)
self.controller = self.bestfirst
# self.window = Window(self.screen, self.events, 300, 200, "caption")
self.grid = Grid(50,50,*self.size)
self.last_distance_grid_update = time() - 10
self.update_distance_grid()
self.done = False
self.register_events()
self.spin()
def setup_pygame(self):
pygame.init()
# Set the width and height of the screen [width, height]
self.size = (700, 500)
self.screen = pygame.display.set_mode(self.size)
self.font = pygame.font.SysFont("arial",10)
pygame.display.set_caption("My Game")
def draw_string(self,string,x,y,color=Draw.BLACK):
Draw.draw_string(self.screen,self.font,string,x,y,color)
def draw(self):
self.grid.draw(self.screen)
self.lane.draw(self.screen)
# Draw car
for car in self.cars:
if self.controller is not None:
if hasattr(self.controller,"action"):
car.draw(self.screen, self.controller.action)
self.controller.draw(self.screen, car)
else:
car.draw(self.screen)
def input(self):
# get mouse info
cursor = pygame.mouse.get_pos()
(left_button, middle_button, right_button) = pygame.mouse.get_pressed()
keys = pygame.key.get_pressed()
if self.lane.selected is not None:
if keys[pygame.K_DELETE]:
self.lane.remove_support_point(self.lane.selected)
self.lane.selected = None
self.update_distance_grid()
if keys[pygame.K_SPACE]:
for car in self.cars:
# save original speed
if not hasattr(car,"speed_on"):
car.speed_on = car.speed
# toggle speed
car.speed = car.speed_on - car.speed
if keys[pygame.K_RETURN]:
self.controller = self.human if self.controller != self.human else self.onestep
def update_distance_grid(self):
# return
if time() - self.last_distance_grid_update > 1 / 5:
self.last_distance_grid_update = time()
for i in range(self.grid.width):
for j in range(self.grid.height):
x,y = self.grid.xs[i], self.grid.ys[j]
closest_idx = self.lane.closest_sampled_idx(x, y)
distance = Utils.distance_between(
(self.lane.sampled_x[closest_idx],self.lane.sampled_y[closest_idx]),
(x,y))
# diff = np.array([self.lane.sampled_x[closest_idx]-x,self.lane.sampled_y[closest_idx]-y])
# distance = math.sqrt(np.sum(np.square(diff)))
self.grid.data[i,j] = distance*distance
def register_events(self):
self.events.register_callback("quit", self.on_quit)
self.events.register_callback("laneupdate", self.on_laneupdate)
def on_quit(self, args):
self.done = True
def on_laneupdate(self, lane):
if lane == self.lane:
if self.lane.selected is None:
self.update_distance_grid()
# pass
def spin(self):
# Loop until the user clicks the close button.
self.done = False
# Used to manage how fast the screen updates
clock = pygame.time.Clock()
self.last_time = time()
# -------- Main Program Loop -----------
while not self.done:
dt = time()-self.last_time
self.last_time = time()
# --- Main event loop
for event in pygame.event.get(): # User did something
if event.type in self.events.pygame_mappings:
self.events.fire_callbacks(self.events.pygame_mappings[event.type], event)
# if event.type == pygame.QUIT: # If user clicked close
# done = True # Flag that we are done so we exit this loop
self.input()
# --- Game logic should go here
if self.controller is not None:
for car in self.cars:
action = self.controller.compute_action(car)
self.controller.action = action
car.forward(action,dt)
# --- Drawing code should go here
# First, clear the screen to white. Don't put other drawing commands
# above this, or they will be erased with this command.
self.screen.fill(Draw.WHITE)
self.draw()
# --- Go ahead and update the screen with what we've drawn.
pygame.display.flip()
# --- Limit to 60 frames per second
clock.tick(60)
# Close the window and quit.
# If you forget this line, the program will 'hang'
# on exit if running from IDLE.
pygame.quit()
import numpy as np
import Arr_Time as arrtime
import statistics
import math
# import matplotlib.pyplot as plt
finaltt = []
finalvel = []
finalvol = []
while globalV.simnumber <= globalV.totalsim:
globalV.all_vehs = []
#Simulate one-direction two-lane traffic from 10th to 14th on Peachtree
# Initialize Road Network
# 13rd to 14th
ln4 = Lane(0.115, None, 3)
# 12nd to 13rd
ln3 = Lane(0.125, ln4, 2)
# 11st to 12nd
ln2 = Lane(0.14, ln3, 1)
# 10th to 11st
ln1 = Lane(0.165, ln2, 0)
ln1.lastLn = None
ln2.lastLn = ln1
ln3.lastLn = ln2
ln4.lastLn = ln3
all_ln = [ln1, ln2, ln3, ln4]
# Generate B-type event list (Vehicle entry at each street) done in Arr_Time.py
import random
#get csv functionality to store the data
#store 1 row per generation with columns of avg wait, avg throughput, best wait, best throughput
import csv
#import visualization functionality
import showsim
#import led_test
import math
#keep track of time
ts = time.time()
#make the toward lanes and the away lanes
toward = []
for i in range(7):
toward.append(Lane(10,1))
away = []
for i in range(4):
away.append(Lane(10,-1))
#make the intersection
I = Intersection(toward, away)
#store the networks' througputs and wait times
#gets cleared every epoch
throughs = []
waits = []
#stores average throughs and waits for all the generations
avg_throughs = []
def main():
d = CarDespawner()
s1 = CarSpawner(0.5, [d])
s2 = CarSpawner(0.5, [d])
r = Roundabout(1)
lane1 = Lane(5, s1, None, r, Direction.SOUTH)
lane2 = Lane(5, s2, None, r, Direction.WEST)
lane3 = Lane(5, r, Direction.NORTH, d, None)
for i in range(20):
d.update()
r.update()
lane1.update()
lane2.update()
lane3.update()
s1.update()
s2.update()
print(lane1)
print(lane2)
print()
print(r)
print()
print(lane3)
print("-" * 100)
def main():
d = CarDespawner()
s1 = CarSpawner(0.5, [d])
s2 = CarSpawner(0.5, [d])
inter = Intersection()
lane1 = Lane(5, s1, None, inter, Direction.SOUTH)
lane2 = Lane(5, s2, None, inter, Direction.WEST)
lane3 = Lane(5, inter, Direction.NORTH, d, None)
for i in range(100):
d.update()
lane1.update()
lane2.update()
lane3.update()
inter.update()
s1.update()
s2.update()
print(lane3, end=" ||| ")
print(inter, end=" ||| ")
print(lane1)
print(" " * 51, end="")
print(lane2)
print("-" * 100)
