import sumolib from math import dist, ceil, sqrt, log, cos, sin, atan2 from random import randint, uniform from PySide6.QtGui import QPainter from PySide6.QtCore import QPointF, Signal, QObject class updateSignals(QObject): updateDisp = Signal(tuple) addGraphPt = Signal(tuple) class Car(): 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.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 = 1.3#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)) for r,rn in zip(route,route[1:]): self.route.append(r) conn=r.getConnections(rn) if(len(conn)==0): continue edge=self.map.getLane(conn[0].getViaLaneID()).getEdge() self.route.append(edge) 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) 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() connection = self.route[prevInd].getConnections(self.route[edgeInd])[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 = list(self.controller.getCarsOnLane(edge.getID(), laneInd)) # doit y avoir moyen de le faire en gardant les filters 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 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 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 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 tti, sai = self.calcTti(self.distToInter, self.v, vmax, self.a) 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 # 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 or (tti + leader.T + marg) < ltti: self.v = min(vmax, self.v + self.a*dt) else:# sinon on s'arrete net self.v = 0 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,self.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 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.pos = self.pos.copy() copy.laneShape = self.laneShape copy.laneId = self.laneId copy.vmax = self.vmax return copy