Mercurial > hg > nsaunier > traffic-intelligence

comparison trafficintelligence/moving.py @ 1099:4ab5c63c13a3

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
first working version of newell model
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Wed, 20 Feb 2019 00:08:20 -0500
parents 469e36eea158
children 1e833fd8490d
comparison
equal deleted inserted replaced
1098:469e36eea158 1099:4ab5c63c13a3
1260 def updateCurvilinearPositions(self, method, instant, timeStep, _nextAlignmentIdx = None, maxSpeed = None, acceleration = None): 1260 def updateCurvilinearPositions(self, method, instant, timeStep, _nextAlignmentIdx = None, maxSpeed = None, acceleration = None):
1261 '''Update curvilinear position of user at new instant''' 1261 '''Update curvilinear position of user at new instant'''
1262 1262
1263 if method == 'newell': 1263 if method == 'newell':
1264 if self.curvilinearPositions is None: # vehicle without positions, all vehicles should have leader (?) 1264 if self.curvilinearPositions is None: # vehicle without positions, all vehicles should have leader (?)
1265 if self.leader is None: 1265 if self.instantAtX0 is None:
1266 s = (timeStep*instant-self.initialHeadway)*self.desiredSpeed 1266 if self.leader is None:
1267 self.timeInterval = TimeInterval(instant, instant) 1267 s = (timeStep*instant-self.initialHeadway)*self.desiredSpeed
1268 self.curvilinearPositions = CurvilinearTrajectory([s], [0.], [self.initialAlignmentIdx]) 1268 self.timeInterval = TimeInterval(instant, instant)
1269 self.curvilinearVelocities = CurvilinearTrajectory() 1269 self.curvilinearPositions = CurvilinearTrajectory([s], [0.], [self.initialAlignmentIdx])
1270 self.instantAtX0 = self.initialHeadway 1270 self.curvilinearVelocities = CurvilinearTrajectory()
1271 elif self.leader.curvilinearPositions is not None and self.leader.curvilinearPositions.getSCoordAt(-1) > self.d and len(self.leader.curvilinearPositions) >=2: 1271 self.instantAtX0 = self.initialHeadway# to avoid next test
1272 firstInstantAfterD = self.leader.getLastInstant() 1272 elif self.leader.curvilinearPositions is not None and self.leader.curvilinearPositions.getSCoordAt(-1) > self.d and len(self.leader.curvilinearPositions) >=2:
1273 while self.leader.existsAtInstant(firstInstantAfterD) and self.leader.getCurvilinearPositionAtInstant(firstInstantAfterD-1)[0] > self.d:# find first instant after d 1273 firstInstantAfterD = self.leader.getLastInstant()
1274 firstInstantAfterD -= 1 # if not recorded position before self.d, we extrapolate linearly from first known position 1274 while self.leader.existsAtInstant(firstInstantAfterD) and self.leader.getCurvilinearPositionAtInstant(firstInstantAfterD-1)[0] > self.d:# find first instant after d
1275 leaderSpeed = self.leader.getCurvilinearVelocityAtInstant(firstInstantAfterD)[0] 1275 firstInstantAfterD -= 1 # if not recorded position before self.d, we extrapolate linearly from first known position
1276 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 1276 leaderSpeed = self.leader.getCurvilinearVelocityAtInstant(firstInstantAfterD-1)[0]
1277 if self.instantAtX0 < self.initialHeadway: #obj appears at instant initialHeadway at x=0 with desiredSpeed 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
1278 self.instantAtX0 = self.initialHeadway 1278 if self.instantAtX0 < self.initialHeadway: #obj appears at instant initialHeadway at x=0 with desiredSpeed
1279 if self.instantAtX0 is not None and instant >= self.instantAtX0: 1279 self.instantAtX0 = self.initialHeadway
1280 elif instant*timeStep >= self.instantAtX0:
1280 firstInstant = int(ceil(self.instantAtX0/timeStep)) 1281 firstInstant = int(ceil(self.instantAtX0/timeStep))
1281 self.timeInterval = TimeInterval(firstInstant, firstInstant) 1282 self.timeInterval = TimeInterval(firstInstant, firstInstant)
1282 freeFlowCoord = (firstInstant*timeStep - self.instantAtX0)*self.desiredSpeed 1283 freeFlowCoord = (firstInstant*timeStep - self.instantAtX0)*self.desiredSpeed
1283 # constrainedCoord at firstInstant = xn-1(t = firstInstant*timeStep-self.tau)-self.d 1284 # constrainedCoord at firstInstant = xn-1(t = firstInstant*timeStep-self.tau)-self.d
1284 t = firstInstant*timeStep-self.tau 1285 t = firstInstant*timeStep-self.tau
1285 i = int(floor(t/timeStep)) 1286 i = int(floor(t/timeStep))
1286 leaderSpeed = self.leader.getCurvilinearVelocityAtInstant(i)[0] 1287 leaderSpeed = self.leader.getCurvilinearVelocityAtInstant(i)[0]
1287 constrainedCoord = self.leader.getCurvilinearPositionAtInstant(i)[0]+leaderSpeed*(t-i*timeStep)-self.d 1288 constrainedCoord = self.leader.getCurvilinearPositionAtInstant(i)[0]+leaderSpeed*(t-i*timeStep)-self.d
1288 self.curvilinearPositions = CurvilinearTrajectory([min(freeFlowCoord, constrainedCoord)], [0.], [self.initialAlignmentIdx]) 1289 self.curvilinearPositions = CurvilinearTrajectory([min(freeFlowCoord, constrainedCoord)], [0.], [self.initialAlignmentIdx])
1289 self.curvilinearVelocities = CurvilinearTrajectory() 1290 self.curvilinearVelocities = CurvilinearTrajectory()
1290 print(firstInstant+1, instant+1)
1291 for i in range(firstInstant+1, instant+1): 1291 for i in range(firstInstant+1, instant+1):
1292 s1 = self.curvilinearPositions.getSCoordAt(-1) 1292 s1 = self.curvilinearPositions.getSCoordAt(-1)
1293 freeFlowCoord = s1 + timeStep*self.desiredSpeed 1293 freeFlowCoord = s1 + timeStep*self.desiredSpeed
1294 constrainedCoord = self.leader.interpolateCurvilinearPositions(i-self.tau/timeStep)[0]-self.d 1294 constrainedCoord = self.leader.interpolateCurvilinearPositions(i-self.tau/timeStep)[0]-self.d
1295 s2 = min(freeFlowCoord, constrainedCoord) 1295 s2 = min(freeFlowCoord, constrainedCoord)