Mercurial Hosting > traffic-intelligence

comparison trafficintelligence/moving.py @ 1100:1e833fd8490d

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
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: