import sumolib from math import dist, ceil, sqrt 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] nextEdge = self.route[edgeInd+1] inverted = not (startEdge.getToNode()==nextEdge.getFromNode() or startEdge.getToNode()==nextEdge.getToNode()) laneId = 0 if not inverted else 1 lane = startEdge.getLane(laneId) vmax = lane.getSpeed() laneShape = lane.getShape() if(inverted): laneShape = laneShape.reverse() 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 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 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 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 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) 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.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 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