amélioré la detection du leader
This commit is contained in:
parent
e932b3253b
commit
4a6f5bf8e0
GPG Key ID:
0DD993BFB2B307DB
81
Car.py
81
Car.py
@ -1,21 +1,29 @@
|
||||
import sumolib
|
||||
import math
|
||||
from math import dist
|
||||
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]
|
||||
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)
|
||||
self.vmax=lane.getSpeed()
|
||||
self.laneShape=lane.getShape()
|
||||
vmax = lane.getSpeed()
|
||||
laneShape = lane.getShape()
|
||||
if(inverted):
|
||||
self.laneShape.reverse()
|
||||
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]
|
||||
|
||||
self.infoWidg.setVal("Index",f"{self.index}/{len(self.route)}")
|
||||
self.infoWidg.setVal("Edge",self.route[self.index])
|
||||
@ -30,6 +38,8 @@ class Car:
|
||||
self.index=0
|
||||
self.laneInd=0
|
||||
self.route=[]
|
||||
self.laneShape=None
|
||||
self.laneId=0
|
||||
self.leader=None
|
||||
|
||||
self.pos=[0,0]
|
||||
@ -37,6 +47,8 @@ class Car:
|
||||
self.a=10
|
||||
self.b=20
|
||||
|
||||
self.vmax=0
|
||||
|
||||
self.infoWidg.setVal("Position",self.pos)
|
||||
self.infoWidg.setVal("Vitesse",self.v)
|
||||
self.infoWidg.setVal("Index",f"{self.index}/{len(route)}")
|
||||
@ -63,6 +75,40 @@ class Car:
|
||||
|
||||
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(shapeInd == self.laneInd and edgeInd == self.index):
|
||||
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:
|
||||
if(shapeInd == self.laneInd and edgeInd == self.index):
|
||||
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:
|
||||
return closest
|
||||
return
|
||||
|
||||
def draw(self,painter):
|
||||
pt = QPointF(*self.pos)
|
||||
painter.drawEllipse(pt,3,3)
|
||||
@ -87,26 +133,15 @@ class Car:
|
||||
self.infoWidg.addSpeedPoint(1,self.controller.t,vsec)
|
||||
|
||||
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])
|
||||
self.leader=self.getLeader(50)
|
||||
|
||||
if(de<df and db<minDist):
|
||||
minDist=db
|
||||
leader=l
|
||||
self.leader=leader
|
||||
|
||||
self.conduite(self.vmax,leader,dt)
|
||||
self.conduite(self.vmax,self.leader,dt)
|
||||
|
||||
lgt=self.v*dt
|
||||
|
||||
while(lgt>0):
|
||||
endPos=self.laneShape[self.laneInd+1]
|
||||
l=math.dist(self.pos,endPos)
|
||||
l=dist(self.pos,endPos)
|
||||
if lgt>=l:
|
||||
lgt-=l
|
||||
pos=list(self.laneShape[-1])
|
||||
@ -127,4 +162,4 @@ class Car:
|
||||
|
||||
self.infoWidg.setVal("Position", self.pos)
|
||||
self.infoWidg.setVal("Vitesse", self.v)
|
||||
self.infoWidg.setVal("Leader", leader if leader is None else leader.id)
|
||||
self.infoWidg.setVal("Leader", self.leader if self.leader is None else self.leader.id)
|
||||
|
||||
@ -8,7 +8,7 @@ from math import ceil
|
||||
|
||||
class carInfo(QWidget):
|
||||
def __init__(self):
|
||||
super(carInfo,self).__init__()
|
||||
super().__init__()
|
||||
self.ui = Ui_carInfo()
|
||||
self.ui.setupUi(self)
|
||||
self.maxV = 0
|
||||
@ -30,7 +30,7 @@ class carInfo(QWidget):
|
||||
self.speedSeries[ind].remove(0)
|
||||
self.speedSeries[ind].append(t,val)
|
||||
|
||||
if(self.speedSeries[ind].count()>2):
|
||||
if self.speedSeries[ind].count()>2:
|
||||
i = self.speedSeries[ind].count()-1
|
||||
pm2 = self.speedSeries[ind].at(i-2)
|
||||
pm1 = self.speedSeries[ind].at(i-1)
|
||||
@ -38,13 +38,13 @@ class carInfo(QWidget):
|
||||
diffL = (pm1.y() - pm2.y())/(pm1.x() - pm2.x())
|
||||
diffR = (pm0.y() - pm1.y())/(pm0.x() - pm1.x())
|
||||
diff2 = diffR - diffL
|
||||
if(diff2 == 0):
|
||||
if diff2 == 0:
|
||||
self.speedSeries[ind].remove(i-1)
|
||||
|
||||
xAxis = self.chart.axes(Qt.Horizontal,self.speedSeries[ind])[0]
|
||||
xAxis.setRange(max(t-500,0),t)
|
||||
|
||||
if(val>self.maxV):
|
||||
if val>self.maxV:
|
||||
self.maxV = val
|
||||
yAxis = self.chart.axes(Qt.Vertical, self.speedSeries[ind])[0]
|
||||
yAxis.setMax(ceil(val+1))
|
||||
@ -53,7 +53,7 @@ class carInfo(QWidget):
|
||||
|
||||
def setVal(self,key,val):
|
||||
obj = self.findChild(QLabel,key)
|
||||
if(obj is None):
|
||||
if obj is None:
|
||||
return
|
||||
obj.setText(f"{key} : {val}")
|
||||
obj.update()
|
||||
@ -63,11 +63,12 @@ class CarController:
|
||||
self.map=parentMap
|
||||
self.cars=[]
|
||||
self.t=0
|
||||
self.infoWidget=None
|
||||
|
||||
def fromPath(self,path):
|
||||
self.cars=[]
|
||||
self.t=0
|
||||
while(self.infoWidget.count() != 0):
|
||||
while self.infoWidget.count() != 0:
|
||||
self.infoWidget.removeItem(0)
|
||||
|
||||
for vehicle in sumolib.xml.parse(path,"vehicle"):
|
||||
@ -79,16 +80,15 @@ class CarController:
|
||||
for car in self.cars:
|
||||
car.prepareRoute()
|
||||
|
||||
def getCarsOnEdge(self,edgeID): # on devrai probablement utiliser une map ici (dict en python?) mais flemme
|
||||
res=[]
|
||||
for car in self.cars:
|
||||
cid=car.route[car.index].getID()
|
||||
if(cid==edgeID):
|
||||
res.append(car)
|
||||
return res
|
||||
def getCarsOnEdge(self,edgeID):
|
||||
return filter(lambda c: c.route[c.index].getID()==edgeID,self.cars)
|
||||
|
||||
def getCarsOnLane(self,edgeID,laneID):
|
||||
cars = self.getCarsOnEdge(edgeID)
|
||||
return cars #filter(lambda c: c.laneId == laneID,cars)
|
||||
|
||||
def update(self):
|
||||
if(self.map.net is None):
|
||||
if self.map.net is None:
|
||||
return
|
||||
self.t+=1
|
||||
for car in self.cars:
|
||||
@ -98,7 +98,7 @@ class CarController:
|
||||
for ind,car in enumerate(self.cars):
|
||||
selectedId = self.infoWidget.currentIndex()
|
||||
selectedCar = self.cars[selectedId]
|
||||
if(ind == selectedId):
|
||||
if ind == selectedId:
|
||||
painter.setPen(Qt.green)
|
||||
elif(selectedCar.leader is not None and selectedCar.leader.id == car.id):
|
||||
painter.setPen(Qt.red)
|
||||
@ -108,4 +108,3 @@ class CarController:
|
||||
|
||||
def setInfoWidget(self, widget):
|
||||
self.infoWidget = widget
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user