Mercurial Hosting > traffic-intelligence
comparison trafficintelligence/moving.py @ 1097:b3f8b26ee838
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): |