import sumolib from math import dist, ceil, sqrt, log from random import randint 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.v=0 self.a=10 self.b=20 self.vmax=0 self.gamma = 5 self.delta = 0 self.T = 0.3 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.leaderDist = l+carComing[0] 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 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()): 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, flemme 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) 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() #painter.drawEllipse(pt,100,100) 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 vbar=(self.v+vleader)/2 bbar=(bleader+self.b)/2 # S = vleader * 3.6 * 0.6 Si=self.leaderDist-vleader*1.3 #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 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.conduite(self.vmax,self.leader,dt) lgt=self.v*dt 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