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