SUMO - Simulation of Urban MObility
MSLaneChangerSublane.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-2018 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 /****************************************************************************/
16 // Performs sub-lane changing of vehicles
17 /****************************************************************************/
18 
19 // ===========================================================================
20 // included modules
21 // ===========================================================================
22 #include <config.h>
23 
24 #include "MSLaneChangerSublane.h"
25 #include "MSNet.h"
26 #include "MSVehicle.h"
27 #include "MSVehicleType.h"
28 #include "MSVehicleTransfer.h"
29 #include "MSGlobals.h"
30 #include <cassert>
31 #include <iterator>
32 #include <cstdlib>
33 #include <cmath>
36 #include <utils/geom/GeomHelper.h>
37 
38 
39 // ===========================================================================
40 // DEBUG constants
41 // ===========================================================================
42 #define DEBUG_COND (vehicle->getLaneChangeModel().debugVehicle())
43 //#define DEBUG_COND (vehicle->getID() == "disabled")
44 //#define DEBUG_DECISION
45 //#define DEBUG_ACTIONSTEPS
46 //#define DEBUG_STATE
47 //#define DEBUG_MANEUVER
48 //#define DEBUG_SURROUNDING
49 
50 // ===========================================================================
51 // member method definitions
52 // ===========================================================================
53 MSLaneChangerSublane::MSLaneChangerSublane(const std::vector<MSLane*>* lanes, bool allowChanging) :
54  MSLaneChanger(lanes, allowChanging) {
55  // initialize siblings
56  if (myChanger.front().lane->isInternal()) {
57  for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
58  for (ChangerIt ce2 = myChanger.begin(); ce2 != myChanger.end(); ++ce2) {
59  if (ce != ce2 && ce->lane->getIncomingLanes().front().lane == ce2->lane->getIncomingLanes().front().lane) {
60  //std::cout << "addSibling lane=" << ce->lane->getID() << " offset=" << ce2->lane->getIndex() - ce->lane->getIndex() << "\n";
61  ce->siblings.push_back(ce2->lane->getIndex() - ce->lane->getIndex());
62  }
63  }
64  }
65  }
66 }
67 
68 
70 
71 void
74  // Prepare myChanger with a safe state.
75  for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
76  ce->ahead = ce->lane->getPartialBeyond();
77 // std::cout << SIMTIME << " initChanger lane=" << ce->lane->getID() << " vehicles=" << toString(ce->lane->myVehicles) << "\n";
78 // std::cout << SIMTIME << " initChanger lane=" << ce->lane->getID() << " partial vehicles=" << toString(ce->lane->myPartialVehicles) << "\n";
79 // std::cout << SIMTIME << " initChanger lane=" << ce->lane->getID() << " partial vehicles beyond=" << toString(ce->ahead.toString()) << "\n";
80  }
81 }
82 
83 
84 
85 void
87  MSLaneChanger::updateChanger(vehHasChanged);
88  if (!vehHasChanged) {
89  MSVehicle* lead = myCandi->lead;
90  //std::cout << SIMTIME << " updateChanger lane=" << myCandi->lane->getID() << " lead=" << Named::getIDSecure(lead) << "\n";
91  myCandi->ahead.addLeader(lead, false, 0);
92  MSLane* shadowLane = lead->getLaneChangeModel().getShadowLane();
93  if (shadowLane != nullptr) {
94  const double latOffset = lead->getLane()->getRightSideOnEdge() - shadowLane->getRightSideOnEdge();
95  //std::cout << SIMTIME << " updateChanger shadowLane=" << shadowLane->getID() << " lead=" << Named::getIDSecure(lead) << "\n";
96  (myChanger.begin() + shadowLane->getIndex())->ahead.addLeader(lead, false, latOffset);
97  }
98  }
99  //std::cout << SIMTIME << " updateChanger: lane=" << myCandi->lane->getID() << " lead=" << Named::getIDSecure(myCandi->lead) << " ahead=" << myCandi->ahead.toString() << " vehHasChanged=" << vehHasChanged << "\n";
100  //for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
101  // std::cout << " lane=" << ce->lane->getID() << " vehicles=" << toString(ce->lane->myVehicles) << "\n";
102  //}
103 }
104 
105 
106 bool
108  // variant of change() for the sublane case
110  MSVehicle* vehicle = veh(myCandi);
111 #ifdef DEBUG_ACTIONSTEPS
112  if DEBUG_COND {
113  std::cout << "\nCHANGE" << std::endl;
114 }
115 #endif
116 assert(vehicle->getLane() == (*myCandi).lane);
117  assert(!vehicle->getLaneChangeModel().isChangingLanes());
118  if ( vehicle->isStoppedOnLane()) {
119  registerUnchanged(vehicle);
120  return false;
121  }
122  if (vehicle->isRemoteControlled()) {
123  registerUnchanged(vehicle);
124  return false;
125  }
126  if (!vehicle->isActive()) {
127 #ifdef DEBUG_ACTIONSTEPS
128  if DEBUG_COND {
129  std::cout << SIMTIME << " veh '" << vehicle->getID() << "' skips regular change checks." << std::endl;
130  }
131 #endif
132 
133  bool changed;
134  // let TraCI influence the wish to change lanes during non-actionsteps
135  checkTraCICommands(vehicle);
136 
137  // Resume change
138  changed = continueChangeSublane(vehicle, myCandi);
139 #ifdef DEBUG_ACTIONSTEPS
140  if DEBUG_COND {
141  std::cout << SIMTIME << " veh '" << vehicle->getID() << "' lcm->maneuverDist=" << vehicle->getLaneChangeModel().getManeuverDist()
142  << " lcm->speedLat=" << vehicle->getLaneChangeModel().getSpeedLat() << std::endl;
143  }
144 #endif
145  return changed;
146  }
147 
148 #ifdef DEBUG_ACTIONSTEPS
149  if DEBUG_COND {
150  std::cout << "\n" << SIMTIME << " veh '" << vehicle->getID() << "' at plans sublane maneuver."
151  << std::endl;
152  }
153 #endif
154  vehicle->updateBestLanes(); // needed?
155  for (int i = 0; i < (int) myChanger.size(); ++i) {
156  vehicle->adaptBestLanesOccupation(i, myChanger[i].dens);
157  }
158  // update leaders beyond the current edge for all lanes
159  for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
160  ce->aheadNext = getLeaders(ce, vehicle);
161  }
162 
163  // update expected speeds
164  int sublaneIndex = 0;
165  for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
166  vehicle->getLaneChangeModel().updateExpectedSublaneSpeeds(ce->aheadNext, sublaneIndex, ce->lane->getIndex());
167  for (int offset : ce->siblings) {
168  // treat sibling lanes (internal lanes with the same origin lane) as if they have the same geometry
169  ChangerIt ceSib = ce + offset;
170  vehicle->getLaneChangeModel().updateExpectedSublaneSpeeds(ceSib->aheadNext, sublaneIndex, ceSib->lane->getIndex());
171  }
172  sublaneIndex += ce->ahead.numSublanes();
173  }
174 
176  | (mayChange(1) ? LCA_LEFT : LCA_NONE));
177 
178  StateAndDist right = checkChangeHelper(vehicle, -1, alternatives);
179  StateAndDist left = checkChangeHelper(vehicle, 1, alternatives);
180  StateAndDist current = checkChangeHelper(vehicle, 0, alternatives);
181 
182  StateAndDist decision = vehicle->getLaneChangeModel().decideDirection(current,
183  vehicle->getLaneChangeModel().decideDirection(right, left));
184 #ifdef DEBUG_DECISION
185  if (vehicle->getLaneChangeModel().debugVehicle()) {
186  std::cout << "\n" << SIMTIME << " decision=" << toString((LaneChangeAction)decision.state) << " dir=" << decision.dir << " latDist=" << decision.latDist << " maneuverDist=" << decision.maneuverDist << "\n";
187  }
188 #endif
189  vehicle->getLaneChangeModel().setOwnState(decision.state);
190  vehicle->getLaneChangeModel().setManeuverDist(decision.maneuverDist);
191  if ((decision.state & LCA_WANTS_LANECHANGE) != 0 && (decision.state & LCA_BLOCKED) == 0) {
192  // change if the vehicle wants to and is allowed to change
193 #ifdef DEBUG_MANEUVER
194  if DEBUG_COND {
195  std::cout << SIMTIME << " veh '" << vehicle->getID() << "' performing sublane change..." << std::endl;
196  }
197 #endif
198  return startChangeSublane(vehicle, myCandi, decision.latDist);
199  }
200  // @note this assumes vehicles can instantly abort any maneuvre in case of emergency
201  abortLCManeuver(vehicle);
202 
203  if ((right.state & (LCA_URGENT)) != 0 && (left.state & (LCA_URGENT)) != 0) {
204  // ... wants to go to the left AND to the right
205  // just let them go to the right lane...
206  left.state = 0;
207  }
208  return false;
209 }
210 
211 
212 void
214 #ifdef DEBUG_MANEUVER
215  if DEBUG_COND {
216  std::cout << SIMTIME << " veh '" << vehicle->getID() << "' aborts LC-continuation."
217  << std::endl;
218  }
219 #endif
220  vehicle->getLaneChangeModel().setSpeedLat(0);
221  vehicle->getLaneChangeModel().setManeuverDist(0.);
222  registerUnchanged(vehicle);
223 }
224 
225 
227 MSLaneChangerSublane::checkChangeHelper(MSVehicle* vehicle, int laneOffset, LaneChangeAction alternatives) {
228  StateAndDist result = StateAndDist(0, 0, 0, 0);
229  if (mayChange(laneOffset)) {
230  const std::vector<MSVehicle::LaneQ>& preb = vehicle->getBestLanes();
231  result.state = checkChangeSublane(laneOffset, alternatives, preb, result.latDist, result.maneuverDist);
232  result.dir = laneOffset;
233  if ((result.state & LCA_WANTS_LANECHANGE) != 0 && (result.state & LCA_URGENT) != 0 && (result.state & LCA_BLOCKED) != 0) {
234  (myCandi + laneOffset)->lastBlocked = vehicle;
235  if ((myCandi + laneOffset)->firstBlocked == nullptr) {
236  (myCandi + laneOffset)->firstBlocked = vehicle;
237  }
238  }
239  }
240  return result;
241 }
242 
243 
245 // (used to continue sublane changing in non-action steps).
246 bool
248  // lateral distance to complete maneuver
249  double remLatDist = vehicle->getLaneChangeModel().getManeuverDist();
250  if (remLatDist == 0) {
251  registerUnchanged(vehicle);
252  return false;
253  }
254  const double nextLatDist = SPEED2DIST(vehicle->getLaneChangeModel().computeSpeedLat(remLatDist, remLatDist));
255 #ifdef DEBUG_MANEUVER
256  if DEBUG_COND {
257  std::cout << SIMTIME << " vehicle '" << vehicle->getID() << "' continueChangeSublane()"
258  << " remLatDist=" << remLatDist << " nextLatDist=" << nextLatDist
259  << std::endl;
260  }
261 #endif
262 
263  const bool changed = startChangeSublane(vehicle, from, nextLatDist);
264  return changed;
265 }
266 
267 
268 bool
270  // Prevent continuation of LC beyond lane borders if change is not allowed
271  const double distToRightLaneBorder = latDist < 0 ? vehicle->getLane()->getWidth() * 0.5 + vehicle->getLateralPositionOnLane() - vehicle->getWidth() * 0.5 : 0.;
272  const double distToLeftLaneBorder = latDist > 0 ? vehicle->getLane()->getWidth() * 0.5 - vehicle->getLateralPositionOnLane() - vehicle->getWidth() * 0.5 : 0.;
273  // determine direction of LC
274  const int direction = (latDist >= -distToRightLaneBorder && latDist <= distToLeftLaneBorder) ? 0 : (latDist < 0 ? -1 : 1);
275  ChangerIt to = from;
276  if (mayChange(direction)) {
277  to = from + direction;
278  } else {
279  // This may occur during maneuver continuation in non-actionsteps.
280  // TODO: Understand better why and test later if additional sublane actionstep debugging resolves this
281  // (XXX: perhaps one should try to extrapolate check for this case before to avoid maneuver initialization
282  // similar as for continuous LC in MSLaneChanger::checkChange())
283  //assert(false);
284  abortLCManeuver(vehicle);
285  return false;
286  }
287 
288  // The following does:
289  // 1) update vehicles lateral position according to latDist and target lane
290  // 2) distinguish several cases
291  // a) vehicle moves completely within the same lane
292  // b) vehicle intersects another lane
293  // - vehicle must be moved to the lane where it's midpoint is (either old or new)
294  // - shadow vehicle must be created/moved to the other lane if the vehicle intersects it
295  // 3) updated dens of all lanes that hold the vehicle or its shadow
296 
297  vehicle->myState.myPosLat += latDist;
299  vehicle->getLaneChangeModel().setSpeedLat(DIST2SPEED(latDist));
300 #ifdef DEBUG_MANEUVER
301  if DEBUG_COND {
302  std::cout << SIMTIME << " vehicle '" << vehicle->getID() << "' with maneuverDist=" << vehicle->getLaneChangeModel().getManeuverDist()
303  << " and committedSpeed=" << vehicle->getLaneChangeModel().getCommittedSpeed()
304  << " increments lateral position by latDist=" << latDist << std::endl;
305  }
306 #endif
307 #ifdef DEBUG_SURROUNDING
308  if DEBUG_COND {
309  std::cout << SIMTIME << " vehicle '" << vehicle->getID() << "'\n to->ahead=" << to->ahead.toString()
310  << "'\n to->aheadNext=" << to->aheadNext.toString()
311  << std::endl;
312  }
313 #endif
314  const bool completedManeuver = vehicle->getLaneChangeModel().getManeuverDist() - latDist == 0.;
315  vehicle->getLaneChangeModel().setManeuverDist(vehicle->getLaneChangeModel().getManeuverDist() - latDist);
316  vehicle->getLaneChangeModel().updateSafeLatDist(latDist);
317 
318  outputLCStarted(vehicle, from, to, direction);
319  const bool changedToNewLane = checkChangeToNewLane(vehicle, direction, from, to);
320 
321  MSLane* oldShadowLane = vehicle->getLaneChangeModel().getShadowLane();
323  MSLane* shadowLane = vehicle->getLaneChangeModel().getShadowLane();
324  if (shadowLane != nullptr && shadowLane != oldShadowLane) {
325  assert(to != from || oldShadowLane == 0);
326  const double latOffset = vehicle->getLane()->getRightSideOnEdge() - shadowLane->getRightSideOnEdge();
327  (myChanger.begin() + shadowLane->getIndex())->ahead.addLeader(vehicle, false, latOffset);
328  }
329  if (completedManeuver) {
330  outputLCEnded(vehicle, from, to, direction);
331  }
332 
333  // Update maneuver reservations on target lanes
335 
336  // compute new angle of the vehicle from the x- and y-distances travelled within last time step
337  // (should happen last because primaryLaneChanged() also triggers angle computation)
338  // this part of the angle comes from the orientation of our current lane
339  double laneAngle = vehicle->getLane()->getShape().rotationAtOffset(vehicle->getLane()->interpolateLanePosToGeometryPos(vehicle->getPositionOnLane())) ;
340  if (vehicle->getLane()->getShape().length2D() == 0) {
341  if (vehicle->getFurtherLanes().size() == 0) {
342  laneAngle = vehicle->getAngle();
343  } else {
344  laneAngle = vehicle->getFurtherLanes().front()->getShape().rotationAtOffset(-NUMERICAL_EPS);
345  }
346  }
347  // this part of the angle comes from the vehicle's lateral movement
348  double changeAngle = 0;
349  // avoid flicker
350  if (fabs(latDist) > NUMERICAL_EPS) {
351  // angle is between vehicle front and vehicle back (and depending on travelled distance)
352  changeAngle = atan2(latDist, vehicle->getVehicleType().getLength() + SPEED2DIST(vehicle->getSpeed()));
353  }
354 #ifdef DEBUG_MANEUVER
355  if (vehicle->getLaneChangeModel().debugVehicle()) {
356  MSLane* targetLane = vehicle->getLaneChangeModel().getTargetLane();
357  std::cout << SIMTIME << " startChangeSublane()"
358  << " shadowLane=" << (shadowLane != nullptr ? shadowLane->getID() : "NULL")
359  << " targetLane=" << (targetLane != nullptr ? targetLane->getID() : "NULL")
360  << " maneuverDist=" << vehicle->getLaneChangeModel().getManeuverDist()
361  << " latDist=" << latDist
362  << " old=" << Named::getIDSecure(oldShadowLane)
363  << " new=" << Named::getIDSecure(vehicle->getLaneChangeModel().getShadowLane())
364  << " laneA=" << RAD2DEG(laneAngle)
365  << " changeA=" << RAD2DEG(changeAngle)
366  << " oldA=" << RAD2DEG(vehicle->getAngle())
367  << " newA=" << RAD2DEG(laneAngle + changeAngle)
368  << "\n";
369  }
370 #endif
371  vehicle->setAngle(laneAngle + changeAngle, completedManeuver);
372 
373  // check if a traci maneuver must continue
374  if ((vehicle->getLaneChangeModel().getOwnState() & LCA_TRACI) != 0) {
375  if (vehicle->getLaneChangeModel().debugVehicle()) {
376  std::cout << SIMTIME << " continue TraCI-maneuver remainingLatDist=" << vehicle->getLaneChangeModel().getManeuverDist() << "\n";
377  }
379  }
380  from->lane->requireCollisionCheck();
381  to->lane->requireCollisionCheck();
382  return changedToNewLane;
383 }
384 
385 bool
387  const bool changedToNewLane = to != from && fabs(vehicle->getLateralPositionOnLane()) > 0.5 * vehicle->getLane()->getWidth() && mayChange(direction);
388  if (changedToNewLane) {
389  vehicle->myState.myPosLat -= direction * 0.5 * (from->lane->getWidth() + to->lane->getWidth());
390  to->lane->myTmpVehicles.insert(to->lane->myTmpVehicles.begin(), vehicle);
391  to->dens += vehicle->getVehicleType().getLengthWithGap();
393  if (!vehicle->isActive()) {
394  // update leaders beyond the current edge for all lanes
395  // @note to->aheadNext and from->aheadNext are only needed for output in non-action steps.
396  to->aheadNext = getLeaders(to, vehicle);
397  from->aheadNext = getLeaders(from, vehicle);
398  }
399  vehicle->getLaneChangeModel().setLeaderGaps(to->aheadNext);
400  vehicle->getLaneChangeModel().setFollowerGaps(to->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true));
401  vehicle->getLaneChangeModel().setOrigLeaderGaps(from->aheadNext);
402  }
403  vehicle->getLaneChangeModel().startLaneChangeManeuver(from->lane, to->lane, direction);
404  to->ahead.addLeader(vehicle, false, 0);
405  } else {
406  registerUnchanged(vehicle);
407  from->ahead.addLeader(vehicle, false, 0);
408  }
409  return changedToNewLane;
410 }
411 
412 void
415  // non-sublane change started
416  && ((vehicle->getLaneChangeModel().getOwnState() & (LCA_CHANGE_REASONS & ~LCA_SUBLANE)) != 0)
417  && ((vehicle->getLaneChangeModel().getOwnState() & LCA_STAY) == 0)
418  // no changing in previous step (either not wanted or blocked)
419  && (((vehicle->getLaneChangeModel().getPrevState() & (LCA_CHANGE_REASONS & ~LCA_SUBLANE)) == 0)
420  || ((vehicle->getLaneChangeModel().getPrevState() & LCA_STAY) != 0)
421  || ((vehicle->getLaneChangeModel().getPrevState() & LCA_BLOCKED) != 0))
422  ) {
423 #ifdef DEBUG_STATE
424  if DEBUG_COND {
425  std::cout << SIMTIME << " veh=" << vehicle->getID() << " laneChangeStarted state=" << toString((LaneChangeAction)vehicle->getLaneChangeModel().getOwnState())
426  << " prevState=" << toString((LaneChangeAction)vehicle->getLaneChangeModel().getPrevState())
428  << " filtered=" << toString((LaneChangeAction)(vehicle->getLaneChangeModel().getOwnState() & (LCA_CHANGE_REASONS & ~LCA_SUBLANE)))
429  << "\n";
430  }
431 #endif
432  vehicle->getLaneChangeModel().setLeaderGaps(to->aheadNext);
433  vehicle->getLaneChangeModel().setFollowerGaps(to->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true));
434  vehicle->getLaneChangeModel().setOrigLeaderGaps(from->aheadNext);
435  vehicle->getLaneChangeModel().laneChangeOutput("changeStarted", from->lane, to->lane, direction);
436  }
437 }
438 
439 void
440 MSLaneChangerSublane::outputLCEnded(MSVehicle* vehicle, ChangerIt& from, ChangerIt& to, int direction) {
442  // non-sublane change ended
443  && ((vehicle->getLaneChangeModel().getOwnState() & (LCA_CHANGE_REASONS & ~LCA_SUBLANE)) != 0)) {
444  vehicle->getLaneChangeModel().setLeaderGaps(to->aheadNext);
445  vehicle->getLaneChangeModel().setFollowerGaps(to->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true));
446  vehicle->getLaneChangeModel().setOrigLeaderGaps(from->aheadNext);
447  vehicle->getLaneChangeModel().laneChangeOutput("changeEnded", from->lane, to->lane, direction);
448  }
449 }
450 
451 
453 MSLaneChangerSublane::getLeaders(const ChangerIt& target, const MSVehicle* vehicle) const {
454  // get the leading vehicle on the lane to change to
455 #ifdef DEBUG_SURROUNDING
456  if (DEBUG_COND) {
457  std::cout << SIMTIME << " getLeaders lane=" << target->lane->getID() << " ego=" << vehicle->getID() << " ahead=" << target->ahead.toString() << "\n";
458  }
459 #endif
460  MSLeaderDistanceInfo result(target->lane, nullptr, 0);
461  for (int i = 0; i < target->ahead.numSublanes(); ++i) {
462  const MSVehicle* veh = target->ahead[i];
463  if (veh != nullptr) {
464  const double gap = veh->getBackPositionOnLane(target->lane) - vehicle->getPositionOnLane() - vehicle->getVehicleType().getMinGap();
465 #ifdef DEBUG_SURROUNDING
466  if (DEBUG_COND) {
467  std::cout << " ahead lead=" << veh->getID() << " leadBack=" << veh->getBackPositionOnLane() << " gap=" << gap << "\n";
468  }
469 #endif
470  result.addLeader(veh, gap, 0, i);
471  }
472  }
473  // if there are vehicles on the target lane with the same position as ego,
474  // they may not have been added to 'ahead' yet
475  const MSLeaderInfo& aheadSamePos = target->lane->getLastVehicleInformation(nullptr, 0, vehicle->getPositionOnLane(), false);
476  for (int i = 0; i < aheadSamePos.numSublanes(); ++i) {
477  const MSVehicle* veh = aheadSamePos[i];
478  if (veh != nullptr && veh != vehicle) {
479  const double gap = veh->getBackPositionOnLane(target->lane) - vehicle->getPositionOnLane() - vehicle->getVehicleType().getMinGap();
480 #ifdef DEBUG_SURROUNDING
481  if (DEBUG_COND) {
482  std::cout << " further lead=" << veh->getID() << " leadBack=" << veh->getBackPositionOnLane(target->lane) << " gap=" << gap << "\n";
483  }
484 #endif
485  result.addLeader(veh, gap, 0, i);
486  }
487  }
488 
489  if (result.numFreeSublanes() > 0) {
490  MSLane* targetLane = target->lane;
491 
492  double seen = vehicle->getLane()->getLength() - vehicle->getPositionOnLane();
493  double speed = vehicle->getSpeed();
494  double dist = vehicle->getCarFollowModel().brakeGap(speed) + vehicle->getVehicleType().getMinGap();
495  if (seen > dist) {
496 #ifdef DEBUG_SURROUNDING
497  if (DEBUG_COND) {
498  std::cout << " aborting forward search. dist=" << dist << " seen=" << seen << "\n";
499  }
500 #endif
501  return result;
502  }
503  const std::vector<MSLane*>& bestLaneConts = veh(myCandi)->getBestLanesContinuation(targetLane);
504 #ifdef DEBUG_SURROUNDING
505  if (DEBUG_COND) {
506  std::cout << " add consecutive before=" << result.toString() << " dist=" << dist;
507  }
508 #endif
509  target->lane->getLeadersOnConsecutive(dist, seen, speed, vehicle, bestLaneConts, result);
510 #ifdef DEBUG_SURROUNDING
511  if (DEBUG_COND) {
512  std::cout << " after=" << result.toString() << "\n";
513  }
514 #endif
515  }
516  return result;
517 }
518 
519 
520 int
522  int laneOffset,
523  LaneChangeAction alternatives,
524  const std::vector<MSVehicle::LaneQ>& preb,
525  double& latDist,
526  double& maneuverDist) const {
527 
528  ChangerIt target = myCandi + laneOffset;
529  MSVehicle* vehicle = veh(myCandi);
530  const MSLane& neighLane = *(target->lane);
531  int blocked = 0;
532 
533  MSLeaderDistanceInfo neighLeaders = target->aheadNext;
534  MSLeaderDistanceInfo neighFollowers = target->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true);
535  MSLeaderDistanceInfo neighBlockers(&neighLane, vehicle, vehicle->getLane()->getRightSideOnEdge() - neighLane.getRightSideOnEdge());
536  MSLeaderDistanceInfo leaders = myCandi->aheadNext;
537  MSLeaderDistanceInfo followers = myCandi->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true);
538  MSLeaderDistanceInfo blockers(vehicle->getLane(), vehicle, 0);
539 
540 #ifdef DEBUG_SURROUNDING
541  if (DEBUG_COND) std::cout << SIMTIME
542  << " checkChangeSublane: veh=" << vehicle->getID()
543  << " laneOffset=" << laneOffset
544  << "\n leaders=" << leaders.toString()
545  << "\n neighLeaders=" << neighLeaders.toString()
546  << "\n followers=" << followers.toString()
547  << "\n neighFollowers=" << neighFollowers.toString()
548  << "\n";
549 #endif
550 
551 
552  const int wish = vehicle->getLaneChangeModel().wantsChangeSublane(
553  laneOffset, alternatives,
554  leaders, followers, blockers,
555  neighLeaders, neighFollowers, neighBlockers,
556  neighLane, preb,
557  &(myCandi->lastBlocked), &(myCandi->firstBlocked), latDist, maneuverDist, blocked);
558  int state = blocked | wish;
559 
560  // XXX
561  // do are more careful (but expensive) check to ensure that a
562  // safety-critical leader is not being overlooked
563 
564  // XXX
565  // ensure that a continuous lane change manoeuvre can be completed
566  // before the next turning movement
567 
568  // let TraCI influence the wish to change lanes and the security to take
569  const int oldstate = state;
570  state = vehicle->influenceChangeDecision(state);
571 #ifdef DEBUG_STATE
572  if (DEBUG_COND && state != oldstate) {
573  std::cout << SIMTIME << " veh=" << vehicle->getID() << " stateAfterTraCI=" << toString((LaneChangeAction)state) << " original=" << toString((LaneChangeAction)oldstate) << "\n";
574  }
575 #endif
576  vehicle->getLaneChangeModel().saveLCState(laneOffset, oldstate, state);
577  return state;
578 }
579 
580 /****************************************************************************/
581 
void outputLCStarted(MSVehicle *vehicle, ChangerIt &from, ChangerIt &to, int direction)
optional output for start of lane-change maneuvre
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].
#define DIST2SPEED(x)
Definition: SUMOTime.h:50
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
double length2D() const
Returns the length.
saves leader/follower vehicles and their distances relative to an ego vehicle
Definition: MSLeaderInfo.h:129
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:79
bool isRemoteControlled() const
Returns the information whether the vehicle is fully controlled via TraCI.
Definition: MSVehicle.cpp:5531
double getAngle() const
Returns the vehicle&#39;s direction in radians.
Definition: MSVehicle.h:685
#define SPEED2DIST(x)
Definition: SUMOTime.h:48
#define DEBUG_COND
State myState
This Vehicles driving state (pos and speed)
Definition: MSVehicle.h:1786
virtual std::string toString() const
print a debugging representation
bool checkChangeToNewLane(MSVehicle *vehicle, const int direction, ChangerIt from, ChangerIt to)
check whether the given vehicle has entered the new lane &#39;to->lane&#39; during a sublane LC-step ...
MSLane * getLane() const
Returns the lane the vehicle is on.
Definition: MSVehicle.h:565
void setLeaderGaps(CLeaderDist, double secGap)
virtual void initChanger()
Initialize the changer before looping over all vehicles.
double getManeuverDist() const
Returns the remaining unblocked distance for the current maneuver. (only used by sublane model) ...
void abortLCManeuver(MSVehicle *vehicle)
immediately stop lane-changing and register vehicle as unchanged
double getPositionOnLane() const
Get the vehicle&#39;s position along the lane.
Definition: MSVehicle.h:403
double myPosLat
the stored lateral position
Definition: MSVehicle.h:142
MSVehicle * veh(ConstChangerIt ce) const
Wants go to the right.
StateAndDist checkChangeHelper(MSVehicle *vehicle, int laneOffset, LaneChangeAction alternatives)
helper function that calls checkChangeSublane and sets blocker information
double getLength() const
Returns the lane&#39;s length.
Definition: MSLane.h:514
const PositionVector & getShape() const
Returns this lane&#39;s shape.
Definition: MSLane.h:456
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:71
bool isStoppedOnLane() const
Definition: MSVehicle.cpp:1596
int checkChangeSublane(int laneOffset, LaneChangeAction alternatives, const std::vector< MSVehicle::LaneQ > &preb, double &latDist, double &maneuverDist) const
check whether sub-lane changing in the given direction is desirable and possible
virtual StateAndDist decideDirection(StateAndDist sd1, StateAndDist sd2) const
decide in which direction to move in case both directions are desirable
double rotationAtOffset(double pos) const
Returns the rotation at the given length.
void adaptBestLanesOccupation(int laneIndex, double density)
update occupation from MSLaneChanger
Definition: MSVehicle.cpp:4707
#define RAD2DEG(x)
Definition: GeomHelper.h:39
const std::string & getID() const
Returns the id.
Definition: Named.h:78
Wants go to the left.
void outputLCEnded(MSVehicle *vehicle, ChangerIt &from, ChangerIt &to, int direction)
optional output for end of lane-change maneuvre
double getWidth() const
Returns the lane&#39;s width.
Definition: MSLane.h:530
virtual void updateChanger(bool vehHasChanged)
ChangerIt findCandidate()
Find current candidate. If there is none, myChanger.end() is returned.
used by the sublane model
MSAbstractLaneChangeModel & getLaneChangeModel()
Definition: MSVehicle.cpp:4307
#define SIMTIME
Definition: SUMOTime.h:65
void setFollowerGaps(CLeaderDist follower, double secGap)
Needs to stay on the current lane.
void checkTraCICommands(MSVehicle *vehicle)
Take into account traci LC-commands.
Performs lane changing of vehicles.
Definition: MSLaneChanger.h:48
reasons of lane change
virtual int addLeader(const MSVehicle *veh, double gap, double latOffset=0, int sublane=-1)
const MSCFModel & getCarFollowModel() const
Returns the vehicle&#39;s car following model definition.
Definition: MSVehicle.h:891
int getIndex() const
Returns the lane&#39;s index.
Definition: MSLane.h:537
MSLaneChangerSublane()
Default constructor.
virtual void updateChanger(bool vehHasChanged)
void setSublaneChange(double latDist)
Sets a new sublane-change request.
Definition: MSVehicle.cpp:331
void setAngle(double angle, bool straightenFurther=false)
Set a custom vehicle angle in rad, optionally updates furtherLanePosLat.
Definition: MSVehicle.cpp:1255
blocked in all directions
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:49
The action is urgent (to be defined by lc-model)
void updateBestLanes(bool forceRebuild=false, const MSLane *startLane=0)
computes the best lanes to use in order to continue the route
Definition: MSVehicle.cpp:4325
const std::vector< MSLane * > & getBestLanesContinuation() const
Returns the best sequence of lanes to continue the route starting at myLane.
Definition: MSVehicle.cpp:4668
Position myCachedPosition
Definition: MSVehicle.h:1860
const std::vector< MSLane * > & getFurtherLanes() const
Definition: MSVehicle.h:789
bool startChangeSublane(MSVehicle *vehicle, ChangerIt &from, double latDist)
change by the specified amount and return whether a new lane was entered
virtual void initChanger()
Initialize the changer before looping over all vehicles.
double getMinGap() const
Get the free space in front of vehicles of this class.
virtual bool debugVehicle() const
whether the current vehicles shall be debugged
~MSLaneChangerSublane()
Destructor.
double getLateralPositionOnLane() const
Get the vehicle&#39;s lateral position on the lane.
Definition: MSVehicle.h:440
double getRightSideOnEdge() const
Definition: MSLane.h:1027
void setOrigLeaderGaps(CLeaderDist, double secGap)
bool startLaneChangeManeuver(MSLane *source, MSLane *target, int direction)
start the lane change maneuver and return whether it continues
virtual void updateExpectedSublaneSpeeds(const MSLeaderDistanceInfo &ahead, int sublaneOffset, int laneIndex)
update expected speeds for each sublane of the current edge
bool continueChangeSublane(MSVehicle *vehicle, ChangerIt &from)
Continue a sublane-lane change maneuver and return whether the midpoint was passed in this step...
Influencer & getInfluencer()
Returns the velocity/lane influencer.
Definition: MSVehicle.cpp:5488
MSLeaderDistanceInfo getLeaders(const ChangerIt &target, const MSVehicle *ego) const
get leaders for ego on the given lane
LaneChangeAction
The state of a vehicle&#39;s lane-change behavior.
virtual void setOwnState(const int state)
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:595
static bool haveLCOutput()
whether lanechange-output is active
double interpolateLanePosToGeometryPos(double lanePos) const
Definition: MSLane.h:472
Changer::iterator ChangerIt
the iterator moving over the ChangeElems
double getLength() const
Get vehicle&#39;s length [m].
virtual int wantsChangeSublane(int laneOffset, LaneChangeAction alternatives, const MSLeaderDistanceInfo &leaders, const MSLeaderDistanceInfo &followers, const MSLeaderDistanceInfo &blockers, const MSLeaderDistanceInfo &neighLeaders, const MSLeaderDistanceInfo &neighFollowers, const MSLeaderDistanceInfo &neighBlockers, const MSLane &neighLane, const std::vector< MSVehicle::LaneQ > &preb, MSVehicle **lastBlocked, MSVehicle **firstBlocked, double &latDist, double &targetDistLat, int &blocked)
MSLane * getTargetLane() const
Returns the lane the vehicle has committed to enter during a sublane lane change. ...
virtual double computeSpeedLat(double latDist, double &maneuverDist)
decides the next lateral speed depending on the remaining lane change distance to be covered and upda...
MSAbstractLaneChangeModel::StateAndDist StateAndDist
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:3701
void registerUnchanged(MSVehicle *vehicle)
static bool outputLCStarted()
whether start of maneuvers shall be recorede
#define NUMERICAL_EPS
Definition: config.h:148
int numSublanes() const
Definition: MSLeaderInfo.h:88
MSLane * getShadowLane() const
Returns the lane the vehicle&#39;s shadow is on during continuous/sublane lane change.
void setManeuverDist(const double dist)
Updates the remaining distance for the current maneuver while it is continued within non-action steps...
int numFreeSublanes() const
Definition: MSLeaderInfo.h:92
virtual void updateSafeLatDist(const double travelledLatDist)
Updates the value of safe lateral distances (in SL2015) during maneuver continuation in non-action st...
static bool outputLCEnded()
whether start of maneuvers shall be recorede
const std::vector< LaneQ > & getBestLanes() const
Returns the description of best lanes to use in order to continue the route.
Definition: MSVehicle.cpp:4319
double getSpeed() const
Returns the vehicle&#39;s current speed.
Definition: MSVehicle.h:483
int influenceChangeDecision(int state)
allow TraCI to influence a lane change decision
Definition: MSVehicle.cpp:5512
void laneChangeOutput(const std::string &tag, MSLane *source, MSLane *target, int direction)
called once the vehicle ends a lane change manoeuvre (non-instant)
const std::string & getID() const
Returns the name of the vehicle.
Representation of a lane in the micro simulation.
Definition: MSLane.h:78
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
ChangerIt myCandi
double getWidth() const
Returns the vehicle&#39;s width.
static const Position INVALID
used to indicate that a position is valid
Definition: Position.h:285