128 lines
4.1 KiB
Python
128 lines
4.1 KiB
Python
import sumolib
|
|
import math
|
|
from PySide6.QtGui import QPainter
|
|
from PySide6.QtCore import QPointF
|
|
|
|
class Car:
|
|
def initPath(self):
|
|
startEdge=self.route[self.index]
|
|
nextEdge=self.route[self.index+1]
|
|
|
|
inverted=not (startEdge.getToNode()==nextEdge.getFromNode() or startEdge.getToNode()==nextEdge.getToNode())
|
|
|
|
laneId=0 if not inverted else 1
|
|
lane=startEdge.getLane(laneId)
|
|
self.vmax=lane.getSpeed()
|
|
self.laneShape=lane.getShape()
|
|
if(inverted):
|
|
self.laneShape.reverse()
|
|
|
|
self.infoWidg.setVal("Index",f"{self.index}/{len(self.route)}")
|
|
self.infoWidg.setVal("Edge",self.route[self.index])
|
|
|
|
#print(f"{self.id} : {startEdge.getID()} -> {nextEdge.getID()} via {laneId}")
|
|
|
|
def __init__(self,carID,route,parentMap,parentController,infoWidg):
|
|
self.id=carID
|
|
self.map=parentMap
|
|
self.controller=parentController
|
|
self.infoWidg=infoWidg
|
|
self.index=0
|
|
self.laneInd=0
|
|
self.route=[]
|
|
self.leader=None
|
|
|
|
self.pos=[0,0]
|
|
self.v=0
|
|
self.a=10
|
|
self.b=20
|
|
|
|
self.infoWidg.setVal("Position",self.pos)
|
|
self.infoWidg.setVal("Vitesse",self.v)
|
|
self.infoWidg.setVal("Index",f"{self.index}/{len(route)}")
|
|
|
|
self.rawRoute = 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 draw(self,painter):
|
|
pt = QPointF(*self.pos)
|
|
painter.drawEllipse(pt,3,3)
|
|
|
|
def conduite(self,vmax,leader,dt):
|
|
if(leader is None):
|
|
vleader=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=5
|
|
T=0.3
|
|
vsec=vleader+(S-vmax*T)/(vbar/bbar+T)
|
|
vd=min(self.v+self.a*dt,vmax,vsec)
|
|
self.v=max(0,vd)
|
|
|
|
def update(self,dt):
|
|
leaders=self.controller.getCarsOnEdge(self.route[self.index].getID())
|
|
leader=None
|
|
minDist=self.route[self.index].getLength()
|
|
for l in leaders:
|
|
db=math.dist(self.pos,l.pos)
|
|
de=math.dist(l.pos,self.laneShape[-1])
|
|
df=math.dist(self.pos,self.laneShape[-1])
|
|
|
|
if(de<df and db<minDist):
|
|
minDist=db
|
|
leader=l
|
|
self.leader=leader
|
|
|
|
self.conduite(self.vmax,leader,dt)
|
|
|
|
lgt=self.v*dt
|
|
|
|
while(lgt>0):
|
|
endPos=self.laneShape[self.laneInd+1]
|
|
l=math.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
|
|
|
|
self.infoWidg.setVal("Position", self.pos)
|
|
self.infoWidg.setVal("Vitesse", self.v)
|
|
self.infoWidg.setVal("Leader", leader if leader is None else leader.id)
|