Eclipse 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-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 /****************************************************************************/
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  vehicle->getLaneChangeModel().clearNeighbors();
112 #ifdef DEBUG_ACTIONSTEPS
113  if (DEBUG_COND) {
114  std::cout << "\nCHANGE" << std::endl;
115  }
116 #endif
117  assert(vehicle->getLane() == (*myCandi).lane);
118  assert(!vehicle->getLaneChangeModel().isChangingLanes());
119  if ( vehicle->isStoppedOnLane()) {
120  registerUnchanged(vehicle);
121  return false;
122  }
123  if (!vehicle->isActive()) {
124 #ifdef DEBUG_ACTIONSTEPS
125  if (DEBUG_COND) {
126  std::cout << SIMTIME << " veh '" << vehicle->getID() << "' skips regular change checks." << std::endl;
127  }
128 #endif
129 
130  bool changed;
131  // let TraCI influence the wish to change lanes during non-actionsteps
132  checkTraCICommands(vehicle);
133 
134  // Resume change
135  changed = continueChangeSublane(vehicle, myCandi);
136 #ifdef DEBUG_ACTIONSTEPS
137  if (DEBUG_COND) {
138  std::cout << SIMTIME << " veh '" << vehicle->getID() << "' lcm->maneuverDist=" << vehicle->getLaneChangeModel().getManeuverDist()
139  << " lcm->speedLat=" << vehicle->getLaneChangeModel().getSpeedLat() << std::endl;
140  }
141 #endif
142  return changed;
143  }
144 
145 #ifdef DEBUG_ACTIONSTEPS
146  if (DEBUG_COND) {
147  std::cout << "\n" << SIMTIME << " veh '" << vehicle->getID() << "' plans lanechange maneuver." << std::endl;
148  }
149 #endif
150  vehicle->updateBestLanes(); // needed?
151  for (int i = 0; i < (int) myChanger.size(); ++i) {
152  vehicle->adaptBestLanesOccupation(i, myChanger[i].dens);
153  }
154  // update leaders beyond the current edge for all lanes
155  for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
156  ce->aheadNext = getLeaders(ce, vehicle);
157  }
158 
159  // update expected speeds
160  int sublaneIndex = 0;
161  for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
162  vehicle->getLaneChangeModel().updateExpectedSublaneSpeeds(ce->aheadNext, sublaneIndex, ce->lane->getIndex());
163  for (int offset : ce->siblings) {
164  // treat sibling lanes (internal lanes with the same origin lane) as if they have the same geometry
165  ChangerIt ceSib = ce + offset;
166  vehicle->getLaneChangeModel().updateExpectedSublaneSpeeds(ceSib->aheadNext, sublaneIndex, ceSib->lane->getIndex());
167  }
168  sublaneIndex += ce->ahead.numSublanes();
169  }
170 
172  | (mayChange(1) ? LCA_LEFT : LCA_NONE));
173 
174  StateAndDist right = checkChangeHelper(vehicle, -1, alternatives);
175  StateAndDist left = checkChangeHelper(vehicle, 1, alternatives);
176  StateAndDist current = checkChangeHelper(vehicle, 0, alternatives);
177 
178  StateAndDist decision = vehicle->getLaneChangeModel().decideDirection(current,
179  vehicle->getLaneChangeModel().decideDirection(right, left));
180 #ifdef DEBUG_DECISION
181  if (vehicle->getLaneChangeModel().debugVehicle()) {
182  std::cout << "\n" << SIMTIME << " decision=" << toString((LaneChangeAction)decision.state) << " dir=" << decision.dir << " latDist=" << decision.latDist << " maneuverDist=" << decision.maneuverDist << "\n";
183  }
184 #endif
185  vehicle->getLaneChangeModel().setOwnState(decision.state);
186  if ((decision.state & LCA_WANTS_LANECHANGE) != 0 && (decision.state & LCA_BLOCKED) == 0) {
187  // change if the vehicle wants to and is allowed to change
188 #ifdef DEBUG_MANEUVER
189  if (DEBUG_COND) {
190  std::cout << SIMTIME << " veh '" << vehicle->getID() << "' performing sublane change..." << std::endl;
191  }
192 #endif
193  return startChangeSublane(vehicle, myCandi, decision.latDist, decision.maneuverDist);
194  }
195  // @note this assumes vehicles can instantly abort any maneuvre in case of emergency
196  abortLCManeuver(vehicle);
197 
198  if ((right.state & (LCA_URGENT)) != 0 && (left.state & (LCA_URGENT)) != 0) {
199  // ... wants to go to the left AND to the right
200  // just let them go to the right lane...
201  left.state = 0;
202  }
203  return false;
204 }
205 
206 
207 void
209 #ifdef DEBUG_MANEUVER
210  if (DEBUG_COND) {
211  std::cout << SIMTIME << " veh '" << vehicle->getID() << "' aborts LC-continuation."
212  << std::endl;
213  }
214 #endif
215  const int priorReason = vehicle->getLaneChangeModel().getPrevState() & LCA_CHANGE_REASONS & ~LCA_SUBLANE;
216  const int reason = vehicle->getLaneChangeModel().getOwnState() & LCA_CHANGE_REASONS & ~LCA_SUBLANE;
217  if (priorReason != 0 && priorReason != reason && vehicle->getLaneChangeModel().getPreviousManeuverDist() != 0) {
218  // original from cannot be reconstructed
219  const int priorDirection = vehicle->getLaneChangeModel().getPreviousManeuverDist() > 0 ? 1 : -1;
220 #ifdef DEBUG_MANEUVER
221  if (DEBUG_COND) {
222  std::cout << SIMTIME << " abortLCManeuver priorReason=" << toString((LaneChangeAction)priorReason)
223  << " reason=" << toString((LaneChangeAction)reason) << " veh=" << vehicle->getID() << "\n";
224  }
225 #endif
226  outputLCEnded(vehicle, myCandi, myCandi, priorDirection);
227  }
228  vehicle->getLaneChangeModel().setSpeedLat(0);
229  vehicle->getLaneChangeModel().setManeuverDist(0.);
230  registerUnchanged(vehicle);
231 }
232 
233 
235 MSLaneChangerSublane::checkChangeHelper(MSVehicle* vehicle, int laneOffset, LaneChangeAction alternatives) {
236  StateAndDist result = StateAndDist(0, 0, 0, 0);
237  if (mayChange(laneOffset)) {
238  const std::vector<MSVehicle::LaneQ>& preb = vehicle->getBestLanes();
239  result.state = checkChangeSublane(laneOffset, alternatives, preb, result.latDist, result.maneuverDist);
240  result.dir = laneOffset;
241  if ((result.state & LCA_WANTS_LANECHANGE) != 0 && (result.state & LCA_URGENT) != 0 && (result.state & LCA_BLOCKED) != 0) {
242  (myCandi + laneOffset)->lastBlocked = vehicle;
243  if ((myCandi + laneOffset)->firstBlocked == nullptr) {
244  (myCandi + laneOffset)->firstBlocked = vehicle;
245  }
246  }
247  }
248  return result;
249 }
250 
251 
253 // (used to continue sublane changing in non-action steps).
254 bool
256  // lateral distance to complete maneuver
257  double remLatDist = vehicle->getLaneChangeModel().getManeuverDist();
258  if (remLatDist == 0) {
259  registerUnchanged(vehicle);
260  return false;
261  }
262  const double nextLatDist = SPEED2DIST(vehicle->getLaneChangeModel().computeSpeedLat(remLatDist, remLatDist));
263 #ifdef DEBUG_MANEUVER
264  if (DEBUG_COND) {
265  std::cout << SIMTIME << " vehicle '" << vehicle->getID() << "' continueChangeSublane()"
266  << " remLatDist=" << remLatDist << " nextLatDist=" << nextLatDist
267  << std::endl;
268  }
269 #endif
270 
271  const bool changed = startChangeSublane(vehicle, from, nextLatDist, remLatDist);
272  return changed;
273 }
274 
275 
276 bool
277 MSLaneChangerSublane::startChangeSublane(MSVehicle* vehicle, ChangerIt& from, double latDist, double maneuverDist) {
278  if (vehicle->isRemoteControlled()) {
279  registerUnchanged(vehicle);
280  return false;
281  }
282  // Prevent continuation of LC beyond lane borders if change is not allowed
283  const double distToRightLaneBorder = latDist < 0 ? vehicle->getLane()->getWidth() * 0.5 + vehicle->getLateralPositionOnLane() - vehicle->getWidth() * 0.5 : 0.;
284  const double distToLeftLaneBorder = latDist > 0 ? vehicle->getLane()->getWidth() * 0.5 - vehicle->getLateralPositionOnLane() - vehicle->getWidth() * 0.5 : 0.;
285  // determine direction of LC
286  const int direction = (latDist >= -distToRightLaneBorder && latDist <= distToLeftLaneBorder) ? 0 : (latDist < 0 ? -1 : 1);
287  ChangerIt to = from;
288  if (mayChange(direction)) {
289  to = from + direction;
290  } else {
291  // This may occur during maneuver continuation in non-actionsteps.
292  // TODO: Understand better why and test later if additional sublane actionstep debugging resolves this
293  // (XXX: perhaps one should try to extrapolate check for this case before to avoid maneuver initialization
294  // similar as for continuous LC in MSLaneChanger::checkChange())
295  //assert(false);
296  abortLCManeuver(vehicle);
297  return false;
298  }
299 
300  // The following does:
301  // 1) update vehicles lateral position according to latDist and target lane
302  // 2) distinguish several cases
303  // a) vehicle moves completely within the same lane
304  // b) vehicle intersects another lane
305  // - vehicle must be moved to the lane where it's midpoint is (either old or new)
306  // - shadow vehicle must be created/moved to the other lane if the vehicle intersects it
307  // 3) updated dens of all lanes that hold the vehicle or its shadow
308 
309  vehicle->myState.myPosLat += latDist;
311  vehicle->getLaneChangeModel().setSpeedLat(DIST2SPEED(latDist));
312 #ifdef DEBUG_MANEUVER
313  if (DEBUG_COND) {
314  std::cout << SIMTIME << " vehicle '" << vehicle->getID() << "' with maneuverDist=" << maneuverDist
315  << " and committedSpeed=" << vehicle->getLaneChangeModel().getCommittedSpeed()
316  << " increments lateral position by latDist=" << latDist << std::endl;
317  }
318 #endif
319 #ifdef DEBUG_SURROUNDING
320  if (DEBUG_COND) {
321  std::cout << SIMTIME << " vehicle '" << vehicle->getID() << "'\n to->ahead=" << to->ahead.toString()
322  << "'\n to->aheadNext=" << to->aheadNext.toString()
323  << std::endl;
324  }
325 #endif
326  const bool completedPriorManeuver = fabs(vehicle->getLaneChangeModel().getPreviousManeuverDist()) < NUMERICAL_EPS;
327  const bool completedManeuver = fabs(maneuverDist - latDist) < NUMERICAL_EPS;
328  vehicle->getLaneChangeModel().updateSafeLatDist(latDist);
329 
330  // current maneuver is aborted when direction or reason changes
331  const int priorReason = vehicle->getLaneChangeModel().getPrevState() & LCA_CHANGE_REASONS & ~LCA_SUBLANE;
332  const int reason = vehicle->getLaneChangeModel().getOwnState() & LCA_CHANGE_REASONS & ~LCA_SUBLANE;
333 #ifdef DEBUG_MANEUVER
334  if (DEBUG_COND) {
335  std::cout << SIMTIME << " vehicle '" << vehicle->getID()
336  << "' completedPriorManeuver=" << completedPriorManeuver
337  << " completedManeuver=" << completedManeuver
338  << " priorReason=" << toString((LaneChangeAction)priorReason)
339  << " reason=" << toString((LaneChangeAction)reason)
340  << " priorManeuverDist=" << vehicle->getLaneChangeModel().getPreviousManeuverDist()
341  << " maneuverDist=" << maneuverDist
342  << " latDist=" << latDist
343  << std::endl;
344  }
345 #endif
346  if (!completedManeuver && !completedPriorManeuver && priorReason != 0 &&
347  (vehicle->getLaneChangeModel().getPreviousManeuverDist() * latDist < 0
348  || priorReason != reason)) {
349  const int priorDirection = vehicle->getLaneChangeModel().getPreviousManeuverDist() > 0 ? 1 : -1;
350  // original from cannot be reconstructed
351 #ifdef DEBUG_MANEUVER
352  if (DEBUG_COND) {
353  std::cout << SIMTIME << " startChangeSublane abort priorReason=" << toString((LaneChangeAction)priorReason)
354  << " reason=" << toString((LaneChangeAction)reason) << " veh=" << vehicle->getID() << "\n";
355  }
356 #endif
357  outputLCEnded(vehicle, from, from, priorDirection);
358  }
359 
360  outputLCStarted(vehicle, from, to, direction, maneuverDist);
361  vehicle->getLaneChangeModel().setManeuverDist(maneuverDist - latDist);
362  const bool changedToNewLane = checkChangeToNewLane(vehicle, direction, from, to);
363 
364  MSLane* oldShadowLane = vehicle->getLaneChangeModel().getShadowLane();
366  MSLane* shadowLane = vehicle->getLaneChangeModel().getShadowLane();
367  if (shadowLane != nullptr && shadowLane != oldShadowLane) {
368  assert(to != from || oldShadowLane == 0);
369  const double latOffset = vehicle->getLane()->getRightSideOnEdge() - shadowLane->getRightSideOnEdge();
370  (myChanger.begin() + shadowLane->getIndex())->ahead.addLeader(vehicle, false, latOffset);
371  }
372  if (completedManeuver) {
373  outputLCEnded(vehicle, from, to, direction);
374  }
375 
376  // Update maneuver reservations on target lanes
377  MSLane* targetLane = vehicle->getLaneChangeModel().updateTargetLane();
378  if (!changedToNewLane && targetLane != nullptr
379  && vehicle->getActionStepLength() > DELTA_T) {
380  const int dir = (vehicle->getLaneChangeModel().getManeuverDist() > 0 ? 1 : -1);
381  ChangerIt target = from + dir;
382  const double actionStepDist = dir * vehicle->getVehicleType().getMaxSpeedLat() * vehicle->getActionStepLengthSecs();
383  const double latOffset = vehicle->getLatOffset(targetLane) + actionStepDist;
384  target->ahead.addLeader(vehicle, false, latOffset);
385  //std::cout << SIMTIME << " veh=" << vehicle->getID() << " target=" << targetLane->getID()
386  // << " actionStepDist=" << actionStepDist << " latOffset=" << latOffset
387  // << " targetAhead=" << target->ahead.toString() << "\n";
388  }
389 
390  // compute new angle of the vehicle from the x- and y-distances travelled within last time step
391  // (should happen last because primaryLaneChanged() also triggers angle computation)
392  // this part of the angle comes from the orientation of our current lane
393  double laneAngle = vehicle->getLane()->getShape().rotationAtOffset(vehicle->getLane()->interpolateLanePosToGeometryPos(vehicle->getPositionOnLane())) ;
394  if (vehicle->getLane()->getShape().length2D() == 0) {
395  if (vehicle->getFurtherLanes().size() == 0) {
396  laneAngle = vehicle->getAngle();
397  } else {
398  laneAngle = vehicle->getFurtherLanes().front()->getShape().rotationAtOffset(-NUMERICAL_EPS);
399  }
400  }
401  // this part of the angle comes from the vehicle's lateral movement
402  double changeAngle = 0;
403  // avoid flicker
404  if (fabs(latDist) > NUMERICAL_EPS) {
405  // angle is between vehicle front and vehicle back (and depending on travelled distance)
406  changeAngle = atan2(latDist, vehicle->getVehicleType().getLength() + SPEED2DIST(vehicle->getSpeed()));
407  if (MSNet::getInstance()->lefthand()) {
408  changeAngle *= -1;
409  }
410  }
411 #ifdef DEBUG_MANEUVER
412  if (vehicle->getLaneChangeModel().debugVehicle()) {
413  std::cout << SIMTIME << " startChangeSublane()"
414  << " shadowLane=" << (shadowLane != nullptr ? shadowLane->getID() : "NULL")
415  << " targetLane=" << (targetLane != nullptr ? targetLane->getID() : "NULL")
416  << " maneuverDist=" << vehicle->getLaneChangeModel().getManeuverDist()
417  << " latDist=" << latDist
418  << " old=" << Named::getIDSecure(oldShadowLane)
419  << " new=" << Named::getIDSecure(vehicle->getLaneChangeModel().getShadowLane())
420  << " laneA=" << RAD2DEG(laneAngle)
421  << " changeA=" << RAD2DEG(changeAngle)
422  << " oldA=" << RAD2DEG(vehicle->getAngle())
423  << " newA=" << RAD2DEG(laneAngle + changeAngle)
424  << "\n";
425  }
426 #endif
427  vehicle->setAngle(laneAngle + changeAngle, completedManeuver);
428 
429  // check if a traci maneuver must continue
430  if ((vehicle->getLaneChangeModel().getOwnState() & LCA_TRACI) != 0) {
431  if (vehicle->getLaneChangeModel().debugVehicle()) {
432  std::cout << SIMTIME << " continue TraCI-maneuver remainingLatDist=" << vehicle->getLaneChangeModel().getManeuverDist() << "\n";
433  }
435  }
436  from->lane->requireCollisionCheck();
437  to->lane->requireCollisionCheck();
438  return changedToNewLane;
439 }
440 
441 bool
443  const bool changedToNewLane = to != from && fabs(vehicle->getLateralPositionOnLane()) > 0.5 * vehicle->getLane()->getWidth() && mayChange(direction);
444  if (changedToNewLane) {
445  vehicle->myState.myPosLat -= direction * 0.5 * (from->lane->getWidth() + to->lane->getWidth());
446  to->lane->myTmpVehicles.insert(to->lane->myTmpVehicles.begin(), vehicle);
447  to->dens += vehicle->getVehicleType().getLengthWithGap();
449  if (!vehicle->isActive()) {
450  // update leaders beyond the current edge for all lanes
451  // @note to->aheadNext and from->aheadNext are only needed for output in non-action steps.
452  to->aheadNext = getLeaders(to, vehicle);
453  from->aheadNext = getLeaders(from, vehicle);
454  }
455  vehicle->getLaneChangeModel().setLeaderGaps(to->aheadNext);
456  vehicle->getLaneChangeModel().setFollowerGaps(to->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true));
457  vehicle->getLaneChangeModel().setOrigLeaderGaps(from->aheadNext);
458  }
459  vehicle->getLaneChangeModel().startLaneChangeManeuver(from->lane, to->lane, direction);
460  to->ahead.addLeader(vehicle, false, 0);
461  } else {
462  registerUnchanged(vehicle);
463  from->ahead.addLeader(vehicle, false, 0);
464  }
465  return changedToNewLane;
466 }
467 
468 void
469 MSLaneChangerSublane::outputLCStarted(MSVehicle* vehicle, ChangerIt& from, ChangerIt& to, int direction, double maneuverDist) {
471  // non-sublane change started
472  && ((vehicle->getLaneChangeModel().getOwnState() & (LCA_CHANGE_REASONS & ~LCA_SUBLANE)) != 0)
473  && ((vehicle->getLaneChangeModel().getOwnState() & LCA_STAY) == 0)
474  // no changing for the same reason in previous step (either not wanted or blocked)
477  || ((vehicle->getLaneChangeModel().getPrevState() & LCA_STAY) != 0)
478  || ((vehicle->getLaneChangeModel().getPrevState() & LCA_BLOCKED) != 0))
479  ) {
480 #ifdef DEBUG_STATE
481  if (DEBUG_COND) {
482  std::cout << SIMTIME << " veh=" << vehicle->getID() << " laneChangeStarted state=" << toString((LaneChangeAction)vehicle->getLaneChangeModel().getOwnState())
483  << " prevState=" << toString((LaneChangeAction)vehicle->getLaneChangeModel().getPrevState())
485  << " filtered=" << toString((LaneChangeAction)(vehicle->getLaneChangeModel().getOwnState() & (LCA_CHANGE_REASONS & ~LCA_SUBLANE)))
486  << "\n";
487  }
488 #endif
489  vehicle->getLaneChangeModel().setLeaderGaps(to->aheadNext);
490  vehicle->getLaneChangeModel().setFollowerGaps(to->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true));
491  vehicle->getLaneChangeModel().setOrigLeaderGaps(from->aheadNext);
492  vehicle->getLaneChangeModel().laneChangeOutput("changeStarted", from->lane, to->lane, direction, maneuverDist);
493  }
494 }
495 
496 void
497 MSLaneChangerSublane::outputLCEnded(MSVehicle* vehicle, ChangerIt& from, ChangerIt& to, int direction) {
499  // non-sublane change ended
500  && ((vehicle->getLaneChangeModel().getOwnState() & (LCA_CHANGE_REASONS & ~LCA_SUBLANE)) != 0)) {
501  vehicle->getLaneChangeModel().setLeaderGaps(to->aheadNext);
502  vehicle->getLaneChangeModel().setFollowerGaps(to->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true));
503  vehicle->getLaneChangeModel().setOrigLeaderGaps(from->aheadNext);
504  vehicle->getLaneChangeModel().laneChangeOutput("changeEnded", from->lane, to->lane, direction);
505  }
506 }
507 
508 
510 MSLaneChangerSublane::getLeaders(const ChangerIt& target, const MSVehicle* vehicle) const {
511  // get the leading vehicle on the lane to change to
512 #ifdef DEBUG_SURROUNDING
513  if (DEBUG_COND) {
514  std::cout << SIMTIME << " getLeaders lane=" << target->lane->getID() << " ego=" << vehicle->getID() << " ahead=" << target->ahead.toString() << "\n";
515  }
516 #endif
517  MSLeaderDistanceInfo result(target->lane, nullptr, 0);
518  for (int i = 0; i < target->ahead.numSublanes(); ++i) {
519  const MSVehicle* veh = target->ahead[i];
520  if (veh != nullptr) {
521  const double gap = veh->getBackPositionOnLane(target->lane) - vehicle->getPositionOnLane() - vehicle->getVehicleType().getMinGap();
522 #ifdef DEBUG_SURROUNDING
523  if (DEBUG_COND) {
524  std::cout << " ahead lead=" << veh->getID() << " leadBack=" << veh->getBackPositionOnLane() << " gap=" << gap << "\n";
525  }
526 #endif
527  result.addLeader(veh, gap, 0, i);
528  }
529  }
530  // if there are vehicles on the target lane with the same position as ego,
531  // they may not have been added to 'ahead' yet
532  const MSLeaderInfo& aheadSamePos = target->lane->getLastVehicleInformation(nullptr, 0, vehicle->getPositionOnLane(), false);
533  for (int i = 0; i < aheadSamePos.numSublanes(); ++i) {
534  const MSVehicle* veh = aheadSamePos[i];
535  if (veh != nullptr && veh != vehicle) {
536  const double gap = veh->getBackPositionOnLane(target->lane) - vehicle->getPositionOnLane() - vehicle->getVehicleType().getMinGap();
537 #ifdef DEBUG_SURROUNDING
538  if (DEBUG_COND) {
539  std::cout << " further lead=" << veh->getID() << " leadBack=" << veh->getBackPositionOnLane(target->lane) << " gap=" << gap << "\n";
540  }
541 #endif
542  result.addLeader(veh, gap, 0, i);
543  }
544  }
545 
546  if (result.numFreeSublanes() > 0) {
547  MSLane* targetLane = target->lane;
548 
549  double seen = vehicle->getLane()->getLength() - vehicle->getPositionOnLane();
550  double speed = vehicle->getSpeed();
551  double dist = vehicle->getCarFollowModel().brakeGap(speed) + vehicle->getVehicleType().getMinGap();
552  if (seen > dist) {
553 #ifdef DEBUG_SURROUNDING
554  if (DEBUG_COND) {
555  std::cout << " aborting forward search. dist=" << dist << " seen=" << seen << "\n";
556  }
557 #endif
558  return result;
559  }
560  const std::vector<MSLane*>& bestLaneConts = veh(myCandi)->getBestLanesContinuation(targetLane);
561 #ifdef DEBUG_SURROUNDING
562  if (DEBUG_COND) {
563  std::cout << " add consecutive before=" << result.toString() << " dist=" << dist;
564  }
565 #endif
566  target->lane->getLeadersOnConsecutive(dist, seen, speed, vehicle, bestLaneConts, result);
567 #ifdef DEBUG_SURROUNDING
568  if (DEBUG_COND) {
569  std::cout << " after=" << result.toString() << "\n";
570  }
571 #endif
572  }
573  return result;
574 }
575 
576 
577 int
579  int laneOffset,
580  LaneChangeAction alternatives,
581  const std::vector<MSVehicle::LaneQ>& preb,
582  double& latDist,
583  double& maneuverDist) const {
584 
585  ChangerIt target = myCandi + laneOffset;
586  MSVehicle* vehicle = veh(myCandi);
587  const MSLane& neighLane = *(target->lane);
588  int blocked = 0;
589 
590  MSLeaderDistanceInfo neighLeaders = target->aheadNext;
591  MSLeaderDistanceInfo neighFollowers = target->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true);
592  MSLeaderDistanceInfo neighBlockers(&neighLane, vehicle, vehicle->getLane()->getRightSideOnEdge() - neighLane.getRightSideOnEdge());
593  MSLeaderDistanceInfo leaders = myCandi->aheadNext;
594  MSLeaderDistanceInfo followers = myCandi->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true);
595  MSLeaderDistanceInfo blockers(vehicle->getLane(), vehicle, 0);
596 
597 #ifdef DEBUG_SURROUNDING
598  if (DEBUG_COND) std::cout << SIMTIME
599  << " checkChangeSublane: veh=" << vehicle->getID()
600  << " laneOffset=" << laneOffset
601  << "\n leaders=" << leaders.toString()
602  << "\n neighLeaders=" << neighLeaders.toString()
603  << "\n followers=" << followers.toString()
604  << "\n neighFollowers=" << neighFollowers.toString()
605  << "\n";
606 #endif
607 
608 
609  const int wish = vehicle->getLaneChangeModel().wantsChangeSublane(
610  laneOffset, alternatives,
611  leaders, followers, blockers,
612  neighLeaders, neighFollowers, neighBlockers,
613  neighLane, preb,
614  &(myCandi->lastBlocked), &(myCandi->firstBlocked), latDist, maneuverDist, blocked);
615  int state = blocked | wish;
616 
617  // XXX
618  // do are more careful (but expensive) check to ensure that a
619  // safety-critical leader is not being overlooked
620 
621  // XXX
622  // ensure that a continuous lane change manoeuvre can be completed
623  // before the next turning movement
624 
625  // let TraCI influence the wish to change lanes and the security to take
626  const int oldstate = state;
627  state = vehicle->influenceChangeDecision(state);
628 #ifdef DEBUG_STATE
629  if (DEBUG_COND && state != oldstate) {
630  std::cout << SIMTIME << " veh=" << vehicle->getID() << " stateAfterTraCI=" << toString((LaneChangeAction)state) << " original=" << toString((LaneChangeAction)oldstate) << "\n";
631  }
632 #endif
633  vehicle->getLaneChangeModel().saveLCState(laneOffset, oldstate, state);
634  if (laneOffset != 0) {
635  vehicle->getLaneChangeModel().saveNeighbors(laneOffset, neighFollowers, neighLeaders);
636  }
637  return state;
638 }
639 
640 /****************************************************************************/
641 
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:49
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:133
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:80
bool isRemoteControlled() const
Returns the information whether the vehicle is fully controlled via TraCI.
Definition: MSVehicle.cpp:5860
double getAngle() const
Returns the vehicle&#39;s direction in radians.
Definition: MSVehicle.h:681
#define SPEED2DIST(x)
Definition: SUMOTime.h:47
#define DEBUG_COND
State myState
This Vehicles driving state (pos and speed)
Definition: MSVehicle.h:1833
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:561
void setLeaderGaps(CLeaderDist, double secGap)
void laneChangeOutput(const std::string &tag, MSLane *source, MSLane *target, int direction, double maneuverDist=0)
called once the vehicle ends a lane change manoeuvre (non-instant)
void clearNeighbors()
Clear info on neighboring vehicle from previous step.
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:397
double myPosLat
the stored lateral position
Definition: MSVehicle.h:143
MSVehicle * veh(ConstChangerIt ce) const
Wants go to the right.
void outputLCStarted(MSVehicle *vehicle, ChangerIt &from, ChangerIt &to, int direction, double maneuverDist)
optional output for start of lane-change maneuvre
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
Definition: MSNet.cpp:168
StateAndDist checkChangeHelper(MSVehicle *vehicle, int laneOffset, LaneChangeAction alternatives)
helper function that calls checkChangeSublane and sets blocker information
SUMOTime DELTA_T
Definition: SUMOTime.cpp:35
double getLength() const
Returns the lane&#39;s length.
Definition: MSLane.h:541
const PositionVector & getShape() const
Returns this lane&#39;s shape.
Definition: MSLane.h:478
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 isStoppedOnLane() const
Definition: MSVehicle.cpp:1762
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:5026
#define RAD2DEG(x)
Definition: GeomHelper.h:39
const std::string & getID() const
Returns the id.
Definition: Named.h:77
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:557
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:4609
#define SIMTIME
Definition: SUMOTime.h:64
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)
double getLatOffset(const MSLane *lane) const
Get the offset that that must be added to interpret myState.myPosLat for the given lane...
Definition: MSVehicle.cpp:5294
const MSCFModel & getCarFollowModel() const
Returns the vehicle&#39;s car following model definition.
Definition: MSVehicle.h:894
int getIndex() const
Returns the lane&#39;s index.
Definition: MSLane.h:564
MSLaneChangerSublane()
Default constructor.
double getActionStepLengthSecs() const
Returns the vehicle&#39;s action step length in secs, i.e. the interval between two action points...
Definition: MSVehicle.h:513
virtual void updateChanger(bool vehHasChanged)
void setSublaneChange(double latDist)
Sets a new sublane-change request.
Definition: MSVehicle.cpp:422
void setAngle(double angle, bool straightenFurther=false)
Set a custom vehicle angle in rad, optionally updates furtherLanePosLat.
Definition: MSVehicle.cpp:1410
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)
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
Position myCachedPosition
Definition: MSVehicle.h:1906
const std::vector< MSLane * > & getFurtherLanes() const
Definition: MSVehicle.h:792
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:434
double getRightSideOnEdge() const
Definition: MSLane.h:1068
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
double getMaxSpeedLat() const
Get vehicle&#39;s maximum lateral speed [m/s].
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:5817
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.
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)
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
static bool haveLCOutput()
whether lanechange-output is active
double interpolateLanePosToGeometryPos(double lanePos) const
Definition: MSLane.h:499
Changer::iterator ChangerIt
the iterator moving over the ChangeElems
bool startChangeSublane(MSVehicle *vehicle, ChangerIt &from, double latDist, double maneuverDist)
change by the specified amount and return whether a new lane was entered
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)
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:3993
void registerUnchanged(MSVehicle *vehicle)
static bool outputLCStarted()
whether start of maneuvers shall be recorede
#define NUMERICAL_EPS
Definition: config.h:145
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:4621
double getSpeed() const
Returns the vehicle&#39;s current speed.
Definition: MSVehicle.h:477
int influenceChangeDecision(int state)
allow TraCI to influence a lane change decision
Definition: MSVehicle.cpp:5841
const std::string & getID() const
Returns the name of the vehicle.
Representation of a lane in the micro simulation.
Definition: MSLane.h:83
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