amélioré la detection du leader

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):
+    def getShape(self, edgeInd):
-        startEdge=self.route[self.index]
+        startEdge = self.route[edgeInd]
-        nextEdge=self.route[self.index+1]
+        nextEdge = self.route[edgeInd+1]
 
-        inverted=not (startEdge.getToNode()==nextEdge.getFromNode() or startEdge.getToNode()==nextEdge.getToNode())
+        inverted = not (startEdge.getToNode()==nextEdge.getFromNode() or startEdge.getToNode()==nextEdge.getToNode())
-        
+        laneId = 0 if not inverted else 1
-        laneId=0 if not inverted else 1
+        lane = startEdge.getLane(laneId)
-        lane=startEdge.getLane(laneId)
+        vmax = lane.getSpeed()
-        self.vmax=lane.getSpeed()
+        laneShape = lane.getShape()
-        self.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)}")
@@ -62,6 +74,40 @@ class Car:
         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(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)
@@ -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())
+        self.leader=self.getLeader(50)
-        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):
+        self.conduite(self.vmax,self.leader,dt)
-                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)
+            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)

CarController.py

@@ -8,14 +8,14 @@ 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
-        
+
         self.chart = self.ui.speedGraph.chart()
         #self.chart.setAnimationOptions(QChart.AllAnimations)
-        
+
         speedsNames = ["vmax","vsec","Vitesse (m.s^-1)"]
         self.speedSeries = []
         for ind,s in enumerate(speedsNames):
@@ -29,8 +29,8 @@ class carInfo(QWidget):
         if(self.speedSeries[ind].count()>1 and self.speedSeries[ind].at(1).x()<t-500):
             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,32 +63,32 @@ 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"):
             route=vehicle.route[0].edges.split()
             wId=self.infoWidget.addItem(carInfo(), vehicle.id)
             self.cars.append(Car(vehicle.id,route,self.map,self,self.infoWidget.widget(wId)))
-    
+
     def prepareRoute(self):
         for car in self.cars:
             car.prepareRoute()
 
-    def getCarsOnEdge(self,edgeID): # on devrai probablement utiliser une map ici (dict en python?) mais flemme
+    def getCarsOnEdge(self,edgeID):
-        res=[]
+        return filter(lambda c: c.route[c.index].getID()==edgeID,self.cars)
-        for car in self.cars:
+
-            cid=car.route[car.index].getID()
+    def getCarsOnLane(self,edgeID,laneID):
-            if(cid==edgeID):
+        cars = self.getCarsOnEdge(edgeID)
-                res.append(car)
+        return cars #filter(lambda c: c.laneId == laneID,cars)
-        return res
 
     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
-