Mercurial Hosting > traffic-intelligence
comparison trafficintelligence/moving.py @ 1100:1e833fd8490d
working version of Newell car following model
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Wed, 20 Feb 2019 16:16:19 -0500 |
parents | 4ab5c63c13a3 |
children | 799ef82caa1a |
comparison
equal
deleted
inserted
replaced
1099:4ab5c63c13a3 | 1100:1e833fd8490d |
---|---|
1255 return MovingObject(num = num, timeInterval = timeInterval, positions = positions, velocities = Trajectory([[v.x]*nPoints, [v.y]*nPoints])) | 1255 return MovingObject(num = num, timeInterval = timeInterval, positions = positions, velocities = Trajectory([[v.x]*nPoints, [v.y]*nPoints])) |
1256 | 1256 |
1257 def updatePositions(self): | 1257 def updatePositions(self): |
1258 inter, self.positions, self.velocities = MovingObject.aggregateTrajectories(self.features, self.getTimeInterval()) | 1258 inter, self.positions, self.velocities = MovingObject.aggregateTrajectories(self.features, self.getTimeInterval()) |
1259 | 1259 |
1260 def addNewellAttributes(self, desiredSpeed, tau, d, initialCumulatedHeadway, initialAlignmentIdx): | |
1261 '''adds attributes necessary for Newell car following model | |
1262 using curvilinear trajectories''' | |
1263 # Newell model parameters | |
1264 self.desiredSpeed = desiredSpeed | |
1265 self.tau = tau | |
1266 self.d = d | |
1267 self.leader = None | |
1268 # other attributes necessary for computation | |
1269 self.initialCumulatedHeadway = initialCumulatedHeadway | |
1270 self.initialAlignmentIdx = initialAlignmentIdx | |
1271 self.timeAtS0 = None # time at which the vehicle's position is s=0 on the alignment | |
1272 | |
1260 def updateCurvilinearPositions(self, method, instant, timeStep, _nextAlignmentIdx = None, maxSpeed = None, acceleration = None): | 1273 def updateCurvilinearPositions(self, method, instant, timeStep, _nextAlignmentIdx = None, maxSpeed = None, acceleration = None): |
1261 '''Update curvilinear position of user at new instant''' | 1274 '''Update curvilinear position of user at new instant''' |
1262 | 1275 |
1263 if method == 'newell': | 1276 if method == 'newell': |
1264 if self.curvilinearPositions is None: # vehicle without positions, all vehicles should have leader (?) | 1277 if self.curvilinearPositions is None: # vehicle without positions |
1265 if self.instantAtX0 is None: | 1278 if self.timeAtS0 is None: |
1266 if self.leader is None: | 1279 if self.leader is None: |
1267 s = (timeStep*instant-self.initialHeadway)*self.desiredSpeed | 1280 self.timeAtS0 = self.initialCumulatedHeadway |
1268 self.timeInterval = TimeInterval(instant, instant) | |
1269 self.curvilinearPositions = CurvilinearTrajectory([s], [0.], [self.initialAlignmentIdx]) | |
1270 self.curvilinearVelocities = CurvilinearTrajectory() | |
1271 self.instantAtX0 = self.initialHeadway# to avoid next test | |
1272 elif self.leader.curvilinearPositions is not None and self.leader.curvilinearPositions.getSCoordAt(-1) > self.d and len(self.leader.curvilinearPositions) >=2: | 1281 elif self.leader.curvilinearPositions is not None and self.leader.curvilinearPositions.getSCoordAt(-1) > self.d and len(self.leader.curvilinearPositions) >=2: |
1273 firstInstantAfterD = self.leader.getLastInstant() | 1282 firstInstantAfterD = self.leader.getLastInstant() |
1274 while self.leader.existsAtInstant(firstInstantAfterD) and self.leader.getCurvilinearPositionAtInstant(firstInstantAfterD-1)[0] > self.d:# find first instant after d | 1283 while self.leader.existsAtInstant(firstInstantAfterD) and self.leader.getCurvilinearPositionAtInstant(firstInstantAfterD-1)[0] > self.d:# find first instant after d |
1275 firstInstantAfterD -= 1 # if not recorded position before self.d, we extrapolate linearly from first known position | 1284 firstInstantAfterD -= 1 # if not recorded position before self.d, we extrapolate linearly from first known position |
1276 leaderSpeed = self.leader.getCurvilinearVelocityAtInstant(firstInstantAfterD-1)[0] | 1285 leaderSpeed = self.leader.getCurvilinearVelocityAtInstant(firstInstantAfterD-1)[0] |
1277 self.instantAtX0 = self.tau + instant*timeStep - (self.leader.getCurvilinearPositionAtInstant(firstInstantAfterD)[0]-self.d)/leaderSpeed # second part is the time at which leader is at self.d | 1286 self.timeAtS0 = self.tau + firstInstantAfterD*timeStep - (self.leader.getCurvilinearPositionAtInstant(firstInstantAfterD)[0]-self.d)*timeStep/leaderSpeed # second part is the time at which leader is at self.d |
1278 if self.instantAtX0 < self.initialHeadway: #obj appears at instant initialHeadway at x=0 with desiredSpeed | 1287 if self.timeAtS0 < self.initialCumulatedHeadway: #obj appears at instant initialCumulatedHeadway at x=0 with desiredSpeed |
1279 self.instantAtX0 = self.initialHeadway | 1288 self.timeAtS0 = self.initialCumulatedHeadway |
1280 elif instant*timeStep >= self.instantAtX0: | 1289 elif instant*timeStep > self.timeAtS0: |
1281 firstInstant = int(ceil(self.instantAtX0/timeStep)) | 1290 #firstInstant = int(ceil(self.timeAtS0/timeStep))# this first instant is instant by definition |
1282 self.timeInterval = TimeInterval(firstInstant, firstInstant) | 1291 leaderInstant = instant-self.tau/timeStep |
1283 freeFlowCoord = (firstInstant*timeStep - self.instantAtX0)*self.desiredSpeed | 1292 if self.leader is None: |
1284 # constrainedCoord at firstInstant = xn-1(t = firstInstant*timeStep-self.tau)-self.d | 1293 s = (timeStep*instant-self.timeAtS0)*self.desiredSpeed |
1285 t = firstInstant*timeStep-self.tau | 1294 self.timeInterval = TimeInterval(instant, instant) |
1286 i = int(floor(t/timeStep)) | 1295 self.curvilinearPositions = CurvilinearTrajectory([s], [0.], [self.initialAlignmentIdx]) |
1287 leaderSpeed = self.leader.getCurvilinearVelocityAtInstant(i)[0] | 1296 self.curvilinearVelocities = CurvilinearTrajectory() |
1288 constrainedCoord = self.leader.getCurvilinearPositionAtInstant(i)[0]+leaderSpeed*(t-i*timeStep)-self.d | 1297 elif self.leader.existsAtInstant(leaderInstant): |
1289 self.curvilinearPositions = CurvilinearTrajectory([min(freeFlowCoord, constrainedCoord)], [0.], [self.initialAlignmentIdx]) | 1298 self.timeInterval = TimeInterval(instant, instant) |
1290 self.curvilinearVelocities = CurvilinearTrajectory() | 1299 freeFlowCoord = (instant*timeStep - self.timeAtS0)*self.desiredSpeed |
1291 for i in range(firstInstant+1, instant+1): | 1300 # constrainedCoord at instant = xn-1(t = instant*timeStep-self.tau)-self.d |
1292 s1 = self.curvilinearPositions.getSCoordAt(-1) | 1301 constrainedCoord = self.leader.interpolateCurvilinearPositions(leaderInstant)[0]-self.d |
1293 freeFlowCoord = s1 + timeStep*self.desiredSpeed | 1302 self.curvilinearPositions = CurvilinearTrajectory([min(freeFlowCoord, constrainedCoord)], [0.], [self.initialAlignmentIdx]) |
1294 constrainedCoord = self.leader.interpolateCurvilinearPositions(i-self.tau/timeStep)[0]-self.d | 1303 self.curvilinearVelocities = CurvilinearTrajectory() |
1295 s2 = min(freeFlowCoord, constrainedCoord) | |
1296 self.curvilinearPositions.addPositionSYL(s2, 0., self.initialAlignmentIdx) | |
1297 self.setLastInstant(i) | |
1298 print(i, self.timeInterval) | |
1299 self.curvilinearVelocities.addPositionSYL(s2-s1, 0., None) | |
1300 else: | 1304 else: |
1301 if _nextAlignmentIdx is not None: | 1305 if _nextAlignmentIdx is not None: |
1302 laneChange = (self.curvilinearPositions.getLaneAt(-1), _nextAlignmentIdx) | 1306 laneChange = (self.curvilinearPositions.getLaneAt(-1), _nextAlignmentIdx) |
1303 nextAlignmentIdx = _nextAlignmentIdx | 1307 nextAlignmentIdx = _nextAlignmentIdx |
1304 else: | 1308 else: |