523 lines
20 KiB
Python
523 lines
20 KiB
Python
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
|
|
from itertools import islice
|
|
|
|
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=20
|
|
self.leaderBefore=False
|
|
self.distToInter=0
|
|
self.timeStopped=0
|
|
self.speedPercentage=0
|
|
self.ticksLived=0
|
|
self.timeAtInter = 0
|
|
self.forceThrough = False
|
|
|
|
self.vmax=0
|
|
|
|
self.alpha = 0.1
|
|
self.beta = 0.1
|
|
self.nu = 0
|
|
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))
|
|
self.leaderBefore = False
|
|
while(l<maxDist):
|
|
endPos = laneShape[shapeInd+1]
|
|
|
|
carsReallyHere = filter(lambda c: c.laneInd == shapeInd, carsHere)
|
|
|
|
if l == 0:
|
|
carsReallyHere = filter(lambda c: dist(c.pos,endPos) < dist(self.pos,endPos),carsReallyHere)
|
|
closest = None
|
|
|
|
carsReallyHere = list(carsReallyHere)
|
|
|
|
try:
|
|
closest = min(carsReallyHere, 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
|
|
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
|
|
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):
|
|
print(self.id,"fu")
|
|
return None
|
|
while self.route[prevInd].isSpecial():
|
|
prevInd -= 1
|
|
if prevInd < 0:
|
|
print(self.id, "fu2")
|
|
return None
|
|
inter = self.route[edgeInd-1].getFromNode()
|
|
connections = self.route[prevInd].getConnections(self.route[edgeInd])
|
|
if(len(connections)==0):
|
|
print("aaaaaaaaa")
|
|
return None
|
|
connection = connections[0]
|
|
linkInd = inter.getLinkIndex(connection)
|
|
if(linkInd == -1): # Ca devrait pas arriver, mais de toute évidence ça arrive
|
|
print(self.id, "fu3")
|
|
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: c.nextNonSpecialEdge() == conn[i].getTo(), carsInEdge))
|
|
#carsInEdge = list(carsInEdge)
|
|
#carsInEdge = list(filter(lambda c: c.nextNonSpecialEdge() == self.nextNonSpecialEdge(), 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 nextNonSpecialEdge(self):
|
|
return next(filter(lambda e: not e.isSpecial(), islice(self.route, self.index + 1, None)))
|
|
|
|
def draw(self,painter):
|
|
pt = QPointF(*self.pos)
|
|
if self.forceThrough:
|
|
painter.setPen(Qt.blue)
|
|
elif self.leader is None:
|
|
painter.setPen(Qt.gray)
|
|
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 False and ("f_0" in self.id or "f_1" in self.id or "f_2" in self.id):
|
|
self.v = 0
|
|
return
|
|
|
|
if not self.leaderBefore:
|
|
self.forceThrough = False
|
|
|
|
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):
|
|
if self.v == 0:
|
|
self.timeAtInter += dt
|
|
else:
|
|
self.timeAtInter = 0
|
|
|
|
# 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-vleader) / 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 bloqué dans une dépendance circulaire
|
|
if self.timeAtInter > 10 and self.circularLeaderDep():
|
|
self.forceThrough = True
|
|
|
|
if self.forceThrough:
|
|
self.leader = None # On supprime le leader (pour que seulement la premiere voiture detecte la dependance circulaire)
|
|
self.v = min(vmax, self.v + self.a*dt)
|
|
return
|
|
|
|
|
|
# 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
|
|
#print(tti, leader.T, marg, ltti)
|
|
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)
|
|
|
|
# fonction pour verifier si on as pas une dependence circulaire de leader
|
|
def circularLeaderDep(self):
|
|
l = self.leader
|
|
timeout = 5
|
|
ls = []
|
|
while l is not None and timeout > 0:
|
|
ls.append(l.id)
|
|
if l.id == self.id:
|
|
print(ls)
|
|
return True
|
|
timeout -= 1
|
|
l = l.leader
|
|
return False
|
|
|
|
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)
|
|
|
|
if self.v == 0:
|
|
self.timeStopped += dt
|
|
|
|
self.speedPercentage += self.v/self.vmax
|
|
self.ticksLived += 1
|
|
|
|
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 (inter : {self.leaderBefore})"))
|
|
|
|
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
|