import sumolib from math import dist, ceil, sqrt, log, cos, sin, atan2, pi from random import randint, uniform from PySide6.QtGui import QPainter from PySide6.QtCore import QPointF, Signal, QObject, Qt class updateSignals(QObject): updateDisp = Signal(tuple) addGraphPt = Signal(tuple) class Car(): CLIGNO_NONE = 0 CLIGNO_LEFT = 1 CLIGNO_RIGHT = 2 def getShape(self, edgeInd): startEdge = self.route[edgeInd] laneId = 0 lane = startEdge.getLane(laneId) vmax = lane.getSpeed() laneShape = lane.getShape() return (laneShape, vmax, laneId) def initPath(self): newLane = self.getShape(self.index) self.laneShape = newLane[0] self.vmax = newLane[1] self.laneId = newLane[2] if self.infoWidg is None: return self.signals.updateDisp.emit(("Index",f"{self.index}/{len(self.route)}")) self.signals.updateDisp.emit(("Edge",self.route[self.index])) #print(f"{self.id} : {startEdge.getID()} -> {nextEdge.getID()} via {laneId}") def __init__(self,carID,route,startTime,parentMap,parentController,infoWidg): self.id=carID self.map=parentMap self.controller=parentController self.infoWidg=infoWidg self.index=0 self.laneInd=0 self.route=[] self.laneShape=None self.laneId=0 self.leader=None self.startTime=float(startTime) self.cligno=[] self.pos=[0,0] self.dir=0 self.v=0 self.a=10 self.b=20 self.minSpace=10 self.leaderBefore=False self.distToInter=0 self.vmax=0 self.alpha = 0.1 self.beta = 0.1 self.nu = 0.1 self.gamma = 10 self.delta = 0 self.T = uniform(0.9,1.6) self.size = 3 self.vroom = 0 self.rawRoute = route if infoWidg is None: return self.signals=updateSignals() self.signals.updateDisp.connect(self.infoWidg.setVal) self.signals.addGraphPt.connect(self.infoWidg.addSpeedPoint) self.signals.updateDisp.emit(("Position",self.pos)) self.signals.updateDisp.emit(("Vitesse",self.v)) self.signals.updateDisp.emit(("Index",f"{self.index}/{len(route)}")) def prepareRoute(self): route = list(map(self.map.getEdge,self.rawRoute)) if None in route: print("error planning route, mismatched net/demand?") return for r,rn in zip(route,route[1:]): self.route.append(r) conn=r.getConnections(rn) if(len(conn)==0): continue if conn[0].getDirection() == "l": self.cligno.append(self.CLIGNO_LEFT) elif conn[0].getDirection() == "r": self.cligno.append(self.CLIGNO_RIGHT) else: self.cligno.append(self.CLIGNO_NONE) edge=self.map.getLane(conn[0].getViaLaneID()).getEdge() self.route.append(edge) self.cligno.append(self.CLIGNO_NONE) secEdge=edge.getConnections(rn)[0].getViaLaneID() # Parfois je sais pas pourquoi il coupe les edges internes, mais il marque quand même la connection, ducoup pour contourner while secEdge!="": edge=self.map.getLane(secEdge).getEdge() self.route.append(edge) self.cligno.append(self.CLIGNO_NONE) secEdge=edge.getConnections(rn)[0].getViaLaneID() self.initPath() self.pos=list(self.laneShape[0]) def getLeader(self, maxDist): shapeInd = self.laneInd edgeInd = self.index prevInd = edgeInd laneShape = self.laneShape laneId = self.laneId l = 0 carsHere = self.controller.getCarsOnLane(self.route[edgeInd].getID(), laneId) carsHere = list(filter(lambda c: c.id != self.id, carsHere)) while(l=len(laneShape)-1): shapeInd = 0 edgeInd+=1 carComing = self.getLeaderAtIntersection(prevInd,edgeInd) if(carComing is not None): self.distToInter = l # y as un bug qqu part dans le calcul de la distance, ça saute quand on change d'edge if self.route[edgeInd].isSpecial(): self.distToInter += self.route[edgeInd + 1].getLength() self.leaderDist = carComing[0] self.leaderBefore = True return carComing[1] if(not self.route[edgeInd].isSpecial()): prevInd = edgeInd carsHere = self.controller.getCarsOnLane(self.route[edgeInd].getID(), laneId) carsHere = list(filter(lambda c: c.id != self.id, carsHere)) # me demande pas pourquoi mais si on le convertit pas en liste ici le filter original est modifié if(edgeInd>=len(self.route)-1): return newLane = self.getShape(edgeInd) laneShape = newLane[0] laneId = newLane[2] else: if l == 0: l+=dist(self.pos, closest.pos) else: l+=dist(laneShape[shapeInd], closest.pos) if l <= maxDist: self.leaderDist = l self.leaderBefore = False return closest else: return return def getCurrentEdge(self): return self.route[self.index] def getCarDist(self, car, edge, laneInd, startFromEnd = True): lanes = edge.getLane(laneInd).getShape().copy() if startFromEnd: lanes.reverse() cDist = 0 for i,l in enumerate(lanes[:-1]): if car.laneInd != (i if not startFromEnd else len(lanes)-i-2): cDist += dist(l, lanes[i+1]) else: cDist += dist(l, car.pos) return cDist return cDist def getLeaderAtIntersection(self, prevInd, edgeInd): while(self.route[edgeInd].isSpecial()): return edgeInd = edgeInd + 1 if edgeInd >= len(self.route): return None while self.route[prevInd].isSpecial(): prevInd -= 1 if prevInd < 0: return None inter = self.route[edgeInd-1].getFromNode() connections = self.route[prevInd].getConnections(self.route[edgeInd]) if(len(connections)==0): print("aaaaaaaaa") return connection = connections[0] linkInd = inter.getLinkIndex(connection) if(linkInd == -1): # Ca devrait pas arriver, mais de toute évidence ça arrive return; resp = inter._prohibits[linkInd] # Si je me souvient bien les variables précédées d'un _ doivent pas être touchées? connRaw = inter.getConnections() conn = [0] * len(resp) for c in connRaw: ind = inter.getLinkIndex(c) if(ind == -1): continue conn[ind] = c cars = [] for i,f in enumerate(reversed(resp)): if(f == '0'): continue edge = conn[i].getFrom() laneInd = conn[i].getFromLane().getIndex() intLane = self.map.getLane(conn[i].getViaLaneID()) intLaneLgt = intLane.getLength() carsInEdge = self.controller.getCarsOnLane(edge.getID(), laneInd) carsInEdge = list(filter(lambda c: not c.turnNext(), carsInEdge)) if len(carsInEdge) != 0: carsInEdge = zip([self.getCarDist(c, edge, laneInd)+intLaneLgt for c in carsInEdge], carsInEdge) closest = min(carsInEdge, key=lambda c: c[0]) cars.append(closest) intEdge = intLane.getEdge() carsInEdge = list(self.controller.getCarsOnLane(intEdge.getID(), intLane.getIndex())) if len(carsInEdge) != 0: carsInEdge = zip([self.getCarDist(c, intEdge, intLane.getIndex()) for c in carsInEdge], carsInEdge) closest = min(carsInEdge, key=lambda c: c[0]) cars.append(closest) if(len(cars) == 0): return None cDist,closest = min(cars, key=lambda c: c[0]) return (cDist,closest) def turnNext(self): return self.cligno[self.index] != self.CLIGNO_NONE def draw(self,painter): pt = QPointF(*self.pos) painter.drawEllipse(pt,self.size,self.size) painter.drawLine(self.pos[0], self.pos[1], self.pos[0]+5*cos(self.dir), self.pos[1]+5*sin(self.dir)) if len(self.cligno) != 0 and self.cligno[self.index] != self.CLIGNO_NONE: painter.save() painter.setPen(Qt.yellow) d = self.dir + pi/2 if self.cligno[self.index] == self.CLIGNO_RIGHT: d += pi painter.drawEllipse(pt + self.size * QPointF(cos(d),sin(d)), 3, 3) painter.restore() if self.vroom != 0: painter.save() d=(60-self.vroom)*0.2 painter.translate(pt + QPointF(0,d)) painter.scale(1,-1) font = painter.font(); font.setPixelSize(ceil(self.vroom*0.2)); painter.setFont(font) painter.drawText(QPointF(0,0),"vroom") self.vroom -= 1 painter.restore() #elSize = self.v * self.T #painter.drawEllipse(pt,elSize, elSize) def conduite(self,vmax,leader,dt): """if self.id == "f_00" and self.controller.t%10>5: self.v = 0 return """ if(leader is None): self.v = self.vmax self.updateGraph(self.v, vmax, 0) return vleader=50#self.v bleader=self.b else: vleader=leader.v # vitesse de la voiture leader bleader=leader.b if(self.leaderBefore): if(vleader == 0): return if(self.distToInter > (self.T * self.vmax) or ((self.distToInter / self.vmax) < (self.leaderDist / vleader) - 2 * self.T)): self.v=self.vmax else: self.v = 0 self.updateGraph(self.v, vmax, 0) return vbar=(self.v+vleader)/2 bbar=(bleader+self.b)/2 # S = vleader * 3.6 * 0.6 Si=self.leaderDist-vleader*self.T #S=vf**2 / self.b + vleader**2 / bleader + self.gamma * vf + self.delta T=self.T vsec=vleader+(Si-vmax*T)/(vbar/bbar+T) vd=min(self.v+self.a*dt,vmax,vsec) #vf=min(va,vb2) #va=self.v+2.5*self.a*self.T*(1-(self.v/vd))*sqrt(0.025+(self.v/vd)) #vb2=self.b*self.T+sqrt((self.b)**2*(self.T)**2-self.b*(2*(S-Si)-self.v*self.T-((vleader)**2/bbar))) self.v=max(0,vd) self.updateGraph(self.v, vmax, vsec) def conduiteGipps(self, dt): #Nope Va = self.v + 2.5 * self.a * dt * (1 - self.v/self.vmax) * sqrt(0.025 + self.v/self.vmax) #Vb = self.b * dt + sqrt(self.b**2 * dt**2 - self.b * ) def calcTti(self, dist, v0, vmax, a): ttms = (vmax - v0)/a # "time to max speed", temps pris par la voiture pour atteindre la vmax à partir de sa vitesse actuelle delta = v0**2 + 2*a*dist # delta du trinome qui donne le temps mis pour traverser la distance dist tti = (-v0 + sqrt(delta))/a # "time to intersection", temps mis pour arriver à l'intersection(dist), si il n'y a pas de vmax sai = v0 + tti*a # "speed at intersection", vitesse qu'aura la voiture quand elle arrivera if tti > ttms: # si on atteint vmax avant l'intersection alors ça foire le calcul dbvm = a/2 * ttms**2 + v0 * ttms # "distance before vmax", distance parcouru avant d'atteindre vmax tti = ttms + (dist - dbvm) / vmax # la temps necessaire est donc : temps pour atteindre vmax + temps pour traverser le reste à la vitesse vmax sai = vmax return (tti, sai) def conduiteKrauss(self, vmax, leader, dt): """if self.id == "f_00" and self.controller.t%10>5: self.v = 0 return if self.id == "v_0" and self.controller.t > 5: self.v = 0 self.updateGraph(self.leaderDist, self.distToInter, 0) return """ if leader is None: vd = min(self.v + self.a * dt, vmax) self.v = max(0, vd-self.nu) return vleader = leader.v bleader = leader.b # si on est à une intersection if(self.leaderBefore): # on calcule le temps qu'on va mettre à arriver à l'intersection # et le temps que le leader va mettre nextInternalIndex = self.index # Pour la voiture actuelle, dans l'ideal on calculerait la durée selon la vitesse sur chaque troncon while not self.route[nextInternalIndex].isSpecial(): # Mais pour l'instant on prend juste la vitesse sur le troncon interne (le plus lent en general) nextInternalIndex += 1 tti, sai = self.calcTti(self.distToInter, self.v, self.route[nextInternalIndex].getLane(0).getSpeed(), self.a) # TODO : laneID ltti, lsai = self.calcTti(self.leaderDist, vleader, leader.vmax, leader.a) lta = leader.vmax / leader.a # temps ou le leader accelere (i.e on ne gagne pas de vitesse relative) (on considere que leader.a==self.a) marg = lta + (lsai-sai) / self.a # marge à prendre pour accelerer après l'intersection sans que le leader nous rattrape tts = self.v/self.b # time to stop, temps pour s'arreter si on freine mnt dts = self.v*tts - (self.b*tts**2)/2 # distance to stop, distance parcouru en tts si on freine #print(self.distToInter, self.minSpace, dts) # si on est suffisement loin de l'intersection (i.e on s'en fout du leader) # ou si on as le temps d'arriver à l'intersection avant le leader (plus un marge pour garder un distance de sécu) # alors on accelere pour s'inserer if self.distToInter > self.minSpace + dts or (tti + leader.T + marg) < ltti: self.v = min(vmax, self.v + self.a*dt) #print(self.id, "ca passe") else:# sinon on freine self.v = max(0, self.v - self.b*dt) self.updateGraph(self.v, vmax, 0) return vb = (vleader + self.v) / 2 bb = (bleader + self.b) / 2 vsec = vleader + (self.leaderDist - vleader * self.T - self.minSpace)/((vb/bb) + self.T) vd = min(self.v + self.a * dt, vsec, vmax) self.v = max(0, vd-self.nu) self.updateGraph(self.v, vmax, vsec) def updateGraph(self, v, vmax, vsec): if self.infoWidg is None: return self.signals.addGraphPt.emit((2,self.controller.t,v)) self.signals.addGraphPt.emit((0,self.controller.t,vmax)) self.signals.addGraphPt.emit((1,self.controller.t,vsec)) def update(self,dt): if self.controller.t < self.startTime: return if self.v < 1 and self.getCurrentEdge().isSpecial(): print(f"{self.id} stalled where he souldn't have") self.leader=self.getLeader(100) self.conduiteKrauss(self.vmax,self.leader,dt) lgt=self.v*dt self.dir = atan2(self.laneShape[self.laneInd+1][1]-self.pos[1],self.laneShape[self.laneInd+1][0]-self.pos[0]) while(lgt>0): endPos=self.laneShape[self.laneInd+1] l=dist(self.pos,endPos) if lgt>=l: lgt-=l pos=list(self.laneShape[-1]) self.laneInd+=1 if(self.laneInd>=len(self.laneShape)-1): self.laneInd=0 self.index+=1 if(self.index>=len(self.route)-1): self.index=0 self.controller.destroyCar(self) self.initPath() self.pos=list(self.laneShape[self.laneInd]) continue adv=lgt/l self.pos[0]+=(endPos[0]-self.pos[0])*adv self.pos[1]+=(endPos[1]-self.pos[1])*adv lgt=0 if self.controller.vroomEnable and randint(0,100) == 0: self.vroom = 60 if self.infoWidg is None: return self.signals.updateDisp.emit(("Position", self.pos)) self.signals.updateDisp.emit(("Vitesse", self.v)) self.signals.updateDisp.emit(("Leader", self.leader if self.leader is None else f"{self.leader.id} @ {self.leaderDist:.2f}m")) def __copy__(self): copy = Car(self.id, self.rawRoute, self.startTime, self.map, self.controller, self.infoWidg) copy.route = self.route copy.cligno = self.cligno copy.pos = self.pos.copy() copy.laneShape = self.laneShape copy.laneId = self.laneId copy.vmax = self.vmax return copy