def __init__(self, data, id):
self.data = data
self.block = Block(data)
self.id = id
self.sides = {
'north': IntersectionSide(),
'south': IntersectionSide(),
'east': IntersectionSide(),
'west': IntersectionSide()
}
self.centerNode = None
def __init__(self, layout):
self._initBlocks(layout)
self._initIntersections(layout)
self.layout = layout
startX = layout.getStartX()
startY = layout.getStartY()
startDirName = layout.getJuniorDir()
self.junior = AutoDriver()
self.junior.setup(
Vec2d(startX, startY),
startDirName,
Vec2d(0, 0)
)
self.cars = [self.junior]
self.otherCars = []
self.finish = Block(layout.getFinish())
agentComm = AgentCommunication()
agentGraph = layout.getAgentGraph()
for _ in range(Const.NUM_AGENTS):
startNode = self._getStartNode(agentGraph)
other = Agent(startNode, layout.getAgentGraph(), self, agentComm)
self.cars.append(other)
self.otherCars.append(other)
self.observations = []
agentComm.addAgents(self.otherCars)
self.modelLock = threading.Lock()
self.probCarSet = False
def __init__(self, data, id):
self.data = data
self.block = Block(data)
self.id = id
self.sides = {
'north': IntersectionSide(),
'south': IntersectionSide(),
'east': IntersectionSide(),
'west': IntersectionSide()
}
self.centerNode = None
def __init__(self, layout, QLearner):
self._initBlocks(layout)
self._initIntersections(layout)
self.layout = layout
startX = layout.getStartX()
startY = layout.getStartY()
startDirName = layout.getJuniorDir()
self.junior = AutoDriver(QLearner)
self.junior.setup(Vec2d(startX, startY), startDirName, Vec2d(0, 0))
self.cars = [self.junior]
self.otherCars = []
self.finish = Block(layout.getFinish())
agentComm = AgentCommunication()
if self.layout.getWorldName() != 'highway':
agentGraph = layout.getAgentGraph()
for _ in range(Const.NUM_AGENTS):
startNode = self._getStartNode(agentGraph)
other = Agent(startNode, layout.getAgentGraph(), self,
agentComm)
self.cars.append(other)
self.otherCars.append(other)
else:
agentGraphCarB = layout.getAgentGraphCarB()
startNode = self._getStartNode(agentGraphCarB)
other = Agent(startNode, layout.getAgentGraphCarB(), self,
agentComm)
self.cars.append(other)
self.otherCars.append(other)
agentGraphTruck = layout.getAgentGraphTruck()
startNode = self._getStartNode(agentGraphTruck)
other = Agent(startNode, layout.getAgentGraphTruck(), self,
agentComm)
self.cars.append(other)
self.otherCars.append(other)
self.observations = []
agentComm.addAgents(self.otherCars)
self.modelLock = threading.Lock()
self.probCarSet = False
class Model(object):
def __init__(self, layout):
self._initBlocks(layout)
self._initIntersections(layout)
self.layout = layout
startX = layout.getStartX()
startY = layout.getStartY()
startDirName = layout.getJuniorDir()
self.junior = AutoDriver()
self.junior.setup(
Vec2d(startX, startY),
startDirName,
Vec2d(0, 0)
)
self.cars = [self.junior]
self.otherCars = []
self.finish = Block(layout.getFinish())
agentComm = AgentCommunication()
agentGraph = layout.getAgentGraph()
for _ in range(Const.NUM_AGENTS):
startNode = self._getStartNode(agentGraph)
other = Agent(startNode, layout.getAgentGraph(), self, agentComm)
self.cars.append(other)
self.otherCars.append(other)
self.observations = []
agentComm.addAgents(self.otherCars)
self.modelLock = threading.Lock()
self.probCarSet = False
def _initBlocks(self, layout):
self.blocks = []
for blockData in layout.getBlockData():
block = Block(blockData)
self.blocks.append(block)
def _initIntersections(self, layout):
self.intersections = []
for blockData in layout.getIntersectionNodes():
block = Block(blockData)
self.intersections.append(block)
def _getStartNode(self, agentGraph):
while True:
node = agentGraph.getRandomNode()
pos = node.getPos()
alreadyChosen = False
for car in self.otherCars:
if car.getPos() == pos:
alreadyChosen = True
break
if not alreadyChosen:
return node
def checkVictory(self):
bounds = self.junior.getBounds()
for point in bounds:
if self.finish.containsPoint(point.x, point.y): return True
return False
def checkCollision(self, car):
bounds = car.getBounds()
# check for collision with fixed obstacles
for point in bounds:
if not self.inBounds(point.x, point.y): return True
# check for collision with other cars
for other in self.cars:
if other == car: continue
if other.collides(car.getPos(), bounds): return True
return False
def getIntersection(self, x, y):
for intersection in self.intersections:
if intersection.containsPoint(x, y): return intersection
return None
def inIntersection(self, x, y):
return self.getIntersection(x, y) != None
def inBounds(self, x, y):
if x < 0 or x >= self.getWidth(): return False
if y < 0 or y >= self.getHeight(): return False
for block in self.blocks:
if block.containsPoint(x, y): return False
return True
def getWidth(self):
return self.layout.getWidth()
def getHeight(self):
return self.layout.getHeight()
def getBeliefRows(self):
return self.layout.getBeliefRows()
def getBeliefCols(self):
return self.layout.getBeliefCols()
def getBlocks(self):
return self.blocks
def getFinish(self):
return self.finish
def getCars(self):
return self.cars
def getOtherCars(self):
return self.otherCars
def getJunior(self):
return self.junior
def getAgentGraph(self):
return self.layout.getAgentGraph()
def getJuniorGraph(self):
return self.layout.getJuniorGraph()
def setProbCar(self, beliefs):
self.modelLock.acquire()
total = util.Belief(self.getBeliefRows(), self.getBeliefCols(), 0.0)
for r in range(self.getBeliefRows()):
for c in range(self.getBeliefCols()):
pNot = 1.0
for b in beliefs:
carP = b.getProb(r, c)
pNot *= (1.0 - carP)
p = 1.0 - pNot
total.setProb(r, c, p)
self.probCar = total
self.modelLock.release()
self.probCarSet = True
def getProbCar(self):
if not self.probCarSet: return None
self.modelLock.acquire()
probCar = copy.deepcopy(self.probCar)
self.modelLock.release()
return probCar
def _initIntersections(self, layout):
self.intersections = []
for blockData in layout.getIntersectionNodes():
block = Block(blockData)
self.intersections.append(block)
def _initBlocks(self, layout):
self.blocks = []
for blockData in layout.getBlockData():
block = Block(blockData)
self.blocks.append(block)
class Model(object):
def __init__(self, layout):
self._initBlocks(layout)
self._initIntersections(layout)
self.layout = layout
startX = layout.getStartX()
startY = layout.getStartY()
startDirName = layout.getJuniorDir()
self.junior = AutoDriver()
self.junior.setup(Vec2d(startX, startY), startDirName, Vec2d(0, 0))
self.cars = [self.junior]
self.otherCars = []
self.finish = Block(layout.getFinish())
agentComm = AgentCommunication()
agentGraph = layout.getAgentGraph()
for _ in range(Const.NUM_AGENTS):
startNode = self._getStartNode(agentGraph)
other = Agent(startNode, layout.getAgentGraph(), self, agentComm)
self.cars.append(other)
self.otherCars.append(other)
self.observations = []
agentComm.addAgents(self.otherCars)
self.modelLock = threading.Lock()
self.probCarSet = False
def _initBlocks(self, layout):
self.blocks = []
for blockData in layout.getBlockData():
block = Block(blockData)
self.blocks.append(block)
def _initIntersections(self, layout):
self.intersections = []
for blockData in layout.getIntersectionNodes():
block = Block(blockData)
self.intersections.append(block)
def _getStartNode(self, agentGraph):
while True:
node = agentGraph.getRandomNode()
pos = node.getPos()
alreadyChosen = False
for car in self.otherCars:
if car.getPos() == pos:
alreadyChosen = True
break
if not alreadyChosen:
return node
def checkVictory(self):
bounds = self.junior.getBounds()
for point in bounds:
if self.finish.containsPoint(point.x, point.y): return True
return False
def checkCollision(self, car):
bounds = car.getBounds()
# check for collision with fixed obstacles
for point in bounds:
if not self.inBounds(point.x, point.y): return True
# check for collision with other cars
for other in self.cars:
if other == car: continue
if other.collides(car.getPos(), bounds): return True
return False
def getIntersection(self, x, y):
for intersection in self.intersections:
if intersection.containsPoint(x, y): return intersection
return None
def inIntersection(self, x, y):
return self.getIntersection(x, y) != None
def inBounds(self, x, y):
if x < 0 or x >= self.getWidth(): return False
if y < 0 or y >= self.getHeight(): return False
for block in self.blocks:
if block.containsPoint(x, y): return False
return True
def getWidth(self):
return self.layout.getWidth()
def getHeight(self):
return self.layout.getHeight()
def getBeliefRows(self):
return self.layout.getBeliefRows()
def getBeliefCols(self):
return self.layout.getBeliefCols()
def getBlocks(self):
return self.blocks
def getFinish(self):
return self.finish
def getCars(self):
return self.cars
def getOtherCars(self):
return self.otherCars
def getJunior(self):
return self.junior
def getAgentGraph(self):
return self.layout.getAgentGraph()
def getJuniorGraph(self):
return self.layout.getJuniorGraph()
def setProbCar(self, beliefs):
self.modelLock.acquire()
total = util.Belief(self.getBeliefRows(), self.getBeliefCols(), 0.0)
for r in range(self.getBeliefRows()):
for c in range(self.getBeliefCols()):
pNot = 1.0
for b in beliefs:
carP = b.getProb(r, c)
pNot *= (1.0 - carP)
p = 1.0 - pNot
total.setProb(r, c, p)
self.probCar = total
self.modelLock.release()
self.probCarSet = True
def getProbCar(self):
if not self.probCarSet: return None
self.modelLock.acquire()
probCar = copy.deepcopy(self.probCar)
self.modelLock.release()
return probCar
class Intersection(object):
OPPOSITE = {
'north': 'south',
'south': 'north',
'east': 'west',
'west': 'east'
}
LEFT_TURN = {
'north': 'east',
'south': 'west',
'east': 'south',
'west': 'north'
}
ANGLE_FROM_NORTH = {
'north': 0,
'south': 180,
'east': 90,
'west': 270
}
def __init__(self, data, id):
self.data = data
self.block = Block(data)
self.id = id
self.sides = {
'north': IntersectionSide(),
'south': IntersectionSide(),
'east': IntersectionSide(),
'west': IntersectionSide()
}
self.centerNode = None
def getAllNodes(self):
nodes = []
for sideName in self.sides:
side = self.sides[sideName]
for node in side.getNodes():
nodes.append(node)
if self.centerNode:
nodes.append(self.centerNode)
return nodes
def getAllEdgeStrings(self):
nodes = self.getAllNodes()
allEdges = []
for node in nodes:
others = node.getEdges()
for other in others:
edgeString = '[' + str(node.getId()) + ', ' + str(other.getId()) + ']'
allEdges.append(edgeString)
return allEdges
def getConnectDir(self, other):
if other.block.x1 > self.block.x1: return 'east'
if other.block.x1 < self.block.x1: return 'west'
if other.block.y1 < self.block.y1: return 'north'
if other.block.y1 > self.block.y1: return 'south'
raise Exception('same')
def getNodePos(self, side, isIn, inIsLeft):
width = self.block.getWidth()
height = self.block.getHeight()
center = self.block.getCenter()
isLeft = inIsLeft != isIn
if isLeft:
offset = Vec2d(-width * .2, -height / 2.0)
else:
offset = Vec2d(width * .2, -height / 2.0)
angle = Intersection.ANGLE_FROM_NORTH[side]
offset.rotate(angle)
return center + offset
def connect(self, other):
dir = self.getConnectDir(other)
opposite = Intersection.OPPOSITE[dir]
inPos = other.getNodePos(opposite, True, False)
outPos = self.getNodePos(dir, False, False)
xEqual = abs(inPos.x - outPos.x) < 1
yEqual = abs(inPos.y - outPos.y) < 1
assert xEqual or yEqual
outNode = self.sides[dir].addOut(outPos, dir)
inNode = other.sides[opposite].addIn(inPos, dir)
outNode.addEdge(inNode)
def connectInternal(self):
for sideNameA in self.sides:
for sideNameB in self.sides:
if sideNameA == sideNameB: continue
inNode = self.sides[sideNameA].getIn()
outNode = self.sides[sideNameB].getOut()
if inNode and outNode:
inNode.addEdge(outNode)
if self.isOneSided():
self.createUTurn()
def isOneSided(self):
count = 0
for sideNameA in self.sides:
if self.sides[sideNameA].hasNodes():
count += 1
return count == 1
def getOneSide(self):
for sideName in self.sides:
if self.sides[sideName].hasNodes():
return sideName
raise Exception('not one sided')
def createCenterNode(self):
centerPos = self.block.getCenter()
sideName = self.getOneSide()
centerDir = Intersection.LEFT_TURN[sideName]
self.centerNode = IntersectionNode(centerPos, centerDir)
def createUTurn(self):
self.createCenterNode()
sideName = self.getOneSide()
side = self.sides[sideName]
inNode = side.getIn()
outNode = side.getOut()
inNode.addEdge(self.centerNode)
self.centerNode.addEdge(outNode)
class Intersection(object):
OPPOSITE = {
'north': 'south',
'south': 'north',
'east': 'west',
'west': 'east'
}
LEFT_TURN = {
'north': 'east',
'south': 'west',
'east': 'south',
'west': 'north'
}
ANGLE_FROM_NORTH = {'north': 0, 'south': 180, 'east': 90, 'west': 270}
def __init__(self, data, id):
self.data = data
self.block = Block(data)
self.id = id
self.sides = {
'north': IntersectionSide(),
'south': IntersectionSide(),
'east': IntersectionSide(),
'west': IntersectionSide()
}
self.centerNode = None
def getAllNodes(self):
nodes = []
for sideName in self.sides:
side = self.sides[sideName]
for node in side.getNodes():
nodes.append(node)
if self.centerNode:
nodes.append(self.centerNode)
return nodes
def getAllEdgeStrings(self):
nodes = self.getAllNodes()
allEdges = []
for node in nodes:
others = node.getEdges()
for other in others:
edgeString = '[' + str(node.getId()) + ', ' + str(
other.getId()) + ']'
allEdges.append(edgeString)
return allEdges
def getConnectDir(self, other):
if other.block.x1 > self.block.x1: return 'east'
if other.block.x1 < self.block.x1: return 'west'
if other.block.y1 < self.block.y1: return 'north'
if other.block.y1 > self.block.y1: return 'south'
raise Exception('same')
def getNodePos(self, side, isIn, inIsLeft):
width = self.block.getWidth()
height = self.block.getHeight()
center = self.block.getCenter()
isLeft = inIsLeft != isIn
if isLeft:
offset = Vec2d(-width * .2, -height / 2.0)
else:
offset = Vec2d(width * .2, -height / 2.0)
angle = Intersection.ANGLE_FROM_NORTH[side]
offset.rotate(angle)
return center + offset
def connect(self, other):
dir = self.getConnectDir(other)
opposite = Intersection.OPPOSITE[dir]
inPos = other.getNodePos(opposite, True, False)
outPos = self.getNodePos(dir, False, False)
xEqual = abs(inPos.x - outPos.x) < 1
yEqual = abs(inPos.y - outPos.y) < 1
assert xEqual or yEqual
outNode = self.sides[dir].addOut(outPos, dir)
inNode = other.sides[opposite].addIn(inPos, dir)
outNode.addEdge(inNode)
def connectInternal(self):
for sideNameA in self.sides:
for sideNameB in self.sides:
if sideNameA == sideNameB: continue
inNode = self.sides[sideNameA].getIn()
outNode = self.sides[sideNameB].getOut()
if inNode and outNode:
inNode.addEdge(outNode)
if self.isOneSided():
self.createUTurn()
def isOneSided(self):
count = 0
for sideNameA in self.sides:
if self.sides[sideNameA].hasNodes():
count += 1
return count == 1
def getOneSide(self):
for sideName in self.sides:
if self.sides[sideName].hasNodes():
return sideName
raise Exception('not one sided')
def createCenterNode(self):
centerPos = self.block.getCenter()
sideName = self.getOneSide()
centerDir = Intersection.LEFT_TURN[sideName]
self.centerNode = IntersectionNode(centerPos, centerDir)
def createUTurn(self):
self.createCenterNode()
sideName = self.getOneSide()
side = self.sides[sideName]
inNode = side.getIn()
outNode = side.getOut()
inNode.addEdge(self.centerNode)
self.centerNode.addEdge(outNode)
