Add different CACC-Sim version

modules/CACC/platoonSimulation.py

+import sys
+import IPython
+import bisect
+import time
+import math
+import random
+import collections
+
+import matplotlib
+matplotlib.use('TkAgg')
+from matplotlib import pyplot as plt
+
+class CrashException(Exception):
+  pass
+
+class Vehicle(object):
+  def __init__(self,name,typ,road,initPos,initVelo):
+    
+    self.name = name
+    self.typ = typ
+    self.road = road
+    
+    self.velos = []
+    self.posis = []
+    self.dists = []
+    self.velosVA = []
+    
+    self.posi = 1.0*initPos # to make it a float value
+    if self.typ == "stone":
+      self.velo = 0
+    else:
+      self.velo = initVelo
+    
+    self.engInAcc    = (100 + random.randint(-3,3))/100.0 # constant inaccuracy for my own engine
+    self.sensorInAcc = (100 + random.randint(-3,3))/100.0
+
+  @property
+  def velo(self):
+    return self.velos[-1]
+  @velo.setter
+  def velo(self,value):
+    self.velos.append(value)
+    
+  @property
+  def posi(self):
+    return self.posis[-1]
+  @posi.setter
+  def posi(self,value):
+    self.posis.append(value)
+  
+  @property
+  def dist(self):
+    return self.dists[-1]
+  @dist.setter
+  def dist(self,value):
+    self.dists.append(value)
+  
+  @property
+  def veloVA(self):
+    return self.velosVA[-1]
+  @veloVA.setter
+  def veloVA(self,value):
+    self.velosVA.append(value)
+
+  def __str__(self):
+    retStr = "---"
+    retStr += self.name + ".posi: " + str(self.posi)   + "\n"
+    retStr += self.name + ".velo: " + str(self.velo)   + "\n"
+    retStr += self.name + ".dist: " + str(self.dist)   + "\n"
+    retStr += self.name + ".veloVA: " + str(self.veloVA) + "\n"
+    retStr += self.name + ".engInAcc: " + str(self.engInAcc) + "\n"
+    retStr += self.name + ".sensorInAcc: " + str(self.sensorInAcc) + "\n"
+    retStr += "---\n"
+    return retStr
+
+  def update(self, tStep):
+    
+    oldVelo = self.velo
+    
+    # get sensor data
+    self.updateSensorData(tStep)
+    
+    # udapte position
+    self.posi += tStep*(oldVelo + self.velo)/2
+    
+    # decide on values
+    if self.typ == "fv":
+      if abs(self.dist - IPD) > IPD_TOL:
+        self.velo = self.engInAcc*min(self.veloVA, PS)*(self.dist / IPD)
+      else:
+        self.velo = self.engInAcc*PS
+    
+    if self.typ == "lv":
+      if self.dist < IPD:
+        # self.velo = max(self.velo - 1, 0)
+        self.velo = 0
+      else:
+        self.velo = self.engInAcc*PS
+
+  def updateSensorData(self,tStep):
+    try:
+      oldDist = self.dist
+    except IndexError:
+      oldDist = self.sensorInAcc*self.road.getDistance(self)
+
+    self.dist = self.sensorInAcc*self.road.getDistance(self)
+    self.veloVA = self.velo + (self.dist - oldDist)/tStep    
+    return
+  
+  def __le__(self, other):
+    return self.posi <= other.posi
+  
+  def __lt__(self, other):
+    return self.posi < other.posi
+
+  
+class Road(object):
+  def __init__(self):
+    self.vehicles = []
+  
+  def __str__(self):
+    retStr = ""
+    for vehicle in self.vehicles:
+      retStr += vehicle.__str__()
+    return retStr
+
+  def placeVehicle(self, vehicle):
+    bisect.insort(self.vehicles, vehicle)
+    
+  def update(self,tStep):
+    for vIdx, vehicle in enumerate(self.vehicles):
+      vehicle.update(tStep)
+      if vIdx < len(self.vehicles)-1:
+        if vehicle.posi >= self.vehicles[vIdx+1].posi:
+          raise CrashException("Crash!")
+    
+  def draw(self):
+    minPosi = self.vehicles[0].posi
+    maxPosi = self.vehicles[-1].posi
+    relPos = [int((vehicle.posi - minPosi)/(maxPosi - minPosi)*100) for vehicle in self.vehicles]
+    
+    drawing = ""
+    relDrawProg = 0
+    for vIdx, vehicle in enumerate(self.vehicles):
+      spaceToFill = relPos[vIdx] - relDrawProg
+      distStr = "<" + str(int(self.vehicles[vIdx-1].dist)).zfill(3) + ">"
+      if  spaceToFill >= len(distStr):
+        frontspace = int(math.ceil((spaceToFill - len(distStr))/2.0))
+        backspace  = int(math.floor((spaceToFill - len(distStr))/2.0))
+        for _ in range(frontspace): 
+          drawing += " "; relDrawProg += 1
+        drawing += distStr; relDrawProg += len(distStr)
+        for _ in range(backspace): 
+          drawing += " "; relDrawProg += 1
+      else:
+        for _ in range(spaceToFill):
+          drawing += " "
+          relDrawProg += 1
+      drawing += vehicle.name
+      relDrawProg += 1
+    
+    print("-----------------------------------------------------------------------------------------------------")
+    print(drawing)
+    print("-----------------------------------------------------------------------------------------------------")
+
+  def getDistance(self,vehicle):
+    vIdx = self.vehicles.index(vehicle)
+    if vIdx == len(self.vehicles)-1:
+      return sys.maxsize
+    else:
+      return self.vehicles[vIdx + 1].posi - self.vehicles[vIdx].posi
+
+  def getVeloVA(self, vehicle):
+    vIdx = self.vehicles.index(vehicle)
+    if vIdx == len(self.vehicles)-1:
+      return sys.maxsize
+    else:
+      return self.vehicles[vIdx + 1].velo
+
+
+# GLOBALS
+IPD = 15*100    # in millimetres
+IPD_TOL = 1*100 # in millimetres
+
+PS = 15    # in millimetres per milliseconds
+PS_TOL = 1 # in millimetres per milliseconds
+
+SENSOR_UPDATE_TIME = .5 # in milliseconds
+
+if __name__ == "__main__":
+
+  tStart = 0; tStep  = 1; tEnd = 500
+  
+  road = Road()
+  lv  = Vehicle("L","lv",road,100*100,15); road.placeVehicle(lv)    
+  fv1 = Vehicle("1","fv",road,75*100,20);  road.placeVehicle(fv1)  
+  # fv2 = Vehicle("2","fv",road,50*100,15);  road.placeVehicle(fv2)  
+  
+  # startStone = Vehicle("s","stone",road,0*100,0); road.placeVehicle(startStone)   
+  # endStone   = Vehicle("S","stone",road,400*100,0); road.placeVehicle(endStone)   
+  
+  t = tStart;
+  ts = []
+  while True:
+    
+    try:
+      road.update(SENSOR_UPDATE_TIME)
+    except CrashException:
+      print("CRASH!")
+      break
+    
+    t += SENSOR_UPDATE_TIME; ts.append(t)
+    # print("t = ", t)
+    # road.draw()
+    # time.sleep(0.2)
+    
+    if t > tEnd:
+      break
+  plt.subplot(131)
+  plt.plot([tStart] +  ts, lv.posis,  'm')
+  plt.plot([tStart] +  ts, fv1.posis, 'g')
+  # plt.plot([tStart] +  ts, fv2.posis, 'b')
+  plt.title("Position") 
+  plt.ylim(7000,18000)
+      
+  plt.subplot(132)
+  plt.plot(ts, [PS for t in ts],'r', ts,[PS-PS_TOL for t in ts],'r--', ts,[PS+PS_TOL for t in ts],'r--')
+  plt.plot([tStart] +  ts, lv.velos, 'm--')
+  plt.plot([tStart] +  ts, [d/lv.engInAcc for d in lv.velos], 'm')
+  plt.plot([tStart] +  ts, fv1.velos, 'g--')
+  plt.plot([tStart] +  ts, [d/fv1.engInAcc for d in fv1.velos], 'g')
+  plt.plot(ts, fv1.velosVA, 'm.')
+  # plt.plot([tStart] +  ts, fv2.velos, 'b--')
+  # plt.plot([tStart] +  ts, [d/fv2.engInAcc for d in fv2.velos], 'b')
+  plt.title("Speed") 
+  plt.ylim(13,26)
+  
+  plt.subplot(133)
+  plt.plot(ts,[IPD for t in ts],'r', ts,[IPD-IPD_TOL for t in ts],'r--', ts,[IPD+IPD_TOL for t in ts],'r--')
+  plt.plot(ts, fv1.dists, 'g--')
+  plt.plot(ts, [d/fv1.sensorInAcc for d in fv1.dists], 'g')
+  # plt.plot(ts, fv2.dists, 'b--')
+  # plt.plot(ts, [d/fv2.sensorInAcc for d in fv2.dists], 'b')
+  plt.title("Distance") 
+  plt.ylim(1300,2600)
+  
+  plt.show()
+