Mercurial Hosting > traffic-intelligence
changeset 1098:469e36eea158
work in progress
| author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
|---|---|
| date | Tue, 19 Feb 2019 17:22:48 -0500 |
| parents | b3f8b26ee838 |
| children | 4ab5c63c13a3 |
| files | trafficintelligence/moving.py |
| diffstat | 1 files changed, 56 insertions(+), 52 deletions(-) [+] |
line wrap: on
line diff
--- a/trafficintelligence/moving.py Mon Feb 18 17:23:26 2019 -0500 +++ b/trafficintelligence/moving.py Tue Feb 19 17:22:48 2019 -0500 @@ -1123,7 +1123,7 @@ return self.positions[1][i] def getLaneAt(self, i): - return self.positions[2][i] + return self.lanes[i] def addPositionSYL(self, s, y, lane = None): self.addPositionXY(s,y) @@ -1143,7 +1143,11 @@ p1 = self[0] for i in range(1, self.length()): p2 = self[i] - diff.addPositionSYL(p2[0]-p1[0], p2[1]-p1[1]) + if p2[2] == p1[2]: + laneChange = None + else: + laneChange = (p1[2], p2[2]) + diff.addPositionSYL(p2[0]-p1[0], p2[1]-p1[1], laneChange) p1=p2 if doubleLastPosition and self.length() > 1: diff.addPosition(diff[-1]) @@ -1191,7 +1195,7 @@ super(MovingObject, self).__init__(num, timeInterval) if initCurvilinear: self.curvilinearPositions = positions - self.curvilinearVelocities = velocities + self.curvilinearVelocities = velocities # third component is (previousAlignmentIdx, newAlignmentIdx) or None if no change else: self.positions = positions self.velocities = velocities @@ -1253,65 +1257,65 @@ def updatePositions(self): inter, self.positions, self.velocities = MovingObject.aggregateTrajectories(self.features, self.getTimeInterval()) - def updateCurvilinearPositions(self, method, changeOfAlignment, nextAlignment_idx, timeStep = None, instant = None, previousAlignmentId = None, maxSpeed = None, acceleration = None): + def updateCurvilinearPositions(self, method, instant, timeStep, _nextAlignmentIdx = None, maxSpeed = None, acceleration = None): + '''Update curvilinear position of user at new instant''' if method == 'newell': if self.curvilinearPositions is None: # vehicle without positions, all vehicles should have leader (?) - if self.leader.curvilinearPositions is not None and self.leader.curvilinearPositions.getSCoordAt(-1) > self.dn and len(self.leader.curvilinearPositions) >=2: - leaderSpeed = self.leader.curvilinearVelocities.getSCoordAt(-1) - instantAtX0 = self.tau + instant*timeStep - (self.leader.curvilinearPositions.getSCoordAt(-1)-self.d)/leaderSpeed - if instantAtX0 < obj.initialHeadway: #obj appears at instant initialHeadway at x=0 with desiredSpeed - instantAtX0 = obj.initialHeadway - - firstInstant = int(np.ceil(instantAtX0/timeStep)) + if self.leader is None: + s = (timeStep*instant-self.initialHeadway)*self.desiredSpeed + self.timeInterval = TimeInterval(instant, instant) + self.curvilinearPositions = CurvilinearTrajectory([s], [0.], [self.initialAlignmentIdx]) + self.curvilinearVelocities = CurvilinearTrajectory() + self.instantAtX0 = self.initialHeadway + elif self.leader.curvilinearPositions is not None and self.leader.curvilinearPositions.getSCoordAt(-1) > self.d and len(self.leader.curvilinearPositions) >=2: + firstInstantAfterD = self.leader.getLastInstant() + while self.leader.existsAtInstant(firstInstantAfterD) and self.leader.getCurvilinearPositionAtInstant(firstInstantAfterD-1)[0] > self.d:# find first instant after d + firstInstantAfterD -= 1 # if not recorded position before self.d, we extrapolate linearly from first known position + leaderSpeed = self.leader.getCurvilinearVelocityAtInstant(firstInstantAfterD)[0] + 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 + if self.instantAtX0 < self.initialHeadway: #obj appears at instant initialHeadway at x=0 with desiredSpeed + self.instantAtX0 = self.initialHeadway + if self.instantAtX0 is not None and instant >= self.instantAtX0: + firstInstant = int(ceil(self.instantAtX0/timeStep)) self.timeInterval = TimeInterval(firstInstant, firstInstant) - freeFlowCoord = (firstInstant*timeStep - instantAtX0)*self.desiredSpeed - # constrainedCoord at firstInstant = xn-1(t = firstInstant*timeStep-self.tn)-self.dn - t = firstInstant*timeStep-self.tn + freeFlowCoord = (firstInstant*timeStep - self.instantAtX0)*self.desiredSpeed + # constrainedCoord at firstInstant = xn-1(t = firstInstant*timeStep-self.tau)-self.d + t = firstInstant*timeStep-self.tau i = int(floor(t/timeStep)) leaderSpeed = self.leader.getCurvilinearVelocityAtInstant(i)[0] - constrainedCoord = self.leader.getCurvilinearPositionAtInstant(i)[0]+leaderSpeed*(t-i*timeStep)-self.dn - obj.curvilinearPositions = moving.CurvilinearTrajectory([min(freeFlowCoord, constrainedCoord)], [0.], [nextAlignment_idx])# TODO verify initial alignment index - obj.curvilinearVelocities = moving.CurvilinearTrajectory() + constrainedCoord = self.leader.getCurvilinearPositionAtInstant(i)[0]+leaderSpeed*(t-i*timeStep)-self.d + self.curvilinearPositions = CurvilinearTrajectory([min(freeFlowCoord, constrainedCoord)], [0.], [self.initialAlignmentIdx]) + self.curvilinearVelocities = CurvilinearTrajectory() + print(firstInstant+1, instant+1) for i in range(firstInstant+1, instant+1): - freeFlowCoord = timeStep*self.desiredSpeed + self.curvilinearPositions.getSCoordAt(-1) - #constrainedCoord = - - # if instant <= self.timeInterval[0] < instant + timeStep: - # # #si t < instant de creation du vehicule, la position vaut l'espacement dn entre les deux vehicules - # if leaderVehicle is None: - # self.curvilinearPositions.addPositionSYL( - # -self.dn, - # 0, - # nextAlignment_idx) - # else: - # self.curvilinearPositions.addPositionSYL( - # leaderVehicle.curvilinearPositions[0][0] - self.dn, - # 0, - # nextAlignment_idx) + s1 = self.curvilinearPositions.getSCoordAt(-1) + freeFlowCoord = s1 + timeStep*self.desiredSpeed + constrainedCoord = self.leader.interpolateCurvilinearPositions(i-self.tau/timeStep)[0]-self.d + s2 = min(freeFlowCoord, constrainedCoord) + self.curvilinearPositions.addPositionSYL(s2, 0., self.initialAlignmentIdx) + self.setLastInstant(i) + print(i, self.timeInterval) + self.curvilinearVelocities.addPositionSYL(s2-s1, 0., None) else: - freeFlowCoord = [self.curvilinearPositions[-1][0]+self.desiredSpeed*timeStep, 0, nextAlignment_idx] - if self.leader is None: - self.curvilinearPositions.addPositionSYL(*freeFlowCoord) + if _nextAlignmentIdx is not None: + laneChange = (self.curvilinearPositions.getLaneAt(-1), _nextAlignmentIdx) + nextAlignmentIdx = _nextAlignmentIdx else: - # if leader coord unknown at t-dn, no movement - if instant > self.reactionTime: - previousVehicleCurvilinearPositionAtPrecedentInstant = leaderVehicle.curvilinearPositions[-int(round(self.reactionTime))][0] # t-v.reactionTime - else: - previousVehicleCurvilinearPositionAtPrecedentInstant = \ - leaderVehicle.curvilinearPositions[0][0] - - self.curvilinearPositions.addPositionSYL(previousVehicleCurvilinearPositionAtPrecedentInstant - self.dn, 0, nextAlignment_idx) - - #mise ajour des vitesses - if changeOfAlignment: - self.curvilinearVelocities.addPositionSYL((self.curvilinearPositions[-1][0]-self.curvilinearPositions[-2][0])/timeStep, - (self.curvilinearPositions[-1][1]-self.curvilinearPositions[-2][1])/timeStep, - (previousAlignmentId, nextAlignment_idx)) + laneChange = None + nextAlignmentIdx = self.curvilinearPositions.getLaneAt(-1) + s1 = self.curvilinearPositions.getSCoordAt(-1) + freeFlowCoord = s1 + self.desiredSpeed*timeStep + if self.leader is None: + if self.getLastInstant() < instant: + s2 = freeFlowCoord + self.curvilinearPositions.addPositionSYL(freeFlowCoord, 0., nextAlignmentIdx) else: - self.curvilinearVelocities.addPositionSYL((self.curvilinearPositions[-1][0]-self.curvilinearPositions[-2][0])/timeStep, - (self.curvilinearPositions[-1][1]-self.curvilinearPositions[-2][1])/timeStep, - None) + constrainedCoord = self.leader.interpolateCurvilinearPositions(instant-self.tau/timeStep)[0]-self.d + s2 = min(freeFlowCoord, constrainedCoord) + self.curvilinearPositions.addPositionSYL(s2, 0., nextAlignmentIdx) + self.setLastInstant(instant) + self.curvilinearVelocities.addPositionSYL(s2-s1, 0., laneChange) @staticmethod def concatenate(obj1, obj2, num = None, newFeatureNum = None, computePositions = False):