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, 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)