SUMO - Simulation of Urban MObility
MSLaneChanger.cpp
Go to the documentation of this file.
1 /****************************************************************************/
2 // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3 // Copyright (C) 2002-2017 German Aerospace Center (DLR) and others.
4 /****************************************************************************/
5 //
6 // This program and the accompanying materials
7 // are made available under the terms of the Eclipse Public License v2.0
8 // which accompanies this distribution, and is available at
9 // http://www.eclipse.org/legal/epl-v20.html
10 //
11 /****************************************************************************/
22 // Performs lane changing of vehicles
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 "MSNet.h"
36 #include "MSVehicle.h"
37 #include "MSVehicleType.h"
38 #include "MSVehicleTransfer.h"
39 #include "MSGlobals.h"
40 #include <cassert>
41 #include <iterator>
42 #include <cstdlib>
43 #include <cmath>
46 
47 #define OPPOSITE_OVERTAKING_SAFE_TIMEGAP 0.0
48 // XXX maxLookAhead should be higher if all leaders are stopped and lower when they are jammed/queued
49 #define OPPOSITE_OVERTAKING_MAX_LOOKAHEAD 150.0 // just a guess
50 #define OPPOSITE_OVERTAKING_MAX_LOOKAHEAD_EMERGENCY 1000.0 // just a guess
51 // this is used for finding oncoming vehicles while driving in the opposite direction
52 #define OPPOSITE_OVERTAKING_ONCOMING_LOOKAHEAD 200.0 // just a guess
53 
54 // ===========================================================================
55 // debug defines
56 // ===========================================================================
57 
58 //#define DEBUG_CONTINUE_CHANGE
59 //#define DEBUG_CHECK_CHANGE
60 //#define DEBUG_SURROUNDING_VEHICLES // debug getRealFollower() and getRealLeader()
61 //#define DEBUG_CHANGE_OPPOSITE
62 //#define DEBUG_ACTIONSTEPS
63 //#define DEBUG_STATE
64 //#define DEBUG_CANDIDATE
65 //#define DEBUG_COND (vehicle->getLaneChangeModel().debugVehicle())
66 #define DEBUG_COND (vehicle->isSelected())
67 
68 
69 
70 // ===========================================================================
71 // ChangeElem member method definitions
72 // ===========================================================================
74  lead(0),
75  lane(_lane),
76  hoppedVeh(0),
77  lastBlocked(0),
78  firstBlocked(0),
79  ahead(lane),
80  aheadNext(lane, 0, 0) {
81 }
82 
83 void
85  lane->myTmpVehicles.insert(lane->myTmpVehicles.begin(), vehicle);
86  dens += vehicle->getVehicleType().getLengthWithGap();
87  hoppedVeh = vehicle;
88 }
89 
90 
91 // ===========================================================================
92 // member method definitions
93 // ===========================================================================
94 MSLaneChanger::MSLaneChanger(const std::vector<MSLane*>* lanes, bool allowChanging) :
95  myAllowsChanging(allowChanging),
96  myChangeToOpposite(lanes->front()->getEdge().canChangeToOpposite()) {
97 
98  // Fill the changer with the lane-data.
99  myChanger.reserve(lanes->size());
100  for (std::vector<MSLane*>::const_iterator lane = lanes->begin(); lane != lanes->end(); ++lane) {
101  myChanger.push_back(ChangeElem(*lane));
102  myChanger.back().mayChangeRight = lane != lanes->begin();
103  myChanger.back().mayChangeLeft = (lane + 1) != lanes->end();
104  // avoid changing on internal sibling lane
105  if ((*lane)->isInternal()) {
106  if (myChanger.back().mayChangeRight && (*lane)->getLogicalPredecessorLane() == (*(lane - 1))->getLogicalPredecessorLane()) {
107  myChanger.back().mayChangeRight = false;
108  }
109  if (myChanger.back().mayChangeLeft && (*lane)->getLogicalPredecessorLane() == (*(lane + 1))->getLogicalPredecessorLane()) {
110  myChanger.back().mayChangeLeft = false;
111  }
112  }
113  }
114 }
115 
116 
118 }
119 
120 
121 void
123  // This is what happens in one timestep. After initialization of the
124  // changer, each vehicle will try to change. After that the changer
125  // needs an update to prevent multiple changes of one vehicle.
126  // Finally, the change-result has to be given back to the lanes.
127  initChanger();
128  try {
129  while (vehInChanger()) {
130  const bool haveChanged = change();
131  updateChanger(haveChanged);
132  }
133  updateLanes(t);
134  } catch (const ProcessError&) {
135  // clean up locks or the gui may hang
136  for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
137  ce->lane->releaseVehicles();
138  }
139  throw;
140  }
141 }
142 
143 
144 void
146  // Prepare myChanger with a safe state.
147  for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
148  ce->lead = 0;
149  ce->hoppedVeh = 0;
150  ce->lastBlocked = 0;
151  ce->firstBlocked = 0;
152  ce->dens = 0;
153  ce->lane->getVehiclesSecure();
154 
155  //std::cout << SIMTIME << " initChanger lane=" << ce->lane->getID() << " vehicles=" << toString(ce->lane->myVehicles) << "\n";
156  }
157 }
158 
159 
160 void
161 MSLaneChanger::updateChanger(bool vehHasChanged) {
162  assert(veh(myCandi) != 0);
163 
164  // "Push" the vehicles to the back, i.e. follower becomes vehicle,
165  // vehicle becomes leader, and leader becomes predecessor of vehicle,
166  // if it exists.
167  if (!vehHasChanged || MSGlobals::gLaneChangeDuration > DELTA_T) {
168  //std::cout << SIMTIME << " updateChanger: lane=" << myCandi->lane->getID() << " has new lead=" << veh(myCandi)->getID() << "\n";
169  myCandi->lead = veh(myCandi);
170  }
171 
172  MSLane::VehCont& vehicles = myCandi->lane->myVehicles;
173  vehicles.pop_back();
174  //std::cout << SIMTIME << " updateChanger lane=" << myCandi->lane->getID() << " vehicles=" << toString(myCandi->lane->myVehicles) << "\n";
175 }
176 
177 
178 void
180 
181  // Update the lane's vehicle-container.
182  // First: it is bad style to change other classes members, but for
183  // this release, other attempts were too time-consuming. In a next
184  // release we will change from this lane-centered design to a vehicle-
185  // centered. This will solve many problems.
186  // Second: this swap would be faster if vehicle-containers would have
187  // been pointers, but then I had to change too much of the MSLane code.
188  for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
189  //std::cout << SIMTIME << " updateLanes lane=" << ce->lane->getID() << " myVehicles=" << toString(ce->lane->myVehicles) << " myTmpVehicles=" << toString(ce->lane->myTmpVehicles) << "\n";
190  ce->lane->swapAfterLaneChange(t);
191  ce->lane->releaseVehicles();
192  }
193 }
194 
195 
198  // Find the vehicle in myChanger with the largest position. If there
199  // is no vehicle in myChanger (shouldn't happen) , return myChanger.end().
200  ChangerIt max = myChanger.end();
201 #ifdef DEBUG_CANDIDATE
202  std::cout << SIMTIME << " findCandidate() on edge " << myChanger.begin()->lane->getEdge().getID() << std::endl;
203 #endif
204 
205  for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
206  if (veh(ce) == 0) {
207  continue;
208  }
209 #ifdef DEBUG_CANDIDATE
210  std::cout << " lane = " << ce->lane->getID() << "\n";
211  std::cout << " check vehicle=" << veh(ce)->getID() << " pos=" << veh(ce)->getPositionOnLane() << " lane=" << ce->lane->getID() << " isFrontOnLane=" << veh(ce)->isFrontOnLane(ce->lane) << "\n";
212 #endif
213  if (max == myChanger.end()) {
214 #ifdef DEBUG_CANDIDATE
215  std::cout << " new max vehicle=" << veh(ce)->getID() << " pos=" << veh(ce)->getPositionOnLane() << " lane=" << ce->lane->getID() << " isFrontOnLane=" << veh(ce)->isFrontOnLane(ce->lane) << "\n";
216 #endif
217  max = ce;
218  continue;
219  }
220  assert(veh(ce) != 0);
221  assert(veh(max) != 0);
222  if (veh(max)->getPositionOnLane() < veh(ce)->getPositionOnLane()) {
223 #ifdef DEBUG_CANDIDATE
224  std::cout << " new max vehicle=" << veh(ce)->getID() << " pos=" << veh(ce)->getPositionOnLane() << " lane=" << ce->lane->getID() << " isFrontOnLane=" << veh(ce)->isFrontOnLane(ce->lane) << " oldMaxPos=" << veh(max)->getPositionOnLane() << "\n";
225 #endif
226  max = ce;
227  }
228  }
229  assert(max != myChanger.end());
230  assert(veh(max) != 0);
231  return max;
232 }
233 
234 
235 bool
236 MSLaneChanger::mayChange(int direction) const {
237  if (direction == 0) {
238  return true;
239  }
240  if (!myAllowsChanging) {
241  return false;
242  }
243  if (direction == -1) {
244  return myCandi->mayChangeRight && (myCandi - 1)->lane->allowsVehicleClass(veh(myCandi)->getVehicleType().getVehicleClass());
245  } else if (direction == 1) {
246  return myCandi->mayChangeLeft && (myCandi + 1)->lane->allowsVehicleClass(veh(myCandi)->getVehicleType().getVehicleClass());
247  } else {
248  return false;
249  }
250 }
251 
252 
253 bool
255  // Find change-candidate. If it is on an allowed lane, try to change
256  // to the right (there is a rule in Germany that you have to change
257  // to the right, unless you are overtaking). If change to the right
258  // isn't possible, check if there is a possibility to overtake (on the
259  // left.
260  // If candidate isn't on an allowed lane, changing to an allowed has
261  // priority.
262 
263 #ifdef DEBUG_ACTIONSTEPS
264 // std::cout<< "\nCHANGE" << std::endl;
265 #endif
266 
267 
269  MSVehicle* vehicle = veh(myCandi);
270 
271  if (vehicle->getLaneChangeModel().isChangingLanes()) {
272  return continueChange(vehicle, myCandi);
273  }
274  if (!myAllowsChanging || vehicle->getLaneChangeModel().alreadyChanged() || vehicle->isStoppedOnLane()) {
275  registerUnchanged(vehicle);
276  return false;
277  }
278 
279 
280 #ifndef NO_TRACI
281  if (vehicle->isRemoteControlled()) {
282  registerUnchanged(vehicle);
283  return false;
284  }
285 #endif
286 
287  if (!vehicle->isActive()) {
288 #ifdef DEBUG_ACTIONSTEPS
289  if DEBUG_COND {
290  std::cout << SIMTIME << " veh '" << vehicle->getID() << "' skips regular change checks." << std::endl;
291  }
292 #endif
293  bool changed = false;
294 #ifndef NO_TRACI
295  const int oldstate = vehicle->getLaneChangeModel().getOwnState();
296  // let TraCI influence the wish to change lanes during non-actionsteps
297  checkTraCICommands(vehicle);
298  if (oldstate != vehicle->getLaneChangeModel().getOwnState()) {
299  changed = applyTraCICommands(vehicle);
300  }
301 #endif
302  if (!changed) {
303  registerUnchanged(vehicle);
304  }
305  return changed;
306  }
307 
308  // Check for changes to the opposite lane if vehicle is active
309  std::pair<MSVehicle* const, double> leader = getRealLeader(myCandi);
310  if (myChanger.size() == 1 || vehicle->getLaneChangeModel().isOpposite()) {
311  if (changeOpposite(leader)) {
312  return true;
313  }
314  registerUnchanged(vehicle);
315  return false;
316  }
317 
318  vehicle->updateBestLanes(); // needed?
319  for (int i = 0; i < (int) myChanger.size(); ++i) {
320  vehicle->adaptBestLanesOccupation(i, myChanger[i].dens);
321  }
322 
323  const std::vector<MSVehicle::LaneQ>& preb = vehicle->getBestLanes();
324  // check whether the vehicle wants and is able to change to right lane
325  int stateRight = 0;
326  if (mayChange(-1)) {
327  stateRight = checkChangeWithinEdge(-1, leader, preb);
328  // change if the vehicle wants to and is allowed to change
329  if ((stateRight & LCA_RIGHT) != 0 && (stateRight & LCA_BLOCKED) == 0) {
330  vehicle->getLaneChangeModel().setOwnState(stateRight);
331  startChange(vehicle, myCandi, -1);
332  return true;
333  }
334  if ((stateRight & LCA_RIGHT) != 0 && (stateRight & LCA_URGENT) != 0) {
335  (myCandi - 1)->lastBlocked = vehicle;
336  if ((myCandi - 1)->firstBlocked == 0) {
337  (myCandi - 1)->firstBlocked = vehicle;
338  }
339  }
340  }
341 
342  // check whether the vehicle wants and is able to change to left lane
343  int stateLeft = 0;
344  if (mayChange(1)) {
345  stateLeft = checkChangeWithinEdge(1, leader, preb);
346  // change if the vehicle wants to and is allowed to change
347  if ((stateLeft & LCA_LEFT) != 0 && (stateLeft & LCA_BLOCKED) == 0) {
348  vehicle->getLaneChangeModel().setOwnState(stateLeft);
349  startChange(vehicle, myCandi, 1);
350  return true;
351  }
352  if ((stateLeft & LCA_LEFT) != 0 && (stateLeft & LCA_URGENT) != 0) {
353  (myCandi + 1)->lastBlocked = vehicle;
354  if ((myCandi + 1)->firstBlocked == 0) {
355  (myCandi + 1)->firstBlocked = vehicle;
356  }
357  }
358  }
359 
360  if ((stateRight & LCA_URGENT) != 0 && (stateLeft & LCA_URGENT) != 0) {
361  // ... wants to go to the left AND to the right
362  // just let them go to the right lane...
363  stateLeft = 0;
364  }
365  vehicle->getLaneChangeModel().setOwnState(stateRight | stateLeft);
366 
367  // only emergency vehicles should change to the opposite side on a
368  // multi-lane road
369  if (vehicle->getVehicleType().getVehicleClass() == SVC_EMERGENCY
370  && changeOpposite(leader)) {
371  return true;
372  }
373 
374  registerUnchanged(vehicle);
375  return false;
376 }
377 
378 
379 void
381  myCandi->lane->myTmpVehicles.insert(myCandi->lane->myTmpVehicles.begin(), veh(myCandi));
382  myCandi->dens += vehicle->getVehicleType().getLengthWithGap();
383  vehicle->getLaneChangeModel().unchanged();
384 }
385 
386 
387 
388 void
390 #ifdef DEBUG_STATE
391  const int oldstate = vehicle->getLaneChangeModel().getOwnState();
392 #endif
394 #ifdef DEBUG_STATE
395  if (DEBUG_COND) {
396  const int newstate = vehicle->getLaneChangeModel().getOwnState();
397  std::cout << SIMTIME
398  << " veh=" << vehicle->getID()
399  << " oldState=" << toString((LaneChangeAction) oldstate)
400  << " newState=" << toString((LaneChangeAction) newstate)
401  << ((newstate & LCA_BLOCKED) != 0 ? " (blocked)" : "")
402  << ((newstate & LCA_OVERLAPPING) != 0 ? " (overlap)" : "")
403  << "\n";
404  }
405 #endif
406 }
407 
408 
409 bool
411  // Execute request if not blocked
412  bool changed = false;
413  const int state = vehicle->getLaneChangeModel().getOwnState();
414  const int dir = (state & LCA_RIGHT) != 0 ? -1 : ((state & LCA_LEFT) != 0 ? 1 : 0);
415  const bool execute = dir != 0 && ((state & LCA_BLOCKED) == 0);
416  if (execute) {
417  ChangerIt to = myCandi + dir;
418  bool continuous = vehicle->getLaneChangeModel().startLaneChangeManeuver(myCandi->lane, to->lane, dir);
419  if (continuous) {
420  changed = continueChange(vehicle, myCandi);
421  } else {
422  // insert vehicle into target lane
423  to->registerHop(vehicle);
424  changed = true;
425  }
426  }
427  return changed;
428 }
429 
430 
431 void
432 MSLaneChanger::startChange(MSVehicle* vehicle, ChangerIt& from, int direction) {
433  ChangerIt to = from + direction;
434  // @todo delay entering the target lane until the vehicle intersects it
435  // physically (considering lane width and vehicle width)
436  //if (to->lane->getID() == "beg_1") std::cout << SIMTIME << " startChange to lane=" << to->lane->getID() << " myTmpVehiclesBefore=" << toString(to->lane->myTmpVehicles) << "\n";
437  const bool continuous = vehicle->getLaneChangeModel().startLaneChangeManeuver(from->lane, to->lane, direction);
438  if (continuous) {
439  continueChange(vehicle, myCandi);
440  } else {
441  to->registerHop(vehicle);
442  }
443 }
444 
445 bool
448  const int direction = lcm.getLaneChangeDirection();
449  const bool pastMidpoint = lcm.updateCompletion();
450  vehicle->myState.myPosLat += lcm.getSpeedLat();
452  if (pastMidpoint) {
453  ChangerIt to = from + direction;
454  MSLane* source = myCandi->lane;
455  MSLane* target = to->lane;
456  vehicle->myState.myPosLat -= direction * 0.5 * (source->getWidth() + target->getWidth());
457  lcm.primaryLaneChanged(source, target, direction);
458  to->registerHop(vehicle);
459  } else {
460  from->registerHop(vehicle);
461  }
462  if (!lcm.isChangingLanes()) {
463  vehicle->myState.myPosLat = 0;
464  lcm.endLaneChangeManeuver();
465  }
466  lcm.updateShadowLane();
467  if (lcm.getShadowLane() != 0) {
468  // set as hoppedVeh on the shadow lane so it is found as leader on both lanes
469  ChangerIt shadow = pastMidpoint ? from : from + lcm.getShadowDirection();
470  shadow->hoppedVeh = vehicle;
471  }
472  vehicle->myAngle = vehicle->computeAngle();
473 
474 #ifdef DEBUG_CONTINUE_CHANGE
475  if (DEBUG_COND) {
476  std::cout << SIMTIME
477  << " continueChange veh=" << vehicle->getID()
478  << " from=" << Named::getIDSecure(from->lane)
479  << " dir=" << direction
480  << " pastMidpoint=" << pastMidpoint
481  << " posLat=" << vehicle->getLateralPositionOnLane()
482  //<< " completion=" << lcm.getLaneChangeCompletion()
483  << " shadowLane=" << Named::getIDSecure(lcm.getShadowLane())
484  //<< " shadowHopped=" << Named::getIDSecure(shadow->lane)
485  << "\n";
486  }
487 #endif
488  return pastMidpoint;
489 }
490 
491 
492 std::pair<MSVehicle* const, double>
494  assert(veh(myCandi) != 0);
495  MSVehicle* vehicle = veh(myCandi);
496 #ifdef DEBUG_SURROUNDING_VEHICLES
497  if (DEBUG_COND) {
498  std::cout << SIMTIME << " veh '" << vehicle->getID() << "' looks for leader on lc-target lane '" << target->lane->getID() << "'." << std::endl;
499  }
500 #endif
501  // get the leading vehicle on the lane to change to
502  MSVehicle* neighLead = target->lead;
503 
504 #ifdef DEBUG_SURROUNDING_VEHICLES
505  if (DEBUG_COND) {
506  if (neighLead != 0) {
507  std::cout << "Considering '" << neighLead->getID() << "' at position " << neighLead->getPositionOnLane() << std::endl;
508  }
509  }
510 #endif
511 
512  //if (veh(myCandi)->getID() == "disabled") std::cout << SIMTIME
513  // << " target=" << target->lane->getID()
514  // << " neighLead=" << Named::getIDSecure(neighLead)
515  // << " hopped=" << Named::getIDSecure(target->hoppedVeh)
516  // << " (416)\n";
517  // check whether the hopped vehicle became the leader
518  if (target->hoppedVeh != 0) {
519  double hoppedPos = target->hoppedVeh->getPositionOnLane();
520 #ifdef DEBUG_SURROUNDING_VEHICLES
521  if (DEBUG_COND) {
522  std::cout << "Considering hopped vehicle '" << target->hoppedVeh->getID() << "' at position " << hoppedPos << std::endl;
523  }
524 #endif
525  if (hoppedPos > veh(myCandi)->getPositionOnLane() && (neighLead == 0 || neighLead->getPositionOnLane() > hoppedPos)) {
526  neighLead = target->hoppedVeh;
527  //if (veh(myCandi)->getID() == "flow.21") std::cout << SIMTIME << " neighLead=" << Named::getIDSecure(neighLead) << " (422)\n";
528  }
529  }
530  if (neighLead == 0) {
531 #ifdef DEBUG_SURROUNDING_VEHICLES
532  if (DEBUG_COND) {
533  std::cout << "Looking for leader on consecutive lanes." << std::endl;
534  }
535 #endif
536  // There's no leader on the target lane. Look for leaders on consecutive lanes.
537  // (there might also be partial leaders due to continuous lane changing)
538  MSLane* targetLane = target->lane;
539  const double egoBack = vehicle->getBackPositionOnLane();
540  double leaderBack = targetLane->getLength();
541  for (MSVehicle* pl : targetLane->myPartialVehicles) {
542  double plBack = pl->getBackPositionOnLane(targetLane);
543  if (plBack < leaderBack &&
544  pl->getPositionOnLane(targetLane) + pl->getVehicleType().getMinGap() >= egoBack) {
545  neighLead = pl;
546  leaderBack = plBack;
547  }
548  }
549  if (neighLead != 0) {
550 #ifdef DEBUG_SURROUNDING_VEHICLES
551  if (DEBUG_COND) {
552  std::cout << " found leader=" << neighLead->getID() << " (partial)\n";
553  }
554 #endif
555  return std::pair<MSVehicle*, double>(neighLead, leaderBack - vehicle->getPositionOnLane() - vehicle->getVehicleType().getMinGap());
556  }
557  double seen = myCandi->lane->getLength() - veh(myCandi)->getPositionOnLane();
558  double speed = veh(myCandi)->getSpeed();
559  double dist = veh(myCandi)->getCarFollowModel().brakeGap(speed) + veh(myCandi)->getVehicleType().getMinGap();
560  if (seen > dist) {
561 #ifdef DEBUG_SURROUNDING_VEHICLES
562  if (DEBUG_COND) {
563  std::cout << " found no leader within dist=" << dist << "\n";
564  }
565 #endif
566  return std::pair<MSVehicle* const, double>(static_cast<MSVehicle*>(0), -1);
567  }
568  const std::vector<MSLane*>& bestLaneConts = veh(myCandi)->getBestLanesContinuation(targetLane);
569 
570  std::pair<MSVehicle* const, double> result = target->lane->getLeaderOnConsecutive(dist, seen, speed, *veh(myCandi), bestLaneConts);
571 #ifdef DEBUG_SURROUNDING_VEHICLES
572  if (DEBUG_COND) {
573  std::cout << " found consecutiveLeader=" << Named::getIDSecure(result.first) << "\n";
574  }
575 #endif
576  return result;
577  } else {
578  MSVehicle* candi = veh(myCandi);
579 #ifdef DEBUG_SURROUNDING_VEHICLES
580  if (DEBUG_COND) {
581  std::cout << " found leader=" << neighLead->getID() << "\n";
582  }
583 #endif
584  return std::pair<MSVehicle* const, double>(neighLead, neighLead->getBackPositionOnLane(target->lane) - candi->getPositionOnLane() - candi->getVehicleType().getMinGap());
585  }
586 }
587 
588 
589 std::pair<MSVehicle* const, double>
591  assert(veh(myCandi) != 0);
592 
593 #ifdef DEBUG_SURROUNDING_VEHICLES
594  MSVehicle* vehicle = veh(myCandi);
595  if (DEBUG_COND) {
596  std::cout << SIMTIME << " veh '" << vehicle->getID() << "' looks for follower on lc-target lane '" << target->lane->getID() << "'." << std::endl;
597  }
598 #endif
599  MSVehicle* candi = veh(myCandi);
600  const double candiPos = candi->getPositionOnLane();
601  MSVehicle* neighFollow = veh(target);
602 
603 #ifdef DEBUG_SURROUNDING_VEHICLES
604  if (DEBUG_COND) {
605  if (neighFollow != 0) {
606  std::cout << "veh(target) returns '" << neighFollow->getID() << "' at position " << neighFollow->getPositionOnLane() << std::endl;
607  } else {
608  std::cout << "veh(target) returns none." << std::endl;
609  }
610  }
611 #endif
612 
613 
614 #ifdef DEBUG_SURROUNDING_VEHICLES
615  if (DEBUG_COND) {
616  if (getCloserFollower(candiPos, neighFollow, target->hoppedVeh) != neighFollow) {
617  std::cout << "Hopped vehicle '" << target->hoppedVeh->getID() << "' at position " << target->hoppedVeh->getPositionOnLane() << " is closer." << std::endl;
618  }
619  }
620 #endif
621 
622  // check whether the hopped vehicle became the follower
623  neighFollow = getCloserFollower(candiPos, neighFollow, target->hoppedVeh);
624 
625 
626 #ifdef DEBUG_SURROUNDING_VEHICLES
627  if (DEBUG_COND) {
628  MSVehicle* partialBehind = getCloserFollower(candiPos, neighFollow, target->lane->getPartialBehind(candi));
629  if (partialBehind != 0 && partialBehind != neighFollow) {
630  std::cout << "'Partial behind'-vehicle '" << target->lane->getPartialBehind(candi)->getID() << "' at position " << target->hoppedVeh->getPositionOnLane() << " is closer." << std::endl;
631  }
632  }
633 #endif
634  // or a follower which is partially lapping into the target lane
635  neighFollow = getCloserFollower(candiPos, neighFollow, target->lane->getPartialBehind(candi));
636 
637  if (neighFollow == 0) {
638  CLeaderDist consecutiveFollower = target->lane->getFollowersOnConsecutive(candi, candi->getBackPositionOnLane(), true)[0];
639 #ifdef DEBUG_SURROUNDING_VEHICLES
640  if (DEBUG_COND) {
641  if (consecutiveFollower.first == 0) {
642  std::cout << "no follower found." << std::endl;
643  } else {
644  std::cout << "found follower '" << consecutiveFollower.first->getID() << "' on consecutive lanes." << std::endl;
645  }
646  }
647 #endif
648  return std::make_pair(const_cast<MSVehicle*>(consecutiveFollower.first), consecutiveFollower.second);
649  } else {
650 #ifdef DEBUG_SURROUNDING_VEHICLES
651  if (DEBUG_COND) {
652  std::cout << "found follower '" << neighFollow->getID() << "'." << std::endl;
653  }
654 #endif
655  MSVehicle* candi = veh(myCandi);
656  return std::pair<MSVehicle* const, double>(neighFollow,
657  candi->getPositionOnLane() - candi->getVehicleType().getLength() - neighFollow->getPositionOnLane() - neighFollow->getVehicleType().getMinGap());
658  }
659 }
660 
661 
662 MSVehicle*
663 MSLaneChanger::getCloserFollower(const double maxPos, MSVehicle* follow1, MSVehicle* follow2) {
664  if (follow1 == 0 || follow1->getPositionOnLane() > maxPos) {
665  return follow2;
666  } else if (follow2 == 0 || follow2->getPositionOnLane() > maxPos) {
667  return follow1;
668  } else {
669  if (follow1->getPositionOnLane() > follow2->getPositionOnLane()) {
670  return follow1;
671  } else {
672  return follow2;
673  }
674  }
675 }
676 
677 int
679  int laneOffset,
680  const std::pair<MSVehicle* const, double>& leader,
681  const std::vector<MSVehicle::LaneQ>& preb) const {
682 
683  std::pair<MSVehicle* const, double> neighLead = getRealLeader(myCandi + laneOffset);
684  std::pair<MSVehicle*, double> neighFollow = getRealFollower(myCandi + laneOffset);
685  if (neighLead.first != 0 && neighLead.first == neighFollow.first) {
686  // vehicles should not be leader and follower at the same time to avoid
687  // contradictory behavior
688  neighFollow.first = 0;
689  }
690  ChangerIt target = myCandi + laneOffset;
691  return checkChange(laneOffset, target->lane, leader, neighLead, neighFollow, preb);
692 }
693 
694 int
696  int laneOffset,
697  const MSLane* targetLane,
698  const std::pair<MSVehicle* const, double>& leader,
699  const std::pair<MSVehicle* const, double>& neighLead,
700  const std::pair<MSVehicle* const, double>& neighFollow,
701  const std::vector<MSVehicle::LaneQ>& preb) const {
702 
703  MSVehicle* vehicle = veh(myCandi);
704 
705  // Debug (Leo)
706 #ifdef DEBUG_CHECK_CHANGE
707  if (DEBUG_COND) {
708  std::cout
709  << "\n" << SIMTIME << " checkChange() for vehicle '" << vehicle->getID() << "'"
710  << std::endl;
711  }
712 #endif
713 
714 
715  int blocked = 0;
716  int blockedByLeader = (laneOffset == -1 ? LCA_BLOCKED_BY_RIGHT_LEADER : LCA_BLOCKED_BY_LEFT_LEADER);
717  int blockedByFollower = (laneOffset == -1 ? LCA_BLOCKED_BY_RIGHT_FOLLOWER : LCA_BLOCKED_BY_LEFT_FOLLOWER);
718  // overlap
719  if (neighFollow.first != 0 && neighFollow.second < 0) {
720  blocked |= (blockedByFollower | LCA_OVERLAPPING);
721 
722  // Debug (Leo)
723 #ifdef DEBUG_CHECK_CHANGE
724  if (DEBUG_COND) {
725  std::cout << SIMTIME
726  << " overlapping with follower..."
727  << std::endl;
728  }
729 #endif
730 
731  }
732  if (neighLead.first != 0 && neighLead.second < 0) {
733  blocked |= (blockedByLeader | LCA_OVERLAPPING);
734 
735  // Debug (Leo)
736 #ifdef DEBUG_CHECK_CHANGE
737  if (DEBUG_COND) {
738  std::cout << SIMTIME
739  << " overlapping with leader..."
740  << std::endl;
741  }
742 #endif
743 
744  }
745 
746  double secureFrontGap = MSAbstractLaneChangeModel::NO_NEIGHBOR;
747  double secureBackGap = MSAbstractLaneChangeModel::NO_NEIGHBOR;
748  double secureOrigFrontGap = MSAbstractLaneChangeModel::NO_NEIGHBOR;
749 
750  // safe back gap
751  if ((blocked & blockedByFollower) == 0 && neighFollow.first != 0) {
752  // Calculate secure gap conservatively with vNextFollower / vNextLeader as
753  // extrapolated speeds after the driver's expected reaction time (tau).
754  // NOTE: there exists a possible source for collisions if the follower and the leader
755  // have desynchronized action steps as the extrapolated speeds can be exceeded in this case
756 
757  // Expected reaction time (tau) for the follower-vehicle.
758  // (substracted TS since at this point the vehicles' states are already updated)
759  // XXX: How does the ego vehicle know the value of tau for the neighboring vehicle?
760  const double followerTauRemainder = MAX2(neighFollow.first->getCarFollowModel().getHeadwayTime() - TS, 0.);
761  const double vNextFollower = neighFollow.first->getSpeed() + MAX2(0., followerTauRemainder * neighFollow.first->getAcceleration());
762  const double vNextLeader = vehicle->getSpeed() + MIN2(0., followerTauRemainder * vehicle->getAcceleration());
763  // !!! eigentlich: vsafe braucht die Max. Geschwindigkeit beider Spuren
764  secureBackGap = neighFollow.first->getCarFollowModel().getSecureGap(vNextFollower,
765  vNextLeader, vehicle->getCarFollowModel().getMaxDecel());
766  if (neighFollow.second < secureBackGap) {
767  blocked |= blockedByFollower;
768 
769  // Debug (Leo)
770 #ifdef DEBUG_CHECK_CHANGE
771  if (DEBUG_COND) {
772  std::cout << SIMTIME
773  << " back gap unsafe: "
774  << "gap = " << neighFollow.second
775  << ", secureGap = "
776  << neighFollow.first->getCarFollowModel().getSecureGap(vNextFollower,
777  vNextLeader, vehicle->getCarFollowModel().getMaxDecel())
778  << std::endl;
779  }
780 #endif
781 
782  }
783  }
784 
785  // safe front gap
786  if ((blocked & blockedByLeader) == 0 && neighLead.first != 0) {
787  // Calculate secure gap conservatively with vNextFollower / vNextLeader as
788  // extrapolated speeds after the driver's expected reaction time (tau).
789  // NOTE: there exists a possible source for collisions if the follower and the leader
790  // have desynchronized action steps as the extrapolated speeds can be exceeded in this case
791 
792  // Expected reaction time (tau) for the follower-vehicle.
793  // (substracted TS since at this point the vehicles' states are already updated)
794  // XXX: How does the ego vehicle know the value of tau for the neighboring vehicle?
795  const double followerTauRemainder = MAX2(vehicle->getCarFollowModel().getHeadwayTime() - TS, 0.);
796  const double vNextFollower = vehicle->getSpeed() + MAX2(0., followerTauRemainder * vehicle->getAcceleration());
797  const double vNextLeader = neighLead.first->getSpeed() + MIN2(0., followerTauRemainder * neighLead.first->getAcceleration());
798  // !!! eigentlich: vsafe braucht die Max. Geschwindigkeit beider Spuren
799  secureFrontGap = vehicle->getCarFollowModel().getSecureGap(vNextFollower,
800  vNextLeader, neighLead.first->getCarFollowModel().getMaxDecel());
801  if (neighLead.second < secureFrontGap) {
802  blocked |= blockedByLeader;
803 
804  // Debug (Leo)
805 #ifdef DEBUG_CHECK_CHANGE
806  if (DEBUG_COND) {
807  std::cout << SIMTIME
808  << " front gap unsafe: "
809  << "gap = " << neighLead.second
810  << ", secureGap = "
811  << vehicle->getCarFollowModel().getSecureGap(vNextFollower,
812  vNextLeader, neighLead.first->getCarFollowModel().getMaxDecel())
813  << std::endl;
814  }
815 #endif
816 
817  }
818  }
819  if (leader.first != 0) {
820  secureOrigFrontGap = vehicle->getCarFollowModel().getSecureGap(vehicle->getSpeed(), leader.first->getSpeed(), leader.first->getCarFollowModel().getMaxDecel());
821  }
822 
823  MSAbstractLaneChangeModel::MSLCMessager msg(leader.first, neighLead.first, neighFollow.first);
824  int state = blocked | vehicle->getLaneChangeModel().wantsChange(
825  laneOffset, msg, blocked, leader, neighLead, neighFollow, *targetLane, preb, &(myCandi->lastBlocked), &(myCandi->firstBlocked));
826 
827  if (blocked == 0 && (state & LCA_WANTS_LANECHANGE) != 0 && neighLead.first != 0) {
828  // do a more careful (but expensive) check to ensure that a
829  // safety-critical leader is not being overlooked
830  const double seen = myCandi->lane->getLength() - vehicle->getPositionOnLane();
831  const double speed = vehicle->getSpeed();
832  const double dist = vehicle->getCarFollowModel().brakeGap(speed) + vehicle->getVehicleType().getMinGap();
833  if (seen < dist) {
834  std::pair<MSVehicle* const, double> neighLead2 = targetLane->getCriticalLeader(dist, seen, speed, *vehicle);
835  if (neighLead2.first != 0 && neighLead2.first != neighLead.first
836  && (neighLead2.second < vehicle->getCarFollowModel().getSecureGap(
837  vehicle->getSpeed(), neighLead2.first->getSpeed(), neighLead2.first->getCarFollowModel().getMaxDecel()))) {
838  state |= blockedByLeader;
839  }
840  }
841  }
842  if (blocked == 0 && (state & LCA_WANTS_LANECHANGE)) {
843  // ensure that merging is safe for any upcoming zipper links after changing
844  if (vehicle->unsafeLinkAhead(targetLane)) {
845  state |= blockedByLeader;
846  }
847  }
848 
849  if ((state & LCA_BLOCKED) == 0 && (state & LCA_WANTS_LANECHANGE) != 0 && MSGlobals::gLaneChangeDuration > DELTA_T) {
850  // ensure that a continuous lane change manoeuvre can be completed
851  // before the next turning movement
852  double seen = myCandi->lane->getLength() - vehicle->getPositionOnLane();
853  const double decel = vehicle->getCarFollowModel().getMaxDecel() * STEPS2TIME(MSGlobals::gLaneChangeDuration);
854  const double avgSpeed = 0.5 * (
855  MAX2(0., vehicle->getSpeed() - ACCEL2SPEED(vehicle->getCarFollowModel().getMaxDecel())) +
856  MAX2(0., vehicle->getSpeed() - decel));
857  const double space2change = avgSpeed * STEPS2TIME(MSGlobals::gLaneChangeDuration);
858  // for finding turns it doesn't matter whether we look along the current lane or the target lane
859  const std::vector<MSLane*>& bestLaneConts = vehicle->getBestLanesContinuation();
860  int view = 1;
861  MSLane* nextLane = vehicle->getLane();
862  MSLinkCont::const_iterator link = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
863  while (!nextLane->isLinkEnd(link) && seen <= space2change) {
864  if ((*link)->getDirection() == LINKDIR_LEFT || (*link)->getDirection() == LINKDIR_RIGHT
865  // the lanes after an internal junction are on different
866  // edges and do not allow lane-changing
867  || (nextLane->getEdge().isInternal() && (*link)->getViaLaneOrLane()->getEdge().isInternal())
868  ) {
869  state |= LCA_INSUFFICIENT_SPACE;
870  break;
871  }
872  if ((*link)->getViaLane() == 0) {
873  view++;
874  }
875  nextLane = (*link)->getViaLaneOrLane();
876  seen += nextLane->getLength();
877  // get the next link used
878  link = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
879  }
880  if (nextLane->isLinkEnd(link) && seen < space2change) {
881 #ifdef DEBUG_CHECK_CHANGE
882  if (DEBUG_COND) {
883  std::cout << SIMTIME << " checkChange insufficientSpace: seen=" << seen << " space2change=" << space2change << "\n";
884  }
885 #endif
886  state |= LCA_INSUFFICIENT_SPACE;
887  }
888 
889  if ((state & LCA_BLOCKED) == 0) {
890  // check for dangerous leaders in case the target lane changes laterally between
891  // now and the lane-changing midpoint
892  const double speed = vehicle->getSpeed();
893  seen = myCandi->lane->getLength() - vehicle->getPositionOnLane();
894  nextLane = vehicle->getLane();
895  view = 1;
896  const double dist = vehicle->getCarFollowModel().brakeGap(speed) + vehicle->getVehicleType().getMinGap();
897  MSLinkCont::const_iterator link = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
898  while (!nextLane->isLinkEnd(link) && seen <= space2change && seen <= dist) {
899  nextLane = (*link)->getViaLaneOrLane();
900  MSLane* targetLane = nextLane->getParallelLane(laneOffset);
901  if (targetLane == 0) {
902  state |= LCA_INSUFFICIENT_SPACE;
903  break;
904  } else {
905  std::pair<MSVehicle* const, double> neighLead2 = targetLane->getLeader(vehicle, -seen, std::vector<MSLane*>());
906  if (neighLead2.first != 0 && neighLead2.first != neighLead.first
907  && (neighLead2.second < vehicle->getCarFollowModel().getSecureGap(
908  vehicle->getSpeed(), neighLead2.first->getSpeed(), neighLead2.first->getCarFollowModel().getMaxDecel()))) {
909  state |= blockedByLeader;
910  break;
911  }
912  }
913  if ((*link)->getViaLane() == 0) {
914  view++;
915  }
916  seen += nextLane->getLength();
917  // get the next link used
918  link = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
919  }
920  }
921  }
922  const int oldstate = state;
923 #ifndef NO_TRACI
924  // let TraCI influence the wish to change lanes and the security to take
925  state = vehicle->influenceChangeDecision(state);
926 #endif
927 #ifdef DEBUG_CHECK_CHANGE
928  if (DEBUG_COND) {
929  std::cout << SIMTIME
930  << " veh=" << vehicle->getID()
931  << " oldState=" << toString((LaneChangeAction)oldstate)
932  << " newState=" << toString((LaneChangeAction)state)
933  << ((blocked & LCA_BLOCKED) ? " (blocked)" : "")
934  << ((blocked & LCA_OVERLAPPING) ? " (overlap)" : "")
935  << "\n";
936  }
937 #endif
938  vehicle->getLaneChangeModel().saveState(laneOffset, oldstate, state);
939  if (blocked == 0 && (state & LCA_WANTS_LANECHANGE)) {
940  // this lane change will be executed, save gaps
941  vehicle->getLaneChangeModel().setFollowerGaps(neighFollow, secureBackGap);
942  vehicle->getLaneChangeModel().setLeaderGaps(neighLead, secureFrontGap);
943  vehicle->getLaneChangeModel().setOrigLeaderGaps(leader, secureOrigFrontGap);
944  }
945  return state;
946 }
947 
948 
949 bool
950 MSLaneChanger::changeOpposite(std::pair<MSVehicle*, double> leader) {
951  if (!myChangeToOpposite) {
952  return false;
953  }
955  MSVehicle* vehicle = veh(myCandi);
956  MSLane* source = vehicle->getLane();
957  if (vehicle->isStopped()) {
958  // stopped vehicles obviously should not change lanes. Usually this is
959  // prevent by appropriate bestLane distances
960  return false;
961  }
962  const bool isOpposite = vehicle->getLaneChangeModel().isOpposite();
963  if (!isOpposite && leader.first == 0) {
964  // no reason to change unless there is a leader
965  // or we are changing back to the propper direction
966  // XXX also check whether the leader is so far away as to be irrelevant
967  return false;
968  }
969  MSLane* opposite = source->getOpposite();
970  if (opposite == 0) {
971  return false;
972  }
973 
974  // changing into the opposite direction is always to the left (XXX except for left-hand networkds)
975  int direction = isOpposite ? -1 : 1;
976  std::pair<MSVehicle*, double> neighLead((MSVehicle*)0, -1);
977 
978  // preliminary sanity checks for overtaking space
979  double timeToOvertake;
980  double spaceToOvertake;
981  if (!isOpposite) {
982  assert(leader.first != 0);
983  // find a leader vehicle with sufficient space ahead for merging back
984  const double overtakingSpeed = source->getVehicleMaxSpeed(vehicle); // just a guess
985  const double mergeBrakeGap = vehicle->getCarFollowModel().brakeGap(overtakingSpeed);
986  std::pair<MSVehicle*, double> columnLeader = leader;
987  double egoGap = leader.second;
988  bool foundSpaceAhead = false;
989  double seen = leader.second + leader.first->getVehicleType().getLengthWithGap();
990  std::vector<MSLane*> conts = vehicle->getBestLanesContinuation();
991  while (!foundSpaceAhead) {
992  const double requiredSpaceAfterLeader = (columnLeader.first->getCarFollowModel().getSecureGap(
993  columnLeader.first->getSpeed(), overtakingSpeed, vehicle->getCarFollowModel().getMaxDecel())
994  + vehicle->getVehicleType().getLengthWithGap());
995 
996 
997  // all leader vehicles on the current laneChanger edge are already moved into MSLane::myTmpVehicles
998  const bool checkTmpVehicles = (&columnLeader.first->getLane()->getEdge() == &source->getEdge());
999  std::pair<MSVehicle* const, double> leadLead = columnLeader.first->getLane()->getLeader(
1000  columnLeader.first, columnLeader.first->getPositionOnLane(), conts, requiredSpaceAfterLeader + mergeBrakeGap,
1001  checkTmpVehicles);
1002 
1003 #ifdef DEBUG_CHANGE_OPPOSITE
1004  if (DEBUG_COND) {
1005  std::cout << " leadLead=" << Named::getIDSecure(leadLead.first) << " gap=" << leadLead.second << "\n";
1006  }
1007 #endif
1008  if (leadLead.first == 0) {
1009  foundSpaceAhead = true;
1010  } else {
1011  const double maxLookAhead = (vehicle->getVehicleType().getVehicleClass() == SVC_EMERGENCY
1014  const double requiredSpace = (requiredSpaceAfterLeader
1015  + vehicle->getCarFollowModel().getSecureGap(overtakingSpeed, leadLead.first->getSpeed(), leadLead.first->getCarFollowModel().getMaxDecel()));
1016  if (leadLead.second > requiredSpace) {
1017  foundSpaceAhead = true;
1018  } else {
1019 #ifdef DEBUG_CHANGE_OPPOSITE
1020  if (DEBUG_COND) {
1021  std::cout << " not enough space after columnLeader=" << columnLeader.first->getID() << " required=" << requiredSpace << "\n";
1022  }
1023 #endif
1024  seen += MAX2(0., leadLead.second) + leadLead.first->getVehicleType().getLengthWithGap();
1025  if (seen > maxLookAhead) {
1026 #ifdef DEBUG_CHANGE_OPPOSITE
1027  if (DEBUG_COND) {
1028  std::cout << " cannot changeOpposite due to insufficient free space after columnLeader (seen=" << seen << " columnLeader=" << columnLeader.first->getID() << ")\n";
1029  }
1030 #endif
1031  return false;
1032  }
1033  // see if merging after leadLead is possible
1034  egoGap += columnLeader.first->getVehicleType().getLengthWithGap() + leadLead.second;
1035  columnLeader = leadLead;
1036 #ifdef DEBUG_CHANGE_OPPOSITE
1037  if (DEBUG_COND) {
1038  std::cout << " new columnLeader=" << columnLeader.first->getID() << "\n";
1039  }
1040 #endif
1041  }
1042  }
1043  }
1044 #ifdef DEBUG_CHANGE_OPPOSITE
1045  if (DEBUG_COND) {
1046  std::cout << " compute time/space to overtake for columnLeader=" << columnLeader.first->getID() << " gap=" << columnLeader.second << "\n";
1047  }
1048 #endif
1049  computeOvertakingTime(vehicle, columnLeader.first, egoGap, timeToOvertake, spaceToOvertake);
1050  // check for upcoming stops
1051  if (vehicle->nextStopDist() < spaceToOvertake) {
1052 #ifdef DEBUG_CHANGE_OPPOSITE
1053  if (DEBUG_COND) {
1054  std::cout << " cannot changeOpposite due to upcoming stop (dist=" << vehicle->nextStopDist() << " spaceToOvertake=" << spaceToOvertake << ")\n";
1055  }
1056 #endif
1057  return false;
1058  }
1059  neighLead = opposite->getOppositeLeader(vehicle, timeToOvertake * opposite->getSpeedLimit() * 2 + spaceToOvertake, true);
1060 
1061 #ifdef DEBUG_CHANGE_OPPOSITE
1062  if (DEBUG_COND) {
1063  std::cout << SIMTIME
1064  << " veh=" << vehicle->getID()
1065  << " changeOpposite opposite=" << opposite->getID()
1066  << " lead=" << Named::getIDSecure(leader.first)
1067  << " timeToOvertake=" << timeToOvertake
1068  << " spaceToOvertake=" << spaceToOvertake
1069  << "\n";
1070  }
1071 #endif
1072 
1073  // check for dangerous oncoming leader
1074  if (neighLead.first != 0) {
1075  const MSVehicle* oncoming = neighLead.first;
1076 
1077 #ifdef DEBUG_CHANGE_OPPOSITE
1078  if (DEBUG_COND) {
1079  std::cout << SIMTIME
1080  << " oncoming=" << oncoming->getID()
1081  << " oncomingGap=" << neighLead.second
1082  << " leaderGap=" << leader.second
1083  << "\n";
1084  }
1085 #endif
1086  if (neighLead.second - spaceToOvertake - timeToOvertake * oncoming->getSpeed() < 0) {
1087 
1088 #ifdef DEBUG_CHANGE_OPPOSITE
1089  if (DEBUG_COND) {
1090  std::cout << " cannot changeOpposite due to dangerous oncoming\n";
1091  }
1092 #endif
1093  return false;
1094  }
1095  }
1096  } else {
1097  timeToOvertake = -1;
1098  // look forward as far as possible
1099  spaceToOvertake = std::numeric_limits<double>::max();
1100  leader = source->getOppositeLeader(vehicle, OPPOSITE_OVERTAKING_ONCOMING_LOOKAHEAD, true);
1101  // -1 will use getMaximumBrakeDist() as look-ahead distance
1102  neighLead = opposite->getOppositeLeader(vehicle, -1, false);
1103  }
1104 
1105  // compute remaining space on the opposite side
1106  // 1. the part that remains on the current lane
1107  double usableDist = isOpposite ? vehicle->getPositionOnLane() : source->getLength() - vehicle->getPositionOnLane();
1108  if (usableDist < spaceToOvertake) {
1109  // look forward along the next lanes
1110  const std::vector<MSLane*>& bestLaneConts = vehicle->getBestLanesContinuation();
1111  assert(bestLaneConts.size() >= 1);
1112  std::vector<MSLane*>::const_iterator it = bestLaneConts.begin() + 1;
1113  while (usableDist < spaceToOvertake && it != bestLaneConts.end()) {
1114 #ifdef DEBUG_CHANGE_OPPOSITE
1115  if (DEBUG_COND) {
1116  std::cout << " usableDist=" << usableDist << " opposite=" << Named::getIDSecure((*it)->getOpposite()) << "\n";
1117  }
1118 #endif
1119  if ((*it)->getOpposite() == 0) {
1120  // opposite lane ends
1121  break;
1122  }
1123  // do not overtake past a minor link or turn
1124  if (*(it - 1) != 0) {
1125  MSLink* link = MSLinkContHelper::getConnectingLink(**(it - 1), **it);
1126  if (link == 0 || link->getState() == LINKSTATE_ZIPPER
1127  || (link->getDirection() != LINKDIR_STRAIGHT && vehicle->getVehicleType().getVehicleClass() != SVC_EMERGENCY)
1128  || (!link->havePriority()
1129  // consider traci-influence
1130  && (!vehicle->hasInfluencer() || vehicle->getInfluencer().getRespectJunctionPriority())
1131  // consider junction model parameters
1132  && ((!link->haveRed() && !link->haveYellow()) || !vehicle->ignoreRed(link, true)))) {
1133 #ifdef DEBUG_CHANGE_OPPOSITE
1134  if (DEBUG_COND) {
1135  std::cout << " stop lookahead at link=" << (link == 0 ? "NULL" : link->getViaLaneOrLane()->getID()) << " state=" << (link == 0 ? "?" : toString(link->getState())) << " ignoreRed=" << vehicle->ignoreRed(link, true) << "\n";
1136  }
1137 #endif
1138  break;
1139  }
1140  }
1141  usableDist += (*it)->getLength();
1142  ++it;
1143  }
1144  }
1145  if (!isOpposite && usableDist < spaceToOvertake) {
1146 #ifdef DEBUG_CHANGE_OPPOSITE
1147  if (DEBUG_COND) {
1148  std::cout << " cannot changeOpposite due to insufficient space (seen=" << usableDist << " spaceToOvertake=" << spaceToOvertake << ")\n";
1149  }
1150 #endif
1151  return false;
1152  }
1153 #ifdef DEBUG_CHANGE_OPPOSITE
1154  if (DEBUG_COND) {
1155  std::cout << " usableDist=" << usableDist << " spaceToOvertake=" << spaceToOvertake << " timeToOvertake=" << timeToOvertake << "\n";
1156  }
1157 #endif
1158 
1159  // compute wish to change
1160  // Does "preb" mean "previousBestLanes" ??? If so *rename*
1161  std::vector<MSVehicle::LaneQ> preb = vehicle->getBestLanes();
1162  if (isOpposite) {
1163  // compute the remaining distance that can be drive on the opposite side
1164  // this value will put into LaneQ.length of the leftmost lane
1165  // @note: length counts from the start of the current lane
1166  // @note: see MSLCM_LC2013::_wantsChange @1092 (isOpposite()
1167  MSVehicle::LaneQ& laneQ = preb[preb.size() - 1];
1168  // position on the target lane
1169  const double forwardPos = source->getOppositePos(vehicle->getPositionOnLane());
1170 
1171  // consider usableDist (due to minor links or end of opposite lanes)
1172  laneQ.length = MIN2(laneQ.length, usableDist + forwardPos);
1173  // consider upcoming stops
1174  laneQ.length = MIN2(laneQ.length, vehicle->nextStopDist() + forwardPos);
1175  // consider oncoming leaders
1176  if (leader.first != 0) {
1177  laneQ.length = MIN2(laneQ.length, leader.second / 2 + forwardPos);
1178 #ifdef DEBUG_CHANGE_OPPOSITE
1179  if (DEBUG_COND) {
1180  std::cout << SIMTIME << " found oncoming leader=" << leader.first->getID() << " gap=" << leader.second << "\n";
1181  }
1182 #endif
1183  leader.first = 0; // ignore leader after this
1184  }
1185 #ifdef DEBUG_CHANGE_OPPOSITE
1186  if (DEBUG_COND) {
1187  std::cout << SIMTIME << " veh=" << vehicle->getID() << " remaining dist=" << laneQ.length - forwardPos << " forwardPos=" << forwardPos << " laneQ.length=" << laneQ.length << "\n";
1188  }
1189 #endif
1190  }
1191  std::pair<MSVehicle* const, double> neighFollow = opposite->getOppositeFollower(vehicle);
1192  int state = checkChange(direction, opposite, leader, neighLead, neighFollow, preb);
1193 
1194  bool changingAllowed = (state & LCA_BLOCKED) == 0;
1195  // change if the vehicle wants to and is allowed to change
1196  if ((state & LCA_WANTS_LANECHANGE) != 0 && changingAllowed
1197  // do not change to the opposite direction for cooperative reasons
1198  && (isOpposite || (state & LCA_COOPERATIVE) == 0)) {
1199  vehicle->getLaneChangeModel().startLaneChangeManeuver(source, opposite, direction);
1201  vehicle->myState.myPos = source->getOppositePos(vehicle->myState.myPos);
1204  if (!isOpposite) {
1205  vehicle->myState.myBackPos = source->getOppositePos(vehicle->myState.myBackPos);
1206  }
1207 #ifdef DEBUG_CHANGE_OPPOSITE
1208  if (DEBUG_COND) {
1209  std::cout << SIMTIME << " changing to opposite veh=" << vehicle->getID() << " dir=" << direction << " opposite=" << Named::getIDSecure(opposite) << " state=" << state << "\n";
1210  }
1211 #endif
1212  return true;
1213  }
1214 #ifdef DEBUG_CHANGE_OPPOSITE
1215  if (DEBUG_COND) {
1216  std::cout << SIMTIME << " not changing to opposite veh=" << vehicle->getID() << " dir=" << direction
1217  << " opposite=" << Named::getIDSecure(opposite) << " state=" << toString((LaneChangeAction)state) << "\n";
1218  }
1219 #endif
1220  return false;
1221 }
1222 
1223 
1224 void
1225 MSLaneChanger::computeOvertakingTime(const MSVehicle* vehicle, const MSVehicle* leader, double gap, double& timeToOvertake, double& spaceToOvertake) {
1226  // Assumptions:
1227  // - leader maintains the current speed
1228  // - vehicle merges with maxSpeed ahead of leader
1229  // XXX affected by ticket #860 (the formula is invalid for the current position update rule)
1230 
1231  // first compute these values for the case where vehicle is accelerating
1232  // without upper bound on speed
1233  const double vMax = vehicle->getLane()->getVehicleMaxSpeed(vehicle);
1234  const double v = vehicle->getSpeed();
1235  const double u = leader->getSpeed();
1236  const double a = vehicle->getCarFollowModel().getMaxAccel();
1237  const double d = vehicle->getCarFollowModel().getMaxDecel();
1238  const double g = (
1239  // drive up to the rear of leader
1240  gap + vehicle->getVehicleType().getMinGap()
1241  // drive head-to-head with the leader
1242  + leader->getVehicleType().getLengthWithGap()
1243  // drive past the leader
1244  + vehicle->getVehicleType().getLength()
1245  // allow for safe gap between leader and vehicle
1246  + leader->getCarFollowModel().getSecureGap(v, vMax, d));
1247  const double sign = -1; // XXX recheck
1248  // v*t + t*t*a*0.5 = g + u*t
1249  // solve t
1250  // t = ((u - v - (((((2.0*(u - v))**2.0) + (8.0*a*g))**(1.0/2.0))*sign/2.0))/a)
1251  double t = (u - v - sqrt(4 * (u - v) * (u - v) + 8 * a * g) * sign * 0.5) / a;
1252 
1253  // allow for a safety time gap
1255  // round to multiples of step length (TS)
1256  t = ceil(t / TS) * TS;
1257 
1259  const double timeToMaxSpeed = (vMax - v) / a;
1260 
1261  if (t <= timeToMaxSpeed) {
1262  timeToOvertake = t;
1263  spaceToOvertake = v * t + t * t * a * 0.5;
1264  //if (gDebugFlag1) std::cout << " t below " << timeToMaxSpeed << " vMax=" << vMax << "\n";
1265  } else {
1266  // space until max speed is reached
1267  const double s = v * timeToMaxSpeed + timeToMaxSpeed * timeToMaxSpeed * a * 0.5;
1268  const double m = timeToMaxSpeed;
1269  // s + (t-m) * vMax = g + u*t
1270  // solve t
1271  t = (g - s + m * vMax) / (vMax - u);
1272 
1273  // allow for a safety time gap
1275  // round to multiples of step length (TS)
1276  t = ceil(t / TS) * TS;
1277 
1278  timeToOvertake = t;
1279  spaceToOvertake = s + (t - m) * vMax;
1280  //if (gDebugFlag1) std::cout << " s=" << s << " m=" << m << " vMax=" << vMax << "\n";
1281  }
1282 }
1283 
1284 /****************************************************************************/
1285 
double myPos
the stored position
Definition: MSVehicle.h:140
void laneChange(SUMOTime t)
Start lane-change-process for all vehicles on the edge&#39;e lanes.
bool getRespectJunctionPriority() const
Returns whether junction priority rules shall be respected.
Definition: MSVehicle.h:1399
bool isChangingLanes() const
return true if the vehicle currently performs a lane change maneuver
double getLengthWithGap() const
Get vehicle&#39;s length including the minimum gap [m].
double computeAngle() const
compute the current vehicle angle
Definition: MSVehicle.cpp:1038
static MSVehicle * getCloserFollower(const double maxPos, MSVehicle *follow1, MSVehicle *follow2)
return the closer follower of ego
double getVehicleMaxSpeed(const SUMOVehicle *const veh) const
Returns the lane&#39;s maximum speed, given a vehicle&#39;s speed limit adaptation.
Definition: MSLane.h:475
double brakeGap(const double speed) const
Returns the distance the vehicle needs to halt including driver&#39;s reaction time tau (i...
Definition: MSCFModel.h:289
bool ignoreRed(const MSLink *link, bool canBrake) const
decide whether a red (or yellow light) may be ignore
Definition: MSVehicle.cpp:4880
MSEdge & getEdge() const
Returns the lane&#39;s edge.
Definition: MSLane.h:607
double myAngle
the angle in radians (
Definition: MSVehicle.h:1744
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:83
bool isRemoteControlled() const
Returns the information whether the vehicle is fully controlled via TraCI.
Definition: MSVehicle.cpp:4856
MSLane * lane
the lane the vehicle is on
Definition: MSLaneChanger.h:80
The action is done to help someone else.
std::pair< MSVehicle *const, double > getOppositeLeader(const MSVehicle *ego, double dist, bool oppositeDir) const
Definition: MSLane.cpp:3052
State myState
This Vehicles driving state (pos and speed)
Definition: MSVehicle.h:1679
#define ACCEL2SPEED(x)
Definition: SUMOTime.h:60
MSLane * getLane() const
Returns the lane the vehicle is on.
Definition: MSVehicle.h:564
void setLeaderGaps(CLeaderDist, double secGap)
The vehicle is blocked by left follower.
int checkChange(int laneOffset, const MSLane *targetLane, const std::pair< MSVehicle *const, double > &leader, const std::pair< MSVehicle *const, double > &neighLead, const std::pair< MSVehicle *const, double > &neighFollow, const std::vector< MSVehicle::LaneQ > &preb) const
bool continueChange(MSVehicle *vehicle, ChangerIt &from)
continue a lane change maneuver and return whether the midpoint was passed in this step (used if gLan...
virtual void initChanger()
Initialize the changer before looping over all vehicles.
int getShadowDirection() const
return the direction in which the current shadow lane lies
virtual bool changeOpposite(std::pair< MSVehicle *, double > leader)
const bool myAllowsChanging
double getPositionOnLane() const
Get the vehicle&#39;s position along the lane.
Definition: MSVehicle.h:402
double myPosLat
the stored lateral position
Definition: MSVehicle.h:146
MSVehicle * veh(ConstChangerIt ce) const
std::pair< MSVehicle *const, double > getRealLeader(const ChangerIt &target) const
Wants go to the right.
const bool myChangeToOpposite
whether this edge allows changing to the opposite direction edge
T MAX2(T a, T b)
Definition: StdDefs.h:73
#define OPPOSITE_OVERTAKING_MAX_LOOKAHEAD
SUMOTime DELTA_T
Definition: SUMOTime.cpp:39
double getLength() const
Returns the lane&#39;s length.
Definition: MSLane.h:497
static std::string getIDSecure(const T *obj, const std::string &fallBack="NULL")
get an identifier for Named-like object which may be Null
Definition: Named.h:67
bool isLinkEnd(MSLinkCont::const_iterator &i) const
Definition: MSLane.cpp:1758
bool isStoppedOnLane() const
Definition: MSVehicle.cpp:1337
void adaptBestLanesOccupation(int laneIndex, double density)
update occupation from MSLaneChanger
Definition: MSVehicle.cpp:4065
bool alreadyChanged() const
reset the flag whether a vehicle already moved to false
void startChange(MSVehicle *vehicle, ChangerIt &from, int direction)
start the lane change maneuver (and finish it instantly if gLaneChangeDuration == 0) ...
const std::string & getID() const
Returns the id.
Definition: Named.h:74
#define TS
Definition: SUMOTime.h:51
double length
The overall length which may be driven when using this lane without a lane change.
Definition: MSVehicle.h:803
VehCont myPartialVehicles
The lane&#39;s partial vehicles. This container holds all vehicles that are partially on this lane but wh...
Definition: MSLane.h:1168
Wants go to the left.
double getWidth() const
Returns the lane&#39;s width.
Definition: MSLane.h:513
ChangerIt findCandidate()
Find current candidate. If there is none, myChanger.end() is returned.
This is an uncontrolled, zipper-merge link.
The link is a (hard) left direction.
void checkTraCICommands()
Check for commands issued for the vehicle via TraCI and apply the appropriate state changes For the s...
MSAbstractLaneChangeModel & getLaneChangeModel()
Definition: MSVehicle.cpp:3674
#define SIMTIME
Definition: SUMOTime.h:71
bool isFrontOnLane(const MSLane *lane) const
Returns the information whether the front of the vehicle is on the given lane.
Definition: MSVehicle.cpp:3202
bool vehInChanger() const
Check if there is a single change-candidate in the changer. Returns true if there is one...
void saveState(const int dir, const int stateWithoutTraCI, const int state)
bool applyTraCICommands(MSVehicle *vehicle)
Execute TraCI LC-commands.
The link is a straight direction.
double getMaxAccel() const
Get the vehicle type&#39;s maximum acceleration [m/s^2].
Definition: MSCFModel.h:203
void setFollowerGaps(CLeaderDist follower, double secGap)
void checkTraCICommands(MSVehicle *vehicle)
Take into account traci LC-commands.
A class responsible for exchanging messages between cars involved in lane-change interaction.
virtual bool change()
The vehicle changes lanes (micro only)
const MSCFModel & getCarFollowModel() const
Returns the vehicle&#39;s car following model definition.
Definition: MSVehicle.h:882
virtual void updateChanger(bool vehHasChanged)
VehCont myTmpVehicles
Container for lane-changing vehicles. After completion of lane-change- process, the containers will b...
Definition: MSLane.h:1172
blocked in all directions
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:55
The action is urgent (to be defined by lc-model)
void updateBestLanes(bool forceRebuild=false, const MSLane *startLane=0)
computes the best lanes to use in order to continue the route
Definition: MSVehicle.cpp:3692
const std::vector< MSLane * > & getBestLanesContinuation() const
Returns the best sequence of lanes to continue the route starting at myLane.
Definition: MSVehicle.cpp:4026
#define OPPOSITE_OVERTAKING_MAX_LOOKAHEAD_EMERGENCY
virtual int wantsChange(int laneOffset, MSAbstractLaneChangeModel::MSLCMessager &msgPass, int blocked, const std::pair< MSVehicle *, double > &leader, const std::pair< MSVehicle *, double > &neighLead, const std::pair< MSVehicle *, double > &neighFollow, const MSLane &neighLane, const std::vector< MSVehicle::LaneQ > &preb, MSVehicle **lastBlocked, MSVehicle **firstBlocked)
Called to examine whether the vehicle wants to change using the given laneOffset. This method gets th...
Position myCachedPosition
Definition: MSVehicle.h:1752
std::pair< MSVehicle *const, double > getOppositeFollower(const MSVehicle *ego) const
Definition: MSLane.cpp:3075
int getLaneChangeDirection() const
return the direction of the current lane change maneuver
double getSpeedLimit() const
Returns the lane&#39;s maximum allowed speed.
Definition: MSLane.h:489
void registerHop(MSVehicle *vehicle)
Register that vehicle belongs to Changer Item to after LC decisions.
#define STEPS2TIME(x)
Definition: SUMOTime.h:64
void forceVehicleInsertion(MSVehicle *veh, double pos, MSMoveReminder::Notification notification, double posLat=0)
Inserts the given vehicle at the given position.
Definition: MSLane.cpp:940
T MIN2(T a, T b)
Definition: StdDefs.h:67
The link is a (hard) right direction.
std::pair< MSVehicle *const, double > getLeader(const MSVehicle *veh, const double vehPos, const std::vector< MSLane *> &bestLaneConts, double dist=-1, bool checkTmpVehicles=false) const
Returns the immediate leader of veh and the distance to veh starting on this lane.
Definition: MSLane.cpp:2042
static void computeOvertakingTime(const MSVehicle *vehicle, const MSVehicle *leader, double gap, double &timeToOvertake, double &spaceToOvertake)
Compute the time and space required for overtaking the given leader.
A structure representing the best lanes for continuing the current route starting at &#39;lane&#39;...
Definition: MSVehicle.h:799
bool hasInfluencer() const
Definition: MSVehicle.h:1543
double getMinGap() const
Get the free space in front of vehicles of this class.
double getMaxDecel() const
Get the vehicle type&#39;s maximal comfortable deceleration [m/s^2].
Definition: MSCFModel.h:211
MSLaneChanger()
Default constructor.
MSLane * getOpposite() const
return the opposite direction lane for lane changing or 0
Definition: MSLane.cpp:3012
bool isInternal() const
return whether this edge is an internal edge
Definition: MSEdge.h:229
void updateLanes(SUMOTime t)
std::vector< MSVehicle * > VehCont
Container for vehicles.
Definition: MSLane.h:89
static MSLinkCont::const_iterator succLinkSec(const SUMOVehicle &veh, int nRouteSuccs, const MSLane &succLinkSource, const std::vector< MSLane *> &conts)
Definition: MSLane.cpp:1825
int checkChangeWithinEdge(int laneOffset, const std::pair< MSVehicle *const, double > &leader, const std::vector< MSVehicle::LaneQ > &preb) const
std::pair< MSVehicle *const, double > getRealFollower(const ChangerIt &target) const
double getLateralPositionOnLane() const
Get the vehicle&#39;s lateral position on the lane.
Definition: MSVehicle.h:439
void setOrigLeaderGaps(CLeaderDist, double secGap)
bool startLaneChangeManeuver(MSLane *source, MSLane *target, int direction)
start the lane change maneuver and return whether it continues
#define OPPOSITE_OVERTAKING_ONCOMING_LOOKAHEAD
void primaryLaneChanged(MSLane *source, MSLane *target, int direction)
called once when the vehicles primary lane changes
#define OPPOSITE_OVERTAKING_SAFE_TIMEGAP
std::pair< const MSVehicle *, double > CLeaderDist
Definition: MSLeaderInfo.h:41
Influencer & getInfluencer()
Returns the velocity/lane influencer.
Definition: MSVehicle.cpp:4812
LaneChangeAction
The state of a vehicle&#39;s lane-change behavior.
virtual void setOwnState(const int state)
double getOppositePos(double pos) const
return the corresponding position on the opposite lane
Definition: MSLane.cpp:3021
const MSVehicleType & getVehicleType() const
Returns the vehicle&#39;s type definition.
bool isActive() const
Returns whether the current simulation step is an action point for the vehicle.
Definition: MSVehicle.h:594
virtual ~MSLaneChanger()
Destructor.
virtual double getHeadwayTime() const
Get the driver&#39;s desired headway [s].
Definition: MSCFModel.h:246
The vehicle is blocked being overlapping.
#define DEBUG_COND
double getAcceleration() const
Returns the vehicle&#39;s acceleration in m/s (this is computed as the last step&#39;s mean acceleration in c...
Definition: MSVehicle.h:499
Changer::iterator ChangerIt
the iterator moving over the ChangeElems
std::pair< MSVehicle *const, double > getCriticalLeader(double dist, double seen, double speed, const MSVehicle &veh) const
Returns the most dangerous leader and the distance to him.
Definition: MSLane.cpp:2193
double getLength() const
Get vehicle&#39;s length [m].
Changer myChanger
Container for ChangeElemements, one for every lane in the edge.
double getBackPositionOnLane(const MSLane *lane) const
Get the vehicle&#39;s position relative to the given lane.
Definition: MSVehicle.cpp:3120
void registerUnchanged(MSVehicle *vehicle)
The vehicle does not have enough space to complete a continuous lane and change before the next turni...
long long int SUMOTime
Definition: TraCIDefs.h:51
bool unsafeLinkAhead(const MSLane *lane) const
whether the vehicle may safely move to the given lane with regard to upcoming links ...
Definition: MSVehicle.cpp:4512
MSLane * getShadowLane() const
Returns the lane the vehicle&#39;s shadow is on during continuous/sublane lane change.
bool isStopped() const
Returns whether the vehicle is at a stop.
Definition: MSVehicle.cpp:1332
double getSecureGap(const double speed, const double leaderSpeed, const double leaderMaxDecel) const
Returns the minimum gap to reserve if the leader is braking at maximum (>=0)
Definition: MSCFModel.h:304
MSVehicle * hoppedVeh
last vehicle that changed into this lane
Definition: MSLaneChanger.h:82
const std::vector< LaneQ > & getBestLanes() const
Returns the description of best lanes to use in order to continue the route.
Definition: MSVehicle.cpp:3686
double getSpeed() const
Returns the vehicle&#39;s current speed.
Definition: MSVehicle.h:482
The vehicle is blocked by right leader.
int influenceChangeDecision(int state)
allow TraCI to influence a lane change decision
Definition: MSVehicle.cpp:4836
public emergency vehicles
static SUMOTime gLaneChangeDuration
Definition: MSGlobals.h:88
const std::string & getID() const
Returns the name of the vehicle.
Representation of a lane in the micro simulation.
Definition: MSLane.h:77
double myBackPos
the stored back position
Definition: MSVehicle.h:151
The vehicle is blocked by right follower.
bool mayChange(int direction) const
whether changing to the lane in the given direction should be considered
double getSpeedLat() const
return the lateral speed of the current lane change maneuver
Interface for lane-change models.
SUMOVehicleClass getVehicleClass() const
Get this vehicle type&#39;s vehicle class.
ChangerIt myCandi
static const Position INVALID
used to indicate that a position is valid
Definition: Position.h:277
double nextStopDist() const
return the distance to the next stop or doubleMax if there is none.
Definition: MSVehicle.h:1009
MSLane * getParallelLane(int offset) const
Returns the lane with the given offset parallel to this one or 0 if it does not exist.
Definition: MSLane.cpp:1962
void endLaneChangeManeuver(const MSMoveReminder::Notification reason=MSMoveReminder::NOTIFICATION_LANE_CHANGE)