Eclipse 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-2020 German Aerospace Center (DLR) and others.
4 // This program and the accompanying materials are made available under the
5 // terms of the Eclipse Public License 2.0 which is available at
6 // https://www.eclipse.org/legal/epl-2.0/
7 // This Source Code may also be made available under the following Secondary
8 // Licenses when the conditions for such availability set forth in the Eclipse
9 // Public License 2.0 are satisfied: GNU General Public License, version 2
10 // or later which is available at
11 // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12 // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13 /****************************************************************************/
23 // Performs lane changing of vehicles
24 /****************************************************************************/
25 #include <config.h>
26 
27 #include "MSLaneChanger.h"
28 #include "MSNet.h"
29 #include "MSVehicle.h"
30 #include "MSVehicleType.h"
31 #include "MSVehicleTransfer.h"
32 #include "MSGlobals.h"
33 #include <cassert>
34 #include <iterator>
35 #include <cstdlib>
36 #include <cmath>
41 
42 #define OPPOSITE_OVERTAKING_SAFE_TIMEGAP 0.0
43 #define OPPOSITE_OVERTAKING_SAFETYGAP_HEADWAY_FACTOR 0.0
44 #define OPPOSITE_OVERTAKING_SAFETY_FACTOR 1.2
45 // XXX maxLookAhead should be higher if all leaders are stopped and lower when they are jammed/queued
46 #define OPPOSITE_OVERTAKING_MAX_LOOKAHEAD 150.0 // just a guess
47 #define OPPOSITE_OVERTAKING_MAX_LOOKAHEAD_EMERGENCY 1000.0 // just a guess
48 // this is used for finding oncoming vehicles while driving in the opposite direction
49 #define OPPOSITE_OVERTAKING_ONCOMING_LOOKAHEAD 200.0 // just a guess
50 
51 // ===========================================================================
52 // debug defines
53 // ===========================================================================
54 
55 //#define DEBUG_CONTINUE_CHANGE
56 //#define DEBUG_CHECK_CHANGE
57 //#define DEBUG_SURROUNDING_VEHICLES // debug getRealFollower() and getRealLeader()
58 //#define DEBUG_CHANGE_OPPOSITE
59 //#define DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
60 //#define DEBUG_ACTIONSTEPS
61 //#define DEBUG_STATE
62 //#define DEBUG_CANDIDATE
63 //#define DEBUG_COND (vehicle->getLaneChangeModel().debugVehicle())
64 #define DEBUG_COND (vehicle->isSelected())
65 
66 
67 
68 // ===========================================================================
69 // ChangeElem member method definitions
70 // ===========================================================================
72  lead(nullptr),
73  lane(_lane),
74  hoppedVeh(nullptr),
75  lastBlocked(nullptr),
76  firstBlocked(nullptr),
77  ahead(lane),
78  aheadNext(lane, nullptr, 0) {
79 }
80 
81 void
83  //std::cout << SIMTIME << " registerHop lane=" << lane->getID() << " veh=" << vehicle->getID() << "\n";
84  lane->myTmpVehicles.insert(lane->myTmpVehicles.begin(), vehicle);
85  dens += vehicle->getVehicleType().getLengthWithGap();
86  hoppedVeh = vehicle;
87 }
88 
89 
90 // ===========================================================================
91 // member method definitions
92 // ===========================================================================
93 MSLaneChanger::MSLaneChanger(const std::vector<MSLane*>* lanes, bool allowChanging) :
94  myAllowsChanging(allowChanging),
95  myChangeToOpposite(lanes->front()->getEdge().canChangeToOpposite()) {
96 
97  // Fill the changer with the lane-data.
98  myChanger.reserve(lanes->size());
99  for (std::vector<MSLane*>::const_iterator lane = lanes->begin(); lane != lanes->end(); ++lane) {
100  myChanger.push_back(ChangeElem(*lane));
101  myChanger.back().mayChangeRight = lane != lanes->begin();
102  myChanger.back().mayChangeLeft = (lane + 1) != lanes->end();
103  // avoid changing on internal sibling lane
104  if ((*lane)->isInternal()) {
105  if (myChanger.back().mayChangeRight && (*lane)->getLogicalPredecessorLane() == (*(lane - 1))->getLogicalPredecessorLane()) {
106  myChanger.back().mayChangeRight = false;
107  }
108  if (myChanger.back().mayChangeLeft && (*lane)->getLogicalPredecessorLane() == (*(lane + 1))->getLogicalPredecessorLane()) {
109  myChanger.back().mayChangeLeft = false;
110  }
111  }
112  }
113 }
114 
115 
117 }
118 
119 
120 void
122  // This is what happens in one timestep. After initialization of the
123  // changer, each vehicle will try to change. After that the changer
124  // needs an update to prevent multiple changes of one vehicle.
125  // Finally, the change-result has to be given back to the lanes.
126  initChanger();
127  try {
128  while (vehInChanger()) {
129  const bool haveChanged = change();
130  updateChanger(haveChanged);
131  }
132  updateLanes(t);
133  } catch (const ProcessError&) {
134  // clean up locks or the gui may hang
135  for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
136  ce->lane->releaseVehicles();
137  }
138  throw;
139  }
140 }
141 
142 
143 void
145  // Prepare myChanger with a safe state.
146  for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
147  ce->lead = nullptr;
148  ce->hoppedVeh = nullptr;
149  ce->lastBlocked = nullptr;
150  ce->firstBlocked = nullptr;
151  ce->dens = 0;
152  ce->lane->getVehiclesSecure();
153 
154  //std::cout << SIMTIME << " initChanger lane=" << ce->lane->getID() << " vehicles=" << toString(ce->lane->myVehicles) << "\n";
155  }
156 }
157 
158 
159 void
160 MSLaneChanger::updateChanger(bool vehHasChanged) {
161  assert(veh(myCandi) != 0);
162 
163  // "Push" the vehicles to the back, i.e. follower becomes vehicle,
164  // vehicle becomes leader, and leader becomes predecessor of vehicle,
165  // if it exists.
166  if (!vehHasChanged) {
167  //std::cout << SIMTIME << " updateChanger: lane=" << myCandi->lane->getID() << " has new lead=" << veh(myCandi)->getID() << "\n";
168  myCandi->lead = veh(myCandi);
169  }
170 
171  MSLane::VehCont& vehicles = myCandi->lane->myVehicles;
172  vehicles.pop_back();
173  //std::cout << SIMTIME << " updateChanger lane=" << myCandi->lane->getID() << " vehicles=" << toString(myCandi->lane->myVehicles) << "\n";
174 }
175 
176 
177 void
179 
180  // Update the lane's vehicle-container.
181  // First: it is bad style to change other classes members, but for
182  // this release, other attempts were too time-consuming. In a next
183  // release we will change from this lane-centered design to a vehicle-
184  // centered. This will solve many problems.
185  // Second: this swap would be faster if vehicle-containers would have
186  // been pointers, but then I had to change too much of the MSLane code.
187  for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
188  //std::cout << SIMTIME << " updateLanes lane=" << ce->lane->getID() << " myVehicles=" << toString(ce->lane->myVehicles) << " myTmpVehicles=" << toString(ce->lane->myTmpVehicles) << "\n";
189  ce->lane->swapAfterLaneChange(t);
190  ce->lane->releaseVehicles();
191  }
192 }
193 
194 
197  // Find the vehicle in myChanger with the largest position. If there
198  // is no vehicle in myChanger (shouldn't happen) , return myChanger.end().
199  ChangerIt max = myChanger.end();
200 #ifdef DEBUG_CANDIDATE
201  std::cout << SIMTIME << " findCandidate() on edge " << myChanger.begin()->lane->getEdge().getID() << std::endl;
202 #endif
203 
204  for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
205  if (veh(ce) == nullptr) {
206  continue;
207  }
208 #ifdef DEBUG_CANDIDATE
209  std::cout << " lane = " << ce->lane->getID() << "\n";
210  std::cout << " check vehicle=" << veh(ce)->getID() << " pos=" << veh(ce)->getPositionOnLane() << " lane=" << ce->lane->getID() << " isFrontOnLane=" << veh(ce)->isFrontOnLane(ce->lane) << "\n";
211 #endif
212  if (max == myChanger.end()) {
213 #ifdef DEBUG_CANDIDATE
214  std::cout << " new max vehicle=" << veh(ce)->getID() << " pos=" << veh(ce)->getPositionOnLane() << " lane=" << ce->lane->getID() << " isFrontOnLane=" << veh(ce)->isFrontOnLane(ce->lane) << "\n";
215 #endif
216  max = ce;
217  continue;
218  }
219  assert(veh(ce) != 0);
220  assert(veh(max) != 0);
221  if (veh(max)->getPositionOnLane() < veh(ce)->getPositionOnLane()) {
222 #ifdef DEBUG_CANDIDATE
223  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";
224 #endif
225  max = ce;
226  }
227  }
228  assert(max != myChanger.end());
229  assert(veh(max) != 0);
230  return max;
231 }
232 
233 
234 bool
235 MSLaneChanger::mayChange(int direction) const {
236  if (direction == 0) {
237  return true;
238  }
239  if (!myAllowsChanging) {
240  return false;
241  }
242  if (direction == -1) {
243  return myCandi->mayChangeRight && (myCandi - 1)->lane->allowsVehicleClass(veh(myCandi)->getVehicleType().getVehicleClass());
244  } else if (direction == 1) {
245  return myCandi->mayChangeLeft && (myCandi + 1)->lane->allowsVehicleClass(veh(myCandi)->getVehicleType().getVehicleClass());
246  } else {
247  return false;
248  }
249 }
250 
251 
252 bool
254  // Find change-candidate. If it is on an allowed lane, try to change
255  // to the right (there is a rule in Germany that you have to change
256  // to the right, unless you are overtaking). If change to the right
257  // isn't possible, check if there is a possibility to overtake (on the
258  // left.
259  // If candidate isn't on an allowed lane, changing to an allowed has
260  // priority.
261 
262 #ifdef DEBUG_ACTIONSTEPS
263 // std::cout<< "\nCHANGE" << std::endl;
264 #endif
265 
266 
268  MSVehicle* vehicle = veh(myCandi);
269  vehicle->getLaneChangeModel().clearNeighbors();
270 
271  if (vehicle->getLaneChangeModel().isChangingLanes() && !vehicle->getLaneChangeModel().alreadyChanged()) {
272  return continueChange(vehicle, myCandi);
273  }
274  vehicle->getLaneChangeModel().setSpeedLat(0);
275  if (!myAllowsChanging || vehicle->getLaneChangeModel().alreadyChanged() || vehicle->isStoppedOnLane()) {
276  registerUnchanged(vehicle);
277  return false;
278  }
279 
280  if (!vehicle->isActive()) {
281 #ifdef DEBUG_ACTIONSTEPS
282  if (DEBUG_COND) {
283  std::cout << SIMTIME << " veh '" << vehicle->getID() << "' skips regular change checks." << std::endl;
284  }
285 #endif
286  bool changed = false;
287  const int oldstate = vehicle->getLaneChangeModel().getOwnState();
288  // let TraCI influence the wish to change lanes during non-actionsteps
289  checkTraCICommands(vehicle);
290  if (oldstate != vehicle->getLaneChangeModel().getOwnState()) {
291  changed = applyTraCICommands(vehicle);
292  }
293  if (!changed) {
294  registerUnchanged(vehicle);
295  }
296  return changed;
297  }
298 
299  // Check for changes to the opposite lane if vehicle is active
300  std::pair<MSVehicle* const, double> leader = getRealLeader(myCandi);
301  if (myChanger.size() == 1 || vehicle->getLaneChangeModel().isOpposite() || (!mayChange(-1) && !mayChange(1))) {
302  if (changeOpposite(leader)) {
303  return true;
304  }
305  registerUnchanged(vehicle);
306  return false;
307  }
308 
309  vehicle->updateBestLanes(); // needed?
310  for (int i = 0; i < (int) myChanger.size(); ++i) {
311  vehicle->adaptBestLanesOccupation(i, myChanger[i].dens);
312  }
313 
314  const std::vector<MSVehicle::LaneQ>& preb = vehicle->getBestLanes();
315  // check whether the vehicle wants and is able to change to right lane
316  int stateRight = 0;
317  if (mayChange(-1)) {
318  stateRight = checkChangeWithinEdge(-1, leader, preb);
319  // change if the vehicle wants to and is allowed to change
320  if ((stateRight & LCA_RIGHT) != 0 && (stateRight & LCA_BLOCKED) == 0) {
321  vehicle->getLaneChangeModel().setOwnState(stateRight);
322  return startChange(vehicle, myCandi, -1);
323  }
324  if ((stateRight & LCA_RIGHT) != 0 && (stateRight & LCA_URGENT) != 0) {
325  (myCandi - 1)->lastBlocked = vehicle;
326  if ((myCandi - 1)->firstBlocked == nullptr) {
327  (myCandi - 1)->firstBlocked = vehicle;
328  }
329  }
330  }
331 
332  // check whether the vehicle wants and is able to change to left lane
333  int stateLeft = 0;
334  if (mayChange(1)) {
335  stateLeft = checkChangeWithinEdge(1, leader, preb);
336  // change if the vehicle wants to and is allowed to change
337  if ((stateLeft & LCA_LEFT) != 0 && (stateLeft & LCA_BLOCKED) == 0) {
338  vehicle->getLaneChangeModel().setOwnState(stateLeft);
339  return startChange(vehicle, myCandi, 1);
340  }
341  if ((stateLeft & LCA_LEFT) != 0 && (stateLeft & LCA_URGENT) != 0) {
342  (myCandi + 1)->lastBlocked = vehicle;
343  if ((myCandi + 1)->firstBlocked == nullptr) {
344  (myCandi + 1)->firstBlocked = vehicle;
345  }
346  }
347  }
348 
349  if ((stateRight & LCA_URGENT) != 0 && (stateLeft & LCA_URGENT) != 0) {
350  // ... wants to go to the left AND to the right
351  // just let them go to the right lane...
352  stateLeft = 0;
353  }
354  vehicle->getLaneChangeModel().setOwnState(stateRight | stateLeft);
355 
356  // only emergency vehicles should change to the opposite side on a
357  // multi-lane road
358  if (vehicle->getVehicleType().getVehicleClass() == SVC_EMERGENCY
359  && changeOpposite(leader)) {
360  return true;
361  }
362 
363  registerUnchanged(vehicle);
364  return false;
365 }
366 
367 
368 void
370  //std::cout << SIMTIME << " registerUnchanged lane=" << myCandi->lane->getID() << " veh=" << vehicle->getID() << "\n";
371  myCandi->lane->myTmpVehicles.insert(myCandi->lane->myTmpVehicles.begin(), veh(myCandi));
372  myCandi->dens += vehicle->getVehicleType().getLengthWithGap();
373  vehicle->getLaneChangeModel().unchanged();
374 }
375 
376 
377 
378 void
380 #ifdef DEBUG_STATE
381  const int oldstate = vehicle->getLaneChangeModel().getOwnState();
382 #endif
384 #ifdef DEBUG_STATE
385  if (DEBUG_COND) {
386  const int newstate = vehicle->getLaneChangeModel().getOwnState();
387  std::cout << SIMTIME
388  << " veh=" << vehicle->getID()
389  << " oldState=" << toString((LaneChangeAction) oldstate)
390  << " newState=" << toString((LaneChangeAction) newstate)
391  << ((newstate & LCA_BLOCKED) != 0 ? " (blocked)" : "")
392  << ((newstate & LCA_OVERLAPPING) != 0 ? " (overlap)" : "")
393  << "\n";
394  }
395 #endif
396 }
397 
398 
399 bool
401  // Execute request if not blocked
402  bool changed = false;
403  const int state = vehicle->getLaneChangeModel().getOwnState();
404  const int dir = (state & LCA_RIGHT) != 0 ? -1 : ((state & LCA_LEFT) != 0 ? 1 : 0);
405  const bool execute = dir != 0 && ((state & LCA_BLOCKED) == 0);
406  if (execute) {
407  ChangerIt to = myCandi + dir;
408  bool continuous = vehicle->getLaneChangeModel().startLaneChangeManeuver(myCandi->lane, to->lane, dir);
409  if (continuous) {
410  changed = continueChange(vehicle, myCandi);
411  } else {
412  // insert vehicle into target lane
413  to->registerHop(vehicle);
414  changed = true;
415  }
416  }
417  return changed;
418 }
419 
420 
421 bool
422 MSLaneChanger::startChange(MSVehicle* vehicle, ChangerIt& from, int direction) {
423  if (vehicle->isRemoteControlled()) {
424  registerUnchanged(vehicle);
425  return false;
426  }
427  ChangerIt to = from + direction;
428  // @todo delay entering the target lane until the vehicle intersects it
429  // physically (considering lane width and vehicle width)
430  //if (to->lane->getID() == "beg_1") std::cout << SIMTIME << " startChange to lane=" << to->lane->getID() << " myTmpVehiclesBefore=" << toString(to->lane->myTmpVehicles) << "\n";
431  const bool continuous = vehicle->getLaneChangeModel().startLaneChangeManeuver(from->lane, to->lane, direction);
432  if (continuous) {
433  return continueChange(vehicle, myCandi);
434  } else {
435  to->registerHop(vehicle);
436  to->lane->requireCollisionCheck();
437  return true;
438  }
439 }
440 
441 bool
444  const int direction = lcm.isOpposite() ? 1 : lcm.getLaneChangeDirection();
445  const bool pastMidpoint = lcm.updateCompletion(); // computes lcm.mySpeedLat as a side effect
446  const double speedLat = lcm.isOpposite() ? -lcm.getSpeedLat() : lcm.getSpeedLat();
447  vehicle->myState.myPosLat += SPEED2DIST(speedLat);
449  //std::cout << SIMTIME << " veh=" << vehicle->getID() << " dir=" << direction << " pm=" << pastMidpoint << " speedLat=" << speedLat << " posLat=" << vehicle->myState.myPosLat << "\n";
450  if (pastMidpoint) {
451  MSLane* source = myCandi->lane;
452  MSLane* target = source->getParallelLane(direction);
453  vehicle->myState.myPosLat -= direction * 0.5 * (source->getWidth() + target->getWidth());
454  lcm.primaryLaneChanged(source, target, direction);
455  if (&source->getEdge() == &target->getEdge()) {
456  ChangerIt to = from + direction;
457  to->registerHop(vehicle);
458  }
459  target->requireCollisionCheck();
460  } else {
461  from->registerHop(vehicle);
462  from->lane->requireCollisionCheck();
463  }
464  if (!lcm.isChangingLanes()) {
465  vehicle->myState.myPosLat = 0;
466  lcm.endLaneChangeManeuver();
467  }
468  lcm.updateShadowLane();
469  if (lcm.getShadowLane() != nullptr && &lcm.getShadowLane()->getEdge() == &vehicle->getLane()->getEdge()) {
470  // set as hoppedVeh on the shadow lane so it is found as leader on both lanes
471  ChangerIt shadow = pastMidpoint ? from : from + lcm.getShadowDirection();
472  shadow->hoppedVeh = vehicle;
474  }
475  vehicle->myAngle = vehicle->computeAngle();
476  if (lcm.isOpposite()) {
477  vehicle->myAngle += M_PI;
478  }
479 
480 #ifdef DEBUG_CONTINUE_CHANGE
481  if (DEBUG_COND) {
482  std::cout << SIMTIME
483  << " continueChange veh=" << vehicle->getID()
484  << " from=" << Named::getIDSecure(from->lane)
485  << " dir=" << direction
486  << " speedLat=" << speedLat
487  << " pastMidpoint=" << pastMidpoint
488  << " posLat=" << vehicle->getLateralPositionOnLane()
489  //<< " completion=" << lcm.getLaneChangeCompletion()
490  << " shadowLane=" << Named::getIDSecure(lcm.getShadowLane())
491  //<< " shadowHopped=" << Named::getIDSecure(shadow->lane)
492  << "\n";
493  }
494 #endif
495  return pastMidpoint && lcm.getShadowLane() == nullptr;
496 }
497 
498 
499 std::pair<MSVehicle* const, double>
501  assert(veh(myCandi) != 0);
502  MSVehicle* vehicle = veh(myCandi);
503 #ifdef DEBUG_SURROUNDING_VEHICLES
504  if (DEBUG_COND) {
505  std::cout << SIMTIME << " veh '" << vehicle->getID() << "' looks for leader on lc-target lane '" << target->lane->getID() << "'." << std::endl;
506  }
507 #endif
508  // get the leading vehicle on the lane to change to
509  MSVehicle* neighLead = target->lead;
510 
511 #ifdef DEBUG_SURROUNDING_VEHICLES
512  if (DEBUG_COND) {
513  if (neighLead != 0) {
514  std::cout << "Considering '" << neighLead->getID() << "' at position " << neighLead->getPositionOnLane() << std::endl;
515  }
516  }
517 #endif
518 
519  // check whether the hopped vehicle became the leader
520  if (target->hoppedVeh != nullptr) {
521  double hoppedPos = target->hoppedVeh->getPositionOnLane();
522 #ifdef DEBUG_SURROUNDING_VEHICLES
523  if (DEBUG_COND) {
524  std::cout << "Considering hopped vehicle '" << target->hoppedVeh->getID() << "' at position " << hoppedPos << std::endl;
525  }
526 #endif
527  if (hoppedPos > vehicle->getPositionOnLane() && (neighLead == nullptr || neighLead->getPositionOnLane() > hoppedPos)) {
528  neighLead = target->hoppedVeh;
529  //if (vehicle->getID() == "flow.21") std::cout << SIMTIME << " neighLead=" << Named::getIDSecure(neighLead) << " (422)\n";
530  }
531  }
532  if (neighLead == nullptr) {
533 #ifdef DEBUG_SURROUNDING_VEHICLES
534  if (DEBUG_COND) {
535  std::cout << "Looking for leader on consecutive lanes." << std::endl;
536  }
537 #endif
538  // There's no leader on the target lane. Look for leaders on consecutive lanes.
539  // (there might also be partial leaders due to continuous lane changing)
540  MSLane* targetLane = target->lane;
541  const double egoBack = vehicle->getBackPositionOnLane();
542  double leaderBack = targetLane->getLength();
543  for (MSVehicle* pl : targetLane->myPartialVehicles) {
544  double plBack = pl->getBackPositionOnLane(targetLane);
545  if (plBack < leaderBack &&
546  pl->getPositionOnLane(targetLane) + pl->getVehicleType().getMinGap() >= egoBack) {
547  neighLead = pl;
548  leaderBack = plBack;
549  }
550  }
551  if (neighLead != nullptr) {
552 #ifdef DEBUG_SURROUNDING_VEHICLES
553  if (DEBUG_COND) {
554  std::cout << " found leader=" << neighLead->getID() << " (partial)\n";
555  }
556 #endif
557  return std::pair<MSVehicle*, double>(neighLead, leaderBack - vehicle->getPositionOnLane() - vehicle->getVehicleType().getMinGap());
558  }
559  double seen = myCandi->lane->getLength() - vehicle->getPositionOnLane();
560  double speed = vehicle->getSpeed();
561  double dist = vehicle->getCarFollowModel().brakeGap(speed) + vehicle->getVehicleType().getMinGap();
562  // always check for link leaders while on an internal lane
563  if (seen > dist && !myCandi->lane->isInternal()) {
564 #ifdef DEBUG_SURROUNDING_VEHICLES
565  if (DEBUG_COND) {
566  std::cout << " found no leader within dist=" << dist << "\n";
567  }
568 #endif
569  return std::pair<MSVehicle* const, double>(static_cast<MSVehicle*>(nullptr), -1);
570  }
571  const std::vector<MSLane*>& bestLaneConts = vehicle->getBestLanesContinuation(targetLane);
572 
573  std::pair<MSVehicle* const, double> result = target->lane->getLeaderOnConsecutive(dist, seen, speed, *vehicle, bestLaneConts);
574 #ifdef DEBUG_SURROUNDING_VEHICLES
575  if (DEBUG_COND) {
576  std::cout << " found consecutiveLeader=" << Named::getIDSecure(result.first) << "\n";
577  }
578 #endif
579  return result;
580  } else {
581 #ifdef DEBUG_SURROUNDING_VEHICLES
582  if (DEBUG_COND) {
583  std::cout << " found leader=" << neighLead->getID() << "\n";
584  }
585 #endif
586  return std::pair<MSVehicle* const, double>(neighLead, neighLead->getBackPositionOnLane(target->lane) - vehicle->getPositionOnLane() - vehicle->getVehicleType().getMinGap());
587  }
588 }
589 
590 
591 std::pair<MSVehicle* const, double>
593  assert(veh(myCandi) != 0);
594  MSVehicle* vehicle = veh(myCandi);
595 #ifdef DEBUG_SURROUNDING_VEHICLES
596  if (DEBUG_COND) {
597  std::cout << SIMTIME << " veh '" << vehicle->getID() << "' looks for follower on lc-target lane '" << target->lane->getID() << "'." << std::endl;
598  }
599 #endif
600  const double candiPos = vehicle->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(vehicle));
629  if (partialBehind != 0 && partialBehind != neighFollow) {
630  std::cout << "'Partial behind'-vehicle '" << target->lane->getPartialBehind(vehicle)->getID() << "' at position " << partialBehind->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(vehicle));
636 
637  if (neighFollow == nullptr) {
638  CLeaderDist consecutiveFollower = target->lane->getFollowersOnConsecutive(vehicle, vehicle->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  return std::pair<MSVehicle* const, double>(neighFollow,
656  vehicle->getPositionOnLane() - vehicle->getVehicleType().getLength() - neighFollow->getPositionOnLane() - neighFollow->getVehicleType().getMinGap());
657  }
658 }
659 
660 
661 MSVehicle*
662 MSLaneChanger::getCloserFollower(const double maxPos, MSVehicle* follow1, MSVehicle* follow2) {
663  if (follow1 == nullptr || follow1->getPositionOnLane() > maxPos) {
664  return follow2;
665  } else if (follow2 == nullptr || follow2->getPositionOnLane() > maxPos) {
666  return follow1;
667  } else {
668  if (follow1->getPositionOnLane() > follow2->getPositionOnLane()) {
669  return follow1;
670  } else {
671  return follow2;
672  }
673  }
674 }
675 
676 int
678  int laneOffset,
679  const std::pair<MSVehicle* const, double>& leader,
680  const std::vector<MSVehicle::LaneQ>& preb) const {
681 
682  std::pair<MSVehicle* const, double> neighLead = getRealLeader(myCandi + laneOffset);
683  std::pair<MSVehicle*, double> neighFollow = getRealFollower(myCandi + laneOffset);
684  if (neighLead.first != nullptr && neighLead.first == neighFollow.first) {
685  // vehicles should not be leader and follower at the same time to avoid
686  // contradictory behavior
687  neighFollow.first = 0;
688  }
689  ChangerIt target = myCandi + laneOffset;
690  return checkChange(laneOffset, target->lane, leader, neighLead, neighFollow, preb);
691 }
692 
693 int
695  int laneOffset,
696  const MSLane* targetLane,
697  const std::pair<MSVehicle* const, double>& leader,
698  const std::pair<MSVehicle* const, double>& neighLead,
699  const std::pair<MSVehicle* const, double>& neighFollow,
700  const std::vector<MSVehicle::LaneQ>& preb) const {
701 
702  MSVehicle* vehicle = veh(myCandi);
703 
704 #ifdef DEBUG_CHECK_CHANGE
705  if (DEBUG_COND) {
706  std::cout
707  << "\n" << SIMTIME << " checkChange() for vehicle '" << vehicle->getID() << "'"
708  << std::endl;
709  }
710 #endif
711 
712  int blocked = 0;
713  int blockedByLeader = (laneOffset == -1 ? LCA_BLOCKED_BY_RIGHT_LEADER : LCA_BLOCKED_BY_LEFT_LEADER);
714  int blockedByFollower = (laneOffset == -1 ? LCA_BLOCKED_BY_RIGHT_FOLLOWER : LCA_BLOCKED_BY_LEFT_FOLLOWER);
715  // overlap
716  if (neighFollow.first != nullptr && neighFollow.second < 0) {
717  blocked |= (blockedByFollower | LCA_OVERLAPPING);
718 
719  // Debug (Leo)
720 #ifdef DEBUG_CHECK_CHANGE
721  if (DEBUG_COND) {
722  std::cout << SIMTIME
723  << " overlapping with follower..."
724  << std::endl;
725  }
726 #endif
727 
728  }
729  if (neighLead.first != nullptr && neighLead.second < 0) {
730  blocked |= (blockedByLeader | LCA_OVERLAPPING);
731 
732 #ifdef DEBUG_CHECK_CHANGE
733  if (DEBUG_COND) {
734  std::cout << SIMTIME
735  << " overlapping with leader..."
736  << std::endl;
737  }
738 #endif
739 
740  }
741  double secureFrontGap = MSAbstractLaneChangeModel::NO_NEIGHBOR;
742  double secureBackGap = MSAbstractLaneChangeModel::NO_NEIGHBOR;
743  double secureOrigFrontGap = MSAbstractLaneChangeModel::NO_NEIGHBOR;
744 
745  const double tauRemainder = vehicle->getActionStepLength() == DELTA_T ? 0 : MAX2(vehicle->getCarFollowModel().getHeadwayTime() - TS, 0.);
746  // safe back gap
747  if ((blocked & blockedByFollower) == 0 && neighFollow.first != nullptr) {
748  // Calculate secure gap conservatively with vNextFollower / vNextLeader as
749  // extrapolated speeds after the driver's expected reaction time (tau).
750  // NOTE: there exists a possible source for collisions if the follower and the leader
751  // have desynchronized action steps as the extrapolated speeds can be exceeded in this case
752 
753  // Expected reaction time (tau) for the follower-vehicle.
754  // (substracted TS since at this point the vehicles' states are already updated)
755  const double vNextFollower = neighFollow.first->getSpeed() + MAX2(0., tauRemainder * neighFollow.first->getAcceleration());
756  const double vNextLeader = vehicle->getSpeed() + MIN2(0., tauRemainder * vehicle->getAcceleration());
757  // !!! eigentlich: vsafe braucht die Max. Geschwindigkeit beider Spuren
758  secureBackGap = neighFollow.first->getCarFollowModel().getSecureGap(neighFollow.first, vehicle, vNextFollower,
759  vNextLeader, vehicle->getCarFollowModel().getMaxDecel());
760 #ifdef DEBUG_CHECK_CHANGE
761  if (DEBUG_COND) {
762  std::cout << SIMTIME
763  << " backGap=" << neighFollow.second
764  << " vNextFollower=" << vNextFollower
765  << " vNextEgo=" << vNextLeader
766  << " secureGap=" << secureBackGap
767  << " safetyFactor=" << vehicle->getLaneChangeModel().getSafetyFactor()
768  << " blocked=" << (neighFollow.second < secureBackGap * vehicle->getLaneChangeModel().getSafetyFactor())
769  << "\n";
770  }
771 #endif
772  if (neighFollow.second < secureBackGap * vehicle->getLaneChangeModel().getSafetyFactor()) {
773  blocked |= blockedByFollower;
774  }
775  }
776 
777  // safe front gap
778  if ((blocked & blockedByLeader) == 0 && neighLead.first != nullptr) {
779  // Calculate secure gap conservatively with vNextFollower / vNextLeader as
780  // extrapolated speeds after the driver's expected reaction time (tau).
781  // NOTE: there exists a possible source for collisions if the follower and the leader
782  // have desynchronized action steps as the extrapolated speeds can be exceeded in this case
783 
784  // Expected reaction time (tau) for the follower-vehicle.
785  // (substracted TS since at this point the vehicles' states are already updated)
786  const double vNextFollower = vehicle->getSpeed() + MAX2(0., tauRemainder * vehicle->getAcceleration());
787  const double vNextLeader = neighLead.first->getSpeed() + MIN2(0., tauRemainder * neighLead.first->getAcceleration());
788  // !!! eigentlich: vsafe braucht die Max. Geschwindigkeit beider Spuren
789  secureFrontGap = vehicle->getCarFollowModel().getSecureGap(vehicle, neighLead.first, vNextFollower,
790  vNextLeader, neighLead.first->getCarFollowModel().getMaxDecel());
791 #ifdef DEBUG_CHECK_CHANGE
792  if (DEBUG_COND) {
793  std::cout << SIMTIME
794  << " frontGap=" << neighFollow.second
795  << " vNextEgo=" << vNextFollower
796  << " vNextLeader=" << vNextLeader
797  << " secureGap=" << secureFrontGap
798  << " safetyFactor=" << vehicle->getLaneChangeModel().getSafetyFactor()
799  << " blocked=" << (neighLead.second < secureFrontGap * vehicle->getLaneChangeModel().getSafetyFactor())
800  << "\n";
801  }
802 #endif
803  if (neighLead.second < secureFrontGap * vehicle->getLaneChangeModel().getSafetyFactor()) {
804  blocked |= blockedByLeader;
805  }
806  }
807  if (blocked == 0 && targetLane->hasPedestrians()) {
808  PersonDist nextLeader = targetLane->nextBlocking(vehicle->getBackPositionOnLane(),
809  vehicle->getRightSideOnLane(), vehicle->getRightSideOnLane() + vehicle->getVehicleType().getWidth(),
810  ceil(vehicle->getSpeed() / vehicle->getCarFollowModel().getMaxDecel()));
811  if (nextLeader.first != 0) {
812  const double brakeGap = vehicle->getCarFollowModel().brakeGap(vehicle->getSpeed());
813  // returned gap value is relative to backPosition
814  const double gap = nextLeader.second - vehicle->getVehicleType().getLengthWithGap();
815 #ifdef DEBUG_CHECK_CHANGE
816  if (DEBUG_COND) {
817  std::cout << SIMTIME << " pedestrian on road " + leader.first->getID() << " gap=" << gap << " brakeGap=" << brakeGap << "\n";
818  }
819 #endif
820  if (brakeGap > gap) {
821  blocked |= blockedByLeader;
822 #ifdef DEBUG_CHECK_CHANGE
823  if (DEBUG_COND) {
824  std::cout << SIMTIME << " blocked by pedestrian " + leader.first->getID() << "\n";
825  }
826 #endif
827  }
828  }
829  }
830 
831  if (leader.first != nullptr) {
832  secureOrigFrontGap = vehicle->getCarFollowModel().getSecureGap(vehicle, leader.first, vehicle->getSpeed(), leader.first->getSpeed(), leader.first->getCarFollowModel().getMaxDecel());
833  }
834 
835  MSAbstractLaneChangeModel::MSLCMessager msg(leader.first, neighLead.first, neighFollow.first);
836  int state = blocked | vehicle->getLaneChangeModel().wantsChange(
837  laneOffset, msg, blocked, leader, neighLead, neighFollow, *targetLane, preb, &(myCandi->lastBlocked), &(myCandi->firstBlocked));
838 
839  if (blocked == 0 && (state & LCA_WANTS_LANECHANGE) != 0 && neighLead.first != nullptr) {
840  // do a more careful (but expensive) check to ensure that a
841  // safety-critical leader is not being overlooked
842  // while changing on an intersection, it is not sufficient to abort the
843  // search with a leader on the current lane because all linkLeaders must
844  // be considered as well
845  const double seen = myCandi->lane->getLength() - vehicle->getPositionOnLane();
846  const double speed = vehicle->getSpeed();
847  const double dist = vehicle->getCarFollowModel().brakeGap(speed) + vehicle->getVehicleType().getMinGap();
848  if (seen < dist || myCandi->lane->isInternal()) {
849  std::pair<MSVehicle* const, double> neighLead2 = targetLane->getCriticalLeader(dist, seen, speed, *vehicle);
850  if (neighLead2.first != nullptr && neighLead2.first != neighLead.first) {
851  const double secureGap = vehicle->getCarFollowModel().getSecureGap(vehicle, neighLead2.first, vehicle->getSpeed(),
852  neighLead2.first->getSpeed(), neighLead2.first->getCarFollowModel().getMaxDecel());
853  const double secureGap2 = secureGap * vehicle->getLaneChangeModel().getSafetyFactor();
854 #ifdef DEBUG_SURROUNDING_VEHICLES
855  if (DEBUG_COND) {
856  std::cout << SIMTIME << " found critical leader=" << neighLead2.first->getID()
857  << " gap=" << neighLead2.second << " secGap=" << secureGap << " secGap2=" << secureGap2 << "\n";
858  }
859 #endif
860  if (neighLead2.second < secureGap2) {
861  state |= blockedByLeader;
862  }
863  }
864  }
865  }
866  if (blocked == 0 && (state & LCA_WANTS_LANECHANGE)) {
867  // ensure that merging is safe for any upcoming zipper links after changing
868  if (vehicle->unsafeLinkAhead(targetLane)) {
869  state |= blockedByLeader;
870  }
871  }
872 
873  if ((state & LCA_BLOCKED) == 0 && (state & LCA_WANTS_LANECHANGE) != 0 && MSGlobals::gLaneChangeDuration > DELTA_T) {
874  // Ensure that a continuous lane change manoeuvre can be completed before the next turning movement.
875  // Assume lateral position == 0. (If this should change in the future add + laneOffset*vehicle->getLateralPositionOnLane() to distToNeighLane)
876  const double distToNeighLane = 0.5 * (vehicle->getLane()->getWidth() + targetLane->getWidth());
877  // Extrapolate the LC duration if operating with speed dependent lateral speed.
878  const MSAbstractLaneChangeModel& lcm = vehicle->getLaneChangeModel();
879  const double assumedDecel = lcm.getAssumedDecelForLaneChangeDuration();
880  const double estimatedLCDuration = lcm.estimateLCDuration(vehicle->getSpeed(), distToNeighLane, assumedDecel);
881  if (estimatedLCDuration == -1) {
882  // Can't guarantee that LC will succeed if vehicle is braking -> assert(lcm.myMaxSpeedLatStanding==0)
883 #ifdef DEBUG_CHECK_CHANGE
884  if (DEBUG_COND) {
885  std::cout << SIMTIME << " checkChange() too slow to guarantee completion of continuous lane change."
886  << "\nestimatedLCDuration=" << estimatedLCDuration
887  << "\ndistToNeighLane=" << distToNeighLane
888  << std::endl;
889  }
890 #endif
891  state |= LCA_INSUFFICIENT_SPEED;
892  } else {
893  // Compute covered distance, when braking for the whole lc duration
894  const double decel = vehicle->getCarFollowModel().getMaxDecel() * estimatedLCDuration;
895  const double avgSpeed = 0.5 * (
896  MAX2(0., vehicle->getSpeed() - ACCEL2SPEED(vehicle->getCarFollowModel().getMaxDecel())) +
897  MAX2(0., vehicle->getSpeed() - decel));
898  // Distance required for lane change.
899  const double space2change = avgSpeed * estimatedLCDuration;
900  // Available distance for LC maneuver (distance till next turn)
901  double seen = myCandi->lane->getLength() - vehicle->getPositionOnLane();
902 #ifdef DEBUG_CHECK_CHANGE
903  if (DEBUG_COND) {
904  std::cout << SIMTIME << " checkChange() checking continuous lane change..."
905  << "\ndistToNeighLane=" << distToNeighLane
906  << " estimatedLCDuration=" << estimatedLCDuration
907  << " space2change=" << space2change
908  << " avgSpeed=" << avgSpeed
909  << std::endl;
910  }
911 #endif
912 
913  // for finding turns it doesn't matter whether we look along the current lane or the target lane
914  const std::vector<MSLane*>& bestLaneConts = vehicle->getBestLanesContinuation();
915  int view = 1;
916  const MSLane* nextLane = vehicle->getLane();
917  std::vector<MSLink*>::const_iterator link = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
918  while (!nextLane->isLinkEnd(link) && seen <= space2change) {
919  if ((*link)->getDirection() == LinkDirection::LEFT || (*link)->getDirection() == LinkDirection::RIGHT
920  // the lanes after an internal junction are on different
921  // edges and do not allow lane-changing
922  || (nextLane->getEdge().isInternal() && (*link)->getViaLaneOrLane()->getEdge().isInternal())
923  ) {
924  state |= LCA_INSUFFICIENT_SPACE;
925  break;
926  }
927  if ((*link)->getViaLane() == nullptr) {
928  view++;
929  }
930  nextLane = (*link)->getViaLaneOrLane();
931  seen += nextLane->getLength();
932  // get the next link used
933  link = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
934  }
935 #ifdef DEBUG_CHECK_CHANGE
936  if (DEBUG_COND) {
937  std::cout << " available distance=" << seen << std::endl;
938  }
939 #endif
940  if (nextLane->isLinkEnd(link) && seen < space2change) {
941 #ifdef DEBUG_CHECK_CHANGE
942  if (DEBUG_COND) {
943  std::cout << SIMTIME << " checkChange insufficientSpace: seen=" << seen << " space2change=" << space2change << "\n";
944  }
945 #endif
946  state |= LCA_INSUFFICIENT_SPACE;
947  }
948 
949  if ((state & LCA_BLOCKED) == 0) {
950  // check for dangerous leaders in case the target lane changes laterally between
951  // now and the lane-changing midpoint
952  const double speed = vehicle->getSpeed();
953  seen = myCandi->lane->getLength() - vehicle->getPositionOnLane();
954  nextLane = vehicle->getLane();
955  view = 1;
956  const double dist = vehicle->getCarFollowModel().brakeGap(speed) + vehicle->getVehicleType().getMinGap();
957  std::vector<MSLink*>::const_iterator nextLink = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
958  while (!nextLane->isLinkEnd(nextLink) && seen <= space2change && seen <= dist) {
959  nextLane = (*nextLink)->getViaLaneOrLane();
960  const MSLane* const parallelLane = nextLane->getParallelLane(laneOffset);
961  if (parallelLane == nullptr) {
962  state |= LCA_INSUFFICIENT_SPACE;
963  break;
964  } else {
965  std::pair<MSVehicle* const, double> neighLead2 = parallelLane->getLeader(vehicle, -seen, std::vector<MSLane*>());
966  if (neighLead2.first != nullptr && neighLead2.first != neighLead.first
967  && (neighLead2.second < vehicle->getCarFollowModel().getSecureGap(vehicle, neighLead2.first,
968  vehicle->getSpeed(), neighLead2.first->getSpeed(), neighLead2.first->getCarFollowModel().getMaxDecel()))) {
969  state |= blockedByLeader;
970  break;
971  }
972  }
973  if ((*nextLink)->getViaLane() == nullptr) {
974  view++;
975  }
976  seen += nextLane->getLength();
977  // get the next link used
978  nextLink = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
979  }
980  }
981  }
982  }
983  const int oldstate = state;
984  // let TraCI influence the wish to change lanes and the security to take
985  state = vehicle->influenceChangeDecision(state);
986 #ifdef DEBUG_CHECK_CHANGE
987  if (DEBUG_COND) {
988  std::cout << SIMTIME
989  << " veh=" << vehicle->getID()
990  << " oldState=" << toString((LaneChangeAction)oldstate)
991  << " newState=" << toString((LaneChangeAction)state)
992  << ((blocked & LCA_BLOCKED) ? " (blocked)" : "")
993  << ((blocked & LCA_OVERLAPPING) ? " (overlap)" : "")
994  << "\n";
995  }
996 #endif
997  vehicle->getLaneChangeModel().saveLCState(laneOffset, oldstate, state);
998  if (blocked == 0 && (state & LCA_WANTS_LANECHANGE)) {
999  // this lane change will be executed, save gaps
1000  vehicle->getLaneChangeModel().setFollowerGaps(neighFollow, secureBackGap);
1001  vehicle->getLaneChangeModel().setLeaderGaps(neighLead, secureFrontGap);
1002  vehicle->getLaneChangeModel().setOrigLeaderGaps(leader, secureOrigFrontGap);
1003  }
1004  if (laneOffset != 0) {
1005  vehicle->getLaneChangeModel().saveNeighbors(laneOffset, neighFollow, neighLead);
1006  }
1007  return state;
1008 }
1009 
1010 
1011 bool
1012 MSLaneChanger::changeOpposite(std::pair<MSVehicle*, double> leader) {
1013  if (!myChangeToOpposite) {
1014  return false;
1015  }
1016  myCandi = findCandidate();
1017  MSVehicle* vehicle = veh(myCandi);
1018  MSLane* source = vehicle->getMutableLane();
1019  if (vehicle->getLaneChangeModel().getModelID() == LCM_SL2015) {
1020  // we have warned before but people may still try
1021  return false;
1022  }
1023  if (vehicle->isStopped()) {
1024  // stopped vehicles obviously should not change lanes. Usually this is
1025  // prevent by appropriate bestLane distances
1026  return false;
1027  }
1028  const bool isOpposite = vehicle->getLaneChangeModel().isOpposite();
1029  int ret = 0;
1030  ret = vehicle->influenceChangeDecision(ret);
1031  bool oppositeChangeByTraci = false;
1032  // Check whether a lane change to the opposite direction was requested via TraCI
1033  if ((ret & (LCA_TRACI)) != 0) {
1034  if (isOpposite && (ret & LCA_LEFT) != 0) {
1035  // stay on the opposite side
1036  return false;
1037  }
1038  oppositeChangeByTraci = true;
1039  }
1040  if (!isOpposite && leader.first == 0 && !oppositeChangeByTraci) {
1041  // no reason to change unless there is a leader
1042  // or we are changing back to the propper direction
1043  // XXX also check whether the leader is so far away as to be irrelevant
1044  return false;
1045  }
1046  if (!isOpposite && !oppositeChangeByTraci
1047  && vehicle->getVClass() != SVC_EMERGENCY
1048  && leader.first != 0) {
1049  if (leader.first->signalSet(MSGlobals::gLefthand
1051  // do not try to overtake a vehicle that is about to turn left or wants
1052  // to change left itself
1053 #ifdef DEBUG_CHANGE_OPPOSITE
1054  if (DEBUG_COND) {
1055  std::cout << " not overtaking leader " << leader.first->getID() << " that has blinker set\n";
1056  }
1057 #endif
1058  return false;
1059  } else if (leader.second < 0) {
1060  // leaders is either a junction leader (that cannot be overtaken) or something else is wrong
1061 #ifdef DEBUG_CHANGE_OPPOSITE
1062  if (DEBUG_COND) {
1063  std::cout << " not overtaking leader " << leader.first->getID() << " with gap " << leader.second << "\n";
1064  }
1065 #endif
1066  return false;
1067  }
1068  }
1069 
1070 
1071  MSLane* opposite = source->getOpposite();
1072  //There is no lane for opposite driving
1073  if (opposite == nullptr || !opposite->allowsVehicleClass(vehicle->getVClass())) {
1074  return false;
1075  }
1076  // changing into the opposite direction is always to the left (XXX except for left-hand networkds)
1077  int direction = isOpposite ? -1 : 1;
1078  std::pair<MSVehicle*, double> neighLead((MSVehicle*)nullptr, -1);
1079 
1080  // preliminary sanity checks for overtaking space
1081  double timeToOvertake;
1082  double spaceToOvertake;
1083 
1084  // we need to find two vehicles:
1085  // 1) the leader that shall be overtaken (not necessarily the current leader but one of its leaders that has enough space in front)
1086  // 2) the oncoming vehicle (we need to look past vehicles that are currentlyovertaking through the opposite direction themselves)
1087  //
1088  // if the vehicle is driving normally, then the search for 1) starts on the current lane and 2) on the opposite lane
1089  // if the vehicle is driving on the opposite side then 1) is found on the neighboring lane and 2) on the current lane
1090 
1091  std::pair<MSVehicle*, double> overtaken;
1092 
1093  if (!isOpposite && !oppositeChangeByTraci) {
1094  overtaken = getColumnleader(vehicle, leader);
1095  if (overtaken.first == 0) {
1096  return false;
1097  }
1098 #ifdef DEBUG_CHANGE_OPPOSITE
1099  if (DEBUG_COND) {
1100  std::cout << " compute time/space to overtake for columnLeader=" << overtaken.first->getID() << " egoGap=" << overtaken.second << "\n";
1101  }
1102 #endif
1103  computeOvertakingTime(vehicle, overtaken.first, overtaken.second, timeToOvertake, spaceToOvertake);
1104  // check for upcoming stops
1105  if (vehicle->nextStopDist() < spaceToOvertake) {
1106 #ifdef DEBUG_CHANGE_OPPOSITE
1107  if (DEBUG_COND) {
1108  std::cout << " cannot changeOpposite due to upcoming stop (dist=" << vehicle->nextStopDist() << " spaceToOvertake=" << spaceToOvertake << ")\n";
1109  }
1110 #endif
1111  return false;
1112  }
1113  neighLead = opposite->getOppositeLeader(vehicle, timeToOvertake * opposite->getSpeedLimit() * 2 + spaceToOvertake, true);
1114 
1115 #ifdef DEBUG_CHANGE_OPPOSITE
1116  if (DEBUG_COND) {
1117  std::cout << SIMTIME
1118  << " veh=" << vehicle->getID()
1119  << " changeOpposite opposite=" << opposite->getID()
1120  << " lead=" << Named::getIDSecure(leader.first)
1121  << " timeToOvertake=" << timeToOvertake
1122  << " spaceToOvertake=" << spaceToOvertake
1123  << "\n";
1124  }
1125 #endif
1126  // check for dangerous oncoming leader
1127  if (neighLead.first != 0) {
1128  const MSVehicle* oncoming = neighLead.first;
1129  // conservative: assume that the oncoming vehicle accelerates to its maximum speed
1130  const double oncomingSpeed = oncoming->isStopped() ? 0 : oncoming->getLane()->getVehicleMaxSpeed(oncoming);
1131  const double safetyGap = ((oncomingSpeed + vehicle->getLane()->getVehicleMaxSpeed(vehicle))
1132  * vehicle->getCarFollowModel().getHeadwayTime()
1134  const double surplusGap = neighLead.second - spaceToOvertake - timeToOvertake * oncomingSpeed - safetyGap;
1135 #ifdef DEBUG_CHANGE_OPPOSITE
1136  if (DEBUG_COND) {
1137  std::cout << SIMTIME
1138  << " oncoming=" << oncoming->getID()
1139  << " oncomingGap=" << neighLead.second
1140  << " leaderGap=" << leader.second
1141  << " safetyGap=" << safetyGap
1142  << " surplusGap=" << surplusGap
1143  << "\n";
1144  }
1145 #endif
1146  if (surplusGap < 0) {
1147 
1148 #ifdef DEBUG_CHANGE_OPPOSITE
1149  if (DEBUG_COND) {
1150  std::cout << " cannot changeOpposite due to dangerous oncoming (surplusGap=" << surplusGap << ")\n";
1151  }
1152 #endif
1153 
1154 #ifdef DEBUG_CHANGE_OPPOSITE
1155  if (DEBUG_COND) {
1156  if (oncoming->getLaneChangeModel().isOpposite()) {
1157  std::cout << SIMTIME << " ego=" << vehicle->getID() << " does not changeOpposite due to dangerous oncoming " << oncoming->getID() << " (but the leader is also opposite)\n";
1158  }
1159  }
1160 #endif
1161  return false;
1162  }
1163  }
1164 
1165  } else if (!oppositeChangeByTraci) {
1166  timeToOvertake = -1;
1167  // look forward as far as possible
1168  spaceToOvertake = std::numeric_limits<double>::max();
1170  leader = source->getOppositeLeader(vehicle, dist, true);
1171  double gap = leader.second;
1172  while (leader.first != nullptr && leader.first->getLaneChangeModel().isOpposite() && dist > 0) {
1173  // look beyond leaders that are also driving in the opposite direction until finding an oncoming leader or exhausting the look-ahead distance
1174 #ifdef DEBUG_CHANGE_OPPOSITE
1175  if (DEBUG_COND) {
1176  std::cout << SIMTIME << " ego=" << vehicle->getID() << " opposite leader=" << leader.first->getID() << " gap=" << gap << " is driving against the flow\n";
1177  }
1178 #endif
1179  const double gapToLeaderFront = leader.second + leader.first->getVehicleType().getLengthWithGap();
1180  if (gapToLeaderFront < 0) {
1181  return false;
1182  }
1183  dist -= gapToLeaderFront;
1184  leader = source->getOppositeLeader(leader.first, dist, true);
1185  if (leader.first != 0) {
1186  gap += gapToLeaderFront;
1187  }
1188  }
1189  leader.second = gap;
1190  // -1 will use getMaximumBrakeDist() as look-ahead distance
1191  neighLead = opposite->getOppositeLeader(vehicle, -1, false);
1192  } else {
1193  timeToOvertake = STEPS2TIME(vehicle->getInfluencer().getLaneTimeLineDuration());//todo discuss concept
1194  spaceToOvertake = timeToOvertake * vehicle->getLane()->getVehicleMaxSpeed(vehicle);
1195  }
1196  // compute remaining space on the opposite side
1197  // 1. the part that remains on the current lane
1198  double usableDist = isOpposite ? vehicle->getPositionOnLane() : source->getLength() - vehicle->getPositionOnLane();
1199 
1200  if (usableDist < spaceToOvertake) {
1201  // look forward along the next lanes
1202  const std::vector<MSLane*>& bestLaneConts = vehicle->getBestLanesContinuation();
1203  assert(bestLaneConts.size() >= 1);
1204  std::vector<MSLane*>::const_iterator it = bestLaneConts.begin() + 1;
1205  while (usableDist < spaceToOvertake && it != bestLaneConts.end()) {
1206 #ifdef DEBUG_CHANGE_OPPOSITE
1207  if (DEBUG_COND) {
1208  std::cout << " usableDist=" << usableDist << " opposite=" << Named::getIDSecure((*it)->getOpposite()) << "\n";
1209  }
1210 #endif
1211  if ((*it)->getOpposite() == nullptr || !(*it)->getOpposite()->allowsVehicleClass(vehicle->getVClass())) {
1212  // opposite lane ends
1213  break;
1214  }
1215  // do not overtake past a minor link or turn
1216  const MSLane* const prev = *(it - 1);
1217  if (prev != nullptr) {
1218  const MSLink* link = prev->getLinkTo(*it);
1219  if (link == nullptr || link->getState() == LINKSTATE_ZIPPER
1221  || (!link->havePriority()
1222  // consider traci-influence
1223  && (!vehicle->hasInfluencer() || vehicle->getInfluencer().getRespectJunctionPriority())
1224  // consider junction model parameters
1225  && ((!link->haveRed() && !link->haveYellow()) || !vehicle->ignoreRed(link, true)))) {
1226 #ifdef DEBUG_CHANGE_OPPOSITE
1227  if (DEBUG_COND) {
1228  std::cout << " stop lookahead at link=" << (link == 0 ? "NULL" : link->getViaLaneOrLane()->getID()) << " state=" << (link == 0 ? "?" : toString(link->getState())) << " ignoreRed=" << vehicle->ignoreRed(link, true) << "\n";
1229  }
1230 #endif
1231  break;
1232  }
1233  }
1234  usableDist += (*it)->getLength();
1235  ++it;
1236  }
1237  }
1238  if (!isOpposite && usableDist < spaceToOvertake) {
1239 #ifdef DEBUG_CHANGE_OPPOSITE
1240  if (DEBUG_COND) {
1241  std::cout << " cannot changeOpposite due to insufficient space (seen=" << usableDist << " spaceToOvertake=" << spaceToOvertake << ")\n";
1242  }
1243 #endif
1244  return false;
1245  }
1246 #ifdef DEBUG_CHANGE_OPPOSITE
1247  if (DEBUG_COND) {
1248  std::cout << " usableDist=" << usableDist << " spaceToOvertake=" << spaceToOvertake << " timeToOvertake=" << timeToOvertake << "\n";
1249  }
1250 #endif
1251  // compute wish to change
1252  // Does "preb" mean "previousBestLanes" ??? If so *rename*
1253  std::vector<MSVehicle::LaneQ> preb = vehicle->getBestLanes();
1254  if (isOpposite) {
1255  // compute the remaining distance that can be drive on the opposite side
1256  // this value will put into LaneQ.length of the leftmost lane
1257  // @note: length counts from the start of the current lane
1258  // @note: see MSLCM_LC2013::_wantsChange @1092 (isOpposite()
1259  MSVehicle::LaneQ& laneQ = preb[preb.size() - 1];
1260  // position on the target lane
1261  const double forwardPos = source->getOppositePos(vehicle->getPositionOnLane());
1262 
1263  // consider usableDist (due to minor links or end of opposite lanes)
1264  laneQ.length = MIN2(laneQ.length, usableDist + forwardPos);
1265  // consider upcoming stops
1266  laneQ.length = MIN2(laneQ.length, vehicle->nextStopDist() + forwardPos);
1267  // consider oncoming leaders
1268  if (leader.first != 0) {
1269  if (!leader.first->getLaneChangeModel().isOpposite()) {
1270  MSVehicle* oncoming = leader.first;
1271  const double oncomingSpeed = oncoming->getAcceleration() > 0 ? oncoming->getLane()->getVehicleMaxSpeed(oncoming) : oncoming->getSpeed();
1272  const double safetyGap = ((oncomingSpeed + vehicle->getLane()->getVehicleMaxSpeed(vehicle))
1273  * vehicle->getCarFollowModel().getHeadwayTime()
1275  laneQ.length = MIN2(laneQ.length, leader.second / 2 + forwardPos - safetyGap);
1276 #ifdef DEBUG_CHANGE_OPPOSITE
1277  if (DEBUG_COND) {
1278  std::cout << SIMTIME << " found oncoming leader=" << oncoming->getID() << " gap=" << leader.second << "\n";
1279  }
1280 #endif
1281  } else {
1282 #ifdef DEBUG_CHANGE_OPPOSITE
1283  if (DEBUG_COND) {
1284  std::cout << SIMTIME << " opposite leader=" << leader.first->getID() << " gap=" << leader.second << " is driving against the flow\n";
1285  }
1286 #endif
1287  }
1288  if (neighLead.first != 0) {
1289  overtaken = getColumnleader(vehicle, neighLead);
1290  if (overtaken.first == 0) {
1291 #ifdef DEBUG_CHANGE_OPPOSITE
1292  if (DEBUG_COND) {
1293  std::cout << SIMTIME << " ego=" << vehicle->getID() << " did not find columnleader to overtake\n";
1294  }
1295 #endif
1296  } else {
1297  const double remainingDist = laneQ.length - forwardPos;
1298  computeOvertakingTime(vehicle, overtaken.first, overtaken.second, timeToOvertake, spaceToOvertake);
1299 #ifdef DEBUG_CHANGE_OPPOSITE
1300  if (DEBUG_COND) {
1301  std::cout << SIMTIME << " ego=" << vehicle->getID() << " is overtaking " << overtaken.first->getID()
1302  << " remainingDist=" << remainingDist << " spaceToOvertake=" << spaceToOvertake << " timeToOvertake=" << timeToOvertake << "\n";
1303  }
1304 #endif
1305  if (remainingDist > spaceToOvertake) {
1306  // exaggerate remaining dist so that the vehicle continues
1307  // overtaking (otherwise the lane change model might abort prematurely)
1308  laneQ.length += 1000;
1309  }
1310  }
1311  }
1312  leader.first = 0; // ignore leader after this
1313  }
1314 #ifdef DEBUG_CHANGE_OPPOSITE
1315  if (DEBUG_COND) {
1316  std::cout << SIMTIME << " veh=" << vehicle->getID() << " remaining dist=" << laneQ.length - forwardPos << " forwardPos=" << forwardPos << " laneQ.length=" << laneQ.length << "\n";
1317  }
1318 #endif
1319  }
1320  std::pair<MSVehicle* const, double> neighFollow = opposite->getOppositeFollower(vehicle);
1321  int state = checkChange(direction, opposite, leader, neighLead, neighFollow, preb);
1322  vehicle->getLaneChangeModel().setOwnState(state);
1323 
1324  bool changingAllowed = (state & LCA_BLOCKED) == 0;
1325  // change if the vehicle wants to and is allowed to change
1326  if ((state & LCA_WANTS_LANECHANGE) != 0 && changingAllowed
1327  // do not change to the opposite direction for cooperative reasons
1328  && (isOpposite || (state & LCA_COOPERATIVE) == 0)) {
1329  const bool continuous = vehicle->getLaneChangeModel().startLaneChangeManeuver(source, opposite, direction);
1330 #ifdef DEBUG_CHANGE_OPPOSITE
1331  if (DEBUG_COND) {
1332  std::cout << SIMTIME << " changing to opposite veh=" << vehicle->getID() << " dir=" << direction << " opposite=" << Named::getIDSecure(opposite) << " state=" << state << "\n";
1333  }
1334 #endif
1335  if (continuous) {
1336  continueChange(vehicle, myCandi);
1337  }
1338  return true;
1339  }
1340 #ifdef DEBUG_CHANGE_OPPOSITE
1341  if (DEBUG_COND) {
1342  std::cout << SIMTIME << " not changing to opposite veh=" << vehicle->getID() << " dir=" << direction
1343  << " opposite=" << Named::getIDSecure(opposite) << " state=" << toString((LaneChangeAction)state) << "\n";
1344  }
1345 #endif
1346  return false;
1347 }
1348 
1349 
1350 void
1351 MSLaneChanger::computeOvertakingTime(const MSVehicle* vehicle, const MSVehicle* leader, double gap, double& timeToOvertake, double& spaceToOvertake) {
1352  // Assumptions:
1353  // - leader maintains the current speed
1354  // - vehicle merges with maxSpeed ahead of leader
1355  // XXX affected by ticket #860 (the formula is invalid for the current position update rule)
1356 
1357  // first compute these values for the case where vehicle is accelerating
1358  // without upper bound on speed
1359  const double vMax = vehicle->getLane()->getVehicleMaxSpeed(vehicle);
1360  const double v = vehicle->getSpeed();
1361  const double u = leader->getAcceleration() > 0 ? leader->getLane()->getVehicleMaxSpeed(leader) : leader->getSpeed();
1362  const double a = vehicle->getCarFollowModel().getMaxAccel();
1363  const double d = vehicle->getCarFollowModel().getMaxDecel();
1364  const double g = MAX2(0.0, (
1365  // drive up to the rear of leader
1366  gap + vehicle->getVehicleType().getMinGap()
1367  // drive head-to-head with the leader
1368  + leader->getVehicleType().getLengthWithGap()
1369  // drive past the leader
1370  + vehicle->getVehicleType().getLength()
1371  // allow for safe gap between leader and vehicle
1372  + leader->getCarFollowModel().getSecureGap(leader, vehicle, u, vMax, d)));
1373  const double sign = -1; // XXX recheck
1374  // v*t + t*t*a*0.5 = g + u*t
1375  // solve t
1376  // t = ((u - v - (((((2.0*(u - v))**2.0) + (8.0*a*g))**(1.0/2.0))*sign/2.0))/a)
1377  double t = (u - v - sqrt(4 * (u - v) * (u - v) + 8 * a * g) * sign * 0.5) / a;
1378 #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
1379  if (DEBUG_COND) {
1380  std::cout << " computeOvertakingTime v=" << v << " vMax=" << vMax << " u=" << u << " a=" << a << " d=" << d << " gap=" << gap << " g=" << g << " t=" << t
1381  << " distEgo=" << v* t + t* t* a * 0.5 << " distLead=" << g + u* t
1382  << "\n";
1383  }
1384 #endif
1385  assert(t >= 0);
1386 
1387  // allow for a safety time gap
1389  // round to multiples of step length (TS)
1390  t = ceil(t / TS) * TS;
1391 
1393  const double timeToMaxSpeed = (vMax - v) / a;
1394 
1395 #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
1396  if (DEBUG_COND) {
1397  std::cout << " t=" << t << " tvMax=" << timeToMaxSpeed << "\n";
1398  }
1399 #endif
1400  if (t <= timeToMaxSpeed) {
1401  timeToOvertake = t;
1402  spaceToOvertake = v * t + t * t * a * 0.5;
1403 #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
1404  if (DEBUG_COND) {
1405  std::cout << " sto=" << spaceToOvertake << "\n";
1406  }
1407 #endif
1408  } else {
1409  // space until max speed is reached
1410  const double s = v * timeToMaxSpeed + timeToMaxSpeed * timeToMaxSpeed * a * 0.5;
1411  const double m = timeToMaxSpeed;
1412  // s + (t-m) * vMax = g + u*t
1413  // solve t
1414  t = (g - s + m * vMax) / (vMax - u);
1415  if (t < 0) {
1416  // cannot overtake in time
1417 #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
1418  if (DEBUG_COND) {
1419  std::cout << " t2=" << t << "\n";
1420  }
1421 #endif
1422  timeToOvertake = std::numeric_limits<double>::max();
1423  spaceToOvertake = std::numeric_limits<double>::max();
1424  } else {
1425  // allow for a safety time gap
1427  // round to multiples of step length (TS)
1428  t = ceil(t / TS) * TS;
1429 
1430  timeToOvertake = t;
1431  spaceToOvertake = s + (t - m) * vMax;
1432 #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
1433  if (DEBUG_COND) {
1434  std::cout << " t2=" << t << " s=" << s << " sto=" << spaceToOvertake << " m=" << m << "\n";
1435  }
1436 #endif
1437  }
1438  }
1439  const double safetyFactor = OPPOSITE_OVERTAKING_SAFETY_FACTOR * vehicle->getLaneChangeModel().getOppositeSafetyFactor();
1440  timeToOvertake *= safetyFactor;
1441  spaceToOvertake *= safetyFactor;
1442 #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
1443  if (DEBUG_COND) {
1444  if (safetyFactor != 1) {
1445  std::cout << " applying safetyFactor=" << safetyFactor
1446  << " tto=" << timeToOvertake << " sto=" << spaceToOvertake << "\n";
1447  }
1448  }
1449 #endif
1450 
1451 }
1452 
1453 
1454 
1455 std::pair<MSVehicle*, double>
1456 MSLaneChanger::getColumnleader(MSVehicle* vehicle, std::pair<MSVehicle*, double> leader, double maxLookAhead) {
1457  assert(leader.first != 0);
1458  const MSLane* source = vehicle->getLane();
1459  // find a leader vehicle with sufficient space ahead for merging back
1460  const double overtakingSpeed = source->getVehicleMaxSpeed(vehicle); // just a guess
1461  const double mergeBrakeGap = vehicle->getCarFollowModel().brakeGap(overtakingSpeed);
1462  std::pair<MSVehicle*, double> columnLeader = leader;
1463  double egoGap = leader.second;
1464  bool foundSpaceAhead = false;
1465  double seen = leader.second + leader.first->getVehicleType().getLengthWithGap();
1466  std::vector<MSLane*> conts = vehicle->getBestLanesContinuation();
1467  if (maxLookAhead == std::numeric_limits<double>::max()) {
1468  maxLookAhead = (vehicle->getVehicleType().getVehicleClass() == SVC_EMERGENCY
1471  }
1472 #ifdef DEBUG_CHANGE_OPPOSITE
1473  if (DEBUG_COND) {
1474  std::cout << " getColumnleader vehicle=" << vehicle->getID() << " leader=" << leader.first->getID() << " gap=" << leader.second << " maxLookAhead=" << maxLookAhead << "\n";
1475  }
1476 #endif
1477  const double safetyFactor = OPPOSITE_OVERTAKING_SAFETY_FACTOR * vehicle->getLaneChangeModel().getOppositeSafetyFactor();
1478  while (!foundSpaceAhead) {
1479  const double requiredSpaceAfterLeader = (columnLeader.first->getCarFollowModel().getSecureGap(
1480  columnLeader.first, vehicle,
1481  columnLeader.first->getSpeed(), overtakingSpeed, vehicle->getCarFollowModel().getMaxDecel())
1482  + columnLeader.first->getVehicleType().getMinGap()
1483  + vehicle->getVehicleType().getLengthWithGap());
1484 
1485 
1486  // all leader vehicles on the current laneChanger edge are already moved into MSLane::myTmpVehicles
1487  const bool checkTmpVehicles = (&columnLeader.first->getLane()->getEdge() == &source->getEdge());
1488  std::pair<MSVehicle* const, double> leadLead = columnLeader.first->getLane()->getLeader(
1489  columnLeader.first, columnLeader.first->getPositionOnLane(), conts, requiredSpaceAfterLeader + mergeBrakeGap,
1490  checkTmpVehicles);
1491 
1492 #ifdef DEBUG_CHANGE_OPPOSITE
1493  if (DEBUG_COND) {
1494  std::cout << " leadLead=" << Named::getIDSecure(leadLead.first) << " gap=" << leadLead.second << "\n";
1495  }
1496 #endif
1497  if (leadLead.first == nullptr) {
1498  double availableSpace = columnLeader.first->getLane()->getLength() - columnLeader.first->getPositionOnLane();
1499  const double requiredSpace = safetyFactor * (requiredSpaceAfterLeader
1500  + vehicle->getCarFollowModel().brakeGap(overtakingSpeed));
1501 #ifdef DEBUG_CHANGE_OPPOSITE
1502  if (DEBUG_COND) {
1503  std::cout << " no direct leader found after columnLeader " << columnLeader.first->getID()
1504  << " availableSpace=" << availableSpace
1505  << " req1=" << requiredSpaceAfterLeader
1506  << " req2=" << requiredSpace / safetyFactor
1507  << " req3=" << requiredSpace
1508  << "\n";
1509  }
1510 #endif
1511  if (availableSpace > requiredSpace) {
1512  foundSpaceAhead = true;
1513  } else {
1514  // maybe the columnleader is stopped before a junction or takes a different turn.
1515  // try to find another columnleader on successive lanes
1516  const MSLane* next = getLaneAfter(columnLeader.first->getLane(), conts);
1517 #ifdef DEBUG_CHANGE_OPPOSITE
1518  if (DEBUG_COND) {
1519  std::cout << " look for another leader on lane " << Named::getIDSecure(next) << "\n";
1520  }
1521 #endif
1522  while (next != nullptr && seen < maxLookAhead) {
1523  seen += next->getLength();
1524  MSVehicle* cand = next->getLastAnyVehicle();
1525  if (cand == nullptr) {
1526  availableSpace += next->getLength();
1527  if (availableSpace > requiredSpace) {
1528  foundSpaceAhead = true;
1529  break;
1530  }
1531  } else {
1532  availableSpace += cand->getBackPositionOnLane();
1533  if (availableSpace > requiredSpace) {
1534  foundSpaceAhead = true;
1535  break;
1536  } else {
1537  return getColumnleader(vehicle, std::make_pair(cand, availableSpace + cand->getPositionOnLane()), maxLookAhead - seen);
1538  }
1539  }
1540  }
1541  if (!foundSpaceAhead) {
1542  return std::make_pair(nullptr, -1);
1543  }
1544  }
1545  } else {
1546  const double requiredSpace = safetyFactor * (requiredSpaceAfterLeader
1547  + vehicle->getCarFollowModel().getSecureGap(vehicle, leadLead.first,
1548  overtakingSpeed, leadLead.first->getSpeed(), leadLead.first->getCarFollowModel().getMaxDecel()));
1549 #ifdef DEBUG_CHANGE_OPPOSITE
1550  if (DEBUG_COND) {
1551  std::cout << " leader's leader " << leadLead.first->getID() << " space=" << leadLead.second
1552  << " req1=" << requiredSpaceAfterLeader
1553  << " req2=" << requiredSpace / safetyFactor
1554  << " req3=" << requiredSpace
1555  << "\n";
1556  }
1557 #endif
1558  if (leadLead.second > requiredSpace) {
1559  foundSpaceAhead = true;
1560  } else {
1561 #ifdef DEBUG_CHANGE_OPPOSITE
1562  if (DEBUG_COND) {
1563  std::cout << " not enough space after columnLeader=" << columnLeader.first->getID() << " required=" << requiredSpace << "\n";
1564  }
1565 #endif
1566  seen += MAX2(0., leadLead.second) + leadLead.first->getVehicleType().getLengthWithGap();
1567  if (seen > maxLookAhead) {
1568 #ifdef DEBUG_CHANGE_OPPOSITE
1569  if (DEBUG_COND) {
1570  std::cout << " cannot changeOpposite due to insufficient free space after columnLeader (seen=" << seen << " columnLeader=" << columnLeader.first->getID() << ")\n";
1571  }
1572 #endif
1573  return std::make_pair(nullptr, -1);
1574  }
1575  // see if merging after leadLead is possible
1576  egoGap += columnLeader.first->getVehicleType().getLengthWithGap() + leadLead.second;
1577  columnLeader = leadLead;
1578 #ifdef DEBUG_CHANGE_OPPOSITE
1579  if (DEBUG_COND) {
1580  std::cout << " new columnLeader=" << columnLeader.first->getID() << "\n";
1581  }
1582 #endif
1583  }
1584  }
1585  }
1586  columnLeader.second = egoGap;
1587  return columnLeader;
1588 }
1589 
1590 
1591 MSLane*
1592 MSLaneChanger::getLaneAfter(const MSLane* lane, const std::vector<MSLane*>& conts) {
1593  for (auto it = conts.begin(); it != conts.end(); ++it) {
1594  if (*it == lane) {
1595  if (it + 1 != conts.end()) {
1596  return *(it + 1);
1597  } else {
1598  return nullptr;
1599  }
1600  }
1601  }
1602  return nullptr;
1603 }
1604 
1605 
1606 /****************************************************************************/
#define OPPOSITE_OVERTAKING_MAX_LOOKAHEAD
#define OPPOSITE_OVERTAKING_SAFETYGAP_HEADWAY_FACTOR
#define OPPOSITE_OVERTAKING_MAX_LOOKAHEAD_EMERGENCY
#define OPPOSITE_OVERTAKING_ONCOMING_LOOKAHEAD
#define OPPOSITE_OVERTAKING_SAFETY_FACTOR
#define OPPOSITE_OVERTAKING_SAFE_TIMEGAP
#define DEBUG_COND
std::pair< const MSVehicle *, double > CLeaderDist
Definition: MSLeaderInfo.h:32
std::pair< const MSPerson *, double > PersonDist
Definition: MSPModel.h:39
SUMOTime DELTA_T
Definition: SUMOTime.cpp:37
#define STEPS2TIME(x)
Definition: SUMOTime.h:53
#define SPEED2DIST(x)
Definition: SUMOTime.h:43
#define ACCEL2SPEED(x)
Definition: SUMOTime.h:49
#define TS
Definition: SUMOTime.h:40
#define SIMTIME
Definition: SUMOTime.h:60
long long int SUMOTime
Definition: SUMOTime.h:31
@ SVC_EMERGENCY
public emergency vehicles
@ RIGHT
The link is a (hard) right direction.
@ LEFT
The link is a (hard) left direction.
@ STRAIGHT
The link is a straight direction.
@ LINKSTATE_ZIPPER
This is an uncontrolled, zipper-merge link.
LaneChangeAction
The state of a vehicle's lane-change behavior.
@ LCA_BLOCKED
blocked in all directions
@ LCA_URGENT
The action is urgent (to be defined by lc-model)
@ LCA_BLOCKED_BY_RIGHT_LEADER
The vehicle is blocked by right leader.
@ LCA_INSUFFICIENT_SPACE
The vehicle does not have enough space to complete a continuous change before the next turn.
@ LCA_BLOCKED_BY_LEFT_FOLLOWER
The vehicle is blocked by left follower.
@ LCA_COOPERATIVE
The action is done to help someone else.
@ LCA_INSUFFICIENT_SPEED
Vehicle is too slow to complete a continuous lane change (in case that maxSpeedLatStanding==0)
@ LCA_OVERLAPPING
The vehicle is blocked being overlapping.
@ LCA_LEFT
Wants go to the left.
@ LCA_BLOCKED_BY_RIGHT_FOLLOWER
The vehicle is blocked by right follower.
@ LCA_TRACI
The action is due to a TraCI request.
@ LCA_WANTS_LANECHANGE
lane can change
@ LCA_RIGHT
Wants go to the right.
@ LCA_BLOCKED_BY_LEFT_LEADER
@ LCM_SL2015
T MIN2(T a, T b)
Definition: StdDefs.h:73
T MAX2(T a, T b)
Definition: StdDefs.h:79
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:44
A class responsible for exchanging messages between cars involved in lane-change interaction.
Interface for lane-change models.
void saveLCState(const int dir, const int stateWithoutTraCI, const int state)
void setFollowerGaps(CLeaderDist follower, double secGap)
virtual void setOwnState(const int state)
virtual double getAssumedDecelForLaneChangeDuration() const
Returns a deceleration value which is used for the estimation of the duration of a lane change.
virtual double estimateLCDuration(const double speed, const double remainingManeuverDist, const double decel) const
Calculates the maximal time needed to complete a lane change maneuver if lcMaxSpeedLatFactor and lcMa...
int getLaneChangeDirection() const
return the direction of the current lane change maneuver
MSLane * getShadowLane() const
Returns the lane the vehicle's shadow is on during continuous/sublane lane change.
bool startLaneChangeManeuver(MSLane *source, MSLane *target, int direction)
start the lane change maneuver and return whether it continues
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...
void endLaneChangeManeuver(const MSMoveReminder::Notification reason=MSMoveReminder::NOTIFICATION_LANE_CHANGE)
virtual double getSafetyFactor() const
return factor for modifying the safety constraints of the car-following model
virtual double getOppositeSafetyFactor() const
return factor for modifying the safety constraints for opposite-diretction overtaking of the car-foll...
void setLeaderGaps(CLeaderDist, double secGap)
virtual LaneChangeModel getModelID() const =0
Returns the model's ID;.
void setOrigLeaderGaps(CLeaderDist, double secGap)
void setSpeedLat(double speedLat)
set the lateral speed and update lateral acceleraton
void checkTraCICommands()
Check for commands issued for the vehicle via TraCI and apply the appropriate state changes For the s...
void primaryLaneChanged(MSLane *source, MSLane *target, int direction)
called once when the vehicles primary lane changes
int getShadowDirection() const
return the direction in which the current shadow lane lies
double getSpeedLat() const
return the lateral speed of the current lane change maneuver
bool alreadyChanged() const
reset the flag whether a vehicle already moved to false
bool isChangingLanes() const
return true if the vehicle currently performs a lane change maneuver
void clearNeighbors()
Clear info on neighboring vehicle from previous step.
void saveNeighbors(const int dir, const MSLeaderDistanceInfo &followers, const MSLeaderDistanceInfo &leaders)
Saves the lane change relevant vehicles, which are currently on neighboring lanes in the given direct...
const MSVehicleType & getVehicleType() const
Returns the vehicle's type definition.
SUMOVehicleClass getVClass() const
Returns the vehicle's access class.
bool isStopped() const
Returns whether the vehicle is at a stop.
virtual double getSecureGap(const MSVehicle *const, const MSVehicle *const, 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:328
double getMaxAccel() const
Get the vehicle type's maximum acceleration [m/s^2].
Definition: MSCFModel.h:208
double brakeGap(const double speed) const
Returns the distance the vehicle needs to halt including driver's reaction time tau (i....
Definition: MSCFModel.h:311
double getMaxDecel() const
Get the vehicle type's maximal comfortable deceleration [m/s^2].
Definition: MSCFModel.h:216
virtual double getHeadwayTime() const
Get the driver's desired headway [s].
Definition: MSCFModel.h:257
bool isInternal() const
return whether this edge is an internal edge
Definition: MSEdge.h:256
static bool gLefthand
Whether lefthand-drive is being simulated.
Definition: MSGlobals.h:136
static SUMOTime gLaneChangeDuration
Definition: MSGlobals.h:79
virtual ~MSLaneChanger()
Destructor.
bool applyTraCICommands(MSVehicle *vehicle)
Execute TraCI LC-commands.
bool continueChange(MSVehicle *vehicle, ChangerIt &from)
continue a lane change maneuver and return whether the vehicle has completely moved onto the new lane...
const bool myChangeToOpposite
whether this edge allows changing to the opposite direction edge
static std::pair< MSVehicle *, double > getColumnleader(MSVehicle *vehicle, std::pair< MSVehicle *, double > leader, double maxLookAhead=std::numeric_limits< double >::max())
MSLaneChanger()
Default constructor.
void checkTraCICommands(MSVehicle *vehicle)
Take into account traci LC-commands.
void laneChange(SUMOTime t)
Start lane-change-process for all vehicles on the edge'e lanes.
virtual void initChanger()
Initialize the changer before looping over all vehicles.
bool vehInChanger() const
Check if there is a single change-candidate in the changer. Returns true if there is one.
std::pair< MSVehicle *const, double > getRealFollower(const ChangerIt &target) const
virtual bool change()
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 startChange(MSVehicle *vehicle, ChangerIt &from, int direction)
const bool myAllowsChanging
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.
int checkChangeWithinEdge(int laneOffset, const std::pair< MSVehicle *const, double > &leader, const std::vector< MSVehicle::LaneQ > &preb) const
MSVehicle * veh(ConstChangerIt ce) const
Changer myChanger
Container for ChangeElemements, one for every lane in the edge.
virtual bool changeOpposite(std::pair< MSVehicle *, double > leader)
ChangerIt findCandidate()
Find current candidate. If there is none, myChanger.end() is returned.
bool mayChange(int direction) const
whether changing to the lane in the given direction should be considered
static MSVehicle * getCloserFollower(const double maxPos, MSVehicle *follow1, MSVehicle *follow2)
return the closer follower of ego
void registerUnchanged(MSVehicle *vehicle)
void updateLanes(SUMOTime t)
ChangerIt myCandi
std::pair< MSVehicle *const, double > getRealLeader(const ChangerIt &target) const
Changer::iterator ChangerIt
the iterator moving over the ChangeElems
static MSLane * getLaneAfter(const MSLane *lane, const std::vector< MSLane * > &conts)
return the next lane in conts beyond lane or nullptr
virtual void updateChanger(bool vehHasChanged)
Representation of a lane in the micro simulation.
Definition: MSLane.h:82
MSLane * getParallelLane(int offset, bool includeOpposite=true) const
Returns the lane with the given offset parallel to this one or 0 if it does not exist.
Definition: MSLane.cpp:2211
static std::vector< MSLink * >::const_iterator succLinkSec(const SUMOVehicle &veh, int nRouteSuccs, const MSLane &succLinkSource, const std::vector< MSLane * > &conts)
Definition: MSLane.cpp:2079
std::pair< const MSPerson *, double > nextBlocking(double minPos, double minRight, double maxLeft, double stopTime=0) const
This is just a wrapper around MSPModel::nextBlocking. You should always check using hasPedestrians be...
Definition: MSLane.cpp:3762
const MSLink * getLinkTo(const MSLane *const) const
returns the link to the given lane or nullptr, if it is not connected
Definition: MSLane.cpp:2128
double getSpeedLimit() const
Returns the lane's maximum allowed speed.
Definition: MSLane.h:531
std::vector< MSVehicle * > VehCont
Container for vehicles.
Definition: MSLane.h:92
double getLength() const
Returns the lane's length.
Definition: MSLane.h:539
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:2469
bool isLinkEnd(std::vector< MSLink * >::const_iterator &i) const
Definition: MSLane.h:762
bool allowsVehicleClass(SUMOVehicleClass vclass) const
Definition: MSLane.h:812
double getVehicleMaxSpeed(const SUMOTrafficObject *const veh) const
Returns the lane's maximum speed, given a vehicle's speed limit adaptation.
Definition: MSLane.h:517
std::pair< MSVehicle *const, double > getOppositeFollower(const MSVehicle *ego) const
Definition: MSLane.cpp:3678
bool hasPedestrians() const
whether the lane has pedestrians on it
Definition: MSLane.cpp:3755
void requireCollisionCheck()
require another collision check due to relevant changes in the simulation
Definition: MSLane.h:626
double getOppositePos(double pos) const
return the corresponding position on the opposite lane
Definition: MSLane.cpp:3621
MSVehicle * getLastAnyVehicle() const
returns the last vehicle that is fully or partially on this lane
Definition: MSLane.cpp:2051
VehCont myPartialVehicles
The lane's partial vehicles. This container holds all vehicles that are partially on this lane but wh...
Definition: MSLane.h:1334
MSEdge & getEdge() const
Returns the lane's edge.
Definition: MSLane.h:673
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:2278
MSLane * getOpposite() const
return the opposite direction lane for lane changing or 0
Definition: MSLane.cpp:3612
std::pair< MSVehicle *const, double > getOppositeLeader(const MSVehicle *ego, double dist, bool oppositeDir) const
Definition: MSLane.cpp:3655
double getWidth() const
Returns the lane's width.
Definition: MSLane.h:555
bool getRespectJunctionPriority() const
Returns whether junction priority rules shall be respected.
Definition: MSVehicle.h:1504
SUMOTime getLaneTimeLineDuration()
Definition: MSVehicle.cpp:451
double myPosLat
the stored lateral position
Definition: MSVehicle.h:140
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:77
void updateBestLanes(bool forceRebuild=false, const MSLane *startLane=0)
computes the best lanes to use in order to continue the route
Definition: MSVehicle.cpp:4717
double computeAngle() const
compute the current vehicle angle
Definition: MSVehicle.cpp:1295
bool isStoppedOnLane() const
Definition: MSVehicle.cpp:1442
bool isActive() const
Returns whether the current simulation step is an action point for the vehicle.
Definition: MSVehicle.h:603
bool isFrontOnLane(const MSLane *lane) const
Returns the information whether the front of the vehicle is on the given lane.
Definition: MSVehicle.cpp:4135
MSAbstractLaneChangeModel & getLaneChangeModel()
Definition: MSVehicle.cpp:4699
MSLane * getMutableLane() const
Returns the lane the vehicle is on Non const version indicates that something volatile is going on.
Definition: MSVehicle.h:558
int influenceChangeDecision(int state)
allow TraCI to influence a lane change decision
Definition: MSVehicle.cpp:6093
bool isRemoteControlled() const
Returns the information whether the vehicle is fully controlled via TraCI.
Definition: MSVehicle.cpp:6112
double nextStopDist() const
return the distance to the next stop or doubleMax if there is none.
Definition: MSVehicle.h:1008
void adaptBestLanesOccupation(int laneIndex, double density)
update occupation from MSLaneChanger
Definition: MSVehicle.cpp:5262
double getAcceleration() const
Returns the vehicle's acceleration in m/s (this is computed as the last step's mean acceleration in c...
Definition: MSVehicle.h:483
const std::vector< MSLane * > & getBestLanesContinuation() const
Returns the best sequence of lanes to continue the route starting at myLane.
Definition: MSVehicle.cpp:5106
double getBackPositionOnLane(const MSLane *lane) const
Get the vehicle's position relative to the given lane.
Definition: MSVehicle.cpp:4046
@ VEH_SIGNAL_BLINKER_RIGHT
Right blinker lights are switched on.
Definition: MSVehicle.h:1081
@ VEH_SIGNAL_BLINKER_LEFT
Left blinker lights are switched on.
Definition: MSVehicle.h:1083
SUMOTime getActionStepLength() const
Returns the vehicle's action step length in millisecs, i.e. the interval between two action points.
Definition: MSVehicle.h:494
Influencer & getInfluencer()
Definition: MSVehicle.cpp:6059
double getRightSideOnLane() const
Get the vehicle's lateral position on the lane:
Definition: MSVehicle.cpp:5473
bool unsafeLinkAhead(const MSLane *lane) const
whether the vehicle may safely move to the given lane with regard to upcoming links
Definition: MSVehicle.cpp:5663
double getLateralPositionOnLane() const
Get the vehicle's lateral position on the lane.
Definition: MSVehicle.h:411
double getSpeed() const
Returns the vehicle's current speed.
Definition: MSVehicle.h:458
Position myCachedPosition
Definition: MSVehicle.h:1893
const std::vector< LaneQ > & getBestLanes() const
Returns the description of best lanes to use in order to continue the route.
Definition: MSVehicle.cpp:4711
double myAngle
the angle in radians (
Definition: MSVehicle.h:1885
bool ignoreRed(const MSLink *link, bool canBrake) const
decide whether a red (or yellow light) may be ignore
Definition: MSVehicle.cpp:6136
double getPositionOnLane() const
Get the vehicle's position along the lane.
Definition: MSVehicle.h:374
const MSLane * getLane() const
Returns the lane the vehicle is on.
Definition: MSVehicle.h:550
const MSCFModel & getCarFollowModel() const
Returns the vehicle's car following model definition.
Definition: MSVehicle.h:930
bool hasInfluencer() const
Definition: MSVehicle.h:1651
State myState
This Vehicles driving state (pos and speed)
Definition: MSVehicle.h:1820
double getLengthWithGap() const
Get vehicle's length including the minimum gap [m].
double getWidth() const
Get the width which vehicles of this class shall have when being drawn.
SUMOVehicleClass getVehicleClass() const
Get this vehicle type's vehicle class.
double getMinGap() const
Get the free space in front of vehicles of this class.
double getLength() const
Get vehicle's length [m].
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:66
const std::string & getID() const
Returns the id.
Definition: Named.h:73
static const Position INVALID
used to indicate that a position is valid
Definition: Position.h:282
#define M_PI
Definition: odrSpiral.cpp:40
void registerHop(MSVehicle *vehicle)
Register that vehicle belongs to Changer Item to after LC decisions.
A structure representing the best lanes for continuing the current route starting at 'lane'.
Definition: MSVehicle.h:830
double length
The overall length which may be driven when using this lane without a lane change.
Definition: MSVehicle.h:834