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-2022 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 "MSLink.h"
30 #include "MSVehicle.h"
31 #include "MSVehicleType.h"
32 #include "MSVehicleTransfer.h"
33 #include "MSStop.h"
34 #include "MSGlobals.h"
35 #include <cassert>
36 #include <iterator>
37 #include <cstdlib>
38 #include <cmath>
43 
44 #define OPPOSITE_OVERTAKING_SAFE_TIMEGAP 0.0
45 #define OPPOSITE_OVERTAKING_SAFETYGAP_HEADWAY_FACTOR 0.0
46 #define OPPOSITE_OVERTAKING_SAFETY_FACTOR 1.2
47 // XXX maxLookAhead should be higher if all leaders are stopped and lower when they are jammed/queued
48 #define OPPOSITE_OVERTAKING_MAX_LOOKAHEAD 150.0 // just a guess
49 #define OPPOSITE_OVERTAKING_MAX_LOOKAHEAD_EMERGENCY 1000.0 // just a guess
50 // this is used for finding oncoming vehicles while driving in the opposite direction
51 #define OPPOSITE_OVERTAKING_ONCOMING_LOOKAHEAD 1000.0 // just a guess
52 // do not attempt overtaking maneuvers that would exceed this distance
53 #define OPPOSITE_OVERTAKING_MAX_SPACE_TO_OVERTAKE 1000.0 // just a guess
54 #define OPPOSITE_OVERTAKING_HILLTOP_THRESHOHOLD 5 // (m)
55 
56 // ===========================================================================
57 // debug defines
58 // ===========================================================================
59 
60 //#define DEBUG_CONTINUE_CHANGE
61 //#define DEBUG_CHECK_CHANGE
62 //#define DEBUG_SURROUNDING_VEHICLES // debug getRealFollower() and getRealLeader()
63 //#define DEBUG_CHANGE_OPPOSITE
64 //#define DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
65 //#define DEBUG_ACTIONSTEPS
66 //#define DEBUG_STATE
67 //#define DEBUG_CANDIDATE
68 //#define DEBUG_COND (vehicle->getLaneChangeModel().debugVehicle())
69 #define DEBUG_COND (vehicle->isSelected())
70 //#define DEBUG_COND (true)
71 
72 
73 
74 // ===========================================================================
75 // ChangeElem member method definitions
76 // ===========================================================================
78  lead(nullptr),
79  lane(_lane),
80  hoppedVeh(nullptr),
81  lastBlocked(nullptr),
82  firstBlocked(nullptr),
83  ahead(lane),
84  aheadNext(lane, nullptr, 0) {
85 }
86 
87 void
89  //std::cout << SIMTIME << " registerHop lane=" << lane->getID() << " veh=" << vehicle->getID() << "\n";
90  lane->myTmpVehicles.insert(lane->myTmpVehicles.begin(), vehicle);
91  dens += vehicle->getVehicleType().getLengthWithGap();
92  hoppedVeh = vehicle;
93 }
94 
95 
96 // ===========================================================================
97 // member method definitions
98 // ===========================================================================
99 MSLaneChanger::MSLaneChanger(const std::vector<MSLane*>* lanes, bool allowChanging) :
100  myAllowsChanging(allowChanging),
101  myChangeToOpposite(lanes->front()->getEdge().canChangeToOpposite()) {
102 
103  // Fill the changer with the lane-data.
104  myChanger.reserve(lanes->size());
105  for (std::vector<MSLane*>::const_iterator lane = lanes->begin(); lane != lanes->end(); ++lane) {
106  myChanger.push_back(ChangeElem(*lane));
107  myChanger.back().mayChangeRight = lane != lanes->begin();
108  myChanger.back().mayChangeLeft = (lane + 1) != lanes->end();
109  if ((*lane)->isInternal()) {
110  // avoid changing on internal sibling lane
111  if (myChanger.back().mayChangeRight && (*lane)->getLogicalPredecessorLane() == (*(lane - 1))->getLogicalPredecessorLane()) {
112  myChanger.back().mayChangeRight = false;
113  }
114  if (myChanger.back().mayChangeLeft && (*lane)->getLogicalPredecessorLane() == (*(lane + 1))->getLogicalPredecessorLane()) {
115  myChanger.back().mayChangeLeft = false;
116  }
117  // avoid changing if lanes have different lengths
118  if (myChanger.back().mayChangeRight && (*lane)->getLength() != (*(lane - 1))->getLength()) {
119  //std::cout << " cannot change right from lane=" << (*lane)->getID() << " len=" << (*lane)->getLength() << " to=" << (*(lane - 1))->getID() << " len2=" << (*(lane - 1))->getLength() << "\n";
120  myChanger.back().mayChangeRight = false;
121  }
122  if (myChanger.back().mayChangeLeft && (*lane)->getLength() != (*(lane + 1))->getLength()) {
123  //std::cout << " cannot change left from lane=" << (*lane)->getID() << " len=" << (*lane)->getLength() << " to=" << (*(lane + 1))->getID() << " len2=" << (*(lane + 1))->getLength() << "\n";
124  myChanger.back().mayChangeLeft = false;
125  }
126  }
127  }
128 }
129 
130 
132 }
133 
134 
135 void
137  // This is what happens in one timestep. After initialization of the
138  // changer, each vehicle will try to change. After that the changer
139  // needs an update to prevent multiple changes of one vehicle.
140  // Finally, the change-result has to be given back to the lanes.
141  initChanger();
142  try {
143  while (vehInChanger()) {
144  const bool haveChanged = change();
145  updateChanger(haveChanged);
146  }
147  updateLanes(t);
148  } catch (const ProcessError&) {
149  // clean up locks or the gui may hang
150  for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
151  ce->lane->releaseVehicles();
152  }
153  throw;
154  }
155 }
156 
157 
158 void
160  // Prepare myChanger with a safe state.
161  for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
162  ce->lead = nullptr;
163  ce->hoppedVeh = nullptr;
164  ce->lastBlocked = nullptr;
165  ce->firstBlocked = nullptr;
166  ce->dens = 0;
167  ce->lane->getVehiclesSecure();
168 
169  //std::cout << SIMTIME << " initChanger lane=" << ce->lane->getID() << " vehicles=" << toString(ce->lane->myVehicles) << "\n";
170  }
171 }
172 
173 
174 void
175 MSLaneChanger::updateChanger(bool vehHasChanged) {
176  assert(veh(myCandi) != 0);
177 
178  // "Push" the vehicles to the back, i.e. follower becomes vehicle,
179  // vehicle becomes leader, and leader becomes predecessor of vehicle,
180  // if it exists.
181  if (!vehHasChanged) {
182  //std::cout << SIMTIME << " updateChanger: lane=" << myCandi->lane->getID() << " has new lead=" << veh(myCandi)->getID() << "\n";
183  myCandi->lead = veh(myCandi);
184  }
185 
186  MSLane::VehCont& vehicles = myCandi->lane->myVehicles;
187  vehicles.pop_back();
188  //std::cout << SIMTIME << " updateChanger lane=" << myCandi->lane->getID() << " vehicles=" << toString(myCandi->lane->myVehicles) << "\n";
189 }
190 
191 
192 void
194 
195  // Update the lane's vehicle-container.
196  // First: it is bad style to change other classes members, but for
197  // this release, other attempts were too time-consuming. In a next
198  // release we will change from this lane-centered design to a vehicle-
199  // centered. This will solve many problems.
200  // Second: this swap would be faster if vehicle-containers would have
201  // been pointers, but then I had to change too much of the MSLane code.
202  for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
203  //std::cout << SIMTIME << " updateLanes lane=" << ce->lane->getID() << " myVehicles=" << toString(ce->lane->myVehicles) << " myTmpVehicles=" << toString(ce->lane->myTmpVehicles) << "\n";
204  ce->lane->swapAfterLaneChange(t);
205  ce->lane->releaseVehicles();
206  }
207 }
208 
209 
212  // Find the vehicle in myChanger with the largest position. If there
213  // is no vehicle in myChanger (shouldn't happen) , return myChanger.end().
214  ChangerIt max = myChanger.end();
215 #ifdef DEBUG_CANDIDATE
216  std::cout << SIMTIME << " findCandidate() on edge " << myChanger.begin()->lane->getEdge().getID() << std::endl;
217 #endif
218 
219  for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
220  if (veh(ce) == nullptr) {
221  continue;
222  }
223 #ifdef DEBUG_CANDIDATE
224  std::cout << " lane = " << ce->lane->getID() << "\n";
225  std::cout << " check vehicle=" << veh(ce)->getID() << " pos=" << veh(ce)->getPositionOnLane() << " lane=" << ce->lane->getID() << " isFrontOnLane=" << veh(ce)->isFrontOnLane(ce->lane) << "\n";
226 #endif
227  if (max == myChanger.end()) {
228 #ifdef DEBUG_CANDIDATE
229  std::cout << " new max vehicle=" << veh(ce)->getID() << " pos=" << veh(ce)->getPositionOnLane() << " lane=" << ce->lane->getID() << " isFrontOnLane=" << veh(ce)->isFrontOnLane(ce->lane) << "\n";
230 #endif
231  max = ce;
232  continue;
233  }
234  assert(veh(ce) != 0);
235  assert(veh(max) != 0);
236  if (veh(max)->getPositionOnLane() < veh(ce)->getPositionOnLane()) {
237 #ifdef DEBUG_CANDIDATE
238  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";
239 #endif
240  max = ce;
241  }
242  }
243  assert(max != myChanger.end());
244  assert(veh(max) != 0);
245  return max;
246 }
247 
248 
249 bool
250 MSLaneChanger::mayChange(int direction) const {
251  if (direction == 0) {
252  return true;
253  }
254  if (!myAllowsChanging) {
255  return false;
256  }
258  if (direction == -1) {
259  return myCandi->mayChangeRight && (myCandi - 1)->lane->allowsVehicleClass(svc) && myCandi->lane->allowsChangingRight(svc);
260  } else if (direction == 1) {
261  return myCandi->mayChangeLeft && (myCandi + 1)->lane->allowsVehicleClass(svc) && myCandi->lane->allowsChangingLeft(svc);
262  } else {
263  return false;
264  }
265 }
266 
267 
268 bool
270  // Find change-candidate. If it is on an allowed lane, try to change
271  // to the right (there is a rule in Germany that you have to change
272  // to the right, unless you are overtaking). If change to the right
273  // isn't possible, check if there is a possibility to overtake (on the
274  // left.
275  // If candidate isn't on an allowed lane, changing to an allowed has
276  // priority.
277 
278 #ifdef DEBUG_ACTIONSTEPS
279 // std::cout<< "\nCHANGE" << std::endl;
280 #endif
281 
282 
284  MSVehicle* vehicle = veh(myCandi);
285  vehicle->getLaneChangeModel().clearNeighbors();
286 
287  if (vehicle->getLaneChangeModel().isChangingLanes() && !vehicle->getLaneChangeModel().alreadyChanged()) {
288  return continueChange(vehicle, myCandi);
289  }
290  vehicle->getLaneChangeModel().setSpeedLat(0);
291  if (!myAllowsChanging || vehicle->getLaneChangeModel().alreadyChanged() || vehicle->isStoppedOnLane()) {
292  registerUnchanged(vehicle);
293  return false;
294  }
295 
296  if (!vehicle->isActive()) {
297 #ifdef DEBUG_ACTIONSTEPS
298  if (DEBUG_COND) {
299  std::cout << SIMTIME << " veh '" << vehicle->getID() << "' skips regular change checks." << std::endl;
300  }
301 #endif
302  bool changed = false;
303  const int oldstate = vehicle->getLaneChangeModel().getOwnState();
304  // let TraCI influence the wish to change lanes during non-actionsteps
305  checkTraCICommands(vehicle);
306  if (oldstate != vehicle->getLaneChangeModel().getOwnState()) {
307  changed = applyTraCICommands(vehicle);
308  }
309  if (!changed) {
310  registerUnchanged(vehicle);
311  }
312  return changed;
313  }
314 
315  // Check for changes to the opposite lane if vehicle is active
316  std::pair<MSVehicle* const, double> leader = getRealLeader(myCandi);
317  if (myChanger.size() == 1 || vehicle->getLaneChangeModel().isOpposite() || (!mayChange(-1) && !mayChange(1))) {
318  if (changeOpposite(vehicle, leader)) {
319  return true;
320  }
321  registerUnchanged(vehicle);
322  return false;
323  }
324 
325  vehicle->updateBestLanes(); // needed?
326  for (int i = 0; i < (int) myChanger.size(); ++i) {
327  vehicle->adaptBestLanesOccupation(i, myChanger[i].dens);
328  }
329 
330  const std::vector<MSVehicle::LaneQ>& preb = vehicle->getBestLanes();
331  // check whether the vehicle wants and is able to change to right lane
332  int stateRight = 0;
333  if (mayChange(-1)) {
334  stateRight = checkChangeWithinEdge(-1, leader, preb);
335  // change if the vehicle wants to and is allowed to change
336  if ((stateRight & LCA_RIGHT) != 0 && (stateRight & LCA_BLOCKED) == 0) {
337  vehicle->getLaneChangeModel().setOwnState(stateRight);
338  return startChange(vehicle, myCandi, -1);
339  }
340  if ((stateRight & LCA_RIGHT) != 0 && (stateRight & LCA_URGENT) != 0) {
341  (myCandi - 1)->lastBlocked = vehicle;
342  if ((myCandi - 1)->firstBlocked == nullptr) {
343  (myCandi - 1)->firstBlocked = vehicle;
344  }
345  }
346  }
347 
348  // check whether the vehicle wants and is able to change to left lane
349  int stateLeft = 0;
350  if (mayChange(1)) {
351  stateLeft = checkChangeWithinEdge(1, leader, preb);
352  // change if the vehicle wants to and is allowed to change
353  if ((stateLeft & LCA_LEFT) != 0 && (stateLeft & LCA_BLOCKED) == 0) {
354  vehicle->getLaneChangeModel().setOwnState(stateLeft);
355  return startChange(vehicle, myCandi, 1);
356  }
357  if ((stateLeft & LCA_LEFT) != 0 && (stateLeft & LCA_URGENT) != 0) {
358  (myCandi + 1)->lastBlocked = vehicle;
359  if ((myCandi + 1)->firstBlocked == nullptr) {
360  (myCandi + 1)->firstBlocked = vehicle;
361  }
362  }
363  }
364 
365  if ((stateRight & LCA_URGENT) != 0 && (stateLeft & LCA_URGENT) != 0) {
366  // ... wants to go to the left AND to the right
367  // just let them go to the right lane...
368  stateLeft = 0;
369  }
370  vehicle->getLaneChangeModel().setOwnState(stateRight | stateLeft);
371 
372  // only emergency vehicles should change to the opposite side on a
373  // multi-lane road (or vehicles that need to stop on the opposite side)
374  if ((vehicle->getVehicleType().getVehicleClass() == SVC_EMERGENCY
375  || hasOppositeStop(vehicle))
376  && changeOpposite(vehicle, leader)) {
377  return true;
378  }
379 
380  registerUnchanged(vehicle);
381  return false;
382 }
383 
384 
385 void
387  //std::cout << SIMTIME << " registerUnchanged lane=" << myCandi->lane->getID() << " veh=" << vehicle->getID() << "\n";
388  myCandi->lane->myTmpVehicles.insert(myCandi->lane->myTmpVehicles.begin(), veh(myCandi));
389  myCandi->dens += vehicle->getVehicleType().getLengthWithGap();
390  vehicle->getLaneChangeModel().unchanged();
391 }
392 
393 
394 
395 void
397 #ifdef DEBUG_STATE
398  const int oldstate = vehicle->getLaneChangeModel().getOwnState();
399 #endif
401 #ifdef DEBUG_STATE
402  if (DEBUG_COND) {
403  const int newstate = vehicle->getLaneChangeModel().getOwnState();
404  std::cout << SIMTIME
405  << " veh=" << vehicle->getID()
406  << " oldState=" << toString((LaneChangeAction) oldstate)
407  << " newState=" << toString((LaneChangeAction) newstate)
408  << ((newstate & LCA_BLOCKED) != 0 ? " (blocked)" : "")
409  << ((newstate & LCA_OVERLAPPING) != 0 ? " (overlap)" : "")
410  << "\n";
411  }
412 #endif
413 }
414 
415 
416 bool
418  // Execute request if not blocked
419  bool changed = false;
420  const int state = vehicle->getLaneChangeModel().getOwnState();
421  const int dir = (state & LCA_RIGHT) != 0 ? -1 : ((state & LCA_LEFT) != 0 ? 1 : 0);
422  const bool execute = dir != 0 && ((state & LCA_BLOCKED) == 0);
423  if (execute) {
424  ChangerIt to = myCandi + dir;
425  bool continuous = vehicle->getLaneChangeModel().startLaneChangeManeuver(myCandi->lane, to->lane, dir);
426  if (continuous) {
427  changed = continueChange(vehicle, myCandi);
428  } else {
429  // insert vehicle into target lane
430  to->registerHop(vehicle);
431  changed = true;
432  }
433  }
434  return changed;
435 }
436 
437 
438 bool
439 MSLaneChanger::startChange(MSVehicle* vehicle, ChangerIt& from, int direction) {
440  if (vehicle->isRemoteControlled()) {
441  registerUnchanged(vehicle);
442  return false;
443  }
444  ChangerIt to = from + direction;
445  // @todo delay entering the target lane until the vehicle intersects it
446  // physically (considering lane width and vehicle width)
447  //if (to->lane->getID() == "beg_1") std::cout << SIMTIME << " startChange to lane=" << to->lane->getID() << " myTmpVehiclesBefore=" << toString(to->lane->myTmpVehicles) << "\n";
448  const bool continuous = vehicle->getLaneChangeModel().startLaneChangeManeuver(from->lane, to->lane, direction);
449  if (continuous) {
450  return continueChange(vehicle, myCandi);
451  } else {
452  to->registerHop(vehicle);
453  to->lane->requireCollisionCheck();
454  return true;
455  }
456 }
457 
458 bool
461  const int direction = lcm.isOpposite() ? -lcm.getLaneChangeDirection() : lcm.getLaneChangeDirection();
462  const bool pastMidpoint = lcm.updateCompletion(); // computes lcm.mySpeedLat as a side effect
463  const double speedLat = lcm.isOpposite() ? -lcm.getSpeedLat() : lcm.getSpeedLat();
464  vehicle->myState.myPosLat += SPEED2DIST(speedLat);
466  //std::cout << SIMTIME << " veh=" << vehicle->getID() << " dir=" << direction << " pm=" << pastMidpoint << " speedLat=" << speedLat << " posLat=" << vehicle->myState.myPosLat << "\n";
467  if (pastMidpoint) {
468  MSLane* source = myCandi->lane;
469  MSLane* target = source->getParallelLane(direction);
470  vehicle->myState.myPosLat -= direction * 0.5 * (source->getWidth() + target->getWidth());
471  lcm.primaryLaneChanged(source, target, direction);
472  if (&source->getEdge() == &target->getEdge()) {
473  ChangerIt to = from + direction;
474  to->registerHop(vehicle);
475  }
476  target->requireCollisionCheck();
477  } else {
478  from->registerHop(vehicle);
479  from->lane->requireCollisionCheck();
480  }
481  if (!lcm.isChangingLanes()) {
482  vehicle->myState.myPosLat = 0;
483  lcm.endLaneChangeManeuver();
484  }
485  lcm.updateShadowLane();
486  if (lcm.getShadowLane() != nullptr && &lcm.getShadowLane()->getEdge() == &vehicle->getLane()->getEdge()) {
487  // set as hoppedVeh on the shadow lane so it is found as leader on both lanes
488  myChanger[lcm.getShadowLane()->getIndex()].hoppedVeh = vehicle;
490  }
491  vehicle->myAngle = vehicle->computeAngle();
492  if (lcm.isOpposite()) {
493  vehicle->myAngle += M_PI;
494  }
495 
496 #ifdef DEBUG_CONTINUE_CHANGE
497  if (DEBUG_COND) {
498  std::cout << SIMTIME
499  << " continueChange veh=" << vehicle->getID()
500  << " from=" << Named::getIDSecure(from->lane)
501  << " dir=" << direction
502  << " speedLat=" << speedLat
503  << " pastMidpoint=" << pastMidpoint
504  << " posLat=" << vehicle->getLateralPositionOnLane()
505  << " completion=" << lcm.getLaneChangeCompletion()
506  << " shadowLane=" << Named::getIDSecure(lcm.getShadowLane())
507  //<< " shadowHopped=" << Named::getIDSecure(shadow->lane)
508  << "\n";
509  }
510 #endif
511  return pastMidpoint && lcm.getShadowLane() == nullptr;
512 }
513 
514 
515 std::pair<MSVehicle* const, double>
517  assert(veh(myCandi) != 0);
518  MSVehicle* vehicle = veh(myCandi);
519 #ifdef DEBUG_SURROUNDING_VEHICLES
520  if (DEBUG_COND) {
521  std::cout << SIMTIME << " veh '" << vehicle->getID() << "' looks for leader on lc-target lane '" << target->lane->getID() << "'." << std::endl;
522  }
523 #endif
524  // get the leading vehicle on the lane to change to
525  MSVehicle* neighLead = target->lead;
526 
527 #ifdef DEBUG_SURROUNDING_VEHICLES
528  if (DEBUG_COND) {
529  if (neighLead != 0) {
530  std::cout << "Considering '" << neighLead->getID() << "' at position " << neighLead->getPositionOnLane() << std::endl;
531  }
532  }
533 #endif
534 
535  // check whether the hopped vehicle became the leader
536  if (target->hoppedVeh != nullptr) {
537  double hoppedPos = target->hoppedVeh->getPositionOnLane();
538 #ifdef DEBUG_SURROUNDING_VEHICLES
539  if (DEBUG_COND) {
540  std::cout << "Considering hopped vehicle '" << target->hoppedVeh->getID() << "' at position " << hoppedPos << std::endl;
541  }
542 #endif
543  if (hoppedPos > vehicle->getPositionOnLane() && (neighLead == nullptr || neighLead->getPositionOnLane() > hoppedPos)) {
544  neighLead = target->hoppedVeh;
545  //if (vehicle->getID() == "flow.21") std::cout << SIMTIME << " neighLead=" << Named::getIDSecure(neighLead) << " (422)\n";
546  }
547  }
548  if (neighLead == nullptr) {
549 #ifdef DEBUG_SURROUNDING_VEHICLES
550  if (DEBUG_COND) {
551  std::cout << "Looking for leader on consecutive lanes." << std::endl;
552  }
553 #endif
554  // There's no leader on the target lane. Look for leaders on consecutive lanes.
555  // (there might also be partial leaders due to continuous lane changing)
556  MSLane* targetLane = target->lane;
557  const double egoBack = vehicle->getBackPositionOnLane();
558  double leaderBack = targetLane->getLength();
559  for (MSVehicle* pl : targetLane->myPartialVehicles) {
560  double plBack = pl->getBackPositionOnLane(targetLane);
561  if (plBack < leaderBack &&
562  pl->getPositionOnLane(targetLane) + pl->getVehicleType().getMinGap() >= egoBack) {
563  neighLead = pl;
564  leaderBack = plBack;
565  }
566  }
567  if (neighLead != nullptr) {
568 #ifdef DEBUG_SURROUNDING_VEHICLES
569  if (DEBUG_COND) {
570  std::cout << " found leader=" << neighLead->getID() << " (partial)\n";
571  }
572 #endif
573  return std::pair<MSVehicle*, double>(neighLead, leaderBack - vehicle->getPositionOnLane() - vehicle->getVehicleType().getMinGap());
574  }
575  double seen = myCandi->lane->getLength() - vehicle->getPositionOnLane();
576  double speed = vehicle->getSpeed();
577  double dist = vehicle->getCarFollowModel().brakeGap(speed) + vehicle->getVehicleType().getMinGap();
578  // always check for link leaders while on an internal lane
579  if (seen > dist && !myCandi->lane->isInternal()) {
580 #ifdef DEBUG_SURROUNDING_VEHICLES
581  if (DEBUG_COND) {
582  std::cout << " found no leader within dist=" << dist << "\n";
583  }
584 #endif
585  return std::pair<MSVehicle* const, double>(static_cast<MSVehicle*>(nullptr), -1);
586  }
587  const std::vector<MSLane*>& bestLaneConts = vehicle->getBestLanesContinuation(targetLane);
588 
589  std::pair<MSVehicle* const, double> result = target->lane->getLeaderOnConsecutive(dist, seen, speed, *vehicle, bestLaneConts);
590 #ifdef DEBUG_SURROUNDING_VEHICLES
591  if (DEBUG_COND) {
592  std::cout << " found consecutiveLeader=" << Named::getIDSecure(result.first) << "\n";
593  }
594 #endif
595  return result;
596  } else {
597 #ifdef DEBUG_SURROUNDING_VEHICLES
598  if (DEBUG_COND) {
599  std::cout << " found leader=" << neighLead->getID() << "\n";
600  }
601 #endif
602  return std::pair<MSVehicle* const, double>(neighLead, neighLead->getBackPositionOnLane(target->lane) - vehicle->getPositionOnLane() - vehicle->getVehicleType().getMinGap());
603  }
604 }
605 
606 
607 std::pair<MSVehicle* const, double>
609  assert(veh(myCandi) != 0);
610  MSVehicle* vehicle = veh(myCandi);
611 #ifdef DEBUG_SURROUNDING_VEHICLES
612  if (DEBUG_COND) {
613  std::cout << SIMTIME << " veh '" << vehicle->getID() << "' looks for follower on lc-target lane '" << target->lane->getID() << "'." << std::endl;
614  }
615 #endif
616  const double candiPos = vehicle->getPositionOnLane();
617  MSVehicle* neighFollow = veh(target);
618 
619 #ifdef DEBUG_SURROUNDING_VEHICLES
620  if (DEBUG_COND) {
621  if (neighFollow != 0) {
622  std::cout << "veh(target) returns '" << neighFollow->getID() << "' at position " << neighFollow->getPositionOnLane() << std::endl;
623  } else {
624  std::cout << "veh(target) returns none." << std::endl;
625  }
626  }
627 #endif
628 
629 
630 #ifdef DEBUG_SURROUNDING_VEHICLES
631  if (DEBUG_COND) {
632  if (getCloserFollower(candiPos, neighFollow, target->hoppedVeh) != neighFollow) {
633  std::cout << "Hopped vehicle '" << target->hoppedVeh->getID() << "' at position " << target->hoppedVeh->getPositionOnLane() << " is closer." << std::endl;
634  }
635  }
636 #endif
637 
638  // check whether the hopped vehicle became the follower
639  neighFollow = getCloserFollower(candiPos, neighFollow, target->hoppedVeh);
640 
641 
642 #ifdef DEBUG_SURROUNDING_VEHICLES
643  if (DEBUG_COND) {
644  MSVehicle* partialBehind = getCloserFollower(candiPos, neighFollow, target->lane->getPartialBehind(vehicle));
645  if (partialBehind != 0 && partialBehind != neighFollow) {
646  std::cout << "'Partial behind'-vehicle '" << target->lane->getPartialBehind(vehicle)->getID() << "' at position " << partialBehind->getPositionOnLane() << " is closer." << std::endl;
647  }
648  }
649 #endif
650  // or a follower which is partially lapping into the target lane
651  neighFollow = getCloserFollower(candiPos, neighFollow, target->lane->getPartialBehind(vehicle));
652 
653  if (neighFollow == nullptr || neighFollow == vehicle) {
654  CLeaderDist consecutiveFollower = target->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true)[0];
655 #ifdef DEBUG_SURROUNDING_VEHICLES
656  if (DEBUG_COND) {
657  if (consecutiveFollower.first == 0) {
658  std::cout << "no follower found." << std::endl;
659  } else {
660  std::cout << "found follower '" << consecutiveFollower.first->getID() << "' on consecutive lanes." << std::endl;
661  }
662  }
663 #endif
664  return std::make_pair(const_cast<MSVehicle*>(consecutiveFollower.first), consecutiveFollower.second);
665  } else {
666 #ifdef DEBUG_SURROUNDING_VEHICLES
667  if (DEBUG_COND) {
668  std::cout << "found follower '" << neighFollow->getID() << "'." << std::endl;
669  }
670 #endif
671  return std::pair<MSVehicle* const, double>(neighFollow,
672  vehicle->getPositionOnLane() - vehicle->getVehicleType().getLength() - neighFollow->getPositionOnLane() - neighFollow->getVehicleType().getMinGap());
673  }
674 }
675 
676 
677 MSVehicle*
678 MSLaneChanger::getCloserFollower(const double maxPos, MSVehicle* follow1, MSVehicle* follow2) {
679  if (follow1 == nullptr || follow1->getPositionOnLane() > maxPos) {
680  return follow2;
681  } else if (follow2 == nullptr || follow2->getPositionOnLane() > maxPos) {
682  return follow1;
683  } else {
684  if (follow1->getPositionOnLane() > follow2->getPositionOnLane()) {
685  return follow1;
686  } else {
687  return follow2;
688  }
689  }
690 }
691 
692 int
694  int laneOffset,
695  const std::pair<MSVehicle* const, double>& leader,
696  const std::vector<MSVehicle::LaneQ>& preb) const {
697 
698  std::pair<MSVehicle*, double> follower = getRealFollower(myCandi);
699  std::pair<MSVehicle* const, double> neighLead = getRealLeader(myCandi + laneOffset);
700  std::pair<MSVehicle*, double> neighFollow = getRealFollower(myCandi + laneOffset);
701  if (neighLead.first != nullptr && neighLead.first == neighFollow.first) {
702  // vehicles should not be leader and follower at the same time to avoid
703  // contradictory behavior
704  neighFollow.first = 0;
705  }
706  ChangerIt target = myCandi + laneOffset;
707  return checkChange(laneOffset, target->lane, leader, follower, neighLead, neighFollow, preb);
708 }
709 
710 int
712  int laneOffset,
713  const MSLane* targetLane,
714  const std::pair<MSVehicle* const, double>& leader,
715  const std::pair<MSVehicle* const, double>& follower,
716  const std::pair<MSVehicle* const, double>& neighLead,
717  const std::pair<MSVehicle* const, double>& neighFollow,
718  const std::vector<MSVehicle::LaneQ>& preb) const {
719 
720  MSVehicle* vehicle = veh(myCandi);
721 
722 #ifdef DEBUG_CHECK_CHANGE
723  if (DEBUG_COND) {
724  std::cout
725  << "\n" << SIMTIME << " checkChange() for vehicle '" << vehicle->getID() << "'"
726  << std::endl;
727  }
728 #endif
729 
730  int blocked = 0;
731  int blockedByLeader = (laneOffset == -1 ? LCA_BLOCKED_BY_RIGHT_LEADER : LCA_BLOCKED_BY_LEFT_LEADER);
732  int blockedByFollower = (laneOffset == -1 ? LCA_BLOCKED_BY_RIGHT_FOLLOWER : LCA_BLOCKED_BY_LEFT_FOLLOWER);
733  // overlap
734  if (neighFollow.first != nullptr && neighFollow.second < 0) {
735  blocked |= (blockedByFollower | LCA_OVERLAPPING);
736 
737  // Debug (Leo)
738 #ifdef DEBUG_CHECK_CHANGE
739  if (DEBUG_COND) {
740  std::cout << SIMTIME
741  << " overlapping with follower..."
742  << std::endl;
743  }
744 #endif
745 
746  }
747  if (neighLead.first != nullptr && neighLead.second < 0) {
748  blocked |= (blockedByLeader | LCA_OVERLAPPING);
749 
750 #ifdef DEBUG_CHECK_CHANGE
751  if (DEBUG_COND) {
752  std::cout << SIMTIME
753  << " overlapping with leader..."
754  << std::endl;
755  }
756 #endif
757 
758  }
759  double secureFrontGap = MSAbstractLaneChangeModel::NO_NEIGHBOR;
760  double secureBackGap = MSAbstractLaneChangeModel::NO_NEIGHBOR;
761  double secureOrigFrontGap = MSAbstractLaneChangeModel::NO_NEIGHBOR;
762 
763  const double tauRemainder = vehicle->getActionStepLength() == DELTA_T ? 0 : MAX2(vehicle->getCarFollowModel().getHeadwayTime() - TS, 0.);
764  // safe back gap
765  if ((blocked & blockedByFollower) == 0 && neighFollow.first != nullptr) {
766  // Calculate secure gap conservatively with vNextFollower / vNextLeader as
767  // extrapolated speeds after the driver's expected reaction time (tau).
768  // NOTE: there exists a possible source for collisions if the follower and the leader
769  // have desynchronized action steps as the extrapolated speeds can be exceeded in this case
770 
771  // Expected reaction time (tau) for the follower-vehicle.
772  // (substracted TS since at this point the vehicles' states are already updated)
773  const double vNextFollower = neighFollow.first->getSpeed() + MAX2(0., tauRemainder * neighFollow.first->getAcceleration());
774  const double vNextLeader = vehicle->getSpeed() + MIN2(0., tauRemainder * vehicle->getAcceleration());
775  // !!! eigentlich: vsafe braucht die Max. Geschwindigkeit beider Spuren
776  secureBackGap = neighFollow.first->getCarFollowModel().getSecureGap(neighFollow.first, vehicle, vNextFollower,
777  vNextLeader, vehicle->getCarFollowModel().getMaxDecel());
778 #ifdef DEBUG_CHECK_CHANGE
779  if (DEBUG_COND) {
780  std::cout << SIMTIME
781  << " backGap=" << neighFollow.second
782  << " vNextFollower=" << vNextFollower
783  << " vNextEgo=" << vNextLeader
784  << " secureGap=" << secureBackGap
785  << " safetyFactor=" << vehicle->getLaneChangeModel().getSafetyFactor()
786  << " blocked=" << (neighFollow.second < secureBackGap * vehicle->getLaneChangeModel().getSafetyFactor())
787  << "\n";
788  }
789 #endif
790  if (neighFollow.second < secureBackGap * vehicle->getLaneChangeModel().getSafetyFactor()) {
791  blocked |= blockedByFollower;
792  }
793  }
794 
795  // safe front gap
796  if ((blocked & blockedByLeader) == 0 && neighLead.first != nullptr) {
797  // Calculate secure gap conservatively with vNextFollower / vNextLeader as
798  // extrapolated speeds after the driver's expected reaction time (tau).
799  // NOTE: there exists a possible source for collisions if the follower and the leader
800  // have desynchronized action steps as the extrapolated speeds can be exceeded in this case
801 
802  // Expected reaction time (tau) for the follower-vehicle.
803  // (substracted TS since at this point the vehicles' states are already updated)
804  const double vNextFollower = vehicle->getSpeed() + MAX2(0., tauRemainder * vehicle->getAcceleration());
805  const double vNextLeader = neighLead.first->getSpeed() + MIN2(0., tauRemainder * neighLead.first->getAcceleration());
806  // !!! eigentlich: vsafe braucht die Max. Geschwindigkeit beider Spuren
807  secureFrontGap = vehicle->getCarFollowModel().getSecureGap(vehicle, neighLead.first, vNextFollower,
808  vNextLeader, neighLead.first->getCarFollowModel().getMaxDecel());
809 #ifdef DEBUG_CHECK_CHANGE
810  if (DEBUG_COND) {
811  std::cout << SIMTIME
812  << " frontGap=" << neighFollow.second
813  << " vNextEgo=" << vNextFollower
814  << " vNextLeader=" << vNextLeader
815  << " secureGap=" << secureFrontGap
816  << " safetyFactor=" << vehicle->getLaneChangeModel().getSafetyFactor()
817  << " blocked=" << (neighLead.second < secureFrontGap * vehicle->getLaneChangeModel().getSafetyFactor())
818  << "\n";
819  }
820 #endif
821  if (neighLead.second < secureFrontGap * vehicle->getLaneChangeModel().getSafetyFactor()) {
822  blocked |= blockedByLeader;
823  }
824  }
825  if (blocked == 0 && targetLane->hasPedestrians()) {
826  PersonDist nextLeader = targetLane->nextBlocking(vehicle->getBackPositionOnLane(),
827  vehicle->getRightSideOnLane(), vehicle->getRightSideOnLane() + vehicle->getVehicleType().getWidth(),
828  ceil(vehicle->getSpeed() / vehicle->getCarFollowModel().getMaxDecel()));
829  if (nextLeader.first != 0) {
830  const double brakeGap = vehicle->getCarFollowModel().brakeGap(vehicle->getSpeed());
831  // returned gap value is relative to backPosition
832  const double gap = nextLeader.second - vehicle->getVehicleType().getLengthWithGap();
833 #ifdef DEBUG_CHECK_CHANGE
834  if (DEBUG_COND) {
835  std::cout << SIMTIME << " pedestrian on road " + leader.first->getID() << " gap=" << gap << " brakeGap=" << brakeGap << "\n";
836  }
837 #endif
838  if (brakeGap > gap) {
839  blocked |= blockedByLeader;
840 #ifdef DEBUG_CHECK_CHANGE
841  if (DEBUG_COND) {
842  std::cout << SIMTIME << " blocked by pedestrian " + leader.first->getID() << "\n";
843  }
844 #endif
845  }
846  }
847  }
848 
849  if (leader.first != nullptr) {
850  secureOrigFrontGap = vehicle->getCarFollowModel().getSecureGap(vehicle, leader.first, vehicle->getSpeed(), leader.first->getSpeed(), leader.first->getCarFollowModel().getMaxDecel());
851  }
852 
853  MSAbstractLaneChangeModel::MSLCMessager msg(leader.first, neighLead.first, neighFollow.first);
854  int state = blocked | vehicle->getLaneChangeModel().wantsChange(
855  laneOffset, msg, blocked, leader, follower, neighLead, neighFollow, *targetLane, preb, &(myCandi->lastBlocked), &(myCandi->firstBlocked));
856 
857  if (blocked == 0 && (state & LCA_WANTS_LANECHANGE) != 0 && neighLead.first != nullptr) {
858  // do a more careful (but expensive) check to ensure that a
859  // safety-critical leader is not being overlooked
860  // while changing on an intersection, it is not sufficient to abort the
861  // search with a leader on the current lane because all linkLeaders must
862  // be considered as well
863  const double seen = myCandi->lane->getLength() - vehicle->getPositionOnLane();
864  const double speed = vehicle->getSpeed();
865  const double dist = vehicle->getCarFollowModel().brakeGap(speed) + vehicle->getVehicleType().getMinGap();
866  if (seen < dist || myCandi->lane->isInternal()) {
867  std::pair<MSVehicle* const, double> neighLead2 = targetLane->getCriticalLeader(dist, seen, speed, *vehicle);
868  if (neighLead2.first != nullptr && neighLead2.first != neighLead.first) {
869  const double secureGap = vehicle->getCarFollowModel().getSecureGap(vehicle, neighLead2.first, vehicle->getSpeed(),
870  neighLead2.first->getSpeed(), neighLead2.first->getCarFollowModel().getMaxDecel());
871  const double secureGap2 = secureGap * vehicle->getLaneChangeModel().getSafetyFactor();
872 #ifdef DEBUG_SURROUNDING_VEHICLES
873  if (DEBUG_COND) {
874  std::cout << SIMTIME << " found critical leader=" << neighLead2.first->getID()
875  << " gap=" << neighLead2.second << " secGap=" << secureGap << " secGap2=" << secureGap2 << "\n";
876  }
877 #endif
878  if (neighLead2.second < secureGap2) {
879  state |= blockedByLeader;
880  }
881  }
882  }
883  }
884  if (blocked == 0 && (state & LCA_WANTS_LANECHANGE)) {
885  // ensure that merging is safe for any upcoming zipper links after changing
886  if (vehicle->unsafeLinkAhead(targetLane)) {
887  state |= blockedByLeader;
888  }
889  }
890 
891  if ((state & LCA_BLOCKED) == 0 && (state & LCA_WANTS_LANECHANGE) != 0 && MSGlobals::gLaneChangeDuration > DELTA_T) {
892  // Ensure that a continuous lane change manoeuvre can be completed before the next turning movement.
893  // Assume lateral position == 0. (If this should change in the future add + laneOffset*vehicle->getLateralPositionOnLane() to distToNeighLane)
894  const double distToNeighLane = 0.5 * (vehicle->getLane()->getWidth() + targetLane->getWidth());
895  // Extrapolate the LC duration if operating with speed dependent lateral speed.
896  const MSAbstractLaneChangeModel& lcm = vehicle->getLaneChangeModel();
897  const double assumedDecel = lcm.getAssumedDecelForLaneChangeDuration();
898  const double estimatedLCDuration = lcm.estimateLCDuration(vehicle->getSpeed(), distToNeighLane, assumedDecel, (state & LCA_URGENT) != 0);
899  if (estimatedLCDuration == -1) {
900  // Can't guarantee that LC will succeed if vehicle is braking -> assert(lcm.myMaxSpeedLatStanding==0)
901 #ifdef DEBUG_CHECK_CHANGE
902  if (DEBUG_COND) {
903  std::cout << SIMTIME << " checkChange() too slow to guarantee completion of continuous lane change."
904  << "\nestimatedLCDuration=" << estimatedLCDuration
905  << "\ndistToNeighLane=" << distToNeighLane
906  << std::endl;
907  }
908 #endif
909  state |= LCA_INSUFFICIENT_SPEED;
910  } else {
911  // Compute covered distance, when braking for the whole lc duration
912  const double decel = vehicle->getCarFollowModel().getMaxDecel() * estimatedLCDuration;
913  const double avgSpeed = 0.5 * (
914  MAX2(0., vehicle->getSpeed() - ACCEL2SPEED(vehicle->getCarFollowModel().getMaxDecel())) +
915  MAX2(0., vehicle->getSpeed() - decel));
916  // Distance required for lane change.
917  const double space2change = avgSpeed * estimatedLCDuration;
918  // Available distance for LC maneuver (distance till next turn)
919  double seen = myCandi->lane->getLength() - vehicle->getPositionOnLane();
920 #ifdef DEBUG_CHECK_CHANGE
921  if (DEBUG_COND) {
922  std::cout << SIMTIME << " checkChange() checking continuous lane change..."
923  << "\ndistToNeighLane=" << distToNeighLane
924  << " estimatedLCDuration=" << estimatedLCDuration
925  << " space2change=" << space2change
926  << " avgSpeed=" << avgSpeed
927  << std::endl;
928  }
929 #endif
930 
931  // for finding turns it doesn't matter whether we look along the current lane or the target lane
932  const std::vector<MSLane*>& bestLaneConts = vehicle->getBestLanesContinuation();
933  int view = 1;
934  const MSLane* nextLane = vehicle->getLane();
935  std::vector<MSLink*>::const_iterator link = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
936  while (!nextLane->isLinkEnd(link) && seen <= space2change) {
937  if ((*link)->getDirection() == LinkDirection::LEFT || (*link)->getDirection() == LinkDirection::RIGHT
938  // the lanes after an internal junction are on different
939  // edges and do not allow lane-changing
940  || (nextLane->getEdge().isInternal() && (*link)->getViaLaneOrLane()->getEdge().isInternal())
941  ) {
942  state |= LCA_INSUFFICIENT_SPACE;
943  break;
944  }
945  if ((*link)->getViaLane() == nullptr) {
946  view++;
947  }
948  nextLane = (*link)->getViaLaneOrLane();
949  seen += nextLane->getLength();
950  // get the next link used
951  link = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
952  }
953 #ifdef DEBUG_CHECK_CHANGE
954  if (DEBUG_COND) {
955  std::cout << " available distance=" << seen << std::endl;
956  }
957 #endif
958  if (nextLane->isLinkEnd(link) && seen < space2change) {
959 #ifdef DEBUG_CHECK_CHANGE
960  if (DEBUG_COND) {
961  std::cout << SIMTIME << " checkChange insufficientSpace: seen=" << seen << " space2change=" << space2change << "\n";
962  }
963 #endif
964  state |= LCA_INSUFFICIENT_SPACE;
965  }
966 
967  if ((state & LCA_BLOCKED) == 0) {
968  // check for dangerous leaders in case the target lane changes laterally between
969  // now and the lane-changing midpoint
970  const double speed = vehicle->getSpeed();
971  seen = myCandi->lane->getLength() - vehicle->getPositionOnLane();
972  nextLane = vehicle->getLane();
973  view = 1;
974  const double dist = vehicle->getCarFollowModel().brakeGap(speed) + vehicle->getVehicleType().getMinGap();
975  std::vector<MSLink*>::const_iterator nextLink = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
976  while (!nextLane->isLinkEnd(nextLink) && seen <= space2change && seen <= dist) {
977  nextLane = (*nextLink)->getViaLaneOrLane();
978  const MSLane* const parallelLane = nextLane->getParallelLane(laneOffset);
979  if (parallelLane == nullptr) {
980  state |= LCA_INSUFFICIENT_SPACE;
981  break;
982  } else {
983  std::pair<MSVehicle* const, double> neighLead2 = parallelLane->getLeader(vehicle, -seen, std::vector<MSLane*>());
984  if (neighLead2.first != nullptr && neighLead2.first != neighLead.first
985  && (neighLead2.second < vehicle->getCarFollowModel().getSecureGap(vehicle, neighLead2.first,
986  vehicle->getSpeed(), neighLead2.first->getSpeed(), neighLead2.first->getCarFollowModel().getMaxDecel()))) {
987  state |= blockedByLeader;
988  break;
989  }
990  }
991  if ((*nextLink)->getViaLane() == nullptr) {
992  view++;
993  }
994  seen += nextLane->getLength();
995  // get the next link used
996  nextLink = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
997  }
998  }
999  }
1000  }
1001  const int oldstate = state;
1002  // let TraCI influence the wish to change lanes and the security to take
1003  state = vehicle->influenceChangeDecision(state);
1004 #ifdef DEBUG_CHECK_CHANGE
1005  if (DEBUG_COND) {
1006  std::cout << SIMTIME
1007  << " veh=" << vehicle->getID()
1008  << " oldState=" << toString((LaneChangeAction)oldstate)
1009  << " newState=" << toString((LaneChangeAction)state)
1010  << ((blocked & LCA_BLOCKED) ? " (blocked)" : "")
1011  << ((blocked & LCA_OVERLAPPING) ? " (overlap)" : "")
1012  << "\n";
1013  }
1014 #endif
1015  vehicle->getLaneChangeModel().saveLCState(laneOffset, oldstate, state);
1016  if (blocked == 0 && (state & LCA_WANTS_LANECHANGE)) {
1017  // this lane change will be executed, save gaps
1018  vehicle->getLaneChangeModel().setFollowerGaps(neighFollow, secureBackGap);
1019  vehicle->getLaneChangeModel().setLeaderGaps(neighLead, secureFrontGap);
1020  vehicle->getLaneChangeModel().setOrigLeaderGaps(leader, secureOrigFrontGap);
1021  }
1022  if (laneOffset != 0) {
1023  vehicle->getLaneChangeModel().saveNeighbors(laneOffset, neighFollow, neighLead);
1024  }
1025  return state;
1026 }
1027 
1028 bool
1030  if (vehicle->hasStops()) {
1031  const MSStop& stop = vehicle->getNextStop();
1032  if (stop.isOpposite && vehicle->nextStopDist() < OPPOSITE_OVERTAKING_MAX_LOOKAHEAD) {
1033  return true;
1034  }
1035  }
1036  return false;
1037 }
1038 
1039 
1040 
1041 std::vector<MSVehicle::LaneQ>
1042 MSLaneChanger::getBestLanesOpposite(MSVehicle* vehicle, const MSLane* stopLane, double oppositeLength) {
1043  const bool isOpposite = vehicle->getLaneChangeModel().isOpposite();
1044  const MSEdge* forward = isOpposite ? vehicle->getLane()->getEdge().getOppositeEdge()->getNormalSuccessor() : vehicle->getLane()->getEdge().getNormalSuccessor();
1045  const MSEdge* opposite = forward->getOppositeEdge();
1046  const int numForward = forward->getNumLanes();
1047  const int numOpposite = opposite->getNumLanes();
1048  const std::vector<MSLane*>& oLanes = opposite->getLanes();
1049  std::vector<MSVehicle::LaneQ> preb = vehicle->getBestLanes();
1050  for (int i = 0; i < numOpposite; i++) {
1051  preb.push_back(preb.back());
1052  preb.back().lane = oLanes[numOpposite - 1 - i];
1053  preb.back().length = oppositeLength;
1054  if (isOpposite) {
1055  preb.back().bestLaneOffset = -1 - i;
1056  //std::cout << " oi=" << i << " bestOffset =" << preb.back().bestLaneOffset << "\n";
1057  }
1058  }
1059  if (stopLane != nullptr) {
1060  const int stopIndex = numForward + numOpposite - stopLane->getIndex() - 1;
1061  for (int i = 0; i < (int)preb.size(); i++) {
1062  preb[i].bestLaneOffset = stopIndex - i;
1063  preb[i].length = vehicle->getLaneChangeModel().getForwardPos() + vehicle->nextStopDist();
1064  //std::cout << " oi2=" << i << " stopIndex=" << stopIndex << " bestOffset =" << preb[i].bestLaneOffset << " stopDist=" << vehicle->nextStopDist() << " length=" << preb[i].length << "\n";
1065  }
1066  }
1067 #ifdef DEBUG_CHANGE_OPPOSITE
1068  if (DEBUG_COND) {
1069  std::cout << SIMTIME << " getBestLanesOpposite " << vehicle->getID() << " isOpposite=" << isOpposite << "\n";
1070  for (int i = 0; i < (int)preb.size(); i++) {
1071  std::cout << " i=" << i << " lane=" << preb[i].lane->getID() << " bestOffset=" << preb[i].bestLaneOffset << " length=" << preb[i].length << "\n";
1072  }
1073  }
1074 #endif
1075  return preb;
1076 }
1077 
1078 
1079 bool
1080 MSLaneChanger::changeOpposite(MSVehicle* vehicle, std::pair<MSVehicle*, double> leader) {
1081  // Evaluate lane-changing between opposite direction lanes
1082  if (!myChangeToOpposite) {
1083  return false;
1084  }
1085  const bool isOpposite = vehicle->getLaneChangeModel().isOpposite();
1086  MSLane* source = vehicle->getMutableLane();
1087  MSLane* opposite = isOpposite ? source->getParallelLane(1) : source->getOpposite();
1088 
1089 #ifdef DEBUG_CHANGE_OPPOSITE
1091  if (DEBUG_COND) {
1092  std::cout << SIMTIME << " veh=" << vehicle->getID() << " considerChangeOpposite source=" << source->getID() << " opposite=" << Named::getIDSecure(opposite) << " lead=" << Named::getIDSecure(leader.first) << "\n";
1093  }
1094 #endif
1095  //There is no lane for opposite driving
1096  if (opposite == nullptr) {
1097  return false;
1098  }
1099  if (vehicle->isStopped()) {
1100  // stopped vehicles obviously should not change lanes. Usually this is
1101  // prevent by appropriate bestLane distances
1102  return false;
1103  }
1104  int ret = 0;
1105  ret = vehicle->influenceChangeDecision(ret);
1106  bool oppositeChangeByTraci = false;
1107  // Check whether a lane change to the opposite direction was requested via TraCI
1108  if ((ret & (LCA_TRACI)) != 0) {
1109  if (isOpposite && (ret & LCA_LEFT) != 0) {
1110  // stay on the opposite side
1111  return false;
1112  }
1113  oppositeChangeByTraci = true;
1114  }
1115  if (!isOpposite && !oppositeChangeByTraci && !source->allowsChangingLeft(vehicle->getVClass())) {
1116  // lane changing explicitly forbidden from this lane
1117 #ifdef DEBUG_CHANGE_OPPOSITE
1118  if (DEBUG_COND) {
1119  std::cout << " not overtaking due to changeLeft restriction\n";
1120  }
1121 #endif
1122  return false;
1123  }
1124 
1125  //lane for opposite driving is not permitted
1126  if (!opposite->allowsVehicleClass(vehicle->getVClass())) {
1127  return false;
1128  }
1129 
1130  const MSLane* oncomingLane = isOpposite ? source : opposite;
1131  // changing into the opposite direction is always to the left (XXX except for left-hand networkds)
1132  int direction = isOpposite ? -1 : 1;
1133  std::pair<MSVehicle*, double> neighLead(nullptr, -1);
1134 
1135  // distance that can safely be driven on the opposite side
1136  double surplusGap = std::numeric_limits<double>::max();
1137 
1138  // we need to find two vehicles:
1139  // 1) the leader that shall be overtaken (not necessarily the current leader but one of its leaders that has enough space in front)
1140  // 2) the oncoming vehicle (we need to look past vehicles that are currently overtaking through the opposite direction themselves)
1141  //
1142  // if the vehicle is driving normally, then the search for 1) starts on the current lane and 2) on the opposite lane
1143  // if the vehicle is driving on the opposite side then 1) is found on the neighboring lane and 2) on the current lane
1144 
1145  std::pair<MSVehicle*, double> overtaken(nullptr, -1);
1146  std::pair<MSVehicle*, double> oncoming(nullptr, -1);
1147  // the maximum speed while overtaking (may be lowered if slow vehicles are
1148  // currently overtaking ahead of vehicle)
1149  double vMax = vehicle->getLane()->getVehicleMaxSpeed(vehicle);
1150  double oncomingSpeed = oncomingLane->getSpeedLimit();
1151 
1152  // check for opposite direction stop
1153  if (!oppositeChangeByTraci && hasOppositeStop(vehicle)) {
1154  const std::vector<MSVehicle::LaneQ> preb = getBestLanesOpposite(vehicle, vehicle->getNextStop().lane, -1);
1155  const int laneIndex = vehicle->getLaneChangeModel().getNormalizedLaneIndex();
1156  const int bestOffset = preb[laneIndex].bestLaneOffset;
1157  //std::cout << SIMTIME << " veh=" << vehicle->getID() << " laneIndex=" << laneIndex << " bestOffset=" << bestOffset << " target=" << target->getID() << "\n";
1158 
1159  // compute safety constraints (assume vehicle is safe once stop is reached)
1160  const double spaceToStop = vehicle->nextStopDist();
1161  const double timeToStopForward = spaceToStop / MAX2(vehicle->getSpeed(), vehicle->getCarFollowModel().getMaxAccel());
1162  const double timeToStopLateral = (MSGlobals::gLaneChangeDuration > 0
1165  ? bestOffset * SUMO_const_laneWidth / vehicle->getVehicleType().getMaxSpeedLat()
1166  : 0.));
1167  const double timeToStop = MAX2(timeToStopForward, timeToStopLateral);
1168  if (!isOpposite) {
1169  // we keep neighLead distinct from oncoming because it determines blocking on the neigh lane
1170  // but also look for an oncoming leader to compute safety constraint
1171  const double searchDist = timeToStop * oncomingLane->getSpeedLimit() * 2 + spaceToStop;
1172  neighLead = oncomingLane->getOppositeLeader(vehicle, searchDist, true);
1173  oncoming = getOncomingVehicle(oncomingLane, neighLead, searchDist, vMax);
1174  } else {
1175  double searchDist = OPPOSITE_OVERTAKING_ONCOMING_LOOKAHEAD;
1176  oncoming = oncomingLane->getOppositeLeader(vehicle, searchDist, true);
1177  oncoming = getOncomingVehicle(oncomingLane, oncoming, searchDist, vMax);
1178  }
1179  surplusGap = computeSurplusGap(vehicle, opposite, oncoming, timeToStop, spaceToStop, oncomingSpeed);
1180  if (!isOpposite && surplusGap < 0) {
1181 #ifdef DEBUG_CHANGE_OPPOSITE
1182  if (DEBUG_COND) {
1183  std::cout << " cannot changeOppositeStop due to dangerous oncoming spaceToStop=" << spaceToStop
1184  << " timeToStopForward=" << timeToStopForward << " timeToStopLateral=" << timeToStopLateral << " surplusGap=" << surplusGap << "\n";
1185  }
1186 #endif
1187  return false;
1188  }
1189 
1190  if (bestOffset > 0) {
1191  MSLane* const target = preb[laneIndex + 1].lane;
1192  neighLead = target->getOppositeLeader(vehicle, OPPOSITE_OVERTAKING_MAX_LOOKAHEAD, true);
1193  std::pair<MSVehicle* const, double> neighFollow = target->getOppositeFollower(vehicle);
1194  return checkChangeOpposite(vehicle, 1, target, leader, neighLead, neighFollow, preb);
1195  } else {
1196  // return prematurely (due to foe?)
1197  //return checkChangeOpposite(vehicle, -1, target, leader, neighLead, neighFollow, preb);
1198  return false;
1199  }
1200  }
1201 
1202  if (!isOpposite && leader.first == nullptr && !oppositeChangeByTraci) {
1203  // no reason to change unless there is a leader
1204  // or we are changing back to the propper direction
1205  // XXX also check whether the leader is so far away as to be irrelevant
1206  return false;
1207  }
1208  if (!isOpposite && !oppositeChangeByTraci
1209  && vehicle->getVClass() != SVC_EMERGENCY
1210  && leader.first != nullptr) {
1211  if (leader.first->signalSet(MSGlobals::gLefthand
1213  // do not try to overtake a vehicle that is about to turn left or wants
1214  // to change left itself
1215 #ifdef DEBUG_CHANGE_OPPOSITE
1216  if (DEBUG_COND) {
1217  std::cout << " not overtaking leader " << leader.first->getID() << " that has blinker set\n";
1218  }
1219 #endif
1220  return false;
1221  } else if (leader.second < 0) {
1222  // leaders is either a junction leader (that cannot be overtaken) or something else is wrong
1223 #ifdef DEBUG_CHANGE_OPPOSITE
1224  if (DEBUG_COND) {
1225  std::cout << " not overtaking leader " << leader.first->getID() << " with gap " << leader.second << "\n";
1226  }
1227 #endif
1228  return false;
1229  }
1230  }
1231 
1232  // checks for overtaking space
1233  double timeToOvertake = std::numeric_limits<double>::max();
1234  double spaceToOvertake = std::numeric_limits<double>::max();
1235 
1236  if (oppositeChangeByTraci) {
1237  timeToOvertake = STEPS2TIME(vehicle->getInfluencer().getLaneTimeLineDuration());//todo discuss concept
1238  spaceToOvertake = timeToOvertake * vehicle->getLane()->getVehicleMaxSpeed(vehicle);
1239  } else {
1240  if (isOpposite) {
1241  // -1 will use getMaximumBrakeDist() as look-ahead distance
1242  neighLead = opposite->getOppositeLeader(vehicle, -1, false);
1243  // make sure that overlapping vehicles on the neighboring lane are found by starting search at the back position
1244  overtaken = opposite->getLeader(vehicle, opposite->getOppositePos(vehicle->getBackPositionOnLane()), vehicle->getBestLanesContinuation(opposite));
1245  overtaken.second -= vehicle->getVehicleType().getLength();
1246  if (overtaken.first == nullptr && neighLead.first != nullptr) {
1247  overtaken = neighLead;
1248  }
1249  if (overtaken.first != nullptr) {
1250  overtaken = getColumnleader(vehicle, overtaken);
1251  }
1252 #ifdef DEBUG_CHANGE_OPPOSITE
1253  if (DEBUG_COND) {
1254  std::cout << " leaderOnSource=" << Named::getIDSecure(oncoming.first) << " gap=" << oncoming.second << "\n";
1255  std::cout << " leaderOnTarget=" << Named::getIDSecure(neighLead.first) << " gap=" << neighLead.second << "\n";
1256  std::cout << " overtaken=" << Named::getIDSecure(overtaken.first) << " gap=" << overtaken.second << "\n";
1257  }
1258 #endif
1259  } else {
1260  overtaken = getColumnleader(vehicle, leader);
1261  }
1262 
1263  if (overtaken.first == 0) {
1264  if (!isOpposite) {
1265  // no reason to change to the opposite side
1266  return false;
1267  }
1268  } else {
1269 #ifdef DEBUG_CHANGE_OPPOSITE
1270  if (DEBUG_COND) {
1271  std::cout << " compute time/space to overtake for columnLeader=" << overtaken.first->getID() << " egoGap=" << overtaken.second << "\n";
1272  }
1273 #endif
1274  // there might be leader vehicles on the opposite side that also drive
1275  // against the flow which are slower than ego (must be factored into
1276  // overtaking time)
1277  computeOvertakingTime(vehicle, vMax, overtaken.first, overtaken.second, timeToOvertake, spaceToOvertake);
1278 #ifdef DEBUG_CHANGE_OPPOSITE
1279  if (DEBUG_COND) {
1280  std::cout << SIMTIME
1281  << " veh=" << vehicle->getID()
1282  << " changeOpposite opposite=" << opposite->getID()
1283  << " lead=" << Named::getIDSecure(leader.first)
1284  << " timeToOvertake=" << timeToOvertake
1285  << " spaceToOvertake=" << spaceToOvertake
1286  << "\n";
1287  }
1288 #endif
1289 
1290  if (!isOpposite && spaceToOvertake > OPPOSITE_OVERTAKING_MAX_SPACE_TO_OVERTAKE) {
1291 #ifdef DEBUG_CHANGE_OPPOSITE
1292  if (DEBUG_COND) {
1293  std::cout << " cannot changeOpposite (cannot overtake fast leader " << Named::getIDSecure(overtaken.first) << " v=" << overtaken.first->getSpeed() << ")\n";
1294  }
1295 #endif
1296  return false;
1297  }
1298  }
1299 
1300  // if we have a leader vehicle that is driving in the opposite
1301  // direction, it may slow us down (update vMax)
1302  if (!isOpposite) {
1303  assert(timeToOvertake != std::numeric_limits<double>::max());
1304  assert(spaceToOvertake != std::numeric_limits<double>::max());
1305  // we keep neighLead distinct from oncoming because it determines blocking on the neigh lane
1306  // but also look for an oncoming leader to compute safety constraint
1307  double searchDist = timeToOvertake * oncomingLane->getSpeedLimit() * 2 + spaceToOvertake;
1308  neighLead = oncomingLane->getOppositeLeader(vehicle, searchDist, true);
1309  oncoming = getOncomingVehicle(oncomingLane, neighLead, searchDist, vMax, overtaken.first);
1310  } else {
1311  double searchDist = OPPOSITE_OVERTAKING_ONCOMING_LOOKAHEAD;
1312  oncoming = oncomingLane->getOppositeLeader(vehicle, searchDist, true);
1313  oncoming = getOncomingVehicle(oncomingLane, oncoming, searchDist, vMax, overtaken.first);
1314  }
1315  if (overtaken.first != nullptr && vMax != vehicle->getLane()->getVehicleMaxSpeed(vehicle)) {
1316  // recompute overtaking time with slow opposite leader
1317  computeOvertakingTime(vehicle, vMax, overtaken.first, overtaken.second, timeToOvertake, spaceToOvertake);
1318 #ifdef DEBUG_CHANGE_OPPOSITE
1319  if (DEBUG_COND) {
1320  std::cout << " recomputed overtaking time with vMax=" << vMax
1321  << " timeToOvertake=" << timeToOvertake
1322  << " spaceToOvertake=" << spaceToOvertake
1323  << "\n";
1324  }
1325 #endif
1326  }
1327  if (!isOpposite) {
1328  if (spaceToOvertake > OPPOSITE_OVERTAKING_MAX_SPACE_TO_OVERTAKE) {
1329 #ifdef DEBUG_CHANGE_OPPOSITE
1330  if (DEBUG_COND) {
1331  std::cout << " cannot changeOpposite (cannot overtake fast leader " << Named::getIDSecure(overtaken.first) << " v=" << overtaken.first->getSpeed() << ")\n";
1332  }
1333 #endif
1334  return false;
1335  }
1336  // check for upcoming stops
1337  if (vehicle->nextStopDist() < spaceToOvertake) {
1338 #ifdef DEBUG_CHANGE_OPPOSITE
1339  if (DEBUG_COND) {
1340  std::cout << " cannot changeOpposite due to upcoming stop (dist=" << vehicle->nextStopDist() << " spaceToOvertake=" << spaceToOvertake << ")\n";
1341  }
1342 #endif
1343  return false;
1344  }
1345  assert(timeToOvertake != std::numeric_limits<double>::max());
1346  assert(spaceToOvertake != std::numeric_limits<double>::max());
1347  }
1348 
1349  // check for dangerous oncoming leader
1350  surplusGap = computeSurplusGap(vehicle, opposite, oncoming, timeToOvertake, spaceToOvertake, oncomingSpeed);
1351  if (!isOpposite && surplusGap < 0) {
1352 #ifdef DEBUG_CHANGE_OPPOSITE
1353  if (DEBUG_COND) {
1354  std::cout << " cannot changeOpposite due to dangerous oncoming (surplusGap=" << surplusGap << ")\n";
1355  }
1356 #endif
1357 
1358 #ifdef DEBUG_CHANGE_OPPOSITE
1359  if (DEBUG_COND) {
1360  if (oncoming.first->getLaneChangeModel().isOpposite()) {
1361  std::cout << SIMTIME << " ego=" << vehicle->getID() << " does not changeOpposite due to dangerous oncoming " << oncoming.first->getID() << " (but the leader is also opposite)\n";
1362  }
1363  }
1364 #endif
1365  return false;
1366  }
1367  }
1368  // compute remaining space on the opposite side
1369  // 1. the part that remains on the current lane
1370  double usableDist = isOpposite ? vehicle->getPositionOnLane() : source->getLength() - vehicle->getPositionOnLane();
1371 
1372  if (usableDist < spaceToOvertake) {
1373  // look forward along the next lanes
1374  const std::vector<MSLane*>& bestLaneConts = vehicle->getBestLanesContinuation();
1375  assert(bestLaneConts.size() >= 1);
1376  std::vector<MSLane*>::const_iterator it = bestLaneConts.begin() + 1;
1377  while (usableDist < spaceToOvertake && it != bestLaneConts.end()) {
1378 #ifdef DEBUG_CHANGE_OPPOSITE
1379  if (DEBUG_COND) {
1380  std::cout << " usableDist=" << usableDist << " opposite=" << Named::getIDSecure((*it)->getOpposite()) << "\n";
1381  }
1382 #endif
1383  if ((*it)->getOpposite() == nullptr || !(*it)->getOpposite()->allowsVehicleClass(vehicle->getVClass())) {
1384  // opposite lane ends
1385  break;
1386  }
1387  // do not overtake past a minor link or turn
1388  const MSLane* const prev = *(it - 1);
1389  if (prev != nullptr) {
1390  const MSLink* link = prev->getLinkTo(*it);
1391  if (link == nullptr || link->getState() == LINKSTATE_ZIPPER
1393  || (!link->havePriority()
1394  // consider traci-influence
1395  && (!vehicle->hasInfluencer() || vehicle->getInfluencer().getRespectJunctionPriority())
1396  // consider junction model parameters
1397  && ((!link->haveRed() && !link->haveYellow()) || !vehicle->ignoreRed(link, true)))) {
1398 #ifdef DEBUG_CHANGE_OPPOSITE
1399  if (DEBUG_COND) {
1400  std::cout << " stop lookahead at link=" << (link == 0 ? "NULL" : link->getViaLaneOrLane()->getID()) << " state=" << (link == 0 ? "?" : toString(link->getState())) << " ignoreRed=" << vehicle->ignoreRed(link, true) << "\n";
1401  }
1402 #endif
1403  break;
1404  }
1405  }
1406  usableDist += (*it)->getLength();
1407  ++it;
1408  }
1409  }
1410  if (!isOpposite && usableDist < spaceToOvertake) {
1411 #ifdef DEBUG_CHANGE_OPPOSITE
1412  if (DEBUG_COND) {
1413  std::cout << " cannot changeOpposite due to insufficient space (seen=" << usableDist << " spaceToOvertake=" << spaceToOvertake << ")\n";
1414  }
1415 #endif
1416  return false;
1417  }
1418  if (!isOpposite && MSNet::getInstance()->hasElevation() && !overtaken.first->isStopped()) {
1419  // do not overtake before the top of a hill
1420  double searchDist = timeToOvertake * oncomingLane->getSpeedLimit() * 1.5 * vehicle->getLaneChangeModel().getOppositeSafetyFactor() + spaceToOvertake;
1421  int view = vehicle->getLane()->isInternal() ? 1 : 0;
1422  bool foundHill = vehicle->getSlope() > 0;
1423  if (foundHilltop(vehicle, foundHill, searchDist, vehicle->getBestLanesContinuation(), view, vehicle->getPositionOnLane(), vehicle->getPosition().z(), OPPOSITE_OVERTAKING_HILLTOP_THRESHOHOLD)) {
1424  return false;
1425  }
1426  }
1427 #ifdef DEBUG_CHANGE_OPPOSITE
1428  if (DEBUG_COND) {
1429  std::cout << " usableDist=" << usableDist << " spaceToOvertake=" << spaceToOvertake << " timeToOvertake=" << timeToOvertake << "\n";
1430  }
1431 #endif
1432  // compute wish to change
1433  double oppositeLength = vehicle->getBestLanes().back().length;
1434  if (isOpposite) {
1435  // compute the remaining distance that can be driven on the opposite side
1436  // this value will put into oppositeLength of the opposite lanes
1437  // @note: length counts from the start of the current lane
1438  // @note: see MSLaneChangeModel::LC2013::_wantsChange @1092 (isOpposite()
1439  // position on the target lane
1440  const double forwardPos = source->getOppositePos(vehicle->getPositionOnLane());
1441 
1442  // consider usableDist (due to minor links or end of opposite lanes)
1443  oppositeLength = MIN2(oppositeLength, usableDist + forwardPos);
1444  // consider upcoming stops
1445  oppositeLength = MIN2(oppositeLength, vehicle->nextStopDist() + forwardPos);
1446 #ifdef DEBUG_CHANGE_OPPOSITE
1447  if (DEBUG_COND) {
1448  std::cout << " laneQLength=" << oppositeLength << " usableDist=" << usableDist << " forwardPos=" << forwardPos << " stopDist=" << vehicle->nextStopDist() << "\n";
1449  }
1450 #endif
1451  // consider oncoming leaders
1452  const MSVehicle* oncomingVeh = oncoming.first;
1453  if (oncomingVeh != 0) {
1454  if (!oncomingVeh->getLaneChangeModel().isOpposite() && oncomingVeh->getLaneChangeModel().getShadowLane() != source) {
1455  const double egoSpeedFraction = MIN2(0.5, vMax / (vMax + oncomingSpeed));
1456  oppositeLength = MIN2(oppositeLength, forwardPos + oncoming.second * egoSpeedFraction);
1457 #ifdef DEBUG_CHANGE_OPPOSITE
1458  if (DEBUG_COND) {
1459  std::cout << SIMTIME << " found oncoming leader=" << oncomingVeh->getID() << " gap=" << oncoming.second
1460  << " egoSpeedFraction=" << egoSpeedFraction << " newDist=" << oppositeLength << "\n";
1461  }
1462 #endif
1463  } else {
1464 #ifdef DEBUG_CHANGE_OPPOSITE
1465  if (DEBUG_COND) {
1466  std::cout << SIMTIME << " opposite leader=" << oncomingVeh->getID() << " gap=" << oncoming.second << " is driving against the flow\n";
1467  }
1468 #endif
1469  }
1470  if (neighLead.first != nullptr) {
1471  if (overtaken.first == nullptr) {
1472 #ifdef DEBUG_CHANGE_OPPOSITE
1473  if (DEBUG_COND) {
1474  std::cout << SIMTIME << " ego=" << vehicle->getID() << " did not find columnleader to overtake\n";
1475  }
1476 #endif
1477  } else {
1478  if (surplusGap > 0) {
1479  // exaggerate remaining dist so that the vehicle continues
1480  // overtaking (otherwise the lane change model might abort prematurely)
1481  oppositeLength += 1000;
1482  } else {
1483  // return from the opposite ahead of the unpassable column leader (unless overlapping)
1484  if (overtaken.second > 0) {
1485  oppositeLength = MIN2(oppositeLength, forwardPos + overtaken.second);
1486  }
1487  // (don't set the distance so low as to imply emergency braking)
1488  oppositeLength = MAX2(oppositeLength, forwardPos + vehicle->getCarFollowModel().brakeGap(vehicle->getSpeed()));
1489  }
1490 #ifdef DEBUG_CHANGE_OPPOSITE
1491  if (DEBUG_COND) {
1492  std::cout << SIMTIME << " ego=" << vehicle->getID() << " is overtaking " << overtaken.first->getID()
1493  << " surplusGap=" << surplusGap
1494  << " spaceToOvertake=" << spaceToOvertake
1495  << " timeToOvertake=" << timeToOvertake
1496  << " timeToOvertake=" << timeToOvertake
1497  << " final laneQLength=" << oppositeLength
1498  << "\n";
1499  }
1500 #endif
1501  }
1502  }
1503  } else {
1504  if (overtaken.first == nullptr) {
1505  // there is no reason to stay on the opposite side
1506  std::pair<MSVehicle* const, double> neighFollow = opposite->getOppositeFollower(vehicle);
1507  if (neighFollow.first == nullptr) {
1508  oppositeLength = forwardPos;
1509  } else {
1510  const double secureGap = neighFollow.first->getCarFollowModel().getSecureGap(
1511  neighFollow.first, vehicle, neighFollow.first->getSpeed(), vehicle->getSpeed(), vehicle->getCarFollowModel().getMaxDecel());
1512 #ifdef DEBUG_CHANGE_OPPOSITE
1513  if (DEBUG_COND) {
1514  std::cout << SIMTIME << " ego=" << vehicle->getID() << " neighFollow=" << neighFollow.first->getID() << " gap=" << neighFollow.second << " secureGap=" << secureGap << "\n";
1515  }
1516 #endif
1517  if (neighFollow.second > secureGap) {
1518  // back gap is safe for immidiate return
1519  oppositeLength = forwardPos;
1520  }
1521  }
1522  }
1523  }
1524  leader.first = 0; // ignore leader after this
1525 #ifdef DEBUG_CHANGE_OPPOSITE
1526  if (DEBUG_COND) {
1527  std::cout << SIMTIME << " veh=" << vehicle->getID() << " remaining dist=" << oppositeLength - forwardPos << " forwardPos=" << forwardPos << " oppositeLength=" << oppositeLength << "\n";
1528  }
1529 #endif
1530  }
1531  std::vector<MSVehicle::LaneQ> preb = getBestLanesOpposite(vehicle, nullptr, oppositeLength);
1532  std::pair<MSVehicle* const, double> neighFollow = opposite->getOppositeFollower(vehicle);
1533  return checkChangeOpposite(vehicle, direction, opposite, leader, neighLead, neighFollow, preb);
1534 }
1535 
1536 
1537 std::pair<MSVehicle* const, double>
1538 MSLaneChanger::getOncomingVehicle(const MSLane* opposite, std::pair<MSVehicle*, double> oncoming, double searchDist, double& vMax, const MSVehicle* overtaken) {
1539  double gap = oncoming.second;
1540  while (oncoming.first != nullptr && (oncoming.first->getLaneChangeModel().isOpposite() || oncoming.first->getLaneChangeModel().getShadowLane() == opposite)) {
1541  searchDist -= (oncoming.first->getVehicleType().getLengthWithGap() + MAX2(0.0, oncoming.second));
1542  // leader is itself overtaking through the opposite side. find real oncoming vehicle
1543  gap += oncoming.first->getVehicleType().getLengthWithGap();
1544  if (oncoming.first != overtaken) {
1545  vMax = MIN2(vMax, oncoming.first->getSpeed());
1546  } // else: might be the case if we are overtaking a vehicle that is stopped on the opposite side
1547 #ifdef DEBUG_CHANGE_OPPOSITE
1548  if (gDebugFlag5) {
1549  std::cout << SIMTIME << " oncoming=" << oncoming.first->getID() << " isOpposite gap=" << oncoming.second
1550  << " totalGap=" << gap << " searchDist=" << searchDist << " vMax=" << vMax << "\n";
1551  }
1552 #endif
1553  if (searchDist < 0) {
1554  break;
1555  }
1556  // getOppositeLeader resets the search postion by ego length and may thus create cycles
1557  if (oncoming.first->getLaneChangeModel().getShadowLane() != opposite) {
1558  opposite = oncoming.first->getLane();
1559  }
1560  oncoming = opposite->getFollower(oncoming.first, oncoming.first->getPositionOnLane(opposite), searchDist, true);
1561  if (oncoming.first != nullptr) {
1562  gap += oncoming.second;
1563  }
1564  }
1565  oncoming.second = gap;
1566  return oncoming;
1567 }
1568 
1569 
1570 double
1571 MSLaneChanger::computeSurplusGap(const MSVehicle* vehicle, const MSLane* opposite, std::pair<MSVehicle*, double> oncoming, double timeToOvertake, double spaceToOvertake, double& oncomingSpeed) {
1572  double surplusGap = std::numeric_limits<double>::max();
1573  const MSVehicle* oncomingVeh = oncoming.first;
1574  if (oncomingVeh != 0 && !oncomingVeh->getLaneChangeModel().isOpposite() && oncomingVeh->getLaneChangeModel().getShadowLane() != opposite) {
1575  // conservative: assume that the oncoming vehicle accelerates to its maximum speed
1576  oncomingSpeed = oncomingVeh->isStopped() ? 0 : oncomingVeh->getLane()->getVehicleMaxSpeed(oncomingVeh);
1577  const double safetyGap = ((oncomingSpeed + vehicle->getLane()->getVehicleMaxSpeed(vehicle))
1578  * vehicle->getCarFollowModel().getHeadwayTime()
1580  surplusGap = oncoming.second - spaceToOvertake - timeToOvertake * oncomingSpeed - safetyGap;
1581 #ifdef DEBUG_CHANGE_OPPOSITE
1582  if (DEBUG_COND) {
1583  std::cout << SIMTIME
1584  << " oncoming=" << oncomingVeh->getID()
1585  << " oncomingGap=" << oncoming.second
1586  << " safetyGap=" << safetyGap
1587  << " surplusGap=" << surplusGap
1588  << "\n";
1589  }
1590 #endif
1591  }
1592  return surplusGap;
1593 }
1594 
1595 bool
1596 MSLaneChanger::foundHilltop(MSVehicle* vehicle, bool foundHill, double searchDist, const std::vector<MSLane*>& bestLanes, int view, double pos, double lastMax, double hilltopThreshold) {
1597  if (view >= (int)bestLanes.size()) {
1598  return false;
1599  }
1600  MSLane* lane = bestLanes[view];
1601  double laneDist = 0;
1602  const PositionVector& shape = lane->getShape();
1603  double lastZ = lastMax;
1604  for (int i = 1; i < (int)shape.size(); i++) {
1605  const double dist = lane->interpolateGeometryPosToLanePos(shape[i - 1].distanceTo(shape[i]));
1606  laneDist += dist;
1607  if (laneDist > pos) {
1608  const double z = shape[i].z();
1609  if (z > lastMax) {
1610  lastMax = z;
1611  }
1612  if (z > lastZ) {
1613  foundHill = true;
1614  }
1615  lastZ = z;
1616 #ifdef DEBUG_CHANGE_OPPOSITE
1617  if (DEBUG_COND) {
1618  std::cout << SIMTIME << " foundHill=" << foundHill << " searchDist=" << searchDist << " lastMax=" << lastMax << " lane=" << lane->getID() << " laneDist=" << laneDist << " z=" << z << "\n";
1619  }
1620 #endif
1621  if (foundHill && z < lastMax) {
1622  const double drop = lastMax - z;
1623  //std::cout << SIMTIME << " searchDist=" << searchDist << " hillDrop=" << drop << " lastMax=" << lastMax << " lane=" << lane->getID() << " laneDist=" << laneDist << " z=" << z << "\n";
1624  if (drop > hilltopThreshold) {
1625 #ifdef DEBUG_CHANGE_OPPOSITE
1626  if (DEBUG_COND) {
1627  std::cout << " cannot changeOpposite before the top of a hill searchDist=" << searchDist << " hillDrop=" << drop
1628  << " lastMax=" << lastMax << " lane=" << lane->getID() << " laneDist=" << laneDist << " z=" << z << "\n";
1629  }
1630 #endif
1631  return true;
1632  }
1633  }
1634  if (pos == 0) {
1635  searchDist -= dist;
1636  } else {
1637  searchDist -= laneDist - pos;
1638  pos = 0;
1639  }
1640  if (searchDist <= 0) {
1641  return false;
1642  }
1643  }
1644  }
1645  return foundHilltop(vehicle, foundHill, searchDist, bestLanes, view + 1, 0, lastMax, hilltopThreshold);
1646 }
1647 
1648 
1649 bool
1651  MSVehicle* vehicle,
1652  int laneOffset,
1653  MSLane* targetLane,
1654  const std::pair<MSVehicle* const, double>& leader,
1655  const std::pair<MSVehicle* const, double>& neighLead,
1656  const std::pair<MSVehicle* const, double>& neighFollow,
1657  const std::vector<MSVehicle::LaneQ>& preb) {
1658  const bool isOpposite = vehicle->getLaneChangeModel().isOpposite();
1659  MSLane* source = vehicle->getMutableLane();
1660  const std::pair<MSVehicle* const, double> follower(nullptr, -1);
1661  int state = checkChange(laneOffset, targetLane, leader, follower, neighLead, neighFollow, preb);
1662  vehicle->getLaneChangeModel().setOwnState(state);
1663  bool changingAllowed = (state & LCA_BLOCKED) == 0;
1664  // change if the vehicle wants to and is allowed to change
1665  if ((state & LCA_WANTS_LANECHANGE) != 0 && changingAllowed
1666  // do not change to the opposite direction for cooperative reasons
1667  && (isOpposite || (state & LCA_COOPERATIVE) == 0)) {
1668  const bool continuous = vehicle->getLaneChangeModel().startLaneChangeManeuver(source, targetLane, laneOffset);
1669 #ifdef DEBUG_CHANGE_OPPOSITE
1670  if (DEBUG_COND) {
1671  std::cout << SIMTIME << " changing to opposite veh=" << vehicle->getID() << " dir=" << laneOffset << " opposite=" << Named::getIDSecure(targetLane)
1672  << " state=" << toString((LaneChangeAction)state) << "\n";
1673  }
1674 #endif
1675  if (continuous) {
1676  continueChange(vehicle, myCandi);
1677  }
1678  return true;
1679  }
1680 #ifdef DEBUG_CHANGE_OPPOSITE
1681  if (DEBUG_COND) {
1682  std::cout << SIMTIME << " not changing to opposite veh=" << vehicle->getID() << " dir=" << laneOffset
1683  << " opposite=" << Named::getIDSecure(targetLane) << " state=" << toString((LaneChangeAction)state) << "\n";
1684  }
1685 #endif
1686  return false;
1687 }
1688 
1689 
1690 void
1691 MSLaneChanger::computeOvertakingTime(const MSVehicle* vehicle, double vMax, const MSVehicle* leader, double gap, double& timeToOvertake, double& spaceToOvertake) {
1692  // Assumptions:
1693  // - leader maintains the current speed
1694  // - vehicle merges with maxSpeed ahead of leader
1695  // XXX affected by ticket #860 (the formula is invalid for the current position update rule)
1696 
1697  // first compute these values for the case where vehicle is accelerating
1698  // without upper bound on speed
1699  const double v = vehicle->getSpeed();
1700  const double u = leader->getAcceleration() > 0 ? leader->getLane()->getVehicleMaxSpeed(leader) : leader->getSpeed();
1701  const double a = vehicle->getCarFollowModel().getMaxAccel();
1702  const double d = vehicle->getCarFollowModel().getMaxDecel();
1703  const double g = MAX2(0.0, (
1704  // drive up to the rear of leader
1705  gap + vehicle->getVehicleType().getMinGap()
1706  // drive head-to-head with the leader
1707  + leader->getVehicleType().getLengthWithGap()
1708  // drive past the leader
1709  + vehicle->getVehicleType().getLength()
1710  // allow for safe gap between leader and vehicle
1711  + leader->getCarFollowModel().getSecureGap(leader, vehicle, u, vMax, d))
1712  // time to move between lanes
1713  + (MSGlobals::gSublane ? vMax * vehicle->getLane()->getWidth() / vehicle->getVehicleType().getMaxSpeedLat() : 0));
1714  const double sign = -1; // XXX recheck
1715  // v*t + t*t*a*0.5 = g + u*t
1716  // solve t
1717  // t = ((u - v - (((((2.0*(u - v))**2.0) + (8.0*a*g))**(1.0/2.0))*sign/2.0))/a)
1718  double t = (u - v - sqrt(4 * (u - v) * (u - v) + 8 * a * g) * sign * 0.5) / a;
1719 #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
1720  if (DEBUG_COND) {
1721  std::cout << " computeOvertakingTime v=" << v << " vMax=" << vMax << " u=" << u << " a=" << a << " d=" << d << " gap=" << gap << " g=" << g << " t=" << t
1722  << " distEgo=" << v* t + t* t* a * 0.5 << " distLead=" << g + u* t
1723  << "\n";
1724  }
1725 #endif
1726  assert(t >= 0);
1727  if (vMax <= u) {
1728  // do not try to overtake faster leader
1729  timeToOvertake = std::numeric_limits<double>::max();
1730  spaceToOvertake = std::numeric_limits<double>::max();
1731  return;
1732  }
1733 
1734  // allow for a safety time gap
1736  // round to multiples of step length (TS)
1737  t = ceil(t / TS) * TS;
1738 
1740  const double timeToMaxSpeed = (vMax - v) / a;
1741 
1742 #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
1743  if (DEBUG_COND) {
1744  std::cout << " t=" << t << " tvMax=" << timeToMaxSpeed << "\n";
1745  }
1746 #endif
1747  if (t <= timeToMaxSpeed) {
1748  timeToOvertake = t;
1749  spaceToOvertake = v * t + t * t * a * 0.5;
1750 #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
1751  if (DEBUG_COND) {
1752  std::cout << " sto=" << spaceToOvertake << "\n";
1753  }
1754 #endif
1755  } else {
1756  // space until max speed is reached
1757  const double s = v * timeToMaxSpeed + timeToMaxSpeed * timeToMaxSpeed * a * 0.5;
1758  const double m = timeToMaxSpeed;
1759  // s + (t-m) * vMax = g + u*t
1760  // solve t
1761  t = (g - s + m * vMax) / (vMax - u);
1762  if (t < 0) {
1763  // cannot overtake in time
1764 #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
1765  if (DEBUG_COND) {
1766  std::cout << " t2=" << t << "\n";
1767  }
1768 #endif
1769  timeToOvertake = std::numeric_limits<double>::max();
1770  spaceToOvertake = std::numeric_limits<double>::max();
1771  return;
1772  } else {
1773  // allow for a safety time gap
1775  // round to multiples of step length (TS)
1776  t = ceil(t / TS) * TS;
1777 
1778  timeToOvertake = t;
1779  spaceToOvertake = s + (t - m) * vMax;
1780 #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
1781  if (DEBUG_COND) {
1782  std::cout << " t2=" << t << " s=" << s << " sto=" << spaceToOvertake << " m=" << m << "\n";
1783  }
1784 #endif
1785  }
1786  }
1787  const double safetyFactor = OPPOSITE_OVERTAKING_SAFETY_FACTOR * vehicle->getLaneChangeModel().getOppositeSafetyFactor();
1788  timeToOvertake *= safetyFactor;
1789  spaceToOvertake *= safetyFactor;
1790 #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
1791  if (DEBUG_COND) {
1792  if (safetyFactor != 1) {
1793  std::cout << " applying safetyFactor=" << safetyFactor
1794  << " tto=" << timeToOvertake << " sto=" << spaceToOvertake << "\n";
1795  }
1796  }
1797 #endif
1798 }
1799 
1800 
1801 
1802 std::pair<MSVehicle*, double>
1803 MSLaneChanger::getColumnleader(MSVehicle* vehicle, std::pair<MSVehicle*, double> leader, double maxLookAhead) {
1804  assert(leader.first != 0);
1805  const MSLane* source = vehicle->getLane();
1806  // find a leader vehicle with sufficient space ahead for merging back
1807  const double overtakingSpeed = source->getVehicleMaxSpeed(vehicle); // just a guess
1808  const double mergeBrakeGap = vehicle->getCarFollowModel().brakeGap(overtakingSpeed);
1809  std::pair<MSVehicle*, double> columnLeader = leader;
1810  double egoGap = leader.second;
1811  bool foundSpaceAhead = false;
1812  double seen = leader.second + leader.first->getVehicleType().getLengthWithGap();
1813  std::vector<MSLane*> conts = vehicle->getBestLanesContinuation();
1814  if (maxLookAhead == std::numeric_limits<double>::max()) {
1815  maxLookAhead = (vehicle->getVehicleType().getVehicleClass() == SVC_EMERGENCY
1818  }
1819 #ifdef DEBUG_CHANGE_OPPOSITE
1820  if (DEBUG_COND) {
1821  std::cout << " getColumnleader vehicle=" << vehicle->getID() << " leader=" << leader.first->getID() << " gap=" << leader.second << " maxLookAhead=" << maxLookAhead << "\n";
1822  }
1823 #endif
1824  const double safetyFactor = OPPOSITE_OVERTAKING_SAFETY_FACTOR * vehicle->getLaneChangeModel().getOppositeSafetyFactor();
1825  while (!foundSpaceAhead) {
1826  const double requiredSpaceAfterLeader = (columnLeader.first->getCarFollowModel().getSecureGap(
1827  columnLeader.first, vehicle,
1828  columnLeader.first->getSpeed(), overtakingSpeed, vehicle->getCarFollowModel().getMaxDecel())
1829  + columnLeader.first->getVehicleType().getMinGap()
1830  + vehicle->getVehicleType().getLengthWithGap());
1831 
1832 
1833  // all leader vehicles on the current laneChanger edge are already moved into MSLane::myTmpVehicles
1834  const bool checkTmpVehicles = (&columnLeader.first->getLane()->getEdge() == &source->getEdge());
1835  double searchStart = columnLeader.first->getPositionOnLane();
1836  std::pair<MSVehicle*, double> leadLead = columnLeader.first->getLane()->getLeader(
1837  columnLeader.first, searchStart, conts, requiredSpaceAfterLeader + mergeBrakeGap,
1838  checkTmpVehicles);
1839  std::set<MSVehicle*> seenLeaders;
1840  while (leadLead.first != nullptr && leadLead.first->getLaneChangeModel().isOpposite()) {
1841 #ifdef DEBUG_CHANGE_OPPOSITE
1842  if (DEBUG_COND) {
1843  std::cout << " skipping opposite leadLead=" << leadLead.first->getID() << " gap=" << leadLead.second << "\n";
1844  }
1845 #endif
1846  if (leadLead.second + seen > maxLookAhead || seenLeaders.count(leadLead.first) > 0) {
1847  leadLead.first = nullptr;
1848  break;
1849  }
1850  seenLeaders.insert(leadLead.first);
1851  // found via shadow lane, skip it
1852  const double searchStart2 = searchStart + MAX2(0.0, leadLead.second) + leadLead.first->getVehicleType().getLengthWithGap();
1853  leadLead = columnLeader.first->getLane()->getLeader(
1854  columnLeader.first, searchStart2, conts, requiredSpaceAfterLeader + mergeBrakeGap,
1855  checkTmpVehicles);
1856  leadLead.second += (searchStart2 - searchStart);
1857  }
1858  if (leadLead.first == nullptr) {
1859  double availableSpace = columnLeader.first->getLane()->getLength() - columnLeader.first->getPositionOnLane();
1860  const double requiredSpace = safetyFactor * (requiredSpaceAfterLeader
1861  + vehicle->getCarFollowModel().brakeGap(overtakingSpeed));
1862 #ifdef DEBUG_CHANGE_OPPOSITE
1863  if (DEBUG_COND) {
1864  std::cout << " no direct leader found after columnLeader " << columnLeader.first->getID()
1865  << " availableSpace=" << availableSpace
1866  << " req1=" << requiredSpaceAfterLeader
1867  << " req2=" << requiredSpace / safetyFactor
1868  << " req3=" << requiredSpace
1869  << "\n";
1870  }
1871 #endif
1872  if (availableSpace > requiredSpace) {
1873  foundSpaceAhead = true;
1874  } else {
1875  // maybe the columnleader is stopped before a junction or takes a different turn.
1876  // try to find another columnleader on successive lanes
1877  const bool allowMinor = vehicle->getVehicleType().getVehicleClass() == SVC_EMERGENCY;
1878  const MSLane* next = getLaneAfter(columnLeader.first->getLane(), conts, allowMinor);
1879 #ifdef DEBUG_CHANGE_OPPOSITE
1880  if (DEBUG_COND) {
1881  std::cout << " look for another leader on lane " << Named::getIDSecure(next) << "\n";
1882  }
1883 #endif
1884  while (next != nullptr && seen < maxLookAhead) {
1885  seen += next->getLength();
1886  MSVehicle* cand = next->getLastAnyVehicle();
1887  if (cand == nullptr) {
1888  availableSpace += next->getLength();
1889  if (availableSpace > requiredSpace) {
1890  foundSpaceAhead = true;
1891  break;
1892  }
1893  next = getLaneAfter(next, conts, allowMinor);
1894  } else {
1895  availableSpace += cand->getBackPositionOnLane();
1896  if (availableSpace > requiredSpace) {
1897  foundSpaceAhead = true;
1898  break;
1899  } else {
1900  return getColumnleader(vehicle, std::make_pair(cand, availableSpace + cand->getPositionOnLane()), maxLookAhead - seen);
1901  }
1902  }
1903  }
1904  if (!foundSpaceAhead) {
1905  return std::make_pair(nullptr, -1);
1906  }
1907  }
1908  } else {
1909  const double requiredSpace = safetyFactor * (requiredSpaceAfterLeader
1910  + vehicle->getCarFollowModel().getSecureGap(vehicle, leadLead.first,
1911  overtakingSpeed, leadLead.first->getSpeed(), leadLead.first->getCarFollowModel().getMaxDecel()));
1912 #ifdef DEBUG_CHANGE_OPPOSITE
1913  if (DEBUG_COND) {
1914  std::cout << " leader's leader " << leadLead.first->getID() << " space=" << leadLead.second
1915  << " req1=" << requiredSpaceAfterLeader
1916  << " req2=" << requiredSpace / safetyFactor
1917  << " req3=" << requiredSpace
1918  << "\n";
1919  }
1920 #endif
1921  if (leadLead.second > requiredSpace) {
1922  foundSpaceAhead = true;
1923  } else {
1924 
1925  if (leadLead.second < 0) {
1926  // must be a junction leader or some other dangerous situation
1927 #ifdef DEBUG_CHANGE_OPPOSITE
1928  if (DEBUG_COND) {
1929  std::cout << " leader's leader " << leadLead.first->getID() << " gap=" << leadLead.second << " is junction leader (aborting)\n";
1930  }
1931 #endif
1932  return std::make_pair(nullptr, -1);
1933  }
1934 
1935 #ifdef DEBUG_CHANGE_OPPOSITE
1936  if (DEBUG_COND) {
1937  std::cout << " not enough space after columnLeader=" << columnLeader.first->getID() << " required=" << requiredSpace << "\n";
1938  }
1939 #endif
1940  seen += MAX2(0., leadLead.second) + leadLead.first->getVehicleType().getLengthWithGap();
1941  if (seen > maxLookAhead) {
1942 #ifdef DEBUG_CHANGE_OPPOSITE
1943  if (DEBUG_COND) {
1944  std::cout << " cannot changeOpposite due to insufficient free space after columnLeader (seen=" << seen << " columnLeader=" << columnLeader.first->getID() << ")\n";
1945  }
1946 #endif
1947  return std::make_pair(nullptr, -1);
1948  }
1949  // see if merging after leadLead is possible
1950  egoGap += columnLeader.first->getVehicleType().getLengthWithGap() + leadLead.second;
1951  columnLeader = leadLead;
1952 #ifdef DEBUG_CHANGE_OPPOSITE
1953  if (DEBUG_COND) {
1954  std::cout << " new columnLeader=" << columnLeader.first->getID() << "\n";
1955  }
1956 #endif
1957  }
1958  }
1959  }
1960  columnLeader.second = egoGap;
1961  return columnLeader;
1962 }
1963 
1964 
1965 const MSLane*
1966 MSLaneChanger::getLaneAfter(const MSLane* lane, const std::vector<MSLane*>& conts, bool allowMinor) {
1967  for (auto it = conts.begin(); it != conts.end(); ++it) {
1968  if (*it == lane) {
1969  if (it + 1 != conts.end()) {
1970  // abort on minor link
1971  const MSLane* next = *(it + 1);
1972  const MSLink* link = lane->getLinkTo(next);
1973  if (link == nullptr || (!allowMinor && !link->havePriority())) {
1974  return nullptr;
1975  }
1976  return next;
1977  } else {
1978  return nullptr;
1979  }
1980  }
1981  }
1982  return nullptr;
1983 }
1984 
1985 
1986 /****************************************************************************/
#define OPPOSITE_OVERTAKING_HILLTOP_THRESHOHOLD
#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 OPPOSITE_OVERTAKING_MAX_SPACE_TO_OVERTAKE
#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:32
SUMOVehicleClass
Definition of vehicle classes to differ between different lane usage and authority types.
@ 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
bool gDebugFlag5
Definition: StdDefs.cpp:36
const double SUMO_const_laneWidth
Definition: StdDefs.h:48
T MIN2(T a, T b)
Definition: StdDefs.h:74
T MAX2(T a, T b)
Definition: StdDefs.h:80
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:46
A class responsible for exchanging messages between cars involved in lane-change interaction.
Interface for lane-change models.
double getLaneChangeCompletion() const
Get the current lane change completion ratio.
double getForwardPos() const
get vehicle position relative to the forward direction lane
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, bool urgent) 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
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 int wantsChange(int laneOffset, MSAbstractLaneChangeModel::MSLCMessager &msgPass, int blocked, const std::pair< MSVehicle *, double > &leader, const std::pair< MSVehicle *, double > &follower, 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...
virtual double getOppositeSafetyFactor() const
return factor for modifying the safety constraints for opposite-diretction overtaking of the car-foll...
void setLeaderGaps(CLeaderDist, double secGap)
void setOrigLeaderGaps(CLeaderDist, double secGap)
int getNormalizedLaneIndex()
brief return lane index that treats opposite lanes like normal lanes to the left of the forward lanes
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
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...
MSStop & getNextStop()
const MSVehicleType & getVehicleType() const
Returns the vehicle's type definition.
bool hasStops() const
Returns whether the vehicle has to stop somewhere.
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:351
virtual double brakeGap(const double speed) const
Returns the distance the vehicle needs to halt including driver's reaction time tau (i....
Definition: MSCFModel.h:334
double getMaxAccel() const
Get the vehicle type's maximum acceleration [m/s^2].
Definition: MSCFModel.h:231
double getMaxDecel() const
Get the vehicle type's maximal comfortable deceleration [m/s^2].
Definition: MSCFModel.h:239
virtual double getHeadwayTime() const
Get the driver's desired headway [s].
Definition: MSCFModel.h:280
A road/street connecting two junctions.
Definition: MSEdge.h:77
const MSEdge * getOppositeEdge() const
Returns the opposite direction edge if on exists else a nullptr.
Definition: MSEdge.cpp:1163
const MSEdge * getNormalSuccessor() const
if this edge is an internal edge, return its first normal successor, otherwise the edge itself
Definition: MSEdge.cpp:804
const std::vector< MSLane * > & getLanes() const
Returns this edge's lanes.
Definition: MSEdge.h:168
int getNumLanes() const
Definition: MSEdge.h:172
bool isInternal() const
return whether this edge is an internal edge
Definition: MSEdge.h:262
static double gLateralResolution
Definition: MSGlobals.h:88
static bool gLefthand
Whether lefthand-drive is being simulated.
Definition: MSGlobals.h:157
static bool gSublane
whether sublane simulation is enabled (sublane model or continuous lanechanging)
Definition: MSGlobals.h:148
static SUMOTime gLaneChangeDuration
Definition: MSGlobals.h:85
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.
static bool hasOppositeStop(MSVehicle *vehicle)
whether vehicle has an opposite-direction stop within relevant range
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()
bool startChange(MSVehicle *vehicle, ChangerIt &from, int direction)
const bool myAllowsChanging
int checkChangeWithinEdge(int laneOffset, const std::pair< MSVehicle *const, double > &leader, const std::vector< MSVehicle::LaneQ > &preb) const
MSVehicle * veh(ConstChangerIt ce) const
static bool foundHilltop(MSVehicle *vehicle, bool foundHill, double searchDist, const std::vector< MSLane * > &bestLanes, int view, double pos, double lastMax, double hilltopThreshold)
Changer myChanger
Container for ChangeElemements, one for every lane in the edge.
static double computeSurplusGap(const MSVehicle *vehicle, const MSLane *opposite, std::pair< MSVehicle *, double > oncoming, double timeToOvertake, double spaceToOvertake, double &oncomingSpeed)
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
std::pair< MSVehicle *const, double > getOncomingVehicle(const MSLane *opposite, std::pair< MSVehicle *, double > neighOncoming, double searchDist, double &vMax, const MSVehicle *overtaken=nullptr)
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
int checkChange(int laneOffset, const MSLane *targetLane, const std::pair< MSVehicle *const, double > &leader, const std::pair< MSVehicle *const, double > &follower, const std::pair< MSVehicle *const, double > &neighLead, const std::pair< MSVehicle *const, double > &neighFollow, const std::vector< MSVehicle::LaneQ > &preb) const
static const MSLane * getLaneAfter(const MSLane *lane, const std::vector< MSLane * > &conts, bool allowMinor)
return the next lane in conts beyond lane or nullptr
virtual bool checkChangeOpposite(MSVehicle *vehicle, int laneOffset, 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)
static void computeOvertakingTime(const MSVehicle *vehicle, double vMax, const MSVehicle *leader, double gap, double &timeToOvertake, double &spaceToOvertake)
Compute the time and space required for overtaking the given leader.
bool changeOpposite(MSVehicle *vehicle, std::pair< MSVehicle *, double > leader)
static std::vector< MSVehicle::LaneQ > getBestLanesOpposite(MSVehicle *vehicle, const MSLane *stopLane, double oppositeLength)
add LaneQ for opposite lanes
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:2300
static std::vector< MSLink * >::const_iterator succLinkSec(const SUMOVehicle &veh, int nRouteSuccs, const MSLane &succLinkSource, const std::vector< MSLane * > &conts)
Definition: MSLane.cpp:2165
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:3978
const MSLink * getLinkTo(const MSLane *const) const
returns the link to the given lane or nullptr, if it is not connected
Definition: MSLane.cpp:2214
double getSpeedLimit() const
Returns the lane's maximum allowed speed.
Definition: MSLane.h:533
std::vector< MSVehicle * > VehCont
Container for vehicles.
Definition: MSLane.h:92
std::pair< MSVehicle *const, double > getFollower(const MSVehicle *ego, double egoPos, double dist, bool ignoreMinorLinks) const
Find follower vehicle for the given ego vehicle (which may be on the opposite direction lane)
Definition: MSLane.cpp:3822
double getLength() const
Returns the lane's length.
Definition: MSLane.h:541
bool allowsChangingLeft(SUMOVehicleClass vclass) const
Returns whether the given vehicle class may change left from this lane.
Definition: MSLane.h:819
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:2554
bool isLinkEnd(std::vector< MSLink * >::const_iterator &i) const
Definition: MSLane.h:763
bool allowsVehicleClass(SUMOVehicleClass vclass) const
Definition: MSLane.h:814
double getVehicleMaxSpeed(const SUMOTrafficObject *const veh) const
Returns the lane's maximum speed, given a vehicle's speed limit adaptation.
Definition: MSLane.h:519
std::pair< MSVehicle *const, double > getOppositeFollower(const MSVehicle *ego) const
Definition: MSLane.cpp:3873
bool hasPedestrians() const
whether the lane has pedestrians on it
Definition: MSLane.cpp:3971
int getIndex() const
Returns the lane's index.
Definition: MSLane.h:563
void requireCollisionCheck()
require another collision check due to relevant changes in the simulation
Definition: MSLane.h:627
double getOppositePos(double pos) const
return the corresponding position on the opposite lane
Definition: MSLane.cpp:3817
MSVehicle * getLastAnyVehicle() const
returns the last vehicle that is fully or partially on this lane
Definition: MSLane.cpp:2137
bool isInternal() const
Definition: MSLane.cpp:2122
VehCont myPartialVehicles
The lane's partial vehicles. This container holds all vehicles that are partially on this lane but wh...
Definition: MSLane.h:1348
MSEdge & getEdge() const
Returns the lane's edge.
Definition: MSLane.h:674
const PositionVector & getShape() const
Returns this lane's shape.
Definition: MSLane.h:478
double interpolateGeometryPosToLanePos(double geometryPos) const
Definition: MSLane.h:511
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:2368
MSLane * getOpposite() const
return the neighboring opposite direction lane for lane changing or nullptr
Definition: MSLane.cpp:3802
std::pair< MSVehicle *const, double > getOppositeLeader(const MSVehicle *ego, double dist, bool oppositeDir) const
Definition: MSLane.cpp:3845
double getWidth() const
Returns the lane's width.
Definition: MSLane.h:556
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
Definition: MSNet.cpp:174
bool hasElevation() const
return whether the network contains elevation data
Definition: MSNet.h:778
Definition: MSStop.h:44
const MSLane * lane
The lane to stop at (microsim only)
Definition: MSStop.h:50
bool isOpposite
whether this an opposite-direction stop
Definition: MSStop.h:89
bool getRespectJunctionPriority() const
Returns whether junction priority rules shall be respected (concerns approaching vehicles outside the...
Definition: MSVehicle.h:1472
SUMOTime getLaneTimeLineDuration()
Definition: MSVehicle.cpp:454
double myPosLat
the stored lateral position
Definition: MSVehicle.h:138
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:75
void updateBestLanes(bool forceRebuild=false, const MSLane *startLane=0)
computes the best lanes to use in order to continue the route
Definition: MSVehicle.cpp:5069
double computeAngle() const
compute the current vehicle angle
Definition: MSVehicle.cpp:1330
bool isStoppedOnLane() const
Definition: MSVehicle.cpp:1477
bool isActive() const
Returns whether the current simulation step is an action point for the vehicle.
Definition: MSVehicle.h:597
bool isFrontOnLane(const MSLane *lane) const
Returns the information whether the front of the vehicle is on the given lane.
Definition: MSVehicle.cpp:4465
MSAbstractLaneChangeModel & getLaneChangeModel()
Definition: MSVehicle.cpp:5051
MSLane * getMutableLane() const
Returns the lane the vehicle is on Non const version indicates that something volatile is going on.
Definition: MSVehicle.h:560
int influenceChangeDecision(int state)
allow TraCI to influence a lane change decision
Definition: MSVehicle.cpp:6424
bool isRemoteControlled() const
Returns the information whether the vehicle is fully controlled via TraCI.
Definition: MSVehicle.cpp:6443
double nextStopDist() const
return the distance to the next stop or doubleMax if there is none.
Definition: MSVehicle.h:995
void adaptBestLanesOccupation(int laneIndex, double density)
update occupation from MSLaneChanger
Definition: MSVehicle.cpp:5678
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:485
Position getPosition(const double offset=0) const
Return current position (x/y, cartesian)
Definition: MSVehicle.cpp:1142
const std::vector< MSLane * > & getBestLanesContinuation() const
Returns the best sequence of lanes to continue the route starting at myLane.
Definition: MSVehicle.cpp:5497
double getBackPositionOnLane(const MSLane *lane) const
Get the vehicle's position relative to the given lane.
Definition: MSVehicle.h:396
@ VEH_SIGNAL_BLINKER_RIGHT
Right blinker lights are switched on.
Definition: MSVehicle.h:1068
@ VEH_SIGNAL_BLINKER_LEFT
Left blinker lights are switched on.
Definition: MSVehicle.h:1070
SUMOTime getActionStepLength() const
Returns the vehicle's action step length in millisecs, i.e. the interval between two action points.
Definition: MSVehicle.h:496
Influencer & getInfluencer()
Definition: MSVehicle.cpp:6389
double getRightSideOnLane() const
Get the vehicle's lateral position on the lane:
Definition: MSVehicle.cpp:5888
bool unsafeLinkAhead(const MSLane *lane) const
whether the vehicle may safely move to the given lane with regard to upcoming links
Definition: MSVehicle.cpp:6082
double getLateralPositionOnLane() const
Get the vehicle's lateral position on the lane.
Definition: MSVehicle.h:411
double getSlope() const
Returns the slope of the road at vehicle's position in degrees.
Definition: MSVehicle.cpp:1128
double getSpeed() const
Returns the vehicle's current speed.
Definition: MSVehicle.h:462
Position myCachedPosition
Definition: MSVehicle.h:1882
const std::vector< LaneQ > & getBestLanes() const
Returns the description of best lanes to use in order to continue the route.
Definition: MSVehicle.cpp:5063
double myAngle
the angle in radians (
Definition: MSVehicle.h:1874
bool ignoreRed(const MSLink *link, bool canBrake) const
decide whether a red (or yellow light) may be ignore
Definition: MSVehicle.cpp:6467
double getPositionOnLane() const
Get the vehicle's position along the lane.
Definition: MSVehicle.h:372
const MSLane * getLane() const
Returns the lane the vehicle is on.
Definition: MSVehicle.h:552
const MSCFModel & getCarFollowModel() const
Returns the vehicle's car following model definition.
Definition: MSVehicle.h:917
bool hasInfluencer() const
whether the vehicle is individually influenced (via TraCI or special parameters)
Definition: MSVehicle.h:1633
State myState
This Vehicles driving state (pos and speed)
Definition: MSVehicle.h:1809
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 getMaxSpeedLat() const
Get vehicle's maximum lateral speed [m/s].
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:67
const std::string & getID() const
Returns the id.
Definition: Named.h:74
static const Position INVALID
used to indicate that a position is valid
Definition: Position.h:293
double z() const
Returns the z-position.
Definition: Position.h:65
A list of positions.
#define M_PI
Definition: odrSpiral.cpp:40
void registerHop(MSVehicle *vehicle)
Register that vehicle belongs to Changer Item to after LC decisions.