250 lines
7.9 KiB
Python
250 lines
7.9 KiB
Python
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)
|
|
while(l<maxDist):
|
|
endPos = laneShape[shapeInd+1]
|
|
#carsHere = filter(lambda c: c.laneInd == shapeInd, carsHere)
|
|
|
|
if l == 0:
|
|
carsHere = filter(lambda c: dist(c.pos,endPos) < dist(self.pos,endPos),carsHere)
|
|
closest = None
|
|
|
|
try:
|
|
closest = min(carsHere, key=lambda c: dist(c.pos,laneShape[shapeInd]))
|
|
except ValueError as e:
|
|
if l == 0:
|
|
l+=dist(self.pos,endPos)
|
|
else:
|
|
l+=dist(laneShape[shapeInd],endPos)
|
|
shapeInd+=1
|
|
if(shapeInd>=len(laneShape)-1):
|
|
shapeInd = 0
|
|
edgeInd+=1
|
|
carsHere = self.controller.getCarsOnLane(self.route[edgeInd].getID(), laneId)
|
|
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*0.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
|