diff --git a/modules/CACC/CACC-Module-Test.py b/modules/CACC/CACC-Module-Test.py
index e2c8c8db58e4aaad8bd6e9f3cc0e01a818baf418..e2f631fa5553de5cd237c223c70572d987436ab6 100644
--- a/modules/CACC/CACC-Module-Test.py
+++ b/modules/CACC/CACC-Module-Test.py
@@ -1,96 +1,130 @@
import random
+import IPython
+import sys
import matplotlib
matplotlib.use('TkAgg')
from matplotlib import pyplot as plt
-class Car(object):
- cSpeed = None
- cPos = None
- cDist = None
+class UnderCIPDerror(Exception):
+ pass
- cIPD = None
- IPD = None
- PS = None
+class Car(object):
- def __init__(self, prev, speed, position, IPD, PS):
+ def __init__(self, prev, speed, position, IPD, PS, tStep):
self.plattonPrev = prev
- self.cSpeed = speed
self.cPos = position
- self.updateDis()
- self.cIPD = 0.01*IPD # noch keine cIPD definition existent
self.IPD = IPD
self.PS = PS
+ self.tStep = tStep
self.bcIPD = False
+
+ RAND = 0.5
+ self.SonarSystemUnc = random.uniform(-RAND,RAND) # in cm
+ self.SonarStatisticalUnc = lambda : random.gauss(0,RAND) # in cm
+
+ self.SpeedSystemUnc = random.uniform(-RAND,RAND) # in meter per seconds
+ self.SpeedStatisticalUnc = lambda : random.gauss(0,RAND) # in meter per seconds
+
+ # self.SpeedSystemUnc = random.uniform(0,0) # in meter per seconds
+ # self.SpeedStatisticalUnc = lambda : random.gauss(0,0) # in meter per seconds
+
+ self.EngineSystemUnc = random.uniform(-RAND,RAND) # in meter per seconds
+ # self.EngineSystemUnc = random.uniform(0,0) # in meter per seconds
+
+ self.setSpeed(speed)
+ self.getDistance()
+ self.v_v = self.cSpeed
return None
def getCIPD(self):
- return self.cIPD
+ return 0.01*self.IPD
- def getPos(self):
- return self.cPos
-
def getSpeed(self):
- return self.cSpeed
-
+ if self.cSpeed <= 1e-8:
+ return 0
+ return self.SpeedSystemUnc + self.cSpeed + self.SpeedStatisticalUnc()
+
+ def setSpeed(self,value):
+ if value > 1e-8:
+ self.cSpeed = value
+ self.speed = self.EngineSystemUnc + value
+ else:
+ self.cSpeed = 0
+ self.speed = 0
+
def getDistance(self):
- return self.cDist
+ setLater = False
+ try:
+ self.oldDist = self.dist
+ except AttributeError:
+ setLater = True
+
+ if not self.plattonPrev == None:
+ self.dist = self.SonarSystemUnc + (self.plattonPrev.cPos - self.cPos) + self.SonarStatisticalUnc()
+ else:
+ self.dist = sys.maxsize
+
+ if setLater:
+ self.oldDist = self.dist
+ return self.dist
def updatePos(self):
- self.cPos = self.cPos + self.cSpeed/16 #Ticklaenge. Kann zusammen mit dem i in den Updates angepasst werden. Das Produkt beider muss 1 sein
+ self.cPos += self.cSpeed/float(tStep)
- def updateDis(self):
+ def estimateVeloVA(self):
if not self.plattonPrev == None:
- self.cDist = - self.cPos + self.plattonPrev.getPos()#* random.uniform(0.999, 1.001) # bis zu 0.001 relative Abweichung auf die Entfernungsmessung
+ return self.getSpeed() + (self.dist - self.oldDist)*self.tStep
def updateSpeed(self):
if not self.bcIPD:
- d_c = self.cDist
- v_c = self.cSpeed
+ d = self.getDistance() # sensor data
+ v = self.getSpeed() # sesnor data
IPD = self.IPD
- PS = self.PS
- v_n = None
+ PS = self.PS
+ v_new = None
if not self.plattonPrev == None:
- v_v = self.plattonPrev.getSpeed() #* random.uniform(0.9, 1.1) # bis zu 0.1 relative Abweichung auf die Geschwindigkeitsmessung
- if self.checkInbound(d_c, IPD, 0.01*IPD):
- if self.checkInbound (v_c, v_v, 0.01*v_v):
- if self.checkInbound(v_c, PS, 0.01*PS):
- v_n = PS
+ # v_v = self.plattonPrev.getSpeed() # TODO estimate this speed
+ self.v_v = self.estimateVeloVA()
+ v_v = self.v_v
+ if checkInbound(d, IPD, 0.01*IPD):
+ if checkInbound (v, v_v, 0.01*v_v):
+ if checkInbound(v, PS, 0.01*PS):
+ v_new = PS
else:
- if v_c > PS:
- v_n = v_v - abs(PS-v_v)/4
+ if v > PS:
+ v_new = v_v - abs(PS-v_v)/4
else:
- v_n = v_v + abs(PS-v_v)/4
+ v_new = v_v + abs(PS-v_v)/4
else:
- if v_c > v_v:
- v_n = v_v - abs(PS-v_v)/4
+ if v > v_v:
+ v_new = v_v - abs(PS-v_v)/4
else:
- v_n = v_v + abs(PS-v_v)/4
+ v_new = v_v + abs(PS-v_v)/4
else:
- v_n = min((v_v * (d_c/IPD)**2), v_c*1.1) #Der Exponent gibt an, wie schnell die Aenderung umgesetzt werden soll
+ v_new = (v_v * (d/IPD)**2) #Der Exponent gibt an, wie schnell die Aenderung umgesetzt werden soll
inertia = 0.8 # in [0,1] und gibt die Traegheit des Fahrzeuges an
- self.cSpeed = inertia * v_c + (1- inertia) * v_n
+ self.setSpeed(inertia * v + (1- inertia) * v_new)
+
+ if self.getCIPD() > d:
+ print("raise")
+ raise UnderCIPDerror("under CIPD!")
- self.cIPD = self.IPD * 0.01 #too low
- if self.cIPD > self.cDist:
- self.cSpeed = 0
- self.bcIPD = True
- self.cIPD = -10 #Zur Visualisierung
- else:
- v_v = self.cSpeed
+ else: # this is for the LV
c = 0.8
- self.cSpeed = c * v_c + (1-c) * PS
+ self.v_v = 0
+ self.setSpeed(c * v + (1-c) * PS)
- def checkInbound(self, argument, target, delta): #Ueberall ist ein delta von 0.01 hinterlegt
- if argument > target + delta:
- return False
- elif argument < target - delta:
- return False
- else:
- return True
+def checkInbound(argument, target, delta): #Ueberall ist ein delta von 0.01 hinterlegt
+ if argument > target + delta:
+ return False
+ elif argument < target - delta:
+ return False
+ else:
+ return True
def broadcastPlattonSettings(carList, newIPD, newPS):
for item in carList:
@@ -100,7 +134,6 @@ def broadcastPlattonSettings(carList, newIPD, newPS):
def update(carList):
for item in carList:
item.updatePos()
- item.updateDis()
for item in reversed(carList):
item.updateSpeed()
@@ -108,14 +141,17 @@ if __name__ == "__main__":
maxT = [20, 40, 80, 110, 130,150,170] # times for events
setIPD = [40, 40, 40, 60, 45, 30, 15] # event of IPD (activ until time event)
setPS = [20, 30, 10, 10, 10, 10, 10] # event of PS (activ until time event)
+ # setIPD = [40, 40, 40, 40, 40, 40, 40] # event of IPD (activ until time event)
+ # setPS = [20, 20, 20, 20, 20, 20, 20] # event of PS (activ until time event)
v = [52.0, 50.0, 10.0] # startvektor (Startgeschwindikeit)
- pos = [70, 30, 0] # startvektor (Startposition)
+ pos = [90, 30, 0] # startvektor (Startposition)
+ tStep = 30.0
cars = []
- cars.append(Car(None, v[0], pos[0], setIPD[0], setPS[0]))
- cars.append(Car(cars[0], v[1], pos[1], setIPD[0], setPS[0]))
- cars.append(Car(cars[1], v[2], pos[2], setIPD[0], setPS[0]))
+ cars.append(Car(None, v[0], pos[0], setIPD[0], setPS[0],tStep))
+ cars.append(Car(cars[0], v[1], pos[1], setIPD[0], setPS[0],tStep))
+ cars.append(Car(cars[1], v[2], pos[2], setIPD[0], setPS[0],tStep))
t = []
mesIPD= []
@@ -123,10 +159,13 @@ if __name__ == "__main__":
y_pos = [[],[],[]]
y_speed = [[],[],[]]
+ y_cSpeed = [[],[],[]]
y_dist = [[],[],[]]
+ y_cDist = [[],[],[]]
y_cdist = [[],[],[]]
+ y_v_v = [[],[],[]]
- i = 0.0
+ i = 0.0; end = False
for j in range(0, len(maxT)):
IPD = setIPD[j]
PS = setPS[j]
@@ -138,39 +177,49 @@ if __name__ == "__main__":
mesPS.append(PS)
for k in range(0, len(cars)):
- y_pos[k].append(cars[k].getPos())
+ y_pos[k].append(cars[k].cPos)
y_speed[k].append(cars[k].getSpeed())
+ y_cSpeed[k].append(cars[k].cSpeed)
+ y_v_v[k].append(cars[k].v_v)
for k in range(1, len(cars)):
y_dist[k].append(cars[k].getDistance())
+ y_cDist[k].append(cars[k-1].cPos - cars[k].cPos)
y_cdist[k].append(cars[k].getCIPD())
- i = i+0.0625
- update(cars)
+ i = i+1/tStep
+ try:
+ update(cars)
+ except UnderCIPDerror:
+ print("catch")
+ end = True
+ break
+ if end: break
plt.subplot(131)
- plt.plot(t, y_pos[0], 'r', label='LV')
- plt.plot(t, y_pos[1], 'b', label='FV 1')
- plt.plot(t, y_pos[2], 'g', label='FV 2')
- plt.legend()
+ plt.plot(t, y_pos[0], 'r')
+ plt.plot(t, y_pos[1], 'b')
+ # plt.plot(t, y_pos[2], 'g')
plt.title("Position")
plt.subplot(132)
- plt.plot(t, mesPS, 'r--', label='PS')
- plt.plot(t, y_speed[0], 'r-', label='LV')
- plt.plot(t, y_speed[1], 'b-', label='FV 1')
- plt.plot(t, y_speed[2], 'g-', label='FV 2')
- plt.legend()
+ plt.plot(t, mesPS, 'm--')
+ plt.plot(t, y_speed[0], 'r--')
+ plt.plot(t, y_cSpeed[0], 'r-')
+ plt.plot(t, y_speed[1], 'b--')
+ plt.plot(t, y_cSpeed[1], 'b')
+ plt.plot(t, y_v_v[1], 'g')
+ # plt.plot(t, y_speed[2], 'g')
plt.title("Speed")
plt.subplot(133)
- plt.plot(t, mesIPD, 'r--', label='IPD')
- plt.plot(t, y_dist[1], 'b-', label='FV1 - LV - Distance')
- plt.plot(t, y_cdist[1], 'b--', label='FV1 - cIPD)')
- plt.plot(t, y_dist[2], 'g-', label='FV2 - FV1 - Distance')
- plt.plot(t, y_cdist[2], 'g--', label='FV2 - cIPD')
- plt.legend()
+ plt.plot(t, mesIPD, 'r--')
+ plt.plot(t, y_dist[1], 'b-')
+ plt.plot(t, y_cdist[1], 'b--')
+ plt.plot(t, y_cDist[1], 'b--')
+ # plt.plot(t, y_dist[2], 'g-')
+ # plt.plot(t, y_cdist[2], 'g--')
plt.title("Distance")
plt.show()