Mercurial > hg > nsaunier > traffic-intelligence

comparison trafficintelligence/moving.py @ 1097:b3f8b26ee838

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
modification for simulation
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Mon, 18 Feb 2019 17:23:26 -0500
parents 9a32d63bae3f
children 469e36eea158
comparison
equal deleted inserted replaced
1096:9a32d63bae3f 1097:b3f8b26ee838
2 '''Libraries for moving objects, trajectories...''' 2 '''Libraries for moving objects, trajectories...'''
3 3
4 import copy 4 import copy
5 from math import sqrt, atan2, cos, sin 5 from math import sqrt, atan2, cos, sin
6 6
7 from numpy import median, mean, array, arange, zeros, ones, hypot, NaN, std, floor, float32, argwhere, minimum 7 from numpy import median, mean, array, arange, zeros, ones, hypot, NaN, std, floor, ceil, float32, argwhere, minimum
8 from matplotlib.pyplot import plot, text 8 from matplotlib.pyplot import plot, text
9 from scipy.stats import scoreatpercentile 9 from scipy.stats import scoreatpercentile
10 from scipy.spatial.distance import cdist 10 from scipy.spatial.distance import cdist
11 from scipy.signal import savgol_filter 11 from scipy.signal import savgol_filter
12 12
1114 return self.getXCoordinates() 1114 return self.getXCoordinates()
1115 1115
1116 def getLanes(self): 1116 def getLanes(self):
1117 return self.lanes 1117 return self.lanes
1118 1118
1119 def getSCoordAt(self, i):
1120 return self.positions[0][i]
1121
1122 def getYCoordAt(self, i):
1123 return self.positions[1][i]
1124
1125 def getLaneAt(self, i):
1126 return self.positions[2][i]
1127
1119 def addPositionSYL(self, s, y, lane = None): 1128 def addPositionSYL(self, s, y, lane = None):
1120 self.addPositionXY(s,y) 1129 self.addPositionXY(s,y)
1121 self.lanes.append(lane) 1130 self.lanes.append(lane)
1122 1131
1123 def addPosition(self, p): 1132 def addPosition(self, p):
1176 '''Class for moving objects: a spatio-temporal object 1185 '''Class for moving objects: a spatio-temporal object
1177 with a trajectory and a geometry (constant volume over time) 1186 with a trajectory and a geometry (constant volume over time)
1178 and a usertype (e.g. road user) coded as a number (see userTypeNames) 1187 and a usertype (e.g. road user) coded as a number (see userTypeNames)
1179 ''' 1188 '''
1180 1189
1181 def __init__(self, num = None, timeInterval = None, positions = None, velocities = None, geometry = None, userType = userType2Num['unknown'], nObjects = None): 1190 def __init__(self, num = None, timeInterval = None, positions = None, velocities = None, geometry = None, userType = userType2Num['unknown'], nObjects = None, initCurvilinear = False):
1182 super(MovingObject, self).__init__(num, timeInterval) 1191 super(MovingObject, self).__init__(num, timeInterval)
1183 self.positions = positions 1192 if initCurvilinear:
1184 self.velocities = velocities 1193 self.curvilinearPositions = positions
1194 self.curvilinearVelocities = velocities
1195 else:
1196 self.positions = positions
1197 self.velocities = velocities
1185 self.geometry = geometry 1198 self.geometry = geometry
1186 self.userType = userType 1199 self.userType = userType
1187 self.setNObjects(nObjects) # a feature has None for nObjects 1200 self.setNObjects(nObjects) # a feature has None for nObjects
1188 self.features = None 1201 self.features = None
1189 # compute bounding polygon from trajectory 1202 # compute bounding polygon from trajectory
1238 return MovingObject(num = num, timeInterval = timeInterval, positions = positions, velocities = Trajectory([[v.x]*nPoints, [v.y]*nPoints])) 1251 return MovingObject(num = num, timeInterval = timeInterval, positions = positions, velocities = Trajectory([[v.x]*nPoints, [v.y]*nPoints]))
1239 1252
1240 def updatePositions(self): 1253 def updatePositions(self):
1241 inter, self.positions, self.velocities = MovingObject.aggregateTrajectories(self.features, self.getTimeInterval()) 1254 inter, self.positions, self.velocities = MovingObject.aggregateTrajectories(self.features, self.getTimeInterval())
1242 1255
1243 def updateCurvilinearPositions(self, method, changeOfAlignment, nextAlignment_idx, timeStep = None, time = None, 1256 def updateCurvilinearPositions(self, method, changeOfAlignment, nextAlignment_idx, timeStep = None, instant = None, previousAlignmentId = None, maxSpeed = None, acceleration = None):
1244 leaderVehicle = None, previousAlignmentId = None,
1245 maxSpeed = None, acceleration = None):
1246 1257
1247 if method == 'newell': 1258 if method == 'newell':
1248 if time <= self.timeInterval[0] < time + timeStep: 1259 if self.curvilinearPositions is None: # vehicle without positions, all vehicles should have leader (?)
1249 # #si t < instant de creation du vehicule, la position vaut l'espacement dn entre les deux vehicules 1260 if self.leader.curvilinearPositions is not None and self.leader.curvilinearPositions.getSCoordAt(-1) > self.dn and len(self.leader.curvilinearPositions) >=2:
1250 if leaderVehicle is None: 1261 leaderSpeed = self.leader.curvilinearVelocities.getSCoordAt(-1)
1251 self.curvilinearPositions.addPositionSYL( 1262 instantAtX0 = self.tau + instant*timeStep - (self.leader.curvilinearPositions.getSCoordAt(-1)-self.d)/leaderSpeed
1252 -self.dn, 1263 if instantAtX0 < obj.initialHeadway: #obj appears at instant initialHeadway at x=0 with desiredSpeed
1253 0, 1264 instantAtX0 = obj.initialHeadway
1254 nextAlignment_idx) 1265
1266 firstInstant = int(np.ceil(instantAtX0/timeStep))
1267 self.timeInterval = TimeInterval(firstInstant, firstInstant)
1268 freeFlowCoord = (firstInstant*timeStep - instantAtX0)*self.desiredSpeed
1269 # constrainedCoord at firstInstant = xn-1(t = firstInstant*timeStep-self.tn)-self.dn
1270 t = firstInstant*timeStep-self.tn
1271 i = int(floor(t/timeStep))
1272 leaderSpeed = self.leader.getCurvilinearVelocityAtInstant(i)[0]
1273 constrainedCoord = self.leader.getCurvilinearPositionAtInstant(i)[0]+leaderSpeed*(t-i*timeStep)-self.dn
1274 obj.curvilinearPositions = moving.CurvilinearTrajectory([min(freeFlowCoord, constrainedCoord)], [0.], [nextAlignment_idx])# TODO verify initial alignment index
1275 obj.curvilinearVelocities = moving.CurvilinearTrajectory()
1276 for i in range(firstInstant+1, instant+1):
1277 freeFlowCoord = timeStep*self.desiredSpeed + self.curvilinearPositions.getSCoordAt(-1)
1278 #constrainedCoord =
1279
1280 # if instant <= self.timeInterval[0] < instant + timeStep:
1281 # # #si t < instant de creation du vehicule, la position vaut l'espacement dn entre les deux vehicules
1282 # if leaderVehicle is None:
1283 # self.curvilinearPositions.addPositionSYL(
1284 # -self.dn,
1285 # 0,
1286 # nextAlignment_idx)
1287 # else:
1288 # self.curvilinearPositions.addPositionSYL(
1289 # leaderVehicle.curvilinearPositions[0][0] - self.dn,
1290 # 0,
1291 # nextAlignment_idx)
1292 else:
1293 freeFlowCoord = [self.curvilinearPositions[-1][0]+self.desiredSpeed*timeStep, 0, nextAlignment_idx]
1294 if self.leader is None:
1295 self.curvilinearPositions.addPositionSYL(*freeFlowCoord)
1255 else: 1296 else:
1256 self.curvilinearPositions.addPositionSYL( 1297 # if leader coord unknown at t-dn, no movement
1257 leaderVehicle.curvilinearPositions[0][0] - self.dn, 1298 if instant > self.reactionTime:
1258 0, 1299 previousVehicleCurvilinearPositionAtPrecedentInstant = leaderVehicle.curvilinearPositions[-int(round(self.reactionTime))][0] # t-v.reactionTime
1259 nextAlignment_idx)
1260 else:
1261 if leaderVehicle is None:
1262 self.curvilinearPositions.addPositionSYL(self.curvilinearPositions[-1][0]+self.desiredSpeed*timeStep, 0, nextAlignment_idx)
1263 else:
1264 if time > self.reactionTime:
1265 previousVehicleCurvilinearPositionAtPrecedentTime = \
1266 leaderVehicle.curvilinearPositions[-int(round(self.reactionTime))][0] # t-v.reactionTime
1267 else: 1300 else:
1268 previousVehicleCurvilinearPositionAtPrecedentTime = \ 1301 previousVehicleCurvilinearPositionAtPrecedentInstant = \
1269 leaderVehicle.curvilinearPositions[0][0] 1302 leaderVehicle.curvilinearPositions[0][0]
1270 1303
1271 self.curvilinearPositions.addPositionSYL(previousVehicleCurvilinearPositionAtPrecedentTime - self.dn, 0, nextAlignment_idx) 1304 self.curvilinearPositions.addPositionSYL(previousVehicleCurvilinearPositionAtPrecedentInstant - self.dn, 0, nextAlignment_idx)
1272 1305
1273 #mise ajour des vitesses 1306 #mise ajour des vitesses
1274 if changeOfAlignment: 1307 if changeOfAlignment:
1275 self.velocities.addPositionSYL((self.curvilinearPositions[-1][0]-self.curvilinearPositions[-2][0])/timeStep, 1308 self.curvilinearVelocities.addPositionSYL((self.curvilinearPositions[-1][0]-self.curvilinearPositions[-2][0])/timeStep,
1276 (self.curvilinearPositions[-1][1]-self.curvilinearPositions[-2][1])/timeStep, 1309 (self.curvilinearPositions[-1][1]-self.curvilinearPositions[-2][1])/timeStep,
1277 (previousAlignmentId, nextAlignment_idx)) 1310 (previousAlignmentId, nextAlignment_idx))
1278 else: 1311 else:
1279 self.velocities.addPositionSYL((self.curvilinearPositions[-1][0]-self.curvilinearPositions[-2][0])/timeStep, 1312 self.curvilinearVelocities.addPositionSYL((self.curvilinearPositions[-1][0]-self.curvilinearPositions[-2][0])/timeStep,
1280 (self.curvilinearPositions[-1][1]-self.curvilinearPositions[-2][1])/timeStep, 1313 (self.curvilinearPositions[-1][1]-self.curvilinearPositions[-2][1])/timeStep,
1281 None) 1314 None)
1315
1282 @staticmethod 1316 @staticmethod
1283 def concatenate(obj1, obj2, num = None, newFeatureNum = None, computePositions = False): 1317 def concatenate(obj1, obj2, num = None, newFeatureNum = None, computePositions = False):
1284 '''Concatenates two objects, whether overlapping temporally or not 1318 '''Concatenates two objects, whether overlapping temporally or not
1285 1319
1286 Positions will be recomputed only if computePositions is True 1320 Positions will be recomputed only if computePositions is True
1427 if withOrigin and len(instants)>0: 1461 if withOrigin and len(instants)>0:
1428 plot([instants[0]], [coords[0]], 'ro', **kwargs) 1462 plot([instants[0]], [coords[0]], 'ro', **kwargs)
1429 else: 1463 else:
1430 print('Object {} has no curvilinear positions'.format(self.getNum())) 1464 print('Object {} has no curvilinear positions'.format(self.getNum()))
1431 1465
1466 def interpolateCurvilinearPositions(self, t):
1467 '''Linear interpolation of curvilinear positions, t being a float'''
1468 if hasattr(self, 'curvilinearPositions'):
1469 if self.existsAtInstant(t):
1470 i = int(floor(t))
1471 p1 = self.getCurvilinearPositionAtInstant(i)
1472 p2 = self.getCurvilinearPositionAtInstant(i+1)
1473 alpha = t-float(i)
1474 if alpha < 0.5:
1475 lane = p1[2]
1476 else:
1477 lane = p2[2]
1478 return [(1-alpha)*p1[0]+alpha*p2[0], (1-alpha)*p1[1]+alpha*p2[1], lane]
1479 else:
1480 print('Object {} does not exist at {}'.format(self.getNum(), t))
1481 else:
1482 print('Object {} has no curvilinear positions'.format(self.getNum()))
1483
1432 def setUserType(self, userType): 1484 def setUserType(self, userType):
1433 self.userType = userType 1485 self.userType = userType
1434 1486
1435 def getNObjects(self): 1487 def getNObjects(self):
1436 return self.nObjects 1488 return self.nObjects
1514 def getPositionAtInstant(self, i): 1566 def getPositionAtInstant(self, i):
1515 return self.positions[i-self.getFirstInstant()] 1567 return self.positions[i-self.getFirstInstant()]
1516 1568
1517 def getVelocityAtInstant(self, i): 1569 def getVelocityAtInstant(self, i):
1518 return self.velocities[i-self.getFirstInstant()] 1570 return self.velocities[i-self.getFirstInstant()]
1571
1572 def getCurvilinearPositionAt(self, i):
1573 return self.curvilinearPositions[i]
1574
1575 def getCurvilinearVelocityAt(self, i):
1576 return self.curvilinearVelocities[i]
1577
1578 def getCurvilinearPositionAtInstant(self, i):
1579 return self.curvilinearPositions[i-self.getFirstInstant()]
1580
1581 def getCurvilinearVelocityAtInstant(self, i):
1582 return self.curvilinearVelocities[i-self.getFirstInstant()]
1519 1583
1520 def getXCoordinates(self): 1584 def getXCoordinates(self):
1521 return self.positions.getXCoordinates() 1585 return self.positions.getXCoordinates()
1522 1586
1523 def getYCoordinates(self): 1587 def getYCoordinates(self):