SUMO - Simulation of Urban MObility
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
MSLaneChanger.cpp
Go to the documentation of this file.
1 /****************************************************************************/
11 // Performs lane changing of vehicles
12 /****************************************************************************/
13 // SUMO, Simulation of Urban MObility; see http://sumo-sim.org/
14 // Copyright (C) 2001-2013 DLR (http://www.dlr.de/) and contributors
15 /****************************************************************************/
16 //
17 // This file is part of SUMO.
18 // SUMO is free software: you can redistribute it and/or modify
19 // it under the terms of the GNU General Public License as published by
20 // the Free Software Foundation, either version 3 of the License, or
21 // (at your option) any later version.
22 //
23 /****************************************************************************/
24 
25 // ===========================================================================
26 // included modules
27 // ===========================================================================
28 #ifdef _MSC_VER
29 #include <windows_config.h>
30 #else
31 #include <config.h>
32 #endif
33 
34 #include "MSLaneChanger.h"
35 #include "MSVehicle.h"
36 #include "MSVehicleType.h"
37 #include "MSVehicleTransfer.h"
38 #include "MSGlobals.h"
39 #include <cassert>
40 #include <iterator>
41 #include <cstdlib>
42 #include <cmath>
45 
46 #ifdef CHECK_MEMORY_LEAKS
47 #include <foreign/nvwa/debug_new.h>
48 #endif // CHECK_MEMORY_LEAKS
49 
50 //#define DEBUG_VEHICLE_GUI_SELECTION 1
51 #ifdef DEBUG_VEHICLE_GUI_SELECTION
53 #include <guisim/GUIVehicle.h>
54 #include <guisim/GUILane.h>
55 #endif
56 
57 
58 // ===========================================================================
59 // member method definitions
60 // ===========================================================================
61 MSLaneChanger::MSLaneChanger(std::vector<MSLane*>* lanes, bool allowSwap)
62  : myAllowsSwap(allowSwap) {
63  assert(lanes->size() > 1);
64 
65  // Fill the changer with the lane-data.
66  myChanger.reserve(lanes->size());
67  for (std::vector<MSLane*>::iterator lane = lanes->begin(); lane != lanes->end(); ++lane) {
68  ChangeElem ce;
69  ce.follow = 0;
70  ce.lead = 0;
71  ce.lane = *lane;
72  ce.veh = (*lane)->myVehicles.rbegin();
73  ce.hoppedVeh = 0;
74  ce.lastBlocked = 0;
75  myChanger.push_back(ce);
76  }
77 }
78 
79 
81 
82 
83 void
85  // This is what happens in one timestep. After initialization of the
86  // changer, each vehicle will try to change. After that the changer
87  // nedds an update to prevent multiple changes of one vehicle.
88  // Finally, the change-result has to be given back to the lanes.
89  initChanger();
90  while (vehInChanger()) {
91 
92  bool haveChanged = change();
93  updateChanger(haveChanged);
94  }
95  updateLanes(t);
96 }
97 
98 
99 void
101  // Prepare myChanger with a safe state.
102  for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
103  ce->lead = 0;
104  ce->hoppedVeh = 0;
105  ce->lastBlocked = 0;
106  ce->dens = 0;
107 
108  MSLane::VehCont& vehicles = ce->lane->myVehicles;
109  if (vehicles.empty()) {
110  ce->veh = vehicles.rend();
111  ce->follow = 0;
112  continue;
113  }
114  ce->veh = vehicles.rbegin();
115  if (vehicles.size() == 1) {
116  ce->follow = 0;
117  continue;
118  }
119  ce->follow = *(vehicles.rbegin() + 1);
120  }
121 }
122 
123 
124 bool
126  // Find change-candidate. If it is on an allowed lane, try to change
127  // to the right (there is a rule in Germany that you have to change
128  // to the right, unless you are overtaking). If change to the right
129  // isn't possible, check if there is a possibility to overtake (on the
130  // left.
131  // If candidate isn't on an allowed lane, changing to an allowed has
132  // priority.
134  MSVehicle* vehicle = veh(myCandi);
135 #ifdef DEBUG_VEHICLE_GUI_SELECTION
136  if (gSelected.isSelected(GLO_VEHICLE, static_cast<const GUIVehicle*>(vehicle)->getGlID())) {
137  int bla = 0;
138  }
139 #endif
140  if (vehicle->getLane() != (*myCandi).lane || vehicle->getLaneChangeModel().isChangingLanes()) {
141  // vehicles shadows and changing vehicles are not eligible
142  registerUnchanged(vehicle);
143  return false;
144  }
145 #ifndef NO_TRACI
146  if (vehicle->hasInfluencer() && vehicle->getInfluencer().isVTDControlled()) {
147  return false; // !!! temporary; just because it broke, here
148  }
149 #endif
150  const std::vector<MSVehicle::LaneQ>& preb = vehicle->getBestLanes();
151  assert(preb.size() == myChanger.size());
152  for (int i = 0; i < (int) myChanger.size(); ++i) {
153  ((std::vector<MSVehicle::LaneQ>&) preb)[i].occupation = myChanger[i].dens + preb[i].nextOccupation;
154  }
155 
156  vehicle->getLaneChangeModel().prepareStep();
157  std::pair<MSVehicle* const, SUMOReal> leader = getRealThisLeader(myCandi);
158  // check whether the vehicle wants and is able to change to right lane
159  int state1 = 0;
160  if (myCandi != myChanger.begin() && (myCandi - 1)->lane->allowsVehicleClass(veh(myCandi)->getVehicleType().getVehicleClass())) {
161  std::pair<MSVehicle* const, SUMOReal> rLead = getRealLeader(myCandi - 1);
162  std::pair<MSVehicle* const, SUMOReal> rFollow = getRealFollower(myCandi - 1);
163  state1 = change2right(leader, rLead, rFollow, preb);
164  if ((state1 & LCA_URGENT) != 0 || (state1 & LCA_SPEEDGAIN) != 0) {
165  state1 |= LCA_RIGHT;
166  }
167  bool changingAllowed1 = (state1 & LCA_BLOCKED) == 0;
168  // change if the vehicle wants to and is allowed to change
169  if ((state1 & LCA_RIGHT) != 0 && changingAllowed1) {
170 #ifndef NO_TRACI
171  // inform lane change model about this change
173 #endif
174  startChange(vehicle, myCandi, -1);
175  return true;
176  }
177  if ((state1 & LCA_RIGHT) != 0 && (state1 & LCA_URGENT) != 0) {
178  (myCandi - 1)->lastBlocked = vehicle;
179  }
180  }
181 
182 
183 
184  // check whether the vehicle wants and is able to change to left lane
185  int state2 = 0;
186  if ((myCandi + 1) != myChanger.end() && (myCandi + 1)->lane->allowsVehicleClass(veh(myCandi)->getVehicleType().getVehicleClass())) {
187  std::pair<MSVehicle* const, SUMOReal> lLead = getRealLeader(myCandi + 1);
188  std::pair<MSVehicle* const, SUMOReal> lFollow = getRealFollower(myCandi + 1);
189  state2 = change2left(leader, lLead, lFollow, preb);
190  if ((state2 & LCA_URGENT) != 0 || (state2 & LCA_SPEEDGAIN) != 0) {
191  state2 |= LCA_LEFT;
192  }
193  bool changingAllowed2 = (state2 & LCA_BLOCKED) == 0;
194  //vehicle->getLaneChangeModel().setOwnState(state2|state1);
195  // change if the vehicle wants to and is allowed to change
196  if ((state2 & LCA_LEFT) != 0 && changingAllowed2) {
197 #ifndef NO_TRACI
198  // inform lane change model about this change
200 #endif
201  startChange(vehicle, myCandi, 1);
202  return true;
203  }
204  if ((state2 & LCA_LEFT) != 0 && (state2 & LCA_URGENT) != 0) {
205  (myCandi + 1)->lastBlocked = vehicle;
206  }
207  }
208  vehicle->getLaneChangeModel().setOwnState(state2 | state1);
209 
210  if ((state1 & (LCA_URGENT)) != 0 && (state2 & (LCA_URGENT)) != 0) {
211  // ... wants to go to the left AND to the right
212  // just let them go to the right lane...
213  state2 = 0;
214  vehicle->getLaneChangeModel().setOwnState(state1);
215  }
216  // check whether the vehicles should be swapped
217  if (myAllowsSwap && ((state1 & (LCA_URGENT)) != 0 || (state2 & (LCA_URGENT)) != 0)) {
218  // get the direction ...
219  ChangerIt target;
220  if ((state1 & (LCA_URGENT)) != 0) {
221  // ... wants to go right
222  target = myCandi - 1;
223  }
224  if ((state2 & (LCA_URGENT)) != 0) {
225  // ... wants to go left
226  target = myCandi + 1;
227  }
228  MSVehicle* prohibitor = target->lead;
229  if (target->hoppedVeh != 0) {
230  SUMOReal hoppedPos = target->hoppedVeh->getPositionOnLane();
231  if (prohibitor == 0 || (hoppedPos > vehicle->getPositionOnLane() && prohibitor->getPositionOnLane() > hoppedPos)) {
232  prohibitor = 0;// !!! vehicles should not jump over more than one lanetarget->hoppedVeh;
233  }
234  }
235  if (prohibitor != 0
236  &&
237  ((prohibitor->getLaneChangeModel().getOwnState() & (LCA_URGENT/*|LCA_SPEEDGAIN*/)) != 0
238  &&
239  (prohibitor->getLaneChangeModel().getOwnState() & (LCA_LEFT | LCA_RIGHT))
240  !=
241  (vehicle->getLaneChangeModel().getOwnState() & (LCA_LEFT | LCA_RIGHT))
242  )
243  ) {
244 
245  // check for position and speed
246  if (prohibitor->getVehicleType().getLengthWithGap() - vehicle->getVehicleType().getLengthWithGap() == 0) {
247  // ok, may be swapped
248  // remove vehicle to swap with
249  MSLane::VehCont::iterator i = find(target->lane->myTmpVehicles.begin(), target->lane->myTmpVehicles.end(), prohibitor);
250  if (i != target->lane->myTmpVehicles.end()) {
251  MSVehicle* bla = *i;
252  assert(bla == prohibitor);
253  target->lane->myTmpVehicles.erase(i);
254  // set this vehicle
255  target->hoppedVeh = vehicle;
256  target->lane->myTmpVehicles.insert(target->lane->myTmpVehicles.begin(), vehicle);
257  myCandi->hoppedVeh = prohibitor;
258  myCandi->lane->myTmpVehicles.insert(myCandi->lane->myTmpVehicles.begin(), prohibitor);
259 
260  // leave lane and detectors
263  // patch position and speed
264  SUMOReal p1 = vehicle->getPositionOnLane();
265  vehicle->myState.myPos = prohibitor->myState.myPos;
266  prohibitor->myState.myPos = p1;
267  p1 = vehicle->getSpeed();
268  vehicle->myState.mySpeed = prohibitor->myState.mySpeed;
269  prohibitor->myState.mySpeed = p1;
270  // enter lane and detectors
271  vehicle->enterLaneAtLaneChange(target->lane);
272  prohibitor->enterLaneAtLaneChange(myCandi->lane);
273  // mark lane change
274  vehicle->getLaneChangeModel().changed();
275  prohibitor->getLaneChangeModel().changed();
276  (myCandi)->dens += prohibitor->getVehicleType().getLengthWithGap();
277  (target)->dens += vehicle->getVehicleType().getLengthWithGap();
278  return true;
279  }
280  }
281  }
282  }
283  registerUnchanged(vehicle);
284  return false;
285 }
286 
287 
288 void
290  myCandi->lane->myTmpVehicles.insert(myCandi->lane->myTmpVehicles.begin(), veh(myCandi));
291  vehicle->getLaneChangeModel().unchanged();
292  (myCandi)->dens += vehicle->getVehicleType().getLengthWithGap();
293 }
294 
295 
296 void
297 MSLaneChanger::startChange(MSVehicle* vehicle, ChangerIt& from, int direction) {
298  ChangerIt to = from + direction;
299  to->hoppedVeh = vehicle;
300  // @todo delay entering the target lane until the vehicle intersects it
301  // physically (considering lane width and vehicle width)
302  to->lane->myTmpVehicles.insert(to->lane->myTmpVehicles.begin(), vehicle);
303  const bool continuous = vehicle->getLaneChangeModel().startLaneChangeManeuver(from->lane, to->lane, direction);
304  if (continuous) {
305  from->lane->myTmpVehicles.insert(from->lane->myTmpVehicles.begin(), vehicle);
306  from->dens += vehicle->getVehicleType().getLengthWithGap();
307  }
308  to->dens += to->hoppedVeh->getVehicleType().getLengthWithGap();
309 }
310 
311 
312 std::pair<MSVehicle* const, SUMOReal>
314  // get the leading vehicle on the lane to change to
315  MSVehicle* leader = target->lead;
316  if (leader == 0) {
317  MSLane* targetLane = target->lane;
318  MSVehicle* predP = targetLane->getPartialOccupator();
319  if (predP != 0) {
320  return std::pair<MSVehicle*, SUMOReal>(predP, targetLane->getPartialOccupatorEnd() - veh(myCandi)->getPositionOnLane());
321  }
322  const std::vector<MSLane*>& bestLaneConts = veh(myCandi)->getBestLanesContinuation();
323  MSLinkCont::const_iterator link = targetLane->succLinkSec(*veh(myCandi), 1, *targetLane, bestLaneConts);
324  if (targetLane->isLinkEnd(link)) {
325  return std::pair<MSVehicle*, SUMOReal>(static_cast<MSVehicle*>(0), -1);
326  }
327  MSLane* nextLane = (*link)->getLane();
328  if (nextLane == 0) {
329  return std::pair<MSVehicle*, SUMOReal>(static_cast<MSVehicle*>(0), -1);
330  }
331  leader = nextLane->getLastVehicle();
332  if (leader == 0) {
333  return std::pair<MSVehicle*, SUMOReal>(static_cast<MSVehicle*>(0), -1);
334  }
335  SUMOReal gap =
336  leader->getPositionOnLane() - leader->getVehicleType().getLength()
337  +
338  (myCandi->lane->getLength() - veh(myCandi)->getPositionOnLane() - veh(myCandi)->getVehicleType().getMinGap()); // !!! recheck
339  return std::pair<MSVehicle* const, SUMOReal>(leader, MAX2((SUMOReal) 0, gap));
340  } else {
341  MSVehicle* candi = veh(myCandi);
342  SUMOReal gap = leader->getPositionOnLane() - leader->getVehicleType().getLength() - candi->getPositionOnLane() - candi->getVehicleType().getMinGap();
343  return std::pair<MSVehicle* const, SUMOReal>(leader, MAX2((SUMOReal) 0, gap));
344  }
345 }
346 
347 
348 std::pair<MSVehicle* const, SUMOReal>
350  // get the leading vehicle on the lane to change to
351  MSVehicle* neighLead = target->lead;
352  // check whether the hopped vehicle got the leader
353  if (target->hoppedVeh != 0) {
354  SUMOReal hoppedPos = target->hoppedVeh->getPositionOnLane();
355  if (hoppedPos > veh(myCandi)->getPositionOnLane() && (neighLead == 0 || neighLead->getPositionOnLane() > hoppedPos)) {
356  neighLead = target->hoppedVeh;
357  }
358  }
359  if (neighLead == 0) {
360  MSLane* targetLane = target->lane;
361  MSVehicle* predP = targetLane->getPartialOccupator();
362  if (predP != 0) {
363  return std::pair<MSVehicle*, SUMOReal>(predP, targetLane->getPartialOccupatorEnd() - veh(myCandi)->getPositionOnLane() - veh(myCandi)->getVehicleType().getMinGap());
364  }
365  SUMOReal seen = myCandi->lane->getLength() - veh(myCandi)->getPositionOnLane();
366  SUMOReal speed = veh(myCandi)->getSpeed();
368  if (seen > dist) {
369  return std::pair<MSVehicle* const, SUMOReal>(static_cast<MSVehicle*>(0), -1);
370  }
371  const std::vector<MSLane*>& bestLaneConts = veh(myCandi)->getBestLanesContinuation(targetLane);
372  return target->lane->getLeaderOnConsecutive(dist, seen, speed, *veh(myCandi), bestLaneConts);
373  } else {
374  MSVehicle* candi = veh(myCandi);
375  return std::pair<MSVehicle* const, SUMOReal>(neighLead, neighLead->getPositionOnLane() - neighLead->getVehicleType().getLength() - candi->getPositionOnLane() - candi->getVehicleType().getMinGap());
376  }
377 }
378 
379 
380 std::pair<MSVehicle* const, SUMOReal>
382  MSVehicle* neighFollow = veh(target);
383  // check whether the hopped vehicle got the follower
384  if (target->hoppedVeh != 0) {
385  SUMOReal hoppedPos = target->hoppedVeh->getPositionOnLane();
386  if (hoppedPos <= veh(myCandi)->getPositionOnLane() && (neighFollow == 0 || neighFollow->getPositionOnLane() > hoppedPos)) {
387  neighFollow = target->hoppedVeh;
388  }
389  }
390  if (neighFollow == 0) {
391  SUMOReal speed = target->lane->getSpeedLimit();
392  // in order to look back, we'd need the minimum braking ability of vehicles in the net...
393  // we'll assume it to be 4m/s^2
394  // !!!revisit
395  SUMOReal dist = speed * speed / (2.*4.) + SPEED2DIST(speed);
396  dist = MIN2(dist, (SUMOReal) 500.);
397  MSVehicle* candi = veh(myCandi);
398  SUMOReal seen = candi->getPositionOnLane() - candi->getVehicleType().getLength();
399  return target->lane->getFollowerOnConsecutive(dist, seen, candi->getSpeed(), candi->getPositionOnLane() - candi->getVehicleType().getLength(), 4.5);
400  } else {
401  MSVehicle* candi = veh(myCandi);
402  return std::pair<MSVehicle* const, SUMOReal>(neighFollow, candi->getPositionOnLane() - candi->getVehicleType().getLength() - neighFollow->getPositionOnLane() - neighFollow->getVehicleType().getMinGap());
403  }
404 }
405 
406 
407 
408 
409 void
410 MSLaneChanger::updateChanger(bool vehHasChanged) {
411  assert(myCandi->veh != myCandi->lane->myVehicles.rend());
412 
413  // "Push" the vehicles to the back, i.e. follower becomes vehicle,
414  // vehicle becomes leader, and leader becomes predecessor of vehicle,
415  // if it exists.
416  if (!vehHasChanged) {
417  myCandi->lead = veh(myCandi);
418  }
419  myCandi->veh = myCandi->veh + 1;
420 
421  if (veh(myCandi) == 0) {
422  assert(myCandi->follow == 0);
423  // leader already 0.
424  return;
425  }
426  if (myCandi->veh + 1 == myCandi->lane->myVehicles.rend()) {
427  myCandi->follow = 0;
428  } else {
429  myCandi->follow = *(myCandi->veh + 1);
430  }
431  return;
432 }
433 
434 
435 void
437 
438  // Update the lane's vehicle-container.
439  // First: it is bad style to change other classes members, but for
440  // this release, other attempts were too time-consuming. In a next
441  // release we will change from this lane-centered design to a vehicle-
442  // centered. This will solve many problems.
443  // Second: this swap would be faster if vehicle-containers would have
444  // been pointers, but then I had to change too much of the MSLane code.
445  for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
446 
447  ce->lane->swapAfterLaneChange(t);
448  }
449 }
450 
451 
454  // Find the vehicle in myChanger with the smallest position. If there
455  // is no vehicle in myChanger (shouldn't happen) , return
456  // myChanger.end().
457  ChangerIt max = myChanger.end();
458  for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
459  if (veh(ce) == 0) {
460  continue;
461  }
462  if (max == myChanger.end()) {
463  max = ce;
464  continue;
465  }
466  assert(veh(ce) != 0);
467  assert(veh(max) != 0);
468  if (veh(max)->getPositionOnLane() < veh(ce)->getPositionOnLane()) {
469  max = ce;
470  }
471  }
472  assert(max != myChanger.end());
473  assert(veh(max) != 0);
474  return max;
475 }
476 
477 
478 int
479 MSLaneChanger::change2right(const std::pair<MSVehicle* const, SUMOReal>& leader,
480  const std::pair<MSVehicle* const, SUMOReal>& rLead,
481  const std::pair<MSVehicle* const, SUMOReal>& rFollow,
482  const std::vector<MSVehicle::LaneQ>& preb) const {
483  ChangerIt target = myCandi - 1;
484  int blocked = overlapWithHopped(target)
485  ? target->hoppedVeh->getPositionOnLane() < veh(myCandi)->getPositionOnLane()
488  : 0;
489  // overlap
490  if (rFollow.first != 0 && rFollow.second < 0) {
491  blocked |= (LCA_BLOCKED_BY_RIGHT_FOLLOWER);
492  }
493  if (rLead.first != 0 && rLead.second < 0) {
494  blocked |= (LCA_BLOCKED_BY_RIGHT_LEADER);
495  }
496  // safe back gap
497  if (rFollow.first != 0) {
498  // !!! eigentlich: vsafe braucht die Max. Geschwindigkeit beider Spuren
499  if (rFollow.second < rFollow.first->getCarFollowModel().getSecureGap(rFollow.first->getSpeed(), veh(myCandi)->getSpeed(), veh(myCandi)->getCarFollowModel().getMaxDecel())) {
501  }
502  }
503 
504  // safe front gap
505  if (rLead.first != 0) {
506  // !!! eigentlich: vsafe braucht die Max. Geschwindigkeit beider Spuren
507  if (rLead.second < veh(myCandi)->getCarFollowModel().getSecureGap(veh(myCandi)->getSpeed(), rLead.first->getSpeed(), rLead.first->getCarFollowModel().getMaxDecel())) {
508  blocked |= LCA_BLOCKED_BY_RIGHT_LEADER;
509  }
510  }
511 
512  MSAbstractLaneChangeModel::MSLCMessager msg(leader.first, rLead.first, rFollow.first);
513  return blocked | veh(myCandi)->getLaneChangeModel().wantsChangeToRight(
514  msg, blocked, leader, rLead, rFollow, *(myCandi - 1)->lane, preb, &(myCandi->lastBlocked));
515 }
516 
517 
518 int
519 MSLaneChanger::change2left(const std::pair<MSVehicle* const, SUMOReal>& leader,
520  const std::pair<MSVehicle* const, SUMOReal>& rLead,
521  const std::pair<MSVehicle* const, SUMOReal>& rFollow,
522  const std::vector<MSVehicle::LaneQ>& preb) const {
523  ChangerIt target = myCandi + 1;
524  int blocked = overlapWithHopped(target)
525  ? target->hoppedVeh->getPositionOnLane() < veh(myCandi)->getPositionOnLane()
528  : 0;
529  // overlap
530  if (rFollow.first != 0 && rFollow.second < 0) {
531  blocked |= (LCA_BLOCKED_BY_LEFT_FOLLOWER);
532  }
533  if (rLead.first != 0 && rLead.second < 0) {
534  blocked |= (LCA_BLOCKED_BY_LEFT_LEADER);
535  }
536  // safe back gap
537  if (rFollow.first != 0) {
538  // !!! eigentlich: vsafe braucht die Max. Geschwindigkeit beider Spuren
539  if (rFollow.second < rFollow.first->getCarFollowModel().getSecureGap(rFollow.first->getSpeed(), veh(myCandi)->getSpeed(), veh(myCandi)->getCarFollowModel().getMaxDecel())) {
540  blocked |= LCA_BLOCKED_BY_LEFT_FOLLOWER;
541  }
542  }
543  // safe front gap
544  if (rLead.first != 0) {
545  // !!! eigentlich: vsafe braucht die Max. Geschwindigkeit beider Spuren
546  if (rLead.second < veh(myCandi)->getCarFollowModel().getSecureGap(veh(myCandi)->getSpeed(), rLead.first->getSpeed(), rLead.first->getCarFollowModel().getMaxDecel())) {
547  blocked |= LCA_BLOCKED_BY_LEFT_LEADER;
548  }
549  }
550  MSAbstractLaneChangeModel::MSLCMessager msg(leader.first, rLead.first, rFollow.first);
551  return blocked | veh(myCandi)->getLaneChangeModel().wantsChangeToLeft(
552  msg, blocked, leader, rLead, rFollow, *(myCandi + 1)->lane, preb, &(myCandi->lastBlocked));
553 }
554 
555 
556 
557 
558 /****************************************************************************/
559