Eclipse SUMO - Simulation of Urban MObility
MSLCM_SL2015.cpp
Go to the documentation of this file.
1/****************************************************************************/
2// Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3// Copyright (C) 2013-2022 German Aerospace Center (DLR) and others.
4// This program and the accompanying materials are made available under the
5// terms of the Eclipse Public License 2.0 which is available at
6// https://www.eclipse.org/legal/epl-2.0/
7// This Source Code may also be made available under the following Secondary
8// Licenses when the conditions for such availability set forth in the Eclipse
9// Public License 2.0 are satisfied: GNU General Public License, version 2
10// or later which is available at
11// https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12// SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13/****************************************************************************/
18// A lane change model for heterogeneous traffic (based on sub-lanes)
19/****************************************************************************/
20#include <config.h>
21
22#include <iostream>
25#include <microsim/MSEdge.h>
26#include <microsim/MSLane.h>
27#include <microsim/MSLink.h>
28#include <microsim/MSNet.h>
30#include <microsim/MSGlobals.h>
31#include <microsim/MSStop.h>
34#include "MSLCHelper.h"
35#include "MSLCM_SL2015.h"
36
37// ===========================================================================
38// variable definitions
39// ===========================================================================
40#define MAGIC_OFFSET 1.
41#define LOOK_FORWARD 10.
42
43#define JAM_FACTOR 1.
44
45#define LCA_RIGHT_IMPATIENCE -1.
46#define CUT_IN_LEFT_SPEED_THRESHOLD 27.
47#define MAX_ONRAMP_LENGTH 200.
48
49#define LOOK_AHEAD_MIN_SPEED 0.0
50#define LOOK_AHEAD_SPEED_MEMORY 0.9
51
52#define HELP_DECEL_FACTOR 1.0
53
54#define HELP_OVERTAKE (10.0 / 3.6)
55#define MIN_FALLBEHIND (7.0 / 3.6)
56
57#define URGENCY 2.0
58
59#define KEEP_RIGHT_TIME 5.0 // the number of seconds after which a vehicle should move to the right lane
60
61#define RELGAIN_NORMALIZATION_MIN_SPEED 10.0
62
63#define TURN_LANE_DIST 200.0 // the distance at which a lane leading elsewhere is considered to be a turn-lane that must be avoided
64#define GAIN_PERCEPTION_THRESHOLD 0.05 // the minimum relative speed gain which affects the behavior
65
66#define SPEED_GAIN_MIN_SECONDS 20.0
67
68#define ARRIVALPOS_LAT_THRESHOLD 100.0
69
70// the speed at which the desired lateral gap grows now further
71#define LATGAP_SPEED_THRESHOLD (50 / 3.6)
72// the speed at which the desired lateral gap shrinks now further.
73// @note: when setting LATGAP_SPEED_THRESHOLD = LATGAP_SPEED_THRESHOLD2, no speed-specif reduction of minGapLat is done
74#define LATGAP_SPEED_THRESHOLD2 (50 / 3.6)
75
76// intention to change decays over time
77#define SPEEDGAIN_DECAY_FACTOR 0.5
78// exponential averaging factor for expected sublane speeds
79#define SPEEDGAIN_MEMORY_FACTOR 0.5
80
81#define REACT_TO_STOPPED_DISTANCE 100
82
83
84// ===========================================================================
85// Debug flags
86// ===========================================================================
87//#define DEBUG_MANEUVER
88//#define DEBUG_WANTSCHANGE
89//#define DEBUG_STRATEGIC_CHANGE
90//#define DEBUG_KEEP_LATGAP
91//#define DEBUG_STATE
92//#define DEBUG_ACTIONSTEPS
93//#define DEBUG_COMMITTED_SPEED
94//#define DEBUG_PATCHSPEED
95//#define DEBUG_INFORM
96//#define DEBUG_ROUNDABOUTS
97//#define DEBUG_COOPERATE
98//#define DEBUG_SLOWDOWN
99//#define DEBUG_SAVE_BLOCKER_LENGTH
100//#define DEBUG_BLOCKING
101//#define DEBUG_TRACI
102//#define DEBUG_EXPECTED_SLSPEED
103//#define DEBUG_SLIDING
104//#define DEBUG_COND (myVehicle.getID() == "moped.18" || myVehicle.getID() == "moped.16")
105//#define DEBUG_COND (myVehicle.getID() == "Togliatti_71_0")
106#define DEBUG_COND (myVehicle.isSelected())
107//#define DEBUG_COND (myVehicle.getID() == "pkw150478" || myVehicle.getID() == "pkw150494" || myVehicle.getID() == "pkw150289")
108//#define DEBUG_COND (myVehicle.getID() == "A" || myVehicle.getID() == "B") // fail change to left
109//#define DEBUG_COND (myVehicle.getID() == "disabled") // test stops_overtaking
110//#define DEBUG_COND true
111
112
113// ===========================================================================
114// member method definitions
115// ===========================================================================
118 mySpeedGainProbabilityRight(0),
119 mySpeedGainProbabilityLeft(0),
120 myKeepRightProbability(0),
121 myLeadingBlockerLength(0),
122 myLeftSpace(0),
123 myLookAheadSpeed(LOOK_AHEAD_MIN_SPEED),
124 myLastEdge(nullptr),
125 myCanChangeFully(true),
126 mySafeLatDistRight(0),
127 mySafeLatDistLeft(0),
128 myStrategicParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_STRATEGIC_PARAM, 1)),
129 myCooperativeParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_COOPERATIVE_PARAM, 1)),
130 mySpeedGainParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SPEEDGAIN_PARAM, 1)),
131 myKeepRightParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_KEEPRIGHT_PARAM, 1)),
132 myOppositeParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_OPPOSITE_PARAM, 1)),
133 mySublaneParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SUBLANE_PARAM, 1)),
134 // by default use SUMO_ATTR_LCA_PUSHY. If that is not set, try SUMO_ATTR_LCA_PUSHYGAP
135 myMinGapLat(v.getVehicleType().getMinGapLat()),
136 myPushy(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_PUSHY,
137 1 - (v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_PUSHYGAP,
138 MAX2(NUMERICAL_EPS, myMinGapLat)) /
139 MAX2(NUMERICAL_EPS, myMinGapLat)))),
140 myAssertive(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_ASSERTIVE, 1)),
141 myImpatience(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_IMPATIENCE, 0)),
142 myMinImpatience(myImpatience),
143 myTimeToImpatience(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_TIME_TO_IMPATIENCE, std::numeric_limits<double>::max())),
144 myAccelLat(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_ACCEL_LAT, 1.0)),
145 myTurnAlignmentDist(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_TURN_ALIGNMENT_DISTANCE, 0.0)),
146 myLookaheadLeft(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_LOOKAHEADLEFT, 2.0)),
147 mySpeedGainRight(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SPEEDGAINRIGHT, 0.1)),
148 myLaneDiscipline(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_LANE_DISCIPLINE, 0.0)),
149 mySpeedGainLookahead(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SPEEDGAIN_LOOKAHEAD, 5)),
150 myRoundaboutBonus(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_COOPERATIVE_ROUNDABOUT, myCooperativeParam)),
151 myCooperativeSpeed(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_COOPERATIVE_SPEED, myCooperativeParam)),
152 myKeepRightAcceptanceTime(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_KEEPRIGHT_ACCEPTANCE_TIME, -1)),
153 myOvertakeDeltaSpeedFactor(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_OVERTAKE_DELTASPEED_FACTOR, 0)),
154 mySigmaState(0) {
156}
157
159 changed();
160}
161
162
163void
165 if (mySpeedGainParam <= 0) {
166 myChangeProbThresholdRight = std::numeric_limits<double>::max();
167 myChangeProbThresholdLeft = std::numeric_limits<double>::max();
168 } else {
171 }
173}
174
175
176bool
178 return DEBUG_COND;
179}
180
181
182int
184 int laneOffset,
185 LaneChangeAction alternatives,
186 const MSLeaderDistanceInfo& leaders,
187 const MSLeaderDistanceInfo& followers,
188 const MSLeaderDistanceInfo& blockers,
189 const MSLeaderDistanceInfo& neighLeaders,
190 const MSLeaderDistanceInfo& neighFollowers,
191 const MSLeaderDistanceInfo& neighBlockers,
192 const MSLane& neighLane,
193 const std::vector<MSVehicle::LaneQ>& preb,
194 MSVehicle** lastBlocked,
195 MSVehicle** firstBlocked,
196 double& latDist, double& maneuverDist, int& blocked) {
197
199 const std::string changeType = laneOffset == -1 ? "right" : (laneOffset == 1 ? "left" : "current");
200
201#ifdef DEBUG_MANEUVER
202 if (gDebugFlag2) {
203 std::cout << "\n" << SIMTIME
204 << std::setprecision(gPrecision)
205 << " veh=" << myVehicle.getID()
206 << " lane=" << myVehicle.getLane()->getID()
207 << " neigh=" << neighLane.getID()
208 << " pos=" << myVehicle.getPositionOnLane()
209 << " posLat=" << myVehicle.getLateralPositionOnLane()
210 << " posLatError=" << mySigmaState
211 << " speed=" << myVehicle.getSpeed()
212 << " considerChangeTo=" << changeType
213 << "\n";
214 }
215#endif
216
217 int result = _wantsChangeSublane(laneOffset,
218 alternatives,
219 leaders, followers, blockers,
220 neighLeaders, neighFollowers, neighBlockers,
221 neighLane, preb,
222 lastBlocked, firstBlocked, latDist, maneuverDist, blocked);
223
224 result = keepLatGap(result, leaders, followers, blockers,
225 neighLeaders, neighFollowers, neighBlockers,
226 neighLane, laneOffset, latDist, maneuverDist, blocked);
227
228 result |= getLCA(result, latDist);
229 // take into account lateral acceleration
230#if defined(DEBUG_MANEUVER) || defined(DEBUG_STATE)
231 double latDistTmp = latDist;
232#endif
233 latDist = SPEED2DIST(computeSpeedLat(latDist, maneuverDist, (result & LCA_URGENT) != 0));
234#if defined(DEBUG_MANEUVER) || defined(DEBUG_STATE)
235 if (gDebugFlag2 && latDist != latDistTmp) {
236 std::cout << SIMTIME << " veh=" << myVehicle.getID() << " maneuverDist=" << maneuverDist << " latDist=" << latDistTmp << " mySpeedPrev=" << mySpeedLat << " speedLat=" << DIST2SPEED(latDist) << " latDist2=" << latDist << "\n";
237 }
238
239 if (gDebugFlag2) {
240 if (result & LCA_WANTS_LANECHANGE) {
241 std::cout << SIMTIME
242 << " veh=" << myVehicle.getID()
243 << " wantsChangeTo=" << changeType
244 << " latDist=" << latDist
245 << " maneuverDist=" << maneuverDist
246 << " state=" << toString((LaneChangeAction)result)
247 << ((blocked & LCA_BLOCKED) ? " (blocked)" : "")
248 << ((blocked & LCA_OVERLAPPING) ? " (overlap)" : "")
249 << "\n\n";
250 } else {
251 std::cout << SIMTIME
252 << " veh=" << myVehicle.getID()
253 << " wantsNoChangeTo=" << changeType
254 << " state=" << toString((LaneChangeAction)result)
255 << "\n\n";
256 }
257 }
258#endif
259 gDebugFlag2 = false;
260 return result;
261}
262
263void
266 if (myVehicle.isActive()) {
267 if ((state & (LCA_STRATEGIC | LCA_SPEEDGAIN)) != 0 && (state & LCA_BLOCKED) != 0) {
269 } else {
270 // impatience decays only to the driver-specific level
272 }
273#ifdef DEBUG_STATE
274 if (DEBUG_COND) {
275 std::cout << SIMTIME << " veh=" << myVehicle.getID()
276 << " setOwnState=" << toString((LaneChangeAction)state)
277 << " myMinImpatience=" << myMinImpatience
278 << " myImpatience=" << myImpatience
279 << "\n";
280 }
281#endif
282 if ((state & LCA_STAY) != 0) {
283 myCanChangeFully = true;
284// if (DEBUG_COND) {
285// std::cout << " myCanChangeFully=true\n";
286// }
287 }
288 }
289}
290
291
292void
293MSLCM_SL2015::updateSafeLatDist(const double travelledLatDist) {
294 mySafeLatDistLeft -= travelledLatDist;
295 mySafeLatDistRight += travelledLatDist;
296
297 if (fabs(mySafeLatDistLeft) < NUMERICAL_EPS) {
299 }
300 if (fabs(mySafeLatDistRight) < NUMERICAL_EPS) {
302 }
303}
304
305
306double
307MSLCM_SL2015::patchSpeed(const double min, const double wanted, const double max, const MSCFModel& cfModel) {
309 // negative min speed may be passed when using ballistic updated
310 const double newSpeed = _patchSpeed(MAX2(min, 0.0), wanted, max, cfModel);
311#ifdef DEBUG_PATCHSPEED
312 if (gDebugFlag2) {
313 const std::string patched = (wanted != newSpeed ? " patched=" + toString(newSpeed) : "");
314 std::cout << SIMTIME
315 << " veh=" << myVehicle.getID()
316 << " lane=" << myVehicle.getLane()->getID()
317 << " pos=" << myVehicle.getPositionOnLane()
318 << " v=" << myVehicle.getSpeed()
319 << " min=" << min
320 << " wanted=" << wanted
321 << " max=" << max
322 << patched
323 << "\n\n";
324 }
325#endif
326 gDebugFlag2 = false;
327 return newSpeed;
328}
329
330
331double
332MSLCM_SL2015::_patchSpeed(double min, const double wanted, double max, const MSCFModel& cfModel) {
333 if (wanted <= 0) {
334 return wanted;
335 }
336
337 int state = myOwnState;
338
339 double nVSafe = wanted;
340 bool gotOne = false;
341 // letting vehicles merge in at the end of the lane in case of counter-lane change, step#2
342 // if we want to change and have a blocking leader and there is enough room for him in front of us
343 if (myLeadingBlockerLength != 0) {
345#ifdef DEBUG_PATCHSPEED
346 if (gDebugFlag2) {
347 std::cout << SIMTIME << " veh=" << myVehicle.getID() << " myLeadingBlockerLength=" << myLeadingBlockerLength << " space=" << space << "\n";
348 }
349#endif
350 if (space >= 0) { // XXX space > -MAGIC_OFFSET
351 // compute speed for decelerating towards a place which allows the blocking leader to merge in in front
352 double safe = cfModel.stopSpeed(&myVehicle, myVehicle.getSpeed(), space, MSCFModel::CalcReason::LANE_CHANGE);
353 max = MIN2(max, safe);
354 // if we are approaching this place
355 if (safe < wanted) {
356 if (safe < min) {
358 if (safe >= vMinEmergency) {
359 // permit harder braking if needed and helpful
360 min = MAX2(vMinEmergency, safe);
361 }
362 }
363#ifdef DEBUG_PATCHSPEED
364 if (gDebugFlag2) {
365 std::cout << SIMTIME << " veh=" << myVehicle.getID() << " slowing down for leading blocker, safe=" << safe << (safe + NUMERICAL_EPS < min ? " (not enough)" : "") << "\n";
366 }
367#endif
368 nVSafe = MAX2(min, safe);
369 gotOne = true;
370 }
371 }
372 }
373 const double coopWeight = MAX2(0.0, MIN2(1.0, myCooperativeSpeed));
374 for (std::vector<double>::const_iterator i = myLCAccelerationAdvices.begin(); i != myLCAccelerationAdvices.end(); ++i) {
375 double v = myVehicle.getSpeed() + ACCEL2SPEED(*i);
376 if (v >= min && v <= max) {
377 nVSafe = MIN2(v * coopWeight + (1 - coopWeight) * wanted, nVSafe);
378 gotOne = true;
379#ifdef DEBUG_PATCHSPEED
380 if (gDebugFlag2) {
381 std::cout << SIMTIME << " veh=" << myVehicle.getID() << " got accel=" << (*i) << " nVSafe=" << nVSafe << "\n";
382 }
383#endif
384 } else {
385#ifdef DEBUG_PATCHSPEED
386 if (v < min) {
387 if (gDebugFlag2) {
388 std::cout << SIMTIME << " veh=" << myVehicle.getID() << " ignoring low nVSafe=" << v << " min=" << min << "\n";
389 }
390 } else {
391 if (gDebugFlag2) {
392 std::cout << SIMTIME << " veh=" << myVehicle.getID() << " ignoring high nVSafe=" << v << " max=" << max << "\n";
393 }
394 }
395#endif
396 }
397 }
398
399 if (gotOne && !myDontBrake) {
400#ifdef DEBUG_PATCHSPEED
401 if (gDebugFlag2) {
402 std::cout << SIMTIME << " veh=" << myVehicle.getID() << " got vSafe\n";
403 }
404#endif
405 return nVSafe;
406 }
407
408 // check whether the vehicle is blocked
409 if ((state & LCA_WANTS_LANECHANGE) != 0 && (state & LCA_BLOCKED) != 0) {
410 if ((state & LCA_STRATEGIC) != 0) {
411 // necessary decelerations are controlled via vSafe. If there are
412 // none it means we should speed up
413#if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
414 if (gDebugFlag2) {
415 std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_WANTS_LANECHANGE (strat, no vSafe)\n";
416 }
417#endif
418 return (max + wanted) / 2.0;
419 } else if ((state & LCA_COOPERATIVE) != 0) {
420 // only minor adjustments in speed should be done
421 if ((state & LCA_BLOCKED_BY_LEADER) != 0) {
422#if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
423 if (gDebugFlag2) {
424 std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_BLOCKED_BY_LEADER (coop)\n";
425 }
426#endif
427 return (min + wanted) / 2.0;
428 }
429 if ((state & LCA_BLOCKED_BY_FOLLOWER) != 0) {
430#if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
431 if (gDebugFlag2) {
432 std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_BLOCKED_BY_FOLLOWER (coop)\n";
433 }
434#endif
435 return (max + wanted) / 2.0;
436 }
437 //} else { // VARIANT_16
438 // // only accelerations should be performed
439 // if ((state & LCA_BLOCKED_BY_FOLLOWER) != 0) {
440 // if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_BLOCKED_BY_FOLLOWER\n";
441 // return (max + wanted) / 2.0;
442 // }
443 }
444 }
445
446 /*
447 // decelerate if being a blocking follower
448 // (and does not have to change lanes)
449 if ((state & LCA_AMBLOCKINGFOLLOWER) != 0) {
450 if (fabs(max - myVehicle.getCarFollowModel().maxNextSpeed(myVehicle.getSpeed(), &myVehicle)) < 0.001 && min == 0) { // !!! was standing
451 if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBLOCKINGFOLLOWER (standing)\n";
452 return 0;
453 }
454 if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBLOCKINGFOLLOWER\n";
455
456 //return min; // VARIANT_3 (brakeStrong)
457 return (min + wanted) / 2.0;
458 }
459 if ((state & LCA_AMBACKBLOCKER) != 0) {
460 if (max <= myVehicle.getCarFollowModel().maxNextSpeed(myVehicle.getSpeed(), &myVehicle) && min == 0) { // !!! was standing
461 if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBACKBLOCKER (standing)\n";
462 //return min; VARIANT_9 (backBlockVSafe)
463 return nVSafe;
464 }
465 }
466 if ((state & LCA_AMBACKBLOCKER_STANDING) != 0) {
467 if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBACKBLOCKER_STANDING\n";
468 //return min;
469 return nVSafe;
470 }
471 */
472
473 // accelerate if being a blocking leader or blocking follower not able to brake
474 // (and does not have to change lanes)
475 if ((state & LCA_AMBLOCKINGLEADER) != 0) {
476#if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
477 if (gDebugFlag2) {
478 std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBLOCKINGLEADER\n";
479 }
480#endif
481 return (max + wanted) / 2.0;
482 }
483
484 if ((state & LCA_AMBLOCKINGFOLLOWER_DONTBRAKE) != 0) {
485#if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
486 if (gDebugFlag2) {
487 std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBLOCKINGFOLLOWER_DONTBRAKE\n";
488 }
489#endif
490 /*
491 // VARIANT_4 (dontbrake)
492 if (max <= myVehicle.getCarFollowModel().maxNextSpeed(myVehicle.getSpeed(), &myVehicle) && min == 0) { // !!! was standing
493 return wanted;
494 }
495 return (min + wanted) / 2.0;
496 */
497 }
498 return wanted;
499}
500
501
502void*
503MSLCM_SL2015::inform(void* info, MSVehicle* sender) {
504 Info* pinfo = (Info*) info;
505 if (pinfo->first >= 0) {
506 addLCSpeedAdvice(pinfo->first);
507 }
508 //myOwnState &= 0xffffffff; // reset all bits of MyLCAEnum but only those
509 myOwnState |= pinfo->second;
510#ifdef DEBUG_INFORM
512 std::cout << SIMTIME
513 << " veh=" << myVehicle.getID()
514 << " informedBy=" << sender->getID()
515 << " info=" << pinfo->second
516 << " vSafe=" << pinfo->first
517 << "\n";
518 }
519#else
520 UNUSED_PARAMETER(sender);
521#endif
522 delete pinfo;
523 return (void*) true;
524}
525
526
527void
528MSLCM_SL2015::msg(const CLeaderDist& cld, double speed, int state) {
529 assert(cld.first != 0);
530 ((MSVehicle*)cld.first)->getLaneChangeModel().inform(new Info(speed, state), &myVehicle);
531}
532
533
534double
536 int dir,
537 const CLeaderDist& neighLead,
538 double remainingSeconds) {
539 double plannedSpeed = MIN2(myVehicle.getSpeed(),
541 for (std::vector<double>::const_iterator i = myLCAccelerationAdvices.begin(); i != myLCAccelerationAdvices.end(); ++i) {
542 double v = myVehicle.getSpeed() + ACCEL2SPEED(*i);
544 plannedSpeed = MIN2(plannedSpeed, v);
545 }
546 }
547#ifdef DEBUG_INFORM
548 if (gDebugFlag2) {
549 std::cout << " informLeader speed=" << myVehicle.getSpeed() << " planned=" << plannedSpeed << "\n";
550 }
551#endif
552
553 if ((blocked & LCA_BLOCKED_BY_LEADER) != 0 && neighLead.first != 0) {
554 const MSVehicle* nv = neighLead.first;
556 //std::cout << SIMTIME << " ego=" << myVehicle.getID() << " ignoresDivergentBlockingLeader=" << nv->getID() << "\n";
557 return plannedSpeed;
558 }
559#ifdef DEBUG_INFORM
560 if (gDebugFlag2) std::cout << " blocked by leader nv=" << nv->getID() << " nvSpeed=" << nv->getSpeed() << " needGap="
562#endif
563 // decide whether we want to overtake the leader or follow it
564 const double dv = plannedSpeed - nv->getSpeed();
565 const double overtakeDist = (neighLead.second // drive to back of follower
566 + nv->getVehicleType().getLengthWithGap() // drive to front of follower
567 + myVehicle.getVehicleType().getLength() // ego back reaches follower front
568 + nv->getCarFollowModel().getSecureGap( // save gap to follower
570
571 if ((dv < myOvertakeDeltaSpeedFactor * myVehicle.getLane()->getSpeedLimit() + NUMERICAL_EPS
572 // overtaking on the right on an uncongested highway is forbidden (noOvertakeLCLeft)
574 // not enough space to overtake? (we will start to brake when approaching a dead end)
576 // not enough time to overtake?
577 || dv * remainingSeconds < overtakeDist)
578 && (!neighLead.first->isStopped() || (isOpposite() && neighLead.second >= 0))) {
579 // cannot overtake
580 msg(neighLead, -1, dir | LCA_AMBLOCKINGLEADER);
581 // slow down smoothly to follow leader
582 const double targetSpeed = getCarFollowModel().followSpeed(
583 &myVehicle, myVehicle.getSpeed(), neighLead.second, nv->getSpeed(), nv->getCarFollowModel().getMaxDecel());
584 if (targetSpeed < myVehicle.getSpeed()) {
585 // slow down smoothly to follow leader
586 const double decel = ACCEL2SPEED(MIN2(myVehicle.getCarFollowModel().getMaxDecel(),
587 MAX2(MIN_FALLBEHIND, (myVehicle.getSpeed() - targetSpeed) / remainingSeconds)));
588 //const double nextSpeed = MAX2(0., MIN2(plannedSpeed, myVehicle.getSpeed() - decel));
589 const double nextSpeed = MIN2(plannedSpeed, MAX2(0.0, myVehicle.getSpeed() - decel));
590#ifdef DEBUG_INFORM
591 if (gDebugFlag2) {
592 std::cout << SIMTIME
593 << " cannot overtake leader nv=" << nv->getID()
594 << " dv=" << dv
595 << " remainingSeconds=" << remainingSeconds
596 << " targetSpeed=" << targetSpeed
597 << " nextSpeed=" << nextSpeed
598 << "\n";
599 }
600#endif
601 addLCSpeedAdvice(nextSpeed);
602 return nextSpeed;
603 } else {
604 // leader is fast enough anyway
605#ifdef DEBUG_INFORM
606 if (gDebugFlag2) {
607 std::cout << SIMTIME
608 << " cannot overtake fast leader nv=" << nv->getID()
609 << " dv=" << dv
610 << " remainingSeconds=" << remainingSeconds
611 << " targetSpeed=" << targetSpeed
612 << "\n";
613 }
614#endif
615 addLCSpeedAdvice(targetSpeed);
616 return plannedSpeed;
617 }
618 } else {
619#ifdef DEBUG_INFORM
620 if (gDebugFlag2) {
621 std::cout << SIMTIME
622 << " wants to overtake leader nv=" << nv->getID()
623 << " dv=" << dv
624 << " remainingSeconds=" << remainingSeconds
625 << " currentGap=" << neighLead.second
627 << " overtakeDist=" << overtakeDist
628 << " leftSpace=" << myLeftSpace
629 << " blockerLength=" << myLeadingBlockerLength
630 << "\n";
631 }
632#endif
633 // overtaking, leader should not accelerate
634 msg(neighLead, nv->getSpeed(), dir | LCA_AMBLOCKINGLEADER);
635 return -1;
636 }
637 } else if (neighLead.first != 0) { // (remainUnblocked)
638 // we are not blocked now. make sure we stay far enough from the leader
639 const MSVehicle* nv = neighLead.first;
640 double dv, nextNVSpeed;
642 // XXX: the decrement (HELP_OVERTAKE) should be scaled with timestep length, I think.
643 // It seems to function as an estimate nv's speed in the next simstep!? (so HELP_OVERTAKE should be an acceleration value.)
644 nextNVSpeed = nv->getSpeed() - HELP_OVERTAKE; // conservative
645 dv = SPEED2DIST(myVehicle.getSpeed() - nextNVSpeed);
646 } else {
647 // Estimate neigh's speed after actionstep length
648 // @note The possible breaking can be underestimated by the formula, so this is a potential
649 // source of collisions if actionsteplength>simsteplength.
650 const double nvMaxDecel = HELP_OVERTAKE;
651 nextNVSpeed = nv->getSpeed() - nvMaxDecel * myVehicle.getActionStepLengthSecs(); // conservative
652 // Estimated gap reduction until next action step if own speed stays constant
653 dv = SPEED2DIST(myVehicle.getSpeed() - nextNVSpeed);
654 }
655 const double targetSpeed = getCarFollowModel().followSpeed(
656 &myVehicle, myVehicle.getSpeed(), neighLead.second - dv, nextNVSpeed, nv->getCarFollowModel().getMaxDecel());
657 addLCSpeedAdvice(targetSpeed);
658#ifdef DEBUG_INFORM
659 if (gDebugFlag2) {
660 std::cout << " not blocked by leader nv=" << nv->getID()
661 << " nvSpeed=" << nv->getSpeed()
662 << " gap=" << neighLead.second
663 << " nextGap=" << neighLead.second - dv
665 << " targetSpeed=" << targetSpeed
666 << "\n";
667 }
668#endif
669 return MIN2(targetSpeed, plannedSpeed);
670 } else {
671 // not overtaking
672 return plannedSpeed;
673 }
674}
675
676
677void
679 int dir,
680 const CLeaderDist& neighFollow,
681 double remainingSeconds,
682 double plannedSpeed) {
683 if ((blocked & LCA_BLOCKED_BY_FOLLOWER) != 0 && neighFollow.first != 0) {
684 const MSVehicle* nv = neighFollow.first;
686 //std::cout << SIMTIME << " ego=" << myVehicle.getID() << " ignoresDivergentBlockingFollower=" << nv->getID() << "\n";
687 return;
688 }
689#ifdef DEBUG_INFORM
690 if (gDebugFlag2) std::cout << " blocked by follower nv=" << nv->getID() << " nvSpeed=" << nv->getSpeed() << " needGap="
692#endif
693
694 // are we fast enough to cut in without any help?
695 if (plannedSpeed - nv->getSpeed() >= HELP_OVERTAKE) {
696 const double neededGap = nv->getCarFollowModel().getSecureGap(nv, &myVehicle, nv->getSpeed(), plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel());
697 if ((neededGap - neighFollow.second) / remainingSeconds < (plannedSpeed - nv->getSpeed())) {
698#ifdef DEBUG_INFORM
699 if (gDebugFlag2) {
700 std::cout << " wants to cut in before nv=" << nv->getID() << " without any help neededGap=" << neededGap << "\n";
701 }
702#endif
703 // follower might even accelerate but not to much
704 msg(neighFollow, plannedSpeed - HELP_OVERTAKE, dir | LCA_AMBLOCKINGFOLLOWER);
705 return;
706 }
707 }
708 // decide whether we will request help to cut in before the follower or allow to be overtaken
709
710 // PARAMETERS
711 // assume other vehicle will assume the equivalent of 1 second of
712 // maximum deceleration to help us (will probably be spread over
713 // multiple seconds)
714 // -----------
715 const double helpDecel = nv->getCarFollowModel().getMaxDecel() * HELP_DECEL_FACTOR ;
716
717 // change in the gap between ego and blocker over 1 second (not STEP!)
718 const double neighNewSpeed = MAX2(0., nv->getSpeed() - ACCEL2SPEED(helpDecel));
719 const double neighNewSpeed1s = MAX2(0., nv->getSpeed() - helpDecel);
720 const double dv = plannedSpeed - neighNewSpeed1s;
721 // new gap between follower and self in case the follower does brake for 1s
722 const double decelGap = neighFollow.second + dv;
723 const double secureGap = nv->getCarFollowModel().getSecureGap(nv, &myVehicle, neighNewSpeed1s, plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel());
724#ifdef DEBUG_INFORM
725 if (gDebugFlag2) {
726 std::cout << SIMTIME
727 << " egoV=" << myVehicle.getSpeed()
728 << " egoNV=" << plannedSpeed
729 << " nvNewSpeed=" << neighNewSpeed
730 << " nvNewSpeed1s=" << neighNewSpeed1s
731 << " deltaGap=" << dv
732 << " decelGap=" << decelGap
733 << " secGap=" << secureGap
734 << "\n";
735 }
736#endif
737 if (decelGap > 0 && decelGap >= secureGap) {
738 // if the blocking neighbor brakes it could actually help
739 // how hard does it actually need to be?
740 // to be safe in the next step the following equation has to hold:
741 // vsafe <= followSpeed(gap=currentGap - SPEED2DIST(vsafe), ...)
742 // we compute an upper bound on vsafe by doing the computation twice
743 const double vsafe1 = MAX2(neighNewSpeed, nv->getCarFollowModel().followSpeed(
744 nv, nv->getSpeed(), neighFollow.second + SPEED2DIST(plannedSpeed), plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel()));
745 const double vsafe = MAX2(neighNewSpeed, nv->getCarFollowModel().followSpeed(
746 nv, nv->getSpeed(), neighFollow.second + SPEED2DIST(plannedSpeed - vsafe1), plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel()));
747 // the following assertion cannot be guaranteed because the CFModel handles small gaps differently, see MSCFModel::maximumSafeStopSpeed
748 // assert(vsafe <= vsafe1);
749 msg(neighFollow, vsafe, dir | LCA_AMBLOCKINGFOLLOWER);
750#ifdef DEBUG_INFORM
751 if (gDebugFlag2) {
752 std::cout << " wants to cut in before nv=" << nv->getID()
753 << " vsafe1=" << vsafe1
754 << " vsafe=" << vsafe
755 << " newSecGap=" << nv->getCarFollowModel().getSecureGap(nv, &myVehicle, vsafe, plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel())
756 << "\n";
757 }
758#endif
759 } else if (dv > 0 && dv * remainingSeconds > (secureGap - decelGap + POSITION_EPS)) {
760 // decelerating once is sufficient to open up a large enough gap in time
761 msg(neighFollow, neighNewSpeed, dir | LCA_AMBLOCKINGFOLLOWER);
762#ifdef DEBUG_INFORM
763 if (gDebugFlag2) {
764 std::cout << " wants to cut in before nv=" << nv->getID() << " (eventually)\n";
765 }
766#endif
767 } else if (dir == LCA_MRIGHT && !myAllowOvertakingRight && !nv->congested()) {
768 const double vhelp = MAX2(neighNewSpeed, HELP_OVERTAKE);
769 msg(neighFollow, vhelp, dir | LCA_AMBLOCKINGFOLLOWER);
770#ifdef DEBUG_INFORM
771 if (gDebugFlag2) {
772 std::cout << " wants to cut in before nv=" << nv->getID() << " (nv cannot overtake right)\n";
773 }
774#endif
775 } else {
776 double vhelp = MAX2(nv->getSpeed(), myVehicle.getSpeed() + HELP_OVERTAKE);
777 if (nv->getSpeed() > myVehicle.getSpeed() &&
779 || (dir == LCA_MLEFT && plannedSpeed > CUT_IN_LEFT_SPEED_THRESHOLD) // VARIANT_22 (slowDownLeft)
780 // XXX this is a hack to determine whether the vehicles is on an on-ramp. This information should be retrieved from the network itself
782 )) {
783 // let the follower slow down to increase the likelyhood that later vehicles will be slow enough to help
784 // follower should still be fast enough to open a gap
785 vhelp = MAX2(neighNewSpeed, myVehicle.getSpeed() + HELP_OVERTAKE);
786#ifdef DEBUG_INFORM
787 if (gDebugFlag2) {
788 std::cout << " wants right follower to slow down a bit\n";
789 }
790#endif
791 if ((nv->getSpeed() - myVehicle.getSpeed()) / helpDecel < remainingSeconds) {
792#ifdef DEBUG_INFORM
793 if (gDebugFlag2) {
794 std::cout << " wants to cut in before right follower nv=" << nv->getID() << " (eventually)\n";
795 }
796#endif
797 msg(neighFollow, vhelp, dir | LCA_AMBLOCKINGFOLLOWER);
798 return;
799 }
800 }
801 msg(neighFollow, vhelp, dir | LCA_AMBLOCKINGFOLLOWER);
802 // this follower is supposed to overtake us. slow down smoothly to allow this
803 const double overtakeDist = (neighFollow.second // follower reaches ego back
804 + myVehicle.getVehicleType().getLengthWithGap() // follower reaches ego front
805 + nv->getVehicleType().getLength() // follower back at ego front
806 + myVehicle.getCarFollowModel().getSecureGap( // follower has safe dist to ego
807 &myVehicle, nv, plannedSpeed, vhelp, nv->getCarFollowModel().getMaxDecel()));
808 // speed difference to create a sufficiently large gap
809 const double needDV = overtakeDist / remainingSeconds;
810 // make sure the deceleration is not to strong
812
813#ifdef DEBUG_INFORM
814 if (gDebugFlag2) {
815 std::cout << SIMTIME
816 << " veh=" << myVehicle.getID()
817 << " wants to be overtaken by=" << nv->getID()
818 << " overtakeDist=" << overtakeDist
819 << " vneigh=" << nv->getSpeed()
820 << " vhelp=" << vhelp
821 << " needDV=" << needDV
822 << " vsafe=" << myVehicle.getSpeed() + ACCEL2SPEED(myLCAccelerationAdvices.back())
823 << "\n";
824 }
825#endif
826 }
827 } else if (neighFollow.first != 0) {
828 // we are not blocked no, make sure it remains that way
829 const MSVehicle* nv = neighFollow.first;
830 const double vsafe1 = nv->getCarFollowModel().followSpeed(
831 nv, nv->getSpeed(), neighFollow.second + SPEED2DIST(plannedSpeed), plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel());
832 const double vsafe = nv->getCarFollowModel().followSpeed(
833 nv, nv->getSpeed(), neighFollow.second + SPEED2DIST(plannedSpeed - vsafe1), plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel());
834 msg(neighFollow, vsafe, dir | LCA_AMBLOCKINGFOLLOWER);
835#ifdef DEBUG_INFORM
836 if (gDebugFlag2) {
837 std::cout << " wants to cut in before non-blocking follower nv=" << nv->getID() << "\n";
838 }
839#endif
840 }
841}
842
843double
844MSLCM_SL2015::informLeaders(int blocked, int dir,
845 const std::vector<CLeaderDist>& blockers,
846 double remainingSeconds) {
847 double plannedSpeed = myVehicle.getSpeed();
848 double space = myLeftSpace;
849 if (myLeadingBlockerLength != 0) {
850 // see patchSpeed @todo: refactor
852 if (space <= 0) {
853 // ignore leading blocker
854 space = myLeftSpace;
855 }
856 }
858 plannedSpeed = MIN2(plannedSpeed, safe);
859
860 for (std::vector<CLeaderDist>::const_iterator it = blockers.begin(); it != blockers.end(); ++it) {
861 plannedSpeed = MIN2(plannedSpeed, informLeader(blocked, dir, *it, remainingSeconds));
862 }
863 return plannedSpeed;
864}
865
866
867void
868MSLCM_SL2015::informFollowers(int blocked, int dir,
869 const std::vector<CLeaderDist>& blockers,
870 double remainingSeconds,
871 double plannedSpeed) {
872 // #3727
873 for (std::vector<CLeaderDist>::const_iterator it = blockers.begin(); it != blockers.end(); ++it) {
874 informFollower(blocked, dir, *it, remainingSeconds, plannedSpeed);
875 }
876}
877
878
879void
882 // keep information about strategic change direction
884#ifdef DEBUG_INFORM
885 if (debugVehicle()) {
886 std::cout << SIMTIME
887 << " veh=" << myVehicle.getID()
888 << " prepareStep"
889 << " myCanChangeFully=" << myCanChangeFully
890 << "\n";
891 }
892#endif
894 myLeftSpace = 0;
896 myDontBrake = false;
897 myCFRelated.clear();
898 myCFRelatedReady = false;
899 const double halfWidth = getWidth() * 0.5;
900 double center = getVehicleCenter();
901 mySafeLatDistRight = center - halfWidth;
902 mySafeLatDistLeft = getLeftBorder() - center - halfWidth;
903 // truncate to work around numerical instability between different builds
904 mySpeedGainProbabilityRight = ceil(mySpeedGainProbabilityRight * 100000.0) * 0.00001;
905 mySpeedGainProbabilityLeft = ceil(mySpeedGainProbabilityLeft * 100000.0) * 0.00001;
906 myKeepRightProbability = ceil(myKeepRightProbability * 100000.0) * 0.00001;
907 // updated myExpectedSublaneSpeeds
908 // XXX only do this when (sub)lane changing is possible
909 std::vector<double> newExpectedSpeeds;
910#ifdef DEBUG_INFORM
911 if (DEBUG_COND) {
912 std::cout << SIMTIME << " veh=" << myVehicle.getID() << " myExpectedSublaneSpeeds=" << toString(myExpectedSublaneSpeeds) << "\n";
913 }
914#endif
916 // initialize
917 const MSEdge* currEdge = &myVehicle.getLane()->getEdge();
918 const std::vector<MSLane*>& lanes = currEdge->getLanes();
919 for (std::vector<MSLane*>::const_iterator it_lane = lanes.begin(); it_lane != lanes.end(); ++it_lane) {
920 const int subLanes = MAX2(1, int(ceil((*it_lane)->getWidth() / MSGlobals::gLateralResolution)));
921 for (int i = 0; i < subLanes; ++i) {
922 newExpectedSpeeds.push_back((*it_lane)->getVehicleMaxSpeed(&myVehicle));
923 }
924 }
925 if (currEdge->canChangeToOpposite()) {
926 MSLane* opposite = lanes.back()->getOpposite();
927 const int subLanes = MAX2(1, int(ceil(opposite->getWidth() / MSGlobals::gLateralResolution)));
928 for (int i = 0; i < subLanes; ++i) {
929 newExpectedSpeeds.push_back(lanes.back()->getVehicleMaxSpeed(&myVehicle));
930 }
931 }
932 if (myExpectedSublaneSpeeds.size() > 0) {
933 // copy old values
934 assert(myLastEdge != 0);
935 if (myLastEdge->getSubLaneSides().size() == myExpectedSublaneSpeeds.size()) {
936 const int subLaneShift = computeSublaneShift(myLastEdge, currEdge);
937 if (subLaneShift < std::numeric_limits<int>::max()) {
938 for (int i = 0; i < (int)myExpectedSublaneSpeeds.size(); ++i) {
939 const int newI = i + subLaneShift;
940 if (newI > 0 && newI < (int)newExpectedSpeeds.size()) {
941 newExpectedSpeeds[newI] = myExpectedSublaneSpeeds[i];
942 }
943 }
944 }
945 }
946 }
947 myExpectedSublaneSpeeds = newExpectedSpeeds;
948 myLastEdge = currEdge;
949 }
950 assert(myExpectedSublaneSpeeds.size() == myVehicle.getLane()->getEdge().getSubLaneSides().size());
951 if (mySigma > 0) {
953 }
954}
955
956double
957MSLCM_SL2015::getExtraReservation(int bestLaneOffset) const {
958 if (bestLaneOffset < -1) {
959 return 20;
960 } else if (bestLaneOffset > 1) {
961 return 40;
962 }
963 return 0;
964}
965
966
967double
969 //OUProcess::step(double state, double dt, double timeScale, double noiseIntensity)
970 const double deltaState = OUProcess::step(mySigmaState,
972 MAX2(NUMERICAL_EPS, (1 - mySigma) * 100), mySigma) - mySigmaState;
973 const double scaledDelta = deltaState * myVehicle.getSpeed() / myVehicle.getLane()->getSpeedLimit();
974 return scaledDelta;
975}
976
977double
980}
981
982int
983MSLCM_SL2015::computeSublaneShift(const MSEdge* prevEdge, const MSEdge* curEdge) {
984 // find the first lane that targets the new edge
985 int prevShift = 0;
986 for (const MSLane* const lane : prevEdge->getLanes()) {
987 for (const MSLink* const link : lane->getLinkCont()) {
988 if (&link->getLane()->getEdge() == curEdge) {
989 int curShift = 0;
990 const MSLane* target = link->getLane();
991 const std::vector<MSLane*>& lanes2 = curEdge->getLanes();
992 for (std::vector<MSLane*>::const_iterator it_lane2 = lanes2.begin(); it_lane2 != lanes2.end(); ++it_lane2) {
993 const MSLane* lane2 = *it_lane2;
994 if (lane2 == target) {
995 return prevShift + curShift;
996 }
997 MSLeaderInfo ahead(lane2->getWidth());
998 curShift += ahead.numSublanes();
999 }
1000 assert(false);
1001 }
1002 }
1003 MSLeaderInfo ahead(lane->getWidth());
1004 prevShift -= ahead.numSublanes();
1005 }
1006 return std::numeric_limits<int>::max();
1007}
1008
1009
1010void
1012 if (!myCanChangeFully) {
1013 // do not reset state yet so we can continue our maneuver but acknowledge
1014 // a change to the right (movement should continue due to lane alignment desire)
1015 if (getManeuverDist() < 0) {
1017 }
1018#ifdef DEBUG_STATE
1019 if (DEBUG_COND) {
1020 std::cout << SIMTIME << " veh=" << myVehicle.getID() << " state not reset. maneuverDist=" << getManeuverDist() << "\n";
1021 }
1022#endif
1023 return;
1024 }
1025 myOwnState = 0;
1026 // XX do not reset values for unfinished maneuvers
1030
1031 if (myVehicle.getBestLaneOffset() == 0) {
1032 // if we are not yet on our best lane there might still be unseen blockers
1033 // (during patchSpeed)
1035 myLeftSpace = 0;
1036 }
1039 myDontBrake = false;
1040#if defined(DEBUG_MANEUVER) || defined(DEBUG_STATE)
1041 if (DEBUG_COND) {
1042 std::cout << SIMTIME << " veh=" << myVehicle.getID() << " changed()\n";
1043 }
1044#endif
1045}
1046
1047
1048void
1050 myOwnState = 0;
1055 myLeftSpace = 0;
1058 myDontBrake = false;
1059}
1060
1061
1062int
1064 int laneOffset,
1065 LaneChangeAction alternatives,
1066 const MSLeaderDistanceInfo& leaders,
1067 const MSLeaderDistanceInfo& followers,
1068 const MSLeaderDistanceInfo& blockers,
1069 const MSLeaderDistanceInfo& neighLeaders,
1070 const MSLeaderDistanceInfo& neighFollowers,
1071 const MSLeaderDistanceInfo& neighBlockers,
1072 const MSLane& neighLane,
1073 const std::vector<MSVehicle::LaneQ>& preb,
1074 MSVehicle** lastBlocked,
1075 MSVehicle** firstBlocked,
1076 double& latDist, double& maneuverDist, int& blocked) {
1077
1078 const SUMOTime currentTime = MSNet::getInstance()->getCurrentTimeStep();
1079 // compute bestLaneOffset
1080 MSVehicle::LaneQ curr, neigh, best;
1081 int bestLaneOffset = 0;
1082 double currentDist = 0;
1083 double neighDist = 0;
1084 const MSLane* prebLane = myVehicle.getLane();
1085 if (prebLane->getEdge().isInternal()) {
1086 // internal edges are not kept inside the bestLanes structure
1087 if (isOpposite()) {
1088 prebLane = prebLane->getNormalPredecessorLane();
1089 } else {
1090 prebLane = prebLane->getLinkCont()[0]->getLane();
1091 }
1092 }
1093 // special case: vehicle considers changing to the opposite direction edge
1094 const bool checkOpposite = &neighLane.getEdge() != &myVehicle.getLane()->getEdge();
1095 const int prebOffset = (checkOpposite ? 0 : laneOffset);
1096 for (int p = 0; p < (int) preb.size(); ++p) {
1097 if (preb[p].lane == prebLane && p + laneOffset >= 0) {
1098 assert(p + prebOffset < (int)preb.size());
1099 curr = preb[p];
1100 neigh = preb[p + prebOffset];
1101 currentDist = curr.length;
1102 neighDist = neigh.length;
1103 bestLaneOffset = curr.bestLaneOffset;
1104 // VARIANT_13 (equalBest)
1105 if (bestLaneOffset == 0 && preb[p + prebOffset].bestLaneOffset == 0 && !checkOpposite) {
1106#ifdef DEBUG_WANTSCHANGE
1107 if (gDebugFlag2) {
1108 std::cout << STEPS2TIME(currentTime)
1109 << " veh=" << myVehicle.getID()
1110 << " bestLaneOffsetOld=" << bestLaneOffset
1111 << " bestLaneOffsetNew=" << laneOffset
1112 << "\n";
1113 }
1114#endif
1115 bestLaneOffset = prebOffset;
1116 }
1117 best = preb[p + bestLaneOffset];
1118 break;
1119 }
1120 }
1121 assert(curr.lane != nullptr);
1122 assert(neigh.lane != nullptr);
1123 assert(best.lane != nullptr);
1124 double driveToNextStop = -std::numeric_limits<double>::max();
1125 UNUSED_PARAMETER(driveToNextStop); // XXX use when computing usableDist
1126 if (myVehicle.nextStopDist() < std::numeric_limits<double>::max()
1128 // vehicle can always drive up to stop distance
1129 // @note this information is dynamic and thus not available in updateBestLanes()
1130 // @note: nextStopDist was compute before the vehicle moved
1131 driveToNextStop = myVehicle.nextStopDist();
1132 const double stopPos = getForwardPos() + myVehicle.nextStopDist() - myVehicle.getLastStepDist();
1133#ifdef DEBUG_WANTS_CHANGE
1134 if (DEBUG_COND) {
1135 std::cout << SIMTIME << std::setprecision(gPrecision) << " veh=" << myVehicle.getID()
1136 << " stopDist=" << myVehicle.nextStopDist()
1137 << " lastDist=" << myVehicle.getLastStepDist()
1138 << " stopPos=" << stopPos
1139 << " currentDist=" << currentDist
1140 << " neighDist=" << neighDist
1141 << "\n";
1142 }
1143#endif
1144 currentDist = MAX2(currentDist, stopPos);
1145 neighDist = MAX2(neighDist, stopPos);
1146 }
1147 // direction specific constants
1148 const bool right = (laneOffset == -1);
1149 const bool left = (laneOffset == 1);
1150 const int myLca = (right ? LCA_MRIGHT : (left ? LCA_MLEFT : 0));
1151 const int lcaCounter = (right ? LCA_LEFT : (left ? LCA_RIGHT : LCA_NONE));
1152 const bool changeToBest = (right && bestLaneOffset < 0) || (left && bestLaneOffset > 0) || (laneOffset == 0 && bestLaneOffset == 0);
1153 // keep information about being a leader/follower but remove information
1154 // about previous lane change request or urgency
1155 int ret = (myOwnState & 0xffff0000);
1156
1157 // compute the distance when changing to the neighboring lane
1158 // (ensure we do not lap into the line behind neighLane since there might be unseen blockers)
1159 // minimum distance to move the vehicle fully onto the new lane
1160 double latLaneDist = laneOffset == 0 ? 0. : myVehicle.lateralDistanceToLane(laneOffset);
1161
1162 // VARIANT_5 (disableAMBACKBLOCKER1)
1163 /*
1164 if (leader.first != 0
1165 && (myOwnState & LCA_AMBLOCKINGFOLLOWER_DONTBRAKE) != 0
1166 && (leader.first->getLaneChangeModel().getOwnState() & LCA_AMBLOCKINGFOLLOWER_DONTBRAKE) != 0) {
1167
1168 myOwnState &= (0xffffffff - LCA_AMBLOCKINGFOLLOWER_DONTBRAKE);
1169 if (myVehicle.getSpeed() > SUMO_const_haltingSpeed) {
1170 myOwnState |= LCA_AMBACKBLOCKER;
1171 } else {
1172 ret |= LCA_AMBACKBLOCKER;
1173 myDontBrake = true;
1174 }
1175 }
1176 */
1177
1178#ifdef DEBUG_WANTSCHANGE
1179 if (gDebugFlag2) {
1180 std::cout << STEPS2TIME(currentTime)
1181 << " veh=" << myVehicle.getID()
1182 << " myState=" << toString((LaneChangeAction)myOwnState)
1183 << " firstBlocked=" << Named::getIDSecure(*firstBlocked)
1184 << " lastBlocked=" << Named::getIDSecure(*lastBlocked)
1185 << "\n leaders=" << leaders.toString()
1186 << "\n followers=" << followers.toString()
1187 << "\n blockers=" << blockers.toString()
1188 << "\n neighLeaders=" << neighLeaders.toString()
1189 << "\n neighFollowers=" << neighFollowers.toString()
1190 << "\n neighBlockers=" << neighBlockers.toString()
1191 << "\n changeToBest=" << changeToBest
1192 << " latLaneDist=" << latLaneDist
1193 << "\n expectedSpeeds=" << toString(myExpectedSublaneSpeeds)
1194 << std::endl;
1195 }
1196#endif
1197
1198 ret = slowDownForBlocked(lastBlocked, ret);
1199 // VARIANT_14 (furtherBlock)
1200 if (lastBlocked != firstBlocked) {
1201 ret = slowDownForBlocked(firstBlocked, ret);
1202 }
1203
1204
1205 // we try to estimate the distance which is necessary to get on a lane
1206 // we have to get on in order to keep our route
1207 // we assume we need something that depends on our velocity
1208 // and compare this with the free space on our wished lane
1209 //
1210 // if the free space is somehow less than the space we need, we should
1211 // definitely try to get to the desired lane
1212 //
1213 // this rule forces our vehicle to change the lane if a lane changing is necessary soon
1214 // lookAheadDistance:
1215 // we do not want the lookahead distance to change all the time so we discrectize the speed a bit
1216
1217 // VARIANT_18 (laHyst)
1220 } else {
1221 // FIXME: This strongly dependent on the value of TS, see LC2013 for the fix (l.1153, currently)
1224 }
1225 //myLookAheadSpeed = myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle);
1226
1227 //double laDist = laSpeed > LOOK_FORWARD_SPEED_DIVIDER
1228 // ? laSpeed * LOOK_FORWARD_FAR
1229 // : laSpeed * LOOK_FORWARD_NEAR;
1230 double laDist = myLookAheadSpeed * LOOK_FORWARD * myStrategicParam * (right ? 1 : myLookaheadLeft);
1231 laDist += myVehicle.getVehicleType().getLengthWithGap() * 2.;
1232 // aggressive drivers may elect to use reduced strategic lookahead to optimize speed
1233 /*
1234 if (mySpeedGainProbabilityRight > myChangeProbThresholdRight
1235 || mySpeedGainProbabilityLeft > myChangeProbThresholdLeft) {
1236 laDist *= MAX2(0.0, (1 - myPushy));
1237 laDist *= MAX2(0,0, (1 - myAssertive));
1238 laDist *= MAX2(0,0, (2 - mySpeedGainParam));
1239 }
1240 */
1241
1242 // react to a stopped leader on the current lane
1243 if (bestLaneOffset == 0 && leaders.hasStoppedVehicle()) {
1244 // value is doubled for the check since we change back and forth
1245 // laDist = 0.5 * (myVehicle.getVehicleType().getLengthWithGap() + leader.first->getVehicleType().getLengthWithGap());
1246 // XXX determine length of longest stopped vehicle
1248 } else if (checkOpposite && isOpposite() && neighLeaders.hasStoppedVehicle()) {
1249 // compute exact distance to overtake stopped vehicle
1250 laDist = 0;
1251 for (int i = 0; i < neighLeaders.numSublanes(); ++i) {
1252 CLeaderDist vehDist = neighLeaders[i];
1253 if (vehDist.first != nullptr && vehDist.first->isStopped()) {
1254 laDist = MAX2(laDist, myVehicle.getVehicleType().getMinGap() + vehDist.second + vehDist.first->getVehicleType().getLengthWithGap());
1255 }
1256 }
1257 laDist += myVehicle.getVehicleType().getLength();
1258 }
1259 if (myStrategicParam < 0) {
1260 laDist = -1e3; // never perform strategic change
1261 }
1262
1263 // free space that is available for changing
1264 //const double neighSpeed = (neighLead.first != 0 ? neighLead.first->getSpeed() :
1265 // neighFollow.first != 0 ? neighFollow.first->getSpeed() :
1266 // best.lane->getSpeedLimit());
1267 // @note: while this lets vehicles change earlier into the correct direction
1268 // it also makes the vehicles more "selfish" and prevents changes which are necessary to help others
1269
1270 const double roundaboutBonus = MSLCHelper::getRoundaboutDistBonus(myVehicle, myRoundaboutBonus, curr, neigh, best);
1271 currentDist += roundaboutBonus;
1272 neighDist += roundaboutBonus;
1273
1274 if (laneOffset != 0) {
1275 ret = checkStrategicChange(ret,
1276 neighLane,
1277 laneOffset,
1278 leaders,
1279 neighLeaders,
1280 curr, neigh, best,
1281 bestLaneOffset,
1282 changeToBest,
1283 currentDist,
1284 neighDist,
1285 laDist,
1286 roundaboutBonus,
1287 latLaneDist,
1288 checkOpposite,
1289 latDist);
1290 }
1291
1292 if ((ret & LCA_STAY) != 0 && latDist == 0) {
1293 // ensure that mySafeLatDistLeft / mySafeLatDistRight are up to date for the
1294 // subsquent check with laneOffset = 0
1295 const double center = myVehicle.getCenterOnEdge();
1296 const double neighRight = getNeighRight(neighLane);
1297 updateGaps(neighLeaders, neighRight, center, 1.0, mySafeLatDistRight, mySafeLatDistLeft);
1298 updateGaps(neighFollowers, neighRight, center, 1.0, mySafeLatDistRight, mySafeLatDistLeft);
1299 // remove TraCI flags because it should not be included in "state-without-traci"
1300 ret = getCanceledState(laneOffset);
1301 return ret;
1302 }
1303 if ((ret & LCA_URGENT) != 0) {
1304 // prepare urgent lane change maneuver
1305 if (changeToBest && abs(bestLaneOffset) > 1
1306 && curr.bestContinuations.back()->getLinkCont().size() != 0
1307 ) {
1308 // there might be a vehicle which needs to counter-lane-change one lane further and we cannot see it yet
1309 const double reserve = MIN2(myLeftSpace - MAGIC_OFFSET - myVehicle.getVehicleType().getMinGap(), right ? 20.0 : 40.0);
1311#ifdef DEBUG_WANTSCHANGE
1312 if (gDebugFlag2) {
1313 std::cout << " reserving space for unseen blockers myLeadingBlockerLength=" << myLeadingBlockerLength << "\n";
1314 }
1315#endif
1316 }
1317
1318 // letting vehicles merge in at the end of the lane in case of counter-lane change, step#1
1319 // if there is a leader and he wants to change to the opposite direction
1320 MSVehicle* neighLeadLongest = const_cast<MSVehicle*>(getLongest(neighLeaders).first);
1321 const bool canContinue = curr.bestContinuations.size() > 1;
1322#ifdef DEBUG_WANTSCHANGE
1323 if (DEBUG_COND) {
1324 std::cout << SIMTIME << " veh=" << myVehicle.getID() << " neighLeaders=" << neighLeaders.toString() << " longest=" << Named::getIDSecure(neighLeadLongest) << " firstBlocked=" << Named::getIDSecure(*firstBlocked) << "\n";
1325 }
1326#endif
1327 bool canReserve = MSLCHelper::saveBlockerLength(myVehicle, neighLeadLongest, lcaCounter, myLeftSpace, canContinue, myLeadingBlockerLength);
1328 if (*firstBlocked != neighLeadLongest && tieBrakeLeader(*firstBlocked)) {
1329 canReserve &= MSLCHelper::saveBlockerLength(myVehicle, *firstBlocked, lcaCounter, myLeftSpace, canContinue, myLeadingBlockerLength);
1330 }
1331 if (!canReserve && !isOpposite()) {
1332 // we have a low-priority relief connection
1333 // std::cout << SIMTIME << " veh=" << myVehicle.getID() << " cannotReserve for blockers\n";
1334 myDontBrake = canContinue;
1335 }
1336
1337 std::vector<CLeaderDist> collectLeadBlockers;
1338 std::vector<CLeaderDist> collectFollowBlockers;
1339 int blockedFully = 0; // wether execution of the full maneuver is blocked
1340 maneuverDist = latDist;
1341 const double gapFactor = computeGapFactor(LCA_STRATEGIC);
1342 blocked = checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1343 leaders, followers, blockers,
1344 neighLeaders, neighFollowers, neighBlockers, &collectLeadBlockers, &collectFollowBlockers,
1345 false, gapFactor, &blockedFully);
1346
1347 const double absLaneOffset = fabs(bestLaneOffset != 0 ? bestLaneOffset : latDist / SUMO_const_laneWidth);
1348 const double remainingSeconds = ((ret & LCA_TRACI) == 0 ?
1349 MAX2(STEPS2TIME(TS), myLeftSpace / MAX2(myLookAheadSpeed, NUMERICAL_EPS) / absLaneOffset / URGENCY) :
1351 const double plannedSpeed = informLeaders(blocked, myLca, collectLeadBlockers, remainingSeconds);
1352 // coordinate with direct obstructions
1353 if (plannedSpeed >= 0) {
1354 // maybe we need to deal with a blocking follower
1355 informFollowers(blocked, myLca, collectFollowBlockers, remainingSeconds, plannedSpeed);
1356 }
1357 if (plannedSpeed > 0) {
1358 commitManoeuvre(blocked, blockedFully, leaders, neighLeaders, neighLane, maneuverDist);
1359 }
1360#if defined(DEBUG_WANTSCHANGE) || defined(DEBUG_STATE)
1361 if (gDebugFlag2) {
1362 std::cout << STEPS2TIME(currentTime)
1363 << " veh=" << myVehicle.getID()
1364 << " myLeftSpace=" << myLeftSpace
1365 << " changeFully=" << myCanChangeFully
1366 << " blockedFully=" << toString((LaneChangeAction)blockedFully)
1367 << " remainingSeconds=" << remainingSeconds
1368 << " plannedSpeed=" << plannedSpeed
1369 << " mySafeLatDistRight=" << mySafeLatDistRight
1370 << " mySafeLatDistLeft=" << mySafeLatDistLeft
1371 << "\n";
1372 }
1373#endif
1374 // remove TraCI flags because it should not be included in "state-without-traci"
1375 ret = getCanceledState(laneOffset);
1376 return ret;
1377 }
1378 // VARIANT_15
1379 if (roundaboutBonus > 0) {
1380
1381#ifdef DEBUG_WANTS_CHANGE
1382 if (DEBUG_COND) {
1383 std::cout << STEPS2TIME(currentTime)
1384 << " veh=" << myVehicle.getID()
1385 << " roundaboutBonus=" << roundaboutBonus
1386 << " myLeftSpace=" << myLeftSpace
1387 << "\n";
1388 }
1389#endif
1390 // try to use the inner lanes of a roundabout to increase throughput
1391 // unless we are approaching the exit
1392 if (left) {
1393 ret |= LCA_COOPERATIVE;
1394 if (!cancelRequest(ret | LCA_LEFT, laneOffset)) {
1395 if ((ret & LCA_STAY) == 0) {
1396 latDist = latLaneDist;
1397 maneuverDist = latLaneDist;
1398 blocked = checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1399 leaders, followers, blockers,
1400 neighLeaders, neighFollowers, neighBlockers);
1401 }
1402 return ret;
1403 } else {
1404 ret &= ~LCA_COOPERATIVE;
1405 }
1406 } else {
1408 }
1409 }
1410
1411 // --------
1412
1413 // -------- make place on current lane if blocking follower
1414 //if (amBlockingFollowerPlusNB()) {
1415 // std::cout << myVehicle.getID() << ", " << currentDistAllows(neighDist, bestLaneOffset, laDist)
1416 // << " neighDist=" << neighDist
1417 // << " currentDist=" << currentDist
1418 // << "\n";
1419 //}
1420 const double inconvenience = (latLaneDist < 0
1423#ifdef DEBUG_COOPERATE
1424 if (gDebugFlag2) {
1425 std::cout << STEPS2TIME(currentTime)
1426 << " veh=" << myVehicle.getID()
1427 << " amBlocking=" << amBlockingFollowerPlusNB()
1428 << " state=" << toString((LaneChangeAction)myOwnState)
1429 << " myLca=" << toString((LaneChangeAction)myLca)
1430 << " prevState=" << toString((LaneChangeAction)myPreviousState)
1431 << " inconvenience=" << inconvenience
1432 << " origLatDist=" << getManeuverDist()
1433 << " wantsChangeToHelp=" << (right ? "right" : "left")
1434 << " state=" << myOwnState
1435 << "\n";
1436 }
1437#endif
1438
1439 if (laneOffset != 0
1441 // VARIANT_6 : counterNoHelp
1442 && ((myOwnState & myLca) != 0))
1443 ||
1444 // continue previous cooperative change
1447 // change is in the right direction
1448 && (laneOffset * getManeuverDist() > 0)))
1449 && (inconvenience < myCooperativeParam)
1450 && (changeToBest || currentDistAllows(neighDist, abs(bestLaneOffset) + 1, laDist))) {
1451
1452 // VARIANT_2 (nbWhenChangingToHelp)
1453#ifdef DEBUG_COOPERATE
1454 if (gDebugFlag2) {
1455 std::cout << " wants cooperative change\n";
1456 }
1457#endif
1458
1459 ret |= LCA_COOPERATIVE | LCA_URGENT ;//| LCA_CHANGE_TO_HELP;
1460 if (!cancelRequest(ret | getLCA(ret, latLaneDist), laneOffset)) {
1461 latDist = amBlockingFollowerPlusNB() ? latLaneDist : getManeuverDist();
1462 maneuverDist = latDist;
1463 blocked = checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1464 leaders, followers, blockers,
1465 neighLeaders, neighFollowers, neighBlockers);
1466 return ret;
1467 } else {
1468 ret &= ~(LCA_COOPERATIVE | LCA_URGENT);
1469 }
1470 }
1471
1472 // --------
1473
1474
1477 //if ((blocked & LCA_BLOCKED) != 0) {
1478 // return ret;
1479 //}
1481
1482 // -------- higher speed
1483 //if ((congested(neighLead.first) && neighLead.second < 20) || predInteraction(leader.first)) { //!!!
1484 // return ret;
1485 //}
1486
1487 // iterate over all possible combinations of sublanes this vehicle might cover and check the potential speed
1488 const MSEdge& edge = (isOpposite() ? myVehicle.getLane()->getParallelOpposite() : myVehicle.getLane())->getEdge();
1489 const std::vector<double>& sublaneSides = edge.getSubLaneSides();
1490 assert(sublaneSides.size() == myExpectedSublaneSpeeds.size());
1491 const double vehWidth = getWidth();
1492 const double rightVehSide = getVehicleCenter() - 0.5 * vehWidth;
1493 const double leftVehSide = rightVehSide + vehWidth;
1494 // figure out next speed when staying where we are
1495 double defaultNextSpeed = std::numeric_limits<double>::max();
1497 int leftmostOnEdge = (int)sublaneSides.size() - 1;
1498 while (leftmostOnEdge > 0 && sublaneSides[leftmostOnEdge] > leftVehSide) {
1499 leftmostOnEdge--;
1500 }
1501 int rightmostOnEdge = leftmostOnEdge;
1502 while (rightmostOnEdge > 0 && sublaneSides[rightmostOnEdge] > rightVehSide + NUMERICAL_EPS) {
1503 defaultNextSpeed = MIN2(defaultNextSpeed, myExpectedSublaneSpeeds[rightmostOnEdge]);
1504#ifdef DEBUG_WANTSCHANGE
1505 if (gDebugFlag2) {
1506 std::cout << " adapted to current sublane=" << rightmostOnEdge << " defaultNextSpeed=" << defaultNextSpeed << "\n";
1507 std::cout << " sublaneSides[rightmostOnEdge]=" << sublaneSides[rightmostOnEdge] << " rightVehSide=" << rightVehSide << "\n";
1508 }
1509#endif
1510 rightmostOnEdge--;
1511 }
1512 defaultNextSpeed = MIN2(defaultNextSpeed, myExpectedSublaneSpeeds[rightmostOnEdge]);
1513#ifdef DEBUG_WANTSCHANGE
1514 if (gDebugFlag2) {
1515 std::cout << " adapted to current sublane=" << rightmostOnEdge << " defaultNextSpeed=" << defaultNextSpeed << "\n";
1516 std::cout << " sublaneSides[rightmostOnEdge]=" << sublaneSides[rightmostOnEdge] << " rightVehSide=" << rightVehSide << "\n";
1517 }
1518#endif
1519 double maxGain = -std::numeric_limits<double>::max();
1520 double maxGainRight = -std::numeric_limits<double>::max();
1521 double maxGainLeft = -std::numeric_limits<double>::max();
1522 double latDistNice = std::numeric_limits<double>::max();
1523
1524 const int iMin = MIN2(myVehicle.getLane()->getRightmostSublane(), neighLane.getRightmostSublane());
1525 double leftMax = MAX2(
1527 neighLane.getRightSideOnEdge() + neighLane.getWidth());
1528 double rightMin = MIN2(myVehicle.getLane()->getRightSideOnEdge(), neighLane.getRightSideOnEdge());
1529 if (checkOpposite || isOpposite()) {
1530 leftMax = getLeftBorder();
1531 } else {
1532 assert(leftMax <= edge.getWidth());
1533 }
1534 int sublaneCompact = MAX2(iMin, rightmostOnEdge - 1); // try to compactify to the right by default
1535
1536 const double laneBoundary = laneOffset < 0 ? myVehicle.getLane()->getRightSideOnEdge() : neighLane.getRightSideOnEdge();
1537 // if there is a neighboring lane we could change to, check sublanes on all lanes of the edge
1538 // but restrict maneuver to the currently visible lanes (current, neigh) to ensure safety
1539 // This way we can discover a fast lane beyond the immediate neighbor lane
1540 const double maxLatDist = leftMax - leftVehSide;
1541 const double minLatDist = rightMin - rightVehSide;
1542 const int iStart = laneOffset == 0 ? iMin : 0;
1543 const double rightEnd = laneOffset == 0 ? leftMax : (checkOpposite ? getLeftBorder() : edge.getWidth());
1544#ifdef DEBUG_WANTSCHANGE
1545 if (gDebugFlag2) std::cout
1546 << " checking sublanes rightmostOnEdge=" << rightmostOnEdge
1547 << " rightEnd=" << rightEnd
1548 << " leftmostOnEdge=" << leftmostOnEdge
1549 << " iStart=" << iStart
1550 << " iMin=" << iMin
1551 << " sublaneSides=" << sublaneSides.size()
1552 << " leftMax=" << leftMax
1553 << " minLatDist=" << minLatDist
1554 << " maxLatDist=" << maxLatDist
1555 << " sublaneCompact=" << sublaneCompact
1556 << "\n";
1557#endif
1558 for (int i = iStart; i < (int)sublaneSides.size(); ++i) {
1559 if (sublaneSides[i] + vehWidth < rightEnd) {
1560 // i is the rightmost sublane and the left side of vehicles still fits on the edge,
1561 // compute min speed of all sublanes covered by the vehicle in this case
1562 double vMin = myExpectedSublaneSpeeds[i];
1563 //std::cout << " i=" << i << "\n";
1564 int j = i;
1565 while (vMin > 0 && j < (int)sublaneSides.size() && sublaneSides[j] < sublaneSides[i] + vehWidth) {
1566 vMin = MIN2(vMin, myExpectedSublaneSpeeds[j]);
1567 //std::cout << " j=" << j << " vMin=" << vMin << " sublaneSides[j]=" << sublaneSides[j] << " leftVehSide=" << leftVehSide << " rightVehSide=" << rightVehSide << "\n";
1568 ++j;
1569 }
1570 // check whether the vehicle is between lanes
1571 if (laneOffset != 0 && overlap(sublaneSides[i], sublaneSides[i] + vehWidth, laneBoundary, laneBoundary)) {
1572 vMin *= (1 - myLaneDiscipline);
1573 }
1574 double relativeGain = (vMin - defaultNextSpeed) / MAX2(vMin, RELGAIN_NORMALIZATION_MIN_SPEED);
1575 const double currentLatDist = MIN2(MAX2(sublaneSides[i] - rightVehSide, minLatDist), maxLatDist);
1576 if (currentLatDist > 0 && myVehicle.getLane()->getBidiLane() != nullptr) {
1577 // penalize overtaking on the left if the lane is used in both
1578 // directions
1579 relativeGain *= 0.5;
1580 }
1581 // @note this is biased for changing to the left since we compare the sublanes in ascending order
1582 if (relativeGain > maxGain) {
1583 maxGain = relativeGain;
1584 if (maxGain > GAIN_PERCEPTION_THRESHOLD) {
1585 sublaneCompact = i;
1586 latDist = currentLatDist;
1587#ifdef DEBUG_WANTSCHANGE
1588 if (gDebugFlag2) {
1589 std::cout << " i=" << i << " newLatDist=" << latDist << " relGain=" << relativeGain << "\n";
1590 }
1591#endif
1592 }
1593 } else {
1594 // if anticipated gains to the left are higher then to the right and current gains are equal, prefer left
1595 if (currentLatDist > 0
1596 //&& latDist < 0 // #7184 compensates for #7185
1598 && relativeGain > GAIN_PERCEPTION_THRESHOLD
1599 && maxGain - relativeGain < NUMERICAL_EPS) {
1600 latDist = currentLatDist;
1601 }
1602 }
1603#ifdef DEBUG_WANTSCHANGE
1604 if (gDebugFlag2) {
1605 std::cout << " i=" << i << " rightmostOnEdge=" << rightmostOnEdge << " vMin=" << vMin << " relGain=" << relativeGain << " sublaneCompact=" << sublaneCompact << " curLatDist=" << currentLatDist << "\n";
1606 }
1607#endif
1608 if (currentLatDist < -NUMERICAL_EPS * myVehicle.getActionStepLengthSecs()) {
1609 maxGainRight = MAX2(maxGainRight, relativeGain);
1610 } else if (currentLatDist > NUMERICAL_EPS * myVehicle.getActionStepLengthSecs()) {
1611 maxGainLeft = MAX2(maxGainLeft, relativeGain);
1612 }
1613 const double subAlignDist = sublaneSides[i] - rightVehSide;
1614 if (fabs(subAlignDist) < fabs(latDistNice)) {
1615 latDistNice = subAlignDist;
1616#ifdef DEBUG_WANTSCHANGE
1617 if (gDebugFlag2) std::cout
1618 << " nicest sublane=" << i
1619 << " side=" << sublaneSides[i]
1620 << " rightSide=" << rightVehSide
1621 << " latDistNice=" << latDistNice
1622 << " maxGainR=" << maxGainRight
1623 << " maxGainL=" << maxGainLeft
1624 << "\n";
1625#endif
1626 }
1627 }
1628 }
1629 // updated change probabilities
1630 if (maxGainRight != -std::numeric_limits<double>::max()) {
1631#ifdef DEBUG_WANTSCHANGE
1632 if (gDebugFlag2) {
1633 std::cout << " speedGainR_old=" << mySpeedGainProbabilityRight;
1634 }
1635#endif
1637#ifdef DEBUG_WANTSCHANGE
1638 if (gDebugFlag2) {
1639 std::cout << " speedGainR_new=" << mySpeedGainProbabilityRight << "\n";
1640 }
1641#endif
1642 }
1643 if (maxGainLeft != -std::numeric_limits<double>::max()) {
1644#ifdef DEBUG_WANTSCHANGE
1645 if (gDebugFlag2) {
1646 std::cout << " speedGainL_old=" << mySpeedGainProbabilityLeft;
1647 }
1648#endif
1650#ifdef DEBUG_WANTSCHANGE
1651 if (gDebugFlag2) {
1652 std::cout << " speedGainL_new=" << mySpeedGainProbabilityLeft << "\n";
1653 }
1654#endif
1655 }
1656 // decay if there is no reason for or against changing (only if we have enough information)
1657 if ((fabs(maxGainRight) < NUMERICAL_EPS || maxGainRight == -std::numeric_limits<double>::max())
1658 && (right || (alternatives & LCA_RIGHT) == 0)) {
1660 }
1661 if ((fabs(maxGainLeft) < NUMERICAL_EPS || maxGainLeft == -std::numeric_limits<double>::max())
1662 && (left || (alternatives & LCA_LEFT) == 0)) {
1664 }
1665
1666
1667#ifdef DEBUG_WANTSCHANGE
1668 if (gDebugFlag2) std::cout << SIMTIME
1669 << " veh=" << myVehicle.getID()
1670 << " defaultNextSpeed=" << defaultNextSpeed
1671 << " maxGain=" << maxGain
1672 << " maxGainRight=" << maxGainRight
1673 << " maxGainLeft=" << maxGainLeft
1674 << " latDist=" << latDist
1675 << " latDistNice=" << latDistNice
1676 << " sublaneCompact=" << sublaneCompact
1677 << "\n";
1678#endif
1679
1680 if (!left) {
1681 // ONLY FOR CHANGING TO THE RIGHT
1682 // start keepRight maneuver when no speed loss is expected and continue
1683 // started maneuvers if the loss isn't too big
1684 if (right && myVehicle.getSpeed() > 0 && (maxGainRight >= 0
1685 || ((myPreviousState & LCA_KEEPRIGHT) != 0 && maxGainRight >= -myKeepRightParam))) {
1686 // honor the obligation to keep right (Rechtsfahrgebot)
1687 const double vMax = myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle);
1688 const double roadSpeedFactor = vMax / myVehicle.getLane()->getSpeedLimit(); // differse from speedFactor if vMax < speedLimit
1689 double acceptanceTime;
1690 if (myKeepRightAcceptanceTime == -1) {
1691 // legacy behavior: scale acceptance time with current speed and
1692 // use old hard-coded constant
1693 acceptanceTime = 7 * roadSpeedFactor * MAX2(1.0, myVehicle.getSpeed());
1694 } else {
1695 acceptanceTime = myKeepRightAcceptanceTime * roadSpeedFactor;
1696 if (followers.hasVehicles()) {
1697 // reduce acceptanceTime if a follower vehicle is faster or wants to drive faster
1698 double minFactor = 1.0;
1699 for (int i = 0; i < followers.numSublanes(); ++i) {
1700 CLeaderDist follower = followers[i];
1701 if (follower.first != nullptr && follower.second < 2 * follower.first->getCarFollowModel().brakeGap(follower.first->getSpeed())) {
1702 if (follower.first->getSpeed() >= myVehicle.getSpeed()) {
1703 double factor = MAX2(1.0, myVehicle.getSpeed()) / MAX2(1.0, follower.first->getSpeed());
1704 const double fRSF = follower.first->getLane()->getVehicleMaxSpeed(follower.first) / follower.first->getLane()->getSpeedLimit();
1705 if (fRSF > roadSpeedFactor) {
1706 factor /= fRSF;
1707 }
1708 if (factor < minFactor) {
1709 minFactor = factor;
1710 }
1711 }
1712 }
1713 }
1714 acceptanceTime *= minFactor;
1715 }
1716 }
1717 double fullSpeedGap = MAX2(0., neighDist - myVehicle.getCarFollowModel().brakeGap(vMax));
1718 double fullSpeedDrivingSeconds = MIN2(acceptanceTime, fullSpeedGap / vMax);
1719 CLeaderDist neighLead = getSlowest(neighLeaders);
1720 if (neighLead.first != 0 && neighLead.first->getSpeed() < vMax) {
1721 fullSpeedGap = MAX2(0., MIN2(fullSpeedGap,
1722 neighLead.second - myVehicle.getCarFollowModel().getSecureGap(&myVehicle, neighLead.first,
1723 vMax, neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel())));
1724 fullSpeedDrivingSeconds = MIN2(fullSpeedDrivingSeconds, fullSpeedGap / (vMax - neighLead.first->getSpeed()));
1725 }
1726 const double deltaProb = (myChangeProbThresholdRight * (fullSpeedDrivingSeconds / acceptanceTime) / KEEP_RIGHT_TIME) * myVehicle.getActionStepLengthSecs();
1727 const bool isSlide = preventSliding(latLaneDist);
1728 // stay below threshold
1729 if (!isSlide || !wantsKeepRight(myKeepRightProbability + deltaProb)) {
1730 myKeepRightProbability += deltaProb;
1731 }
1732
1733#ifdef DEBUG_WANTSCHANGE
1734 if (gDebugFlag2) {
1735 std::cout << STEPS2TIME(currentTime)
1736 << " considering keepRight:"
1737 << " vMax=" << vMax
1738 << " neighDist=" << neighDist
1739 << " brakeGap=" << myVehicle.getCarFollowModel().brakeGap(myVehicle.getSpeed())
1740 << " leaderSpeed=" << (neighLead.first == 0 ? -1 : neighLead.first->getSpeed())
1741 << " secGap=" << (neighLead.first == 0 ? -1 : myVehicle.getCarFollowModel().getSecureGap(&myVehicle, neighLead.first,
1742 myVehicle.getSpeed(), neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()))
1743 << " acceptanceTime=" << acceptanceTime
1744 << " fullSpeedGap=" << fullSpeedGap
1745 << " fullSpeedDrivingSeconds=" << fullSpeedDrivingSeconds
1746 << " dProb=" << deltaProb
1747 << " isSlide=" << isSlide
1748 << " keepRight=" << myKeepRightProbability
1749 << " speedGainL=" << mySpeedGainProbabilityLeft
1750 << "\n";
1751 }
1752#endif
1754 /*&& latLaneDist <= -NUMERICAL_EPS * myVehicle.getActionStepLengthSecs()*/) {
1755 ret |= LCA_KEEPRIGHT;
1756 assert(myVehicle.getLane()->getIndex() > neighLane.getIndex() || isOpposite());
1757 if (!cancelRequest(ret | LCA_RIGHT, laneOffset)) {
1758 latDist = latLaneDist;
1759 maneuverDist = latLaneDist;
1760 blocked = checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1761 leaders, followers, blockers,
1762 neighLeaders, neighFollowers, neighBlockers);
1763 return ret;
1764 } else {
1765 ret &= ~LCA_KEEPRIGHT;
1766 }
1767 }
1768 }
1769
1770 const double bidiRightFactor = myVehicle.getLane()->getBidiLane() == nullptr ? 1 : 0.05;
1771#ifdef DEBUG_WANTSCHANGE
1772 if (gDebugFlag2) {
1773 std::cout << STEPS2TIME(currentTime)
1774 << " speedGainR=" << mySpeedGainProbabilityRight
1775 << " speedGainL=" << mySpeedGainProbabilityLeft
1776 << " neighDist=" << neighDist
1777 << " neighTime=" << neighDist / MAX2(.1, myVehicle.getSpeed())
1778 << " rThresh=" << myChangeProbThresholdRight
1779 << " rThresh2=" << myChangeProbThresholdRight* bidiRightFactor
1780 << " latDist=" << latDist
1781 << "\n";
1782 }
1783#endif
1784
1785 // make changing on the right more attractive on bidi edges
1786 if (latDist < 0 && mySpeedGainProbabilityRight >= MAX2(myChangeProbThresholdRight * bidiRightFactor, mySpeedGainProbabilityLeft)
1787 && neighDist / MAX2(.1, myVehicle.getSpeed()) > 20.) {
1788 ret |= LCA_SPEEDGAIN;
1789 if (!cancelRequest(ret | getLCA(ret, latDist), laneOffset)) {
1790 int blockedFully = 0;
1791 maneuverDist = latDist;
1792 blocked = checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1793 leaders, followers, blockers,
1794 neighLeaders, neighFollowers, neighBlockers,
1795 nullptr, nullptr, false, 0, &blockedFully);
1796 //commitManoeuvre(blocked, blockedFully, leaders, neighLeaders, neighLane);
1797 return ret;
1798 } else {
1799 // @note: restore ret so subsequent calls to cancelRequest work correctly
1800 latDist = 0;
1801 ret &= ~LCA_SPEEDGAIN;
1802 }
1803 }
1804 }
1805 if (!right || isOpposite()) {
1806
1807 const bool stayInLane = myVehicle.getLateralPositionOnLane() + latDist < 0.5 * myVehicle.getLane()->getWidth();
1808#ifdef DEBUG_WANTSCHANGE
1809 if (gDebugFlag2) {
1810 std::cout << STEPS2TIME(currentTime)
1811 << " speedGainL=" << mySpeedGainProbabilityLeft
1812 << " speedGainR=" << mySpeedGainProbabilityRight
1813 << " latDist=" << latDist
1814 << " neighDist=" << neighDist
1815 << " neighTime=" << neighDist / MAX2(.1, myVehicle.getSpeed())
1816 << " lThresh=" << myChangeProbThresholdLeft
1817 << " stayInLane=" << stayInLane
1818 << "\n";
1819 }
1820#endif
1821
1823 // if we leave our lane, we should be able to stay in the new
1824 // lane for some time
1825 (stayInLane || neighDist / MAX2(.1, myVehicle.getSpeed()) > SPEED_GAIN_MIN_SECONDS)) {
1826 ret |= LCA_SPEEDGAIN;
1827 if (!cancelRequest(ret + getLCA(ret, latDist), laneOffset)) {
1828 int blockedFully = 0;
1829 maneuverDist = latDist;
1830 blocked = checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1831 leaders, followers, blockers,
1832 neighLeaders, neighFollowers, neighBlockers,
1833 nullptr, nullptr, false, 0, &blockedFully);
1834 //commitManoeuvre(blocked, blockedFully, leaders, neighLeaders, neighLane);
1835 return ret;
1836 } else {
1837 latDist = 0;
1838 ret &= ~LCA_SPEEDGAIN;
1839 }
1840 }
1841 }
1842
1843 double latDistSublane = 0.;
1844 const double halfLaneWidth = myVehicle.getLane()->getWidth() * 0.5;
1845 const double halfVehWidth = getWidth() * 0.5;
1848 && bestLaneOffset == 0
1850 // vehicle is on its final edge, on the correct lane and close to
1851 // its arrival position. Change to the desired lateral position
1855 break;
1857 latDistSublane = -halfLaneWidth + halfVehWidth - myVehicle.getLateralPositionOnLane();
1858 break;
1860 latDistSublane = -myVehicle.getLateralPositionOnLane();
1861 break;
1863 latDistSublane = halfLaneWidth - halfVehWidth - myVehicle.getLateralPositionOnLane();
1864 break;
1865 default:
1866 assert(false);
1867 }
1868#ifdef DEBUG_WANTSCHANGE
1869 if (gDebugFlag2) std::cout << SIMTIME
1870 << " arrivalPosLatProcedure=" << (int)myVehicle.getParameter().arrivalPosLatProcedure
1871 << " arrivalPosLat=" << myVehicle.getParameter().arrivalPosLat << "\n";
1872#endif
1873
1874 } else {
1875
1877 // Check whether the vehicle should adapt its alignment to an upcoming turn
1878 if (myTurnAlignmentDist > 0) {
1879 const std::pair<double, LinkDirection>& turnInfo = myVehicle.getNextTurn();
1880 if (turnInfo.first < myTurnAlignmentDist) {
1881 // Vehicle is close enough to the link to change its default alignment
1882 switch (turnInfo.second) {
1887 break;
1892 break;
1895 default:
1896 break;
1897 }
1898 }
1899 }
1900 switch (align) {
1902 latDistSublane = -halfLaneWidth + halfVehWidth - getPosLat();
1903 break;
1905 latDistSublane = halfLaneWidth - halfVehWidth - getPosLat();
1906 break;
1909 latDistSublane = -getPosLat();
1910 break;
1912 latDistSublane = latDistNice;
1913 break;
1915 latDistSublane = sublaneSides[sublaneCompact] - rightVehSide;
1916 break;
1918 latDistSublane = myVehicle.getLateralPositionOnLane() - getPosLat();
1919 break;
1921 // sublane alignment should not cause the vehicle to leave the lane
1922 const double hw = myVehicle.getLane()->getWidth() / 2 - NUMERICAL_EPS;
1923 const double offset = MAX2(-hw, MIN2(hw, myVehicle.getVehicleType().getPreferredLateralAlignmentOffset()));
1924 latDistSublane = -getPosLat() + offset;
1925 }
1926 break;
1927 default:
1928 break;
1929 }
1930 }
1931 // only factor in preferred lateral alignment if there is no speedGain motivation or it runs in the same direction
1932 if (fabs(latDist) <= NUMERICAL_EPS * myVehicle.getActionStepLengthSecs() ||
1933 latDistSublane * latDist > 0) {
1934
1935#if defined(DEBUG_WANTSCHANGE) || defined(DEBUG_STATE) || defined(DEBUG_MANEUVER)
1936 if (gDebugFlag2) std::cout << SIMTIME
1938 << " mySpeedGainR=" << mySpeedGainProbabilityRight
1939 << " mySpeedGainL=" << mySpeedGainProbabilityLeft
1940 << " latDist=" << latDist
1941 << " latDistSublane=" << latDistSublane
1942 << " relGainSublane=" << computeSpeedGain(latDistSublane, defaultNextSpeed)
1943 << " maneuverDist=" << maneuverDist
1944 << " myCanChangeFully=" << myCanChangeFully
1945 << " myTurnAlignmentDist=" << myTurnAlignmentDist
1946 << " nextTurn=" << myVehicle.getNextTurn().first << ":" << toString(myVehicle.getNextTurn().second)
1947 << " prevState=" << toString((LaneChangeAction)myPreviousState)
1948 << "\n";
1949#endif
1950
1951 if ((latDistSublane < 0 && mySpeedGainProbabilityRight < mySpeedLossProbThreshold)
1952 || (latDistSublane > 0 && mySpeedGainProbabilityLeft < mySpeedLossProbThreshold)
1953 || computeSpeedGain(latDistSublane, defaultNextSpeed) < -mySublaneParam) {
1954 // do not risk losing speed
1955#if defined(DEBUG_WANTSCHANGE)
1956 if (gDebugFlag2) std::cout << " aborting sublane change to avoid speed loss (mySpeedLossProbThreshold=" << mySpeedLossProbThreshold
1957 << " speedGain=" << computeSpeedGain(latDistSublane, defaultNextSpeed) << ")\n";
1958#endif
1959 latDistSublane = 0;
1960 }
1961 // Ignore preferred lateral alignment if we are in the middle of an unfinished non-alignment maneuver into the opposite direction
1962 if (!myCanChangeFully
1964 && ((getManeuverDist() < 0 && latDistSublane > 0) || (getManeuverDist() > 0 && latDistSublane < 0))) {
1965#if defined(DEBUG_WANTSCHANGE)
1966 if (gDebugFlag2) {
1967 std::cout << " aborting sublane change due to prior maneuver\n";
1968 }
1969#endif
1970 latDistSublane = 0;
1971 }
1972 latDist = latDistSublane * (isOpposite() ? -1 : 1);
1973 // XXX first compute preferred adaptation and then override with speed
1974 // (this way adaptation is still done if changing for speedgain is
1975 // blocked)
1976 if (fabs(latDist) >= NUMERICAL_EPS * myVehicle.getActionStepLengthSecs()) {
1977#ifdef DEBUG_WANTSCHANGE
1978 if (gDebugFlag2) std::cout << SIMTIME
1979 << " adapting to preferred alignment=" << toString(myVehicle.getVehicleType().getPreferredLateralAlignment())
1980 << " latDist=" << latDist
1981 << "\n";
1982#endif
1983 ret |= LCA_SUBLANE;
1984 // include prior motivation when sublane-change is part of finishing an ongoing maneuver in the same direction
1985 if (getPreviousManeuverDist() * latDist > 0) {
1986 int priorReason = (myPreviousState & LCA_CHANGE_REASONS & ~LCA_SUBLANE);
1987 ret |= priorReason;
1988#ifdef DEBUG_WANTSCHANGE
1989 if (gDebugFlag2 && priorReason != 0) std::cout << " including prior reason " << toString((LaneChangeAction)priorReason)
1990 << " prevManeuverDist=" << getPreviousManeuverDist() << "\n";
1991#endif
1992 }
1993 if (!cancelRequest(ret + getLCA(ret, latDist), laneOffset)) {
1994 maneuverDist = latDist;
1995 blocked = checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1996 leaders, followers, blockers,
1997 neighLeaders, neighFollowers, neighBlockers);
1998 return ret;
1999 } else {
2000 ret &= ~LCA_SUBLANE;
2001 }
2002 } else {
2003 return ret | LCA_SUBLANE | LCA_STAY;
2004 }
2005 }
2006 latDist = 0;
2007
2008
2009 // --------
2010 /*
2011 if (changeToBest && bestLaneOffset == curr.bestLaneOffset && laneOffset != 0
2012 && (right
2013 ? mySpeedGainProbabilityRight > MAX2(0., mySpeedGainProbabilityLeft)
2014 : mySpeedGainProbabilityLeft > MAX2(0., mySpeedGainProbabilityRight))) {
2015 // change towards the correct lane, speedwise it does not hurt
2016 ret |= LCA_STRATEGIC;
2017 if (!cancelRequest(ret, laneOffset)) {
2018 latDist = latLaneDist;
2019 blocked = checkBlocking(neighLane, latDist, laneOffset,
2020 leaders, followers, blockers,
2021 neighLeaders, neighFollowers, neighBlockers);
2022 return ret;
2023 }
2024 }
2025 */
2026#ifdef DEBUG_WANTSCHANGE
2027 if (gDebugFlag2) {
2028 std::cout << STEPS2TIME(currentTime)
2029 << " veh=" << myVehicle.getID()
2030 << " mySpeedGainR=" << mySpeedGainProbabilityRight
2031 << " mySpeedGainL=" << mySpeedGainProbabilityLeft
2032 << " myKeepRight=" << myKeepRightProbability
2033 << "\n";
2034 }
2035#endif
2036 return ret;
2037}
2038
2039
2040int
2042 // if this vehicle is blocking someone in front, we maybe decelerate to let him in
2043 if ((*blocked) != nullptr) {
2044 double gap = (*blocked)->getPositionOnLane() - (*blocked)->getVehicleType().getLength() - myVehicle.getPositionOnLane() - myVehicle.getVehicleType().getMinGap();
2045#ifdef DEBUG_SLOWDOWN
2046 if (gDebugFlag2) {
2047 std::cout << SIMTIME
2048 << " veh=" << myVehicle.getID()
2049 << " blocked=" << Named::getIDSecure(*blocked)
2050 << " gap=" << gap
2051 << "\n";
2052 }
2053#endif
2054 if (gap > POSITION_EPS) {
2055 //const bool blockedWantsUrgentRight = (((*blocked)->getLaneChangeModel().getOwnState() & LCA_RIGHT != 0)
2056 // && ((*blocked)->getLaneChangeModel().getOwnState() & LCA_URGENT != 0));
2057
2059 //|| blockedWantsUrgentRight // VARIANT_10 (helpblockedRight)
2060 ) {
2061 if ((*blocked)->getSpeed() < SUMO_const_haltingSpeed) {
2063 } else {
2064 state |= LCA_AMBACKBLOCKER;
2065 }
2066 addLCSpeedAdvice(getCarFollowModel().followSpeed(
2068 (gap - POSITION_EPS), (*blocked)->getSpeed(),
2069 (*blocked)->getCarFollowModel().getMaxDecel()));
2070 //(*blocked) = 0; // VARIANT_14 (furtherBlock)
2071 }
2072 }
2073 }
2074 return state;
2075}
2076
2077
2078void MSLCM_SL2015::addLCSpeedAdvice(const double vSafe) {
2079 const double accel = SPEED2ACCEL(vSafe - myVehicle.getSpeed());
2080 myLCAccelerationAdvices.push_back(accel);
2081#ifdef DEBUG_INFORM
2082 if (DEBUG_COND) {
2083 std::cout << SIMTIME << " veh=" << myVehicle.getID() << " accepted LC speed advice "
2084 << "vSafe=" << vSafe << " -> accel=" << accel << "\n";
2085 }
2086#endif
2087}
2088
2089bool
2090MSLCM_SL2015::isBidi(const MSLane* lane) const {
2091 if (lane == myVehicle.getLane()->getBidiLane()) {
2092 return true;
2093 }
2094 for (const MSLane* cand : myVehicle.getBestLanesContinuation()) {
2095 if (cand != nullptr && cand->getBidiLane() == lane) {
2096 return true;
2097 }
2098 }
2099 return false;
2100}
2101
2102void
2103MSLCM_SL2015::updateExpectedSublaneSpeeds(const MSLeaderDistanceInfo& ahead, int sublaneOffset, int laneIndex) {
2104 const std::vector<MSLane*>& lanes = myVehicle.getLane()->getEdge().getLanes();
2105 const std::vector<MSVehicle::LaneQ>& preb = myVehicle.getBestLanes();
2106 const MSLane* lane = isOpposite() ? myVehicle.getLane()->getParallelOpposite() : lanes[laneIndex];
2107 const MSLane* bidi = myVehicle.getLane()->getBidiLane();
2108 const double vMax = lane->getVehicleMaxSpeed(&myVehicle);
2109 assert(preb.size() == lanes.size() || isOpposite());
2110#ifdef DEBUG_EXPECTED_SLSPEED
2111 if (DEBUG_COND) {
2112 std::cout << SIMTIME << " veh=" << myVehicle.getID() << " updateExpectedSublaneSpeeds opposite=" << isOpposite()
2113 << " sublaneOffset=" << sublaneOffset << " laneIndex=" << laneIndex << " lane=" << lane->getID() << " ahead=" << ahead.toString() << "\n";
2114 }
2115#endif
2116
2117 for (int sublane = 0; sublane < (int)ahead.numSublanes(); ++sublane) {
2118 const int edgeSublane = sublane + sublaneOffset;
2119 if (edgeSublane >= (int)myExpectedSublaneSpeeds.size()) {
2120 // this may happen if a sibling lane is wider than the changer lane
2121 continue;
2122 }
2124 // lane allowed, find potential leaders and compute safe speeds
2125 // XXX anticipate future braking if leader has a lower speed than myVehicle
2126 const MSVehicle* leader = ahead[sublane].first;
2127 const double gap = ahead[sublane].second;
2128 double vSafe;
2129 if (leader == nullptr) {
2130 if (hasBlueLight()) {
2131 // can continue from any lane if necessary
2132 vSafe = vMax;
2133 } else {
2134 const int prebIndex = isOpposite() ? (int)preb.size() - 1 : laneIndex;
2135 const double dist = preb[prebIndex].length - myVehicle.getPositionOnLane();
2136 vSafe = getCarFollowModel().followSpeed(&myVehicle, vMax, dist, 0, 0);
2137 }
2138 } else if (bidi != nullptr && leader->getLane()->getBidiLane() != nullptr && isBidi(leader->getLane())) {
2139 // oncoming
2140 if (gap < (1 + mySpeedGainLookahead * 2) * (vMax + leader->getSpeed())) {
2141 vSafe = 0;
2142 } else {
2143 vSafe = vMax;
2144 }
2145#ifdef DEBUG_EXPECTED_SLSPEED
2146 if (DEBUG_COND) {
2147 std::cout << SIMTIME << " updateExpectedSublaneSpeeds sublane=" << sublane << " leader=" << leader->getID() << " bidi=" << bidi->getID() << " gap=" << gap << " vSafe=" << vSafe << "\n";
2148 }
2149#endif
2150 } else {
2151 if (leader->getAcceleration() > 0.5 * leader->getCarFollowModel().getMaxAccel()) {
2152 // assume that the leader will continue accelerating to its maximum speed
2153 vSafe = leader->getLane()->getVehicleMaxSpeed(leader);
2154 } else {
2156 &myVehicle, vMax, gap, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel());
2157#ifdef DEBUG_EXPECTED_SLSPEED
2158 if (DEBUG_COND) {
2159 std::cout << " updateExpectedSublaneSpeeds edgeSublane=" << edgeSublane << " leader=" << leader->getID() << " gap=" << gap << " vSafe=" << vSafe << "\n";
2160 }
2161#endif
2162 vSafe = forecastAverageSpeed(vSafe, vMax, gap, leader->getSpeed());
2163 }
2164 }
2165 // take pedestrians into account
2166 if (lane->getEdge().getPersons().size() > 0 && lane->hasPedestrians()) {
2168 double foeRight, foeLeft;
2169 ahead.getSublaneBorders(sublane, 0, foeRight, foeLeft);
2170 // get all leaders ahead or overlapping
2171 const PersonDist pedLeader = lane->nextBlocking(myVehicle.getPositionOnLane() - myVehicle.getVehicleType().getLength(), foeRight, foeLeft);
2172 if (pedLeader.first != 0) {
2173 const double pedGap = pedLeader.second - myVehicle.getVehicleType().getMinGap() - myVehicle.getVehicleType().getLength();
2174 // we do not know the walking direction here so we take the pedestrian speed as 0
2175 vSafe = MIN2(getCarFollowModel().stopSpeed(&myVehicle, vMax, pedGap),
2176 forecastAverageSpeed(vSafe, vMax, pedGap, 0));
2177#ifdef DEBUG_EXPECTED_SLSPEED
2178 if (DEBUG_COND) {
2179 std::cout << " updateExpectedSublaneSpeeds edgeSublane=" << edgeSublane << " pedLeader=" << pedLeader.first->getID() << " gap=" << pedGap << " vSafe=" << vSafe << "\n";
2180 }
2181#endif
2182 }
2183 }
2184 // take bidi pedestrians into account
2185 if (bidi != nullptr && bidi->getEdge().getPersons().size() > 0 && bidi->hasPedestrians()) {
2187 double foeRight, foeLeft;
2188 ahead.getSublaneBorders(sublane, 0, foeRight, foeLeft);
2189 const double foeRightBidi = bidi->getWidth() - foeLeft;
2190 const double foeLeftBidi = bidi->getWidth() - foeRight;
2191 // get all leaders ahead or overlapping
2192 const double relativeBackPos = myVehicle.getLane()->getLength() - myVehicle.getPositionOnLane() + myVehicle.getLength();
2193 const double stopTime = ceil(myVehicle.getSpeed() / myVehicle.getCarFollowModel().getMaxDecel());
2194 PersonDist pedLeader = bidi->nextBlocking(relativeBackPos, foeRightBidi, foeLeftBidi, stopTime, true);
2195 if (pedLeader.first != 0) {
2196 const double pedGap = pedLeader.second - myVehicle.getVehicleType().getMinGap() - myVehicle.getVehicleType().getLength();
2197 // we do not know the walking direction here so we take the pedestrian speed as 0
2198 vSafe = MIN2(getCarFollowModel().stopSpeed(&myVehicle, vMax, pedGap),
2199 forecastAverageSpeed(vSafe, vMax, pedGap, 0));
2200#ifdef DEBUG_EXPECTED_SLSPEED
2201 if (DEBUG_COND) {
2202 std::cout << " updateExpectedSublaneSpeeds edgeSublane=" << edgeSublane << " pedLeader=" << pedLeader.first->getID() << " (bidi) gap=" << pedGap << " vSafe=" << vSafe << "\n";
2203 }
2204#endif
2205 }
2206 }
2207 vSafe = MIN2(vMax, vSafe);
2208 // forget old data when on the opposite side
2209 const double memoryFactor = isOpposite() ? 0 : pow(SPEEDGAIN_MEMORY_FACTOR, myVehicle.getActionStepLengthSecs());
2210 myExpectedSublaneSpeeds[edgeSublane] = memoryFactor * myExpectedSublaneSpeeds[edgeSublane] + (1 - memoryFactor) * vSafe;
2211 } else {
2212 // lane forbidden
2213 myExpectedSublaneSpeeds[edgeSublane] = -1;
2214 }
2215 }
2216 // XXX deal with leaders on subsequent lanes based on preb
2217}
2218
2219
2220double
2221MSLCM_SL2015::forecastAverageSpeed(double vSafe, double vMax, double gap, double vLeader) const {
2222 const double deltaV = vMax - vLeader;
2223 if (deltaV > 0 && gap / deltaV < mySpeedGainLookahead && mySpeedGainLookahead > 0) {
2224 // anticipate future braking by computing the average
2225 // speed over the next few seconds
2226 const double foreCastTime = mySpeedGainLookahead * 2;
2227 const double gapClosingTime = MAX2(0.0, gap / deltaV);
2228 const double vSafe2 = (gapClosingTime * vSafe + (foreCastTime - gapClosingTime) * vLeader) / foreCastTime;
2229#ifdef DEBUG_EXPECTED_SLSPEED
2230 if (DEBUG_COND && vSafe2 != vSafe) {
2231 std::cout << " foreCastTime=" << foreCastTime << " gapClosingTime=" << gapClosingTime << " extrapolated vSafe=" << vSafe2 << "\n";
2232 }
2233#endif
2234 vSafe = vSafe2;
2235 }
2236 return vSafe;
2237}
2238
2239
2240double
2241MSLCM_SL2015::computeSpeedGain(double latDistSublane, double defaultNextSpeed) const {
2242 double result = std::numeric_limits<double>::max();
2243 const std::vector<double>& sublaneSides = myVehicle.getLane()->getEdge().getSubLaneSides();
2244 const double vehWidth = getWidth();
2245 const double rightVehSide = myVehicle.getCenterOnEdge() - vehWidth * 0.5 + latDistSublane;
2246 const double leftVehSide = rightVehSide + vehWidth;
2247 for (int i = 0; i < (int)sublaneSides.size(); ++i) {
2248 const double leftSide = i + 1 < (int)sublaneSides.size() ? sublaneSides[i + 1] : MAX2(myVehicle.getLane()->getEdge().getWidth(), sublaneSides[i] + POSITION_EPS);
2249 if (overlap(rightVehSide, leftVehSide, sublaneSides[i], leftSide)) {
2250 result = MIN2(result, myExpectedSublaneSpeeds[i]);
2251 }
2252 //std::cout << " i=" << i << " rightVehSide=" << rightVehSide << " leftVehSide=" << leftVehSide << " sublaneR=" << sublaneSides[i] << " sublaneL=" << leftSide << " overlap=" << overlap(rightVehSide, leftVehSide, sublaneSides[i], leftSide) << " speed=" << myExpectedSublaneSpeeds[i] << " result=" << result << "\n";
2253 }
2254 return result - defaultNextSpeed;
2255}
2256
2257
2260 int iMax = -1;
2261 double maxLength = -1;
2262 for (int i = 0; i < ldi.numSublanes(); ++i) {
2263 const MSVehicle* veh = ldi[i].first;
2264 if (veh) {
2265 const double length = veh->getVehicleType().getLength();
2266 if (length > maxLength && tieBrakeLeader(veh)) {
2267 maxLength = length;
2268 iMax = i;
2269 }
2270 }
2271 }
2272 return iMax >= 0 ? ldi[iMax] : std::make_pair(nullptr, -1);
2273}
2274
2275
2276bool
2278 // tie braker if the leader is at the same lane position
2279 return veh != nullptr && (veh->getPositionOnLane() != myVehicle.getPositionOnLane()
2280 || veh->getSpeed() < myVehicle.getSpeed()
2281 || &veh->getLane()->getEdge() != &myVehicle.getLane()->getEdge()
2282 || veh->getLane()->getIndex() > myVehicle.getLane()->getIndex());
2283}
2284
2285
2288 int iMax = 0;
2289 double minSpeed = std::numeric_limits<double>::max();
2290 for (int i = 0; i < ldi.numSublanes(); ++i) {
2291 if (ldi[i].first != 0) {
2292 const double speed = ldi[i].first->getSpeed();
2293 if (speed < minSpeed) {
2294 minSpeed = speed;
2295 iMax = i;
2296 }
2297 }
2298 }
2299 return ldi[iMax];
2300}
2301
2302
2303int
2304MSLCM_SL2015::checkBlocking(const MSLane& neighLane, double& latDist, double maneuverDist, int laneOffset,
2305 const MSLeaderDistanceInfo& leaders,
2306 const MSLeaderDistanceInfo& followers,
2307 const MSLeaderDistanceInfo& /*blockers */,
2308 const MSLeaderDistanceInfo& neighLeaders,
2309 const MSLeaderDistanceInfo& neighFollowers,
2310 const MSLeaderDistanceInfo& /* neighBlockers */,
2311 std::vector<CLeaderDist>* collectLeadBlockers,
2312 std::vector<CLeaderDist>* collectFollowBlockers,
2313 bool keepLatGapManeuver,
2314 double gapFactor,
2315 int* retBlockedFully) {
2316 // truncate latDist according to maxSpeedLat
2317 const double maxDist = SPEED2DIST(getMaxSpeedLat2());
2318 latDist = MAX2(MIN2(latDist, maxDist), -maxDist);
2320 return 0;
2321 }
2322
2323 const double neighRight = getNeighRight(neighLane);
2324 if (!myCFRelatedReady) {
2326 updateCFRelated(followers, myVehicle.getLane()->getRightSideOnEdge(), false);
2327 if (laneOffset != 0) {
2328 updateCFRelated(neighLeaders, neighRight, true);
2329 updateCFRelated(neighFollowers, neighRight, false);
2330 }
2331 myCFRelatedReady = true;
2332 }
2333
2334 // reduce latDist to avoid blockage with overlapping vehicles (no minGapLat constraints)
2335 const double center = myVehicle.getCenterOnEdge();
2336 updateGaps(leaders, myVehicle.getLane()->getRightSideOnEdge(), center, gapFactor, mySafeLatDistRight, mySafeLatDistLeft, false, 0, latDist, collectLeadBlockers);
2337 updateGaps(followers, myVehicle.getLane()->getRightSideOnEdge(), center, gapFactor, mySafeLatDistRight, mySafeLatDistLeft, false, 0, latDist, collectFollowBlockers);
2338 if (laneOffset != 0) {
2339 updateGaps(neighLeaders, neighRight, center, gapFactor, mySafeLatDistRight, mySafeLatDistLeft, false, 0, latDist, collectLeadBlockers);
2340 updateGaps(neighFollowers, neighRight, center, gapFactor, mySafeLatDistRight, mySafeLatDistLeft, false, 0, latDist, collectFollowBlockers);
2341 }
2342#ifdef DEBUG_BLOCKING
2343 if (gDebugFlag2) {
2344 std::cout << " checkBlocking latDist=" << latDist << " mySafeLatDistRight=" << mySafeLatDistRight << " mySafeLatDistLeft=" << mySafeLatDistLeft << "\n";
2345 }
2346#endif
2347 // if we can move at least a little bit in the desired direction, do so (rather than block)
2348 const bool forcedTraCIChange = (myVehicle.hasInfluencer()
2351 if (latDist < 0) {
2352 if (mySafeLatDistRight <= NUMERICAL_EPS) {
2354 } else if (!forcedTraCIChange) {
2355 latDist = MAX2(latDist, -mySafeLatDistRight);
2356 }
2357 } else {
2358 if (mySafeLatDistLeft <= NUMERICAL_EPS) {
2360 } else if (!forcedTraCIChange) {
2361 latDist = MIN2(latDist, mySafeLatDistLeft);
2362 }
2363 }
2364
2365 myCanChangeFully = (maneuverDist == 0 || latDist == maneuverDist);
2366#ifdef DEBUG_BLOCKING
2367 if (gDebugFlag2) {
2368 std::cout << " checkBlocking fully=" << myCanChangeFully << " latDist=" << latDist << " maneuverDist=" << maneuverDist << "\n";
2369 }
2370#endif
2371 // destination sublanes must be safe
2372 // intermediate sublanes must not be blocked by overlapping vehicles
2373
2374 // XXX avoid checking the same leader multiple times
2375 // XXX ensure that only changes within the same lane are undertaken if laneOffset = 0
2376
2377 int blocked = 0;
2378 blocked |= checkBlockingVehicles(&myVehicle, leaders, laneOffset, latDist, myVehicle.getLane()->getRightSideOnEdge(), true,
2379 mySafeLatDistRight, mySafeLatDistLeft, collectLeadBlockers);
2380 blocked |= checkBlockingVehicles(&myVehicle, followers, laneOffset, latDist, myVehicle.getLane()->getRightSideOnEdge(), false,
2381 mySafeLatDistRight, mySafeLatDistLeft, collectFollowBlockers);
2382 if (laneOffset != 0) {
2383 blocked |= checkBlockingVehicles(&myVehicle, neighLeaders, laneOffset, latDist, neighRight, true,
2384 mySafeLatDistRight, mySafeLatDistLeft, collectLeadBlockers);
2385 blocked |= checkBlockingVehicles(&myVehicle, neighFollowers, laneOffset, latDist, neighRight, false,
2386 mySafeLatDistRight, mySafeLatDistLeft, collectFollowBlockers);
2387 }
2388
2389 int blockedFully = 0;
2390 blockedFully |= checkBlockingVehicles(&myVehicle, leaders, laneOffset, maneuverDist, myVehicle.getLane()->getRightSideOnEdge(), true,
2391 mySafeLatDistRight, mySafeLatDistLeft, collectLeadBlockers);
2392 blockedFully |= checkBlockingVehicles(&myVehicle, followers, laneOffset, maneuverDist, myVehicle.getLane()->getRightSideOnEdge(), false,
2393 mySafeLatDistRight, mySafeLatDistLeft, collectFollowBlockers);
2394 if (laneOffset != 0) {
2395 blockedFully |= checkBlockingVehicles(&myVehicle, neighLeaders, laneOffset, maneuverDist, neighRight, true,
2396 mySafeLatDistRight, mySafeLatDistLeft, collectLeadBlockers);
2397 blockedFully |= checkBlockingVehicles(&myVehicle, neighFollowers, laneOffset, maneuverDist, neighRight, false,
2398 mySafeLatDistRight, mySafeLatDistLeft, collectFollowBlockers);
2399 }
2400 if (retBlockedFully != nullptr) {
2401 *retBlockedFully = blockedFully;
2402 }
2403 if (blocked == 0 && !myCanChangeFully && myPushy == 0 && !keepLatGapManeuver) {
2404 // aggressive drivers immediately start moving towards potential
2405 // blockers and only check that the start of their maneuver (latDist) is safe. In
2406 // contrast, cautious drivers need to check latDist and origLatDist to
2407 // ensure that the maneuver can be finished without encroaching on other vehicles.
2408 blocked |= blockedFully;
2409 } else {
2410 // XXX: in case of action step length > simulation step length, pushing may lead to collisions,
2411 // because maneuver is continued until maneuverDist is reached (perhaps set maneuverDist=latDist)
2412 }
2413 if (collectFollowBlockers != nullptr && collectLeadBlockers != nullptr) {
2414 // prevent vehicles from being classified as leader and follower simultaneously
2415 for (std::vector<CLeaderDist>::const_iterator it2 = collectLeadBlockers->begin(); it2 != collectLeadBlockers->end(); ++it2) {
2416 for (std::vector<CLeaderDist>::iterator it = collectFollowBlockers->begin(); it != collectFollowBlockers->end();) {
2417 if ((*it2).first == (*it).first) {
2418#ifdef DEBUG_BLOCKING
2419 if (gDebugFlag2) {
2420 std::cout << " removed follower " << (*it).first->getID() << " because it is already a leader\n";
2421 }
2422#endif
2423 it = collectFollowBlockers->erase(it);
2424 } else {
2425 ++it;
2426 }
2427 }
2428 }
2429 }
2430 return blocked;
2431}
2432
2433
2434int
2436 const MSVehicle* ego, const MSLeaderDistanceInfo& vehicles,
2437 int laneOffset, double latDist, double foeOffset, bool leaders,
2438 double& safeLatGapRight, double& safeLatGapLeft,
2439 std::vector<CLeaderDist>* collectBlockers) const {
2440 // determine borders where safety/no-overlap conditions must hold
2441 const LaneChangeAction blockType = (laneOffset == 0
2443 : (laneOffset > 0
2446 const double vehWidth = getWidth();
2447 const double rightVehSide = ego->getRightSideOnEdge();
2448 const double leftVehSide = rightVehSide + vehWidth;
2449 const double rightVehSideDest = rightVehSide + latDist;
2450 const double leftVehSideDest = leftVehSide + latDist;
2451 const double rightNoOverlap = MIN2(rightVehSideDest, rightVehSide);
2452 const double leftNoOverlap = MAX2(leftVehSideDest, leftVehSide);
2453#ifdef DEBUG_BLOCKING
2454 if (gDebugFlag2) {
2455 std::cout << " checkBlockingVehicles"
2456 << " laneOffset=" << laneOffset
2457 << " latDist=" << latDist
2458 << " foeOffset=" << foeOffset
2459 << " vehRight=" << rightVehSide
2460 << " vehLeft=" << leftVehSide
2461 << " rightNoOverlap=" << rightNoOverlap
2462 << " leftNoOverlap=" << leftNoOverlap
2463 << " destRight=" << rightVehSideDest
2464 << " destLeft=" << leftVehSideDest
2465 << " leaders=" << leaders
2466 << " blockType=" << toString((LaneChangeAction) blockType)
2467 << "\n";
2468 }
2469#endif
2470 int result = 0;
2471 for (int i = 0; i < vehicles.numSublanes(); ++i) {
2472 CLeaderDist vehDist = vehicles[i];
2473 if (vehDist.first != 0 && myCFRelated.count(vehDist.first) == 0) {
2474 const MSVehicle* leader = vehDist.first;
2475 const MSVehicle* follower = ego;
2476 if (!leaders) {
2477 std::swap(leader, follower);
2478 }
2479 // only check the current stripe occupied by foe (transform into edge-coordinates)
2480 double foeRight, foeLeft;
2481 vehicles.getSublaneBorders(i, foeOffset, foeRight, foeLeft);
2482 const bool overlapBefore = overlap(rightVehSide, leftVehSide, foeRight, foeLeft);
2483 const bool overlapDest = overlap(rightVehSideDest, leftVehSideDest, foeRight, foeLeft);
2484 const bool overlapAny = overlap(rightNoOverlap, leftNoOverlap, foeRight, foeLeft);
2485#ifdef DEBUG_BLOCKING
2486 if (gDebugFlag2) {
2487 std::cout << " foe=" << vehDist.first->getID()
2488 << " gap=" << vehDist.second
2489 << " secGap=" << follower->getCarFollowModel().getSecureGap(follower, leader, follower->getSpeed(), leader->getSpeed(), leader->getCarFollowModel().getMaxDecel())
2490 << " foeRight=" << foeRight
2491 << " foeLeft=" << foeLeft
2492 << " overlapBefore=" << overlapBefore
2493 << " overlap=" << overlapAny
2494 << " overlapDest=" << overlapDest
2495 << "\n";
2496 }
2497#endif
2498 if (overlapAny) {
2499 if (vehDist.second < 0) {
2500 if (overlapBefore && !overlapDest) {
2501#ifdef DEBUG_BLOCKING
2502 if (gDebugFlag2) {
2503 std::cout << " ignoring current overlap to come clear\n";
2504 }
2505#endif
2506 } else {
2507#ifdef DEBUG_BLOCKING
2508 if (gDebugFlag2) {
2509 std::cout << " overlap (" << toString((LaneChangeAction)blockType) << ")\n";
2510 }
2511#endif
2512 result |= (blockType | LCA_OVERLAPPING);
2513 if (collectBlockers == nullptr) {
2514 return result;
2515 } else {
2516 collectBlockers->push_back(vehDist);
2517 }
2518 }
2519 } else if (overlapDest || !myCanChangeFully) {
2520 // Estimate state after actionstep (follower may be accelerating!)
2521 // A comparison between secure gap depending on the expected speeds and the extrapolated gap
2522 // determines whether the s is blocking the lane change.
2523 // (Note that the longitudinal state update has already taken effect before LC dynamics (thus "-TS" below), would be affected by #3665)
2524
2525 // Use conservative estimate for time until next action step
2526 // (XXX: how can the ego know the foe's action step length?)
2527 const double timeTillAction = MAX2(follower->getActionStepLengthSecs(), leader->getActionStepLengthSecs()) - TS;
2528 // Ignore decel for follower
2529 const double followerAccel = MAX2(0., follower->getAcceleration());
2530 const double leaderAccel = leader->getAcceleration();
2531 // Expected gap after next actionsteps
2532 const double expectedGap = MSCFModel::gapExtrapolation(timeTillAction, vehDist.second, leader->getSpeed(), follower->getSpeed(), leaderAccel, followerAccel, std::numeric_limits<double>::max(), std::numeric_limits<double>::max());
2533
2534 // Determine expected speeds and corresponding secure gap at the extrapolated timepoint
2535 const double followerExpectedSpeed = follower->getSpeed() + timeTillAction * followerAccel;
2536 const double leaderExpectedSpeed = MAX2(0., leader->getSpeed() + timeTillAction * leaderAccel);
2537 const double expectedSecureGap = follower->getCarFollowModel().getSecureGap(follower, leader, followerExpectedSpeed, leaderExpectedSpeed, leader->getCarFollowModel().getMaxDecel());
2538
2539#if defined(DEBUG_ACTIONSTEPS) && defined(DEBUG_BLOCKING)
2540 if (gDebugFlag2) {
2541 std::cout << " timeTillAction=" << timeTillAction
2542 << " followerAccel=" << followerAccel
2543 << " followerExpectedSpeed=" << followerExpectedSpeed
2544 << " leaderAccel=" << leaderAccel
2545 << " leaderExpectedSpeed=" << leaderExpectedSpeed
2546 << "\n gap=" << vehDist.second
2547 << " gapChange=" << (expectedGap - vehDist.second)
2548 << " expectedGap=" << expectedGap
2549 << " expectedSecureGap=" << expectedSecureGap
2550 << " safeLatGapLeft=" << safeLatGapLeft
2551 << " safeLatGapRight=" << safeLatGapRight
2552 << std::endl;
2553 }
2554#endif
2555
2556 // @note for euler-update, a different value for secureGap2 may be obtained when applying safetyFactor to followerDecel rather than secureGap
2557 const double secureGap2 = expectedSecureGap * getSafetyFactor();
2558 if (expectedGap < secureGap2) {
2559 // Foe is a blocker. Update lateral safe gaps accordingly.
2560 if (foeRight > leftVehSide) {
2561 safeLatGapLeft = MIN2(safeLatGapLeft, foeRight - leftVehSide);
2562 } else if (foeLeft < rightVehSide) {
2563 safeLatGapRight = MIN2(safeLatGapRight, rightVehSide - foeLeft);
2564 }
2565
2566#ifdef DEBUG_BLOCKING
2567 if (gDebugFlag2) {
2568 std::cout << " blocked by " << vehDist.first->getID() << " gap=" << vehDist.second << " expectedGap=" << expectedGap
2569 << " expectedSecureGap=" << expectedSecureGap << " secGap2=" << secureGap2 << " safetyFactor=" << getSafetyFactor()
2570 << " safeLatGapLeft=" << safeLatGapLeft << " safeLatGapRight=" << safeLatGapRight
2571 << "\n";
2572 }
2573#endif
2574 result |= blockType;
2575 if (collectBlockers == nullptr) {
2576 return result;
2577 }
2578#ifdef DEBUG_BLOCKING
2579 } else if (gDebugFlag2 && expectedGap < expectedSecureGap) {
2580 std::cout << " ignore blocker " << vehDist.first->getID() << " gap=" << vehDist.second << " expectedGap=" << expectedGap
2581 << " expectedSecureGap=" << expectedSecureGap << " secGap2=" << secureGap2 << " safetyFactor=" << getSafetyFactor() << "\n";
2582#endif
2583 }
2584 if (collectBlockers != nullptr) {
2585 // collect non-blocking followers as well to make sure
2586 // they remain non-blocking
2587 collectBlockers->push_back(vehDist);
2588 }
2589 }
2590 }
2591 }
2592 }
2593 return result;
2594
2595}
2596
2597
2598void
2599MSLCM_SL2015::updateCFRelated(const MSLeaderDistanceInfo& vehicles, double foeOffset, bool leaders) {
2600 // to ensure that we do not ignore the wrong vehicles due to numerical
2601 // instability we slightly reduce the width
2602 const double vehWidth = myVehicle.getVehicleType().getWidth() - NUMERICAL_EPS;
2603 const double rightVehSide = myVehicle.getCenterOnEdge() - 0.5 * vehWidth;
2604 const double leftVehSide = rightVehSide + vehWidth;
2605#ifdef DEBUG_BLOCKING
2606 if (gDebugFlag2) {
2607 std::cout << " updateCFRelated foeOffset=" << foeOffset << " vehicles=" << vehicles.toString() << "\n";
2608 }
2609#endif
2610 for (int i = 0; i < vehicles.numSublanes(); ++i) {
2611 CLeaderDist vehDist = vehicles[i];
2612 if (vehDist.first != 0 && myCFRelated.count(vehDist.first) == 0) {
2613 double foeRight, foeLeft;
2614 vehicles.getSublaneBorders(i, foeOffset, foeRight, foeLeft);
2615#ifdef DEBUG_BLOCKING
2616 if (gDebugFlag2) {
2617 std::cout << " foe=" << vehDist.first->getID() << " gap=" << vehDist.second
2618 << " sublane=" << i
2619 << " foeOffset=" << foeOffset
2620 << " egoR=" << rightVehSide << " egoL=" << leftVehSide
2621 << " iR=" << foeRight << " iL=" << foeLeft
2622 << " egoV=" << myVehicle.getSpeed() << " foeV=" << vehDist.first->getSpeed()
2623 << " egoE=" << myVehicle.getLane()->getEdge().getID() << " foeE=" << vehDist.first->getLane()->getEdge().getID()
2624 << "\n";
2625 }
2626#endif
2627 if (overlap(rightVehSide, leftVehSide, foeRight, foeLeft) && (vehDist.second >= 0
2628 // avoid deadlock due to #3729
2629 || (!leaders
2632 && vehDist.first->getSpeed() < SUMO_const_haltingSpeed
2633 && -vehDist.second < vehDist.first->getVehicleType().getMinGap()
2634 && &(myVehicle.getLane()->getEdge()) != &(vehDist.first->getLane()->getEdge()))
2635 )) {
2636#ifdef DEBUG_BLOCKING
2637 if (gDebugFlag2) {
2638 std::cout << " ignoring cfrelated foe=" << vehDist.first->getID() << "\n";
2639 }
2640#endif
2641 myCFRelated.insert(vehDist.first);
2642 }
2643 }
2644 }
2645}
2646
2647
2648bool
2649MSLCM_SL2015::overlap(double right, double left, double right2, double left2) {
2650 assert(right <= left);
2651 assert(right2 <= left2);
2652 return left2 >= right + NUMERICAL_EPS && left >= right2 + NUMERICAL_EPS;
2653}
2654
2655
2656int
2657MSLCM_SL2015::lowest_bit(int changeReason) {
2658 if ((changeReason & LCA_STRATEGIC) != 0) {
2659 return LCA_STRATEGIC;
2660 }
2661 if ((changeReason & LCA_COOPERATIVE) != 0) {
2662 return LCA_COOPERATIVE;
2663 }
2664 if ((changeReason & LCA_SPEEDGAIN) != 0) {
2665 return LCA_SPEEDGAIN;
2666 }
2667 if ((changeReason & LCA_KEEPRIGHT) != 0) {
2668 return LCA_KEEPRIGHT;
2669 }
2670 if ((changeReason & LCA_TRACI) != 0) {
2671 return LCA_TRACI;
2672 }
2673 return changeReason;
2674}
2675
2676
2679 // ignore dummy decisions (returned if mayChange() failes)
2680 if (sd1.state == 0) {
2681 return sd2;
2682 } else if (sd2.state == 0) {
2683 return sd1;
2684 }
2685 // LCA_SUBLANE is special because LCA_STAY|LCA_SUBLANE may override another LCA_SUBLANE command
2686 const bool want1 = ((sd1.state & LCA_WANTS_LANECHANGE) != 0) || ((sd1.state & LCA_SUBLANE) != 0 && (sd1.state & LCA_STAY) != 0);
2687 const bool want2 = ((sd2.state & LCA_WANTS_LANECHANGE) != 0) || ((sd2.state & LCA_SUBLANE) != 0 && (sd2.state & LCA_STAY) != 0);
2688 const bool can1 = ((sd1.state & LCA_BLOCKED) == 0);
2689 const bool can2 = ((sd2.state & LCA_BLOCKED) == 0);
2690 int reason1 = lowest_bit(sd1.state & LCA_CHANGE_REASONS);
2691 int reason2 = lowest_bit(sd2.state & LCA_CHANGE_REASONS);
2692#ifdef DEBUG_WANTSCHANGE
2693 if (DEBUG_COND) std::cout << SIMTIME
2694 << " veh=" << myVehicle.getID()
2695 << " state1=" << toString((LaneChangeAction)sd1.state)
2696 << " want1=" << (sd1.state & LCA_WANTS_LANECHANGE)
2697 << " dist1=" << sd1.latDist
2698 << " dir1=" << sd1.dir
2699 << " state2=" << toString((LaneChangeAction)sd2.state)
2700 << " want2=" << (sd2.state & LCA_WANTS_LANECHANGE)
2701 << " dist2=" << sd2.latDist
2702 << " dir2=" << sd2.dir
2703 << " reason1=" << toString((LaneChangeAction)reason1)
2704 << " reason2=" << toString((LaneChangeAction)reason2)
2705 << "\n";
2706#endif
2707 if (want1) {
2708 if (want2) {
2709 // decide whether right or left has higher priority (lower value in enum LaneChangeAction)
2710 if (reason1 < reason2) {
2711 //if (DEBUG_COND) std::cout << " " << (sd1.state & LCA_CHANGE_REASONS) << " < " << (sd2.state & LCA_CHANGE_REASONS) << "\n";
2712 return (!can1 && can2 && sd1.sameDirection(sd2)) ? sd2 : sd1;
2713 //return sd1;
2714 } else if (reason1 > reason2) {
2715 //if (DEBUG_COND) std::cout << " " << (sd1.state & LCA_CHANGE_REASONS) << " > " << (sd2.state & LCA_CHANGE_REASONS) << "\n";
2716 return (!can2 && can1 && sd1.sameDirection(sd2)) ? sd1 : sd2;
2717 //return sd2;
2718 } else {
2719 // same priority.
2720 if ((sd1.state & LCA_SUBLANE) != 0) {
2721 // special treatment: prefer action with dir != 0
2722 if (sd1.dir == 0) {
2723 return sd2;
2724 } else if (sd2.dir == 0) {
2725 return sd1;
2726 } else {
2727 // prefer action that knows more about the desired direction
2728 // @note when deciding between right and left, right is always given as sd1
2729 assert(sd1.dir == -1);
2730 assert(sd2.dir == 1);
2731 if (sd1.latDist <= 0) {
2732 return sd1;
2733 } else if (sd2.latDist >= 0) {
2734 return sd2;
2735 }
2736 // when in doubt, prefer moving to the right
2737 return sd1.latDist <= sd2.latDist ? sd1 : sd2;
2738 }
2739 } else {
2740 if (can1) {
2741 if (can2) {
2742 return fabs(sd1.latDist) > fabs(sd2.latDist) ? sd1 : sd2;
2743 } else {
2744 return sd1;
2745 }
2746 } else {
2747 return sd2;
2748 }
2749 }
2750 }
2751 } else {
2752 return sd1;
2753 }
2754 } else {
2755 return sd2;
2756 }
2757
2758}
2759
2760
2762MSLCM_SL2015::getLCA(int state, double latDist) {
2763 return ((latDist == 0 || (state & LCA_CHANGE_REASONS) == 0)
2764 ? LCA_NONE : (latDist < 0 ? LCA_RIGHT : LCA_LEFT));
2765}
2766
2767
2768int
2770 const MSLane& neighLane,
2771 int laneOffset,
2772 const MSLeaderDistanceInfo& leaders,
2773 const MSLeaderDistanceInfo& neighLeaders,
2774 const MSVehicle::LaneQ& curr,
2775 const MSVehicle::LaneQ& neigh,
2776 const MSVehicle::LaneQ& best,
2777 int bestLaneOffset,
2778 bool changeToBest,
2779 double currentDist,
2780 double neighDist,
2781 double laDist,
2782 double roundaboutBonus,
2783 double latLaneDist,
2784 bool checkOpposite,
2785 double& latDist
2786 ) {
2787 const bool right = (laneOffset == -1);
2788 const bool left = (laneOffset == 1);
2789
2790 const double forwardPos = getForwardPos();
2791 myLeftSpace = currentDist - forwardPos;
2792 const double usableDist = (currentDist - forwardPos - best.occupation * JAM_FACTOR);
2793 //- (best.lane->getVehicleNumber() * neighSpeed)); // VARIANT 9 jfSpeed
2794 const double maxJam = MAX2(neigh.occupation, curr.occupation);
2795 const double neighLeftPlace = MAX2(0., neighDist - forwardPos - maxJam);
2796 // save the left space
2797
2798#ifdef DEBUG_STRATEGIC_CHANGE
2799 if (gDebugFlag2) {
2800 std::cout << SIMTIME
2801 << " veh=" << myVehicle.getID()
2802 << " forwardPos=" << forwardPos
2803 << " laSpeed=" << myLookAheadSpeed
2804 << " laDist=" << laDist
2805 << " currentDist=" << currentDist
2806 << " usableDist=" << usableDist
2807 << " bestLaneOffset=" << bestLaneOffset
2808 << " best.length=" << best.length
2809 << " maxJam=" << maxJam
2810 << " neighLeftPlace=" << neighLeftPlace
2811 << " myLeftSpace=" << myLeftSpace
2812 << "\n";
2813 }
2814#endif
2815
2816 if (laneOffset != 0 && changeToBest && bestLaneOffset == curr.bestLaneOffset
2817 && currentDistDisallows(usableDist, bestLaneOffset, laDist)) {
2819 latDist = latLaneDist;
2820 ret |= LCA_STRATEGIC | LCA_URGENT;
2821 } else {
2822 // VARIANT_20 (noOvertakeRight)
2823 if (left && avoidOvertakeRight() && neighLeaders.hasVehicles()) {
2824 // check for slower leader on the left. we should not overtake but
2825 // rather move left ourselves (unless congested)
2826 // XXX only adapt as much as possible to get a lateral gap
2827 CLeaderDist cld = getSlowest(neighLeaders);
2828 const MSVehicle* nv = cld.first;
2829 if (nv->getSpeed() < myVehicle.getSpeed()) {
2830 const double vSafe = getCarFollowModel().followSpeed(
2831 &myVehicle, myVehicle.getSpeed(), cld.second, nv->getSpeed(), nv->getCarFollowModel().getMaxDecel());
2832 addLCSpeedAdvice(vSafe);
2833 if (vSafe < myVehicle.getSpeed()) {
2835 }
2836#ifdef DEBUG_STRATEGIC_CHANGE
2837 if (gDebugFlag2) {
2838 std::cout << SIMTIME
2839 << " avoid overtaking on the right nv=" << nv->getID()
2840 << " nvSpeed=" << nv->getSpeed()
2841 << " mySpeedGainProbabilityR=" << mySpeedGainProbabilityRight
2842 << " plannedSpeed=" << myVehicle.getSpeed() + ACCEL2SPEED(myLCAccelerationAdvices.back())
2843 << "\n";
2844 }
2845#endif
2846 }
2847 }
2848
2849 // handling reaction to stopped for opposite direction driving NYI
2850 const bool noOpposites = &myVehicle.getLane()->getEdge() == &neighLane.getEdge();
2851 if (laneOffset != 0 && myStrategicParam >= 0 && noOpposites && mustOvertakeStopped(neighLane, leaders, neighLeaders, forwardPos, neighDist, right, latLaneDist, currentDist, latDist)) {
2852 if (latDist == 0) {
2853 ret |= LCA_STAY | LCA_STRATEGIC;
2854 } else {
2855 ret |= LCA_STRATEGIC | LCA_URGENT;
2856 }
2857
2858 } else if (!changeToBest && (currentDistDisallows(neighLeftPlace, abs(bestLaneOffset) + 2, laDist))) {
2859 // the opposite lane-changing direction should be done than the one examined herein
2860 // we'll check whether we assume we could change anyhow and get back in time...
2861 //
2862 // this rule prevents the vehicle from moving in opposite direction of the best lane
2863 // unless the way till the end where the vehicle has to be on the best lane
2864 // is long enough
2865#ifdef DEBUG_STRATEGIC_CHANGE
2866 if (gDebugFlag2) {
2867 std::cout << " veh=" << myVehicle.getID() << " could not change back and forth in time (1) neighLeftPlace=" << neighLeftPlace << "\n";
2868 }
2869#endif
2870 ret |= LCA_STAY | LCA_STRATEGIC;
2871 } else if (
2872 laneOffset != 0
2873 && bestLaneOffset == 0
2874 && !leaders.hasStoppedVehicle()
2875 && neigh.bestContinuations.back()->getLinkCont().size() != 0
2876 && roundaboutBonus == 0
2877 && !checkOpposite
2878 && neighDist < TURN_LANE_DIST
2879 && myStrategicParam >= 0) {
2880 // VARIANT_21 (stayOnBest)
2881 // we do not want to leave the best lane for a lane which leads elsewhere
2882 // unless our leader is stopped or we are approaching a roundabout
2883#ifdef DEBUG_STRATEGIC_CHANGE
2884 if (gDebugFlag2) {
2885 std::cout << " veh=" << myVehicle.getID() << " does not want to leave the bestLane (neighDist=" << neighDist << ")\n";
2886 }
2887#endif
2888 ret |= LCA_STAY | LCA_STRATEGIC;
2889 } else if (right
2890 && bestLaneOffset == 0
2891 && myVehicle.getLane()->getSpeedLimit() > 80. / 3.6
2893 ) {
2894 // let's also regard the case where the vehicle is driving on a highway...
2895 // in this case, we do not want to get to the dead-end of an on-ramp
2896#ifdef DEBUG_STRATEGIC_CHANGE
2897 if (gDebugFlag2) {
2898 std::cout << " veh=" << myVehicle.getID() << " does not want to get stranded on the on-ramp of a highway\n";
2899 }
2900#endif
2901 ret |= LCA_STAY | LCA_STRATEGIC;
2902 }
2903 }
2904 if ((ret & LCA_URGENT) == 0 && getShadowLane() != nullptr &&
2905 // ignore overlap if it goes in the correct direction
2906 bestLaneOffset * myVehicle.getLateralPositionOnLane() <= 0) {
2907 // no decision or decision to stay
2908 // make sure to stay within lane bounds in case the shadow lane ends
2909 //const double requiredDist = MAX2(2 * myVehicle.getLateralOverlap(), getSublaneWidth()) / SUMO_const_laneWidth * laDist;
2910 const double requiredDist = 2 * myVehicle.getLateralOverlap() / SUMO_const_laneWidth * laDist;
2911 double currentShadowDist = -myVehicle.getPositionOnLane();
2912 MSLane* shadowPrev = nullptr;
2913 for (std::vector<MSLane*>::const_iterator it = curr.bestContinuations.begin(); it != curr.bestContinuations.end(); ++it) {
2914 if (*it == nullptr) {
2915 continue;
2916 }
2917 MSLane* shadow = getShadowLane(*it);
2918 if (shadow == nullptr || currentShadowDist >= requiredDist) {
2919 break;
2920 }
2921 if (shadowPrev != nullptr) {
2922 currentShadowDist += shadowPrev->getEdge().getInternalFollowingLengthTo(&shadow->getEdge(), myVehicle.getVClass());
2923 }
2924 currentShadowDist += shadow->getLength();
2925 shadowPrev = shadow;
2926#ifdef DEBUG_STRATEGIC_CHANGE
2927 if (gDebugFlag2) {
2928 std::cout << " shadow=" << shadow->getID() << " currentShadowDist=" << currentShadowDist << "\n";
2929 }
2930#endif
2931 }
2932#ifdef DEBUG_STRATEGIC_CHANGE
2933 if (gDebugFlag2) {
2934 std::cout << " veh=" << myVehicle.getID() << " currentShadowDist=" << currentShadowDist << " requiredDist=" << requiredDist << " overlap=" << myVehicle.getLateralOverlap() << "\n";
2935 }
2936#endif
2937 if (currentShadowDist < requiredDist && currentShadowDist < usableDist) {
2938 myLeftSpace = currentShadowDist;
2940#ifdef DEBUG_STRATEGIC_CHANGE
2941 if (gDebugFlag2) {
2942 std::cout << " must change for shadowLane end latDist=" << latDist << " myLeftSpace=" << myLeftSpace << "\n";
2943 }
2944#endif
2945 ret |= LCA_STRATEGIC | LCA_URGENT | LCA_STAY ;
2946 }
2947 }
2948
2949 // check for overriding TraCI requests
2950#if defined(DEBUG_STRATEGIC_CHANGE) || defined(DEBUG_TRACI)
2951 if (gDebugFlag2) {
2952 std::cout << SIMTIME << " veh=" << myVehicle.getID() << " ret=" << ret;
2953 }
2954#endif
2955 // store state before canceling
2956 getCanceledState(laneOffset) |= ret;
2957 int retTraCI = myVehicle.influenceChangeDecision(ret);
2958 if ((retTraCI & LCA_TRACI) != 0) {
2959 if ((retTraCI & LCA_STAY) != 0) {
2960 ret = retTraCI;
2961 latDist = 0;
2962 } else if (((retTraCI & LCA_RIGHT) != 0 && laneOffset < 0)
2963 || ((retTraCI & LCA_LEFT) != 0 && laneOffset > 0)) {
2964 ret = retTraCI;
2965 latDist = latLaneDist;
2966 }
2967 }
2968#if defined(DEBUG_STRATEGIC_CHANGE) || defined(DEBUG_TRACI)
2969 if (gDebugFlag2) {
2970 std::cout << " reqAfterInfluence=" << ret << " ret=" << ret << "\n";
2971 }
2972#endif
2973 return ret;
2974}
2975
2976
2977bool
2979 double posOnLane, double neighDist, bool right, double latLaneDist, double& currentDist, double& latDist) {
2980 bool mustOvertake = false;
2981 const bool checkOverTakeRight = avoidOvertakeRight();
2982 int rightmost;
2983 int leftmost;
2984 const bool curHasStopped = leaders.hasStoppedVehicle();
2985 const MSLane* neighBeyond = neighLane.getParallelLane(latLaneDist < 0 ? -1 : 1);
2986 const bool hasLaneBeyond = neighBeyond != nullptr && neighBeyond->allowsVehicleClass(myVehicle.getVClass());
2987 if (curHasStopped) {
2988 leaders.getSubLanes(&myVehicle, 0, rightmost, leftmost);
2989 for (int i = rightmost; i <= leftmost; i++) {
2990 const CLeaderDist& leader = leaders[i];
2991 if (leader.first != 0 && leader.first->isStopped() && leader.second < REACT_TO_STOPPED_DISTANCE) {
2992 const double overtakeDist = leader.second + myVehicle.getVehicleType().getLength() + leader.first->getVehicleType().getLengthWithGap();
2993 if (// current destination leaves enough space to overtake the leader
2994 MIN2(neighDist, currentDist) - posOnLane > overtakeDist
2995 // maybe do not overtake on the right at high speed
2996 && (!checkOverTakeRight || !right)
2997 && (!neighLead.hasStoppedVehicle() || hasLaneBeyond)
2998 //&& (neighLead.first == 0 || !neighLead.first->isStopped()
2999 // // neighboring stopped vehicle leaves enough space to overtake leader
3000 // || neighLead.second > overtakeDist))
3001 ) {
3002 // avoid becoming stuck behind a stopped leader
3003 currentDist = myVehicle.getPositionOnLane() + leader.second;
3004 latDist = latLaneDist;
3005 mustOvertake = true;
3006#ifdef DEBUG_WANTS_CHANGE
3007 if (DEBUG_COND) {
3008 std::cout << " veh=" << myVehicle.getID() << " overtake stopped leader=" << leader.first->getID()
3009 << " overtakeDist=" << overtakeDist
3010 << " remaining=" << MIN2(neighDist, currentDist) - posOnLane
3011 << "\n";
3012 }
3013#endif
3014 }
3015 }
3016
3017 }
3018 }
3019 if (!mustOvertake && !curHasStopped && neighLead.hasStoppedVehicle()) {
3020 // #todo fix this if the neigh lane has a different width
3021 const double offset = (latLaneDist < 0 ? -1 : 1) * myVehicle.getLane()->getWidth();
3022 neighLead.getSubLanes(&myVehicle, offset, rightmost, leftmost);
3023 for (int i = 0; i < neighLead.numSublanes(); i++) {
3024 const CLeaderDist& leader = leaders[i];
3025 if (leader.first != 0 && leader.first->isStopped() && leader.second < REACT_TO_STOPPED_DISTANCE) {
3026 mustOvertake = true;
3027 if (i >= rightmost && i <= leftmost) {
3028 latDist = myVehicle.getLateralOverlap() * (latLaneDist > 0 ? -1 : 1);
3029 break;
3030 }
3031 }
3032 }
3033 }
3034 return mustOvertake;
3035}
3036
3037
3038double
3040 return (state & LCA_STRATEGIC) != 0 ? MAX2(0.0, (1.0 - myPushy * (1 + 0.5 * myImpatience))) : 1.0;
3041}
3042
3043
3044int
3046 const MSLeaderDistanceInfo& leaders,
3047 const MSLeaderDistanceInfo& followers,
3048 const MSLeaderDistanceInfo& blockers,
3049 const MSLeaderDistanceInfo& neighLeaders,
3050 const MSLeaderDistanceInfo& neighFollowers,
3051 const MSLeaderDistanceInfo& neighBlockers,
3052 const MSLane& neighLane,
3053 int laneOffset,
3054 double& latDist,
3055 double& maneuverDist,
3056 int& blocked) {
3057
3058 /* @notes
3059 * vehicles may need to compromise between fulfilling lane change objectives
3060 * (LCA_STRATEGIC, LCA_SPEED etc) and maintaining lateral gap. The minimum
3061 * acceptable lateral gap depends on
3062 * - the cultural context (China vs Europe)
3063 * - the driver agressiveness (willingness to encroach on other vehicles to force them to move laterally as well)
3064 * - see @note in checkBlocking
3065 * - the vehicle type (car vs motorcycle)
3066 * - the current speed
3067 * - the speed difference
3068 * - the importance / urgency of the desired maneuver
3069 *
3070 * the object of this method is to evaluate the above circumstances and
3071 * either:
3072 * - allow the current maneuver (state, latDist)
3073 * - to override the current maneuver with a distance-keeping maneuver
3074 *
3075 *
3076 * laneChangeModel/driver parameters
3077 * - bool pushy (willingness to encroach)
3078 * - float minGap at 100km/h (to be interpolated for lower speeds (assume 0 at speed 0)
3079 * - gapFactors (a factor for each of the change reasons
3080 *
3081 * further assumptions
3082 * - the maximum of egoSpeed and deltaSpeed can be used when interpolating minGap
3083 * - distance keeping to the edges of the road can be ignored (for now)
3084 *
3085 * currentMinGap = minGap * min(1.0, max(v, abs(v - vOther)) / 100) * gapFactor[lc_reason]
3086 *
3087 * */
3088
3090 double gapFactor = computeGapFactor(state);
3091 const double oldLatDist = latDist;
3092 const double oldManeuverDist = maneuverDist;
3093
3094 // compute gaps after maneuver
3095 const double halfWidth = getWidth() * 0.5;
3096 // if the current maneuver is blocked we will stay where we are
3097 const double oldCenter = myVehicle.getCenterOnEdge();
3098 // surplus gaps. these are used to collect various constraints
3099 // if they do not permit the desired maneuvre, should override it to better maintain distance
3100 // stay within the current edge
3101 double surplusGapRight = oldCenter - halfWidth;
3102 double surplusGapLeft = getLeftBorder(laneOffset != 0) - oldCenter - halfWidth;
3103 const bool stayInLane = (laneOffset == 0
3104 || ((state & LCA_STRATEGIC) != 0
3105 && (state & LCA_STAY) != 0
3106 // permit wide vehicles to stay on the road
3107 && (surplusGapLeft >= 0 && surplusGapRight >= 0)));
3108
3109 if (isOpposite()) {
3110 std::swap(surplusGapLeft, surplusGapRight);
3111 }
3112#ifdef DEBUG_KEEP_LATGAP
3113 if (gDebugFlag2) {
3114 std::cout << "\n " << SIMTIME << " keepLatGap() laneOffset=" << laneOffset
3115 << " latDist=" << latDist
3116 << " maneuverDist=" << maneuverDist
3117 << " state=" << toString((LaneChangeAction)state)
3118 << " blocked=" << toString((LaneChangeAction)blocked)
3119 << " gapFactor=" << gapFactor
3120 << " stayInLane=" << stayInLane << "\n"
3121 << " stayInEdge: surplusGapRight=" << surplusGapRight << " surplusGapLeft=" << surplusGapLeft << "\n";
3122 }
3123#endif
3124 // staying within the edge overrides all minGap considerations
3125 if (surplusGapLeft < 0 || surplusGapRight < 0) {
3126 gapFactor = 0;
3127 }
3128
3129 // maintain gaps to vehicles on the current lane
3130 // ignore vehicles that are too far behind
3131 const double netOverlap = -myVehicle.getVehicleType().getLength() * 0.5;
3132 updateGaps(leaders, myVehicle.getLane()->getRightSideOnEdge(), oldCenter, gapFactor, surplusGapRight, surplusGapLeft, true);
3133 updateGaps(followers, myVehicle.getLane()->getRightSideOnEdge(), oldCenter, gapFactor, surplusGapRight, surplusGapLeft, true, netOverlap);
3134
3135 if (laneOffset != 0) {
3136 // maintain gaps to vehicles on the target lane
3137 const double neighRight = getNeighRight(neighLane);
3138 updateGaps(neighLeaders, neighRight, oldCenter, gapFactor, surplusGapRight, surplusGapLeft, true);
3139 updateGaps(neighFollowers, neighRight, oldCenter, gapFactor, surplusGapRight, surplusGapLeft, true, netOverlap);
3140 }
3141#ifdef DEBUG_KEEP_LATGAP
3142 if (gDebugFlag2) {
3143 std::cout << " minGapLat: surplusGapRight=" << surplusGapRight << " surplusGapLeft=" << surplusGapLeft << "\n"
3144 << " lastGaps: right=" << myLastLateralGapRight << " left=" << myLastLateralGapLeft << "\n";
3145 }
3146#endif
3147 // we also need to track the physical gap, in addition to the psychological gap
3148 double physicalGapLeft = myLastLateralGapLeft == NO_NEIGHBOR ? surplusGapLeft : myLastLateralGapLeft;
3149 double physicalGapRight = myLastLateralGapRight == NO_NEIGHBOR ? surplusGapRight : myLastLateralGapRight;
3150
3151 const double halfLaneWidth = myVehicle.getLane()->getWidth() * 0.5;
3152 const double posLat = myVehicle.getLateralPositionOnLane() * (isOpposite() ? -1 : 1);
3153 if (stayInLane || laneOffset == 1) {
3154 // do not move past the right boundary of the current lane (traffic wasn't checked there)
3155 // but assume it's ok to be where we are in case we are already beyond
3156 surplusGapRight = MIN2(surplusGapRight, MAX2(0.0, halfLaneWidth + posLat - halfWidth));
3157 physicalGapRight = MIN2(physicalGapRight, MAX2(0.0, halfLaneWidth + posLat - halfWidth));
3158 }
3159 if (stayInLane || laneOffset == -1) {
3160 // do not move past the left boundary of the current lane (traffic wasn't checked there)
3161 // but assume it's ok to be where we are in case we are already beyond
3162 surplusGapLeft = MIN2(surplusGapLeft, MAX2(0.0, halfLaneWidth - posLat - halfWidth));
3163 physicalGapLeft = MIN2(physicalGapLeft, MAX2(0.0, halfLaneWidth - posLat - halfWidth));
3164 }
3165#ifdef DEBUG_KEEP_LATGAP
3166 if (gDebugFlag2) {
3167 std::cout << " stayInLane: surplusGapRight=" << surplusGapRight << " surplusGapLeft=" << surplusGapLeft << "\n";
3168 }
3169#endif
3170
3171 if (surplusGapRight + surplusGapLeft < 0) {
3172 // insufficient lateral space to fulfill all requirements. apportion space proportionally
3173 if ((state & LCA_CHANGE_REASONS) == 0) {
3174 state |= LCA_SUBLANE;
3175 }
3176 const double equalDeficit = 0.5 * (surplusGapLeft + surplusGapRight);
3177 if (surplusGapRight < surplusGapLeft) {
3178 // shift further to the left but no further than there is physical space
3179 const double delta = MIN2(equalDeficit - surplusGapRight, physicalGapLeft);
3180 latDist = delta;
3181 maneuverDist = delta;
3182#ifdef DEBUG_KEEP_LATGAP
3183 if (gDebugFlag2) {
3184 std::cout << " insufficient latSpace, move left: delta=" << delta << "\n";
3185 }
3186#endif
3187 } else {
3188 // shift further to the right but no further than there is physical space
3189 const double delta = MIN2(equalDeficit - surplusGapLeft, physicalGapRight);
3190 latDist = -delta;
3191 maneuverDist = -delta;
3192#ifdef DEBUG_KEEP_LATGAP
3193 if (gDebugFlag2) {
3194 std::cout << " insufficient latSpace, move right: delta=" << delta << "\n";
3195 }
3196#endif
3197 }
3198 } else {
3199 // sufficient space. move as far as the gaps permit
3200 latDist = MAX2(MIN2(latDist, surplusGapLeft), -surplusGapRight);
3201 maneuverDist = MAX2(MIN2(maneuverDist, surplusGapLeft), -surplusGapRight);
3202 if ((state & LCA_KEEPRIGHT) != 0 && maneuverDist != oldManeuverDist) {
3203 // don't start keepRight unless it can be completed
3204 latDist = oldLatDist;
3205 maneuverDist = oldManeuverDist;
3206 }
3207#ifdef DEBUG_KEEP_LATGAP
3208 if (gDebugFlag2) {
3209 std::cout << " adapted latDist=" << latDist << " maneuverDist=" << maneuverDist << " (old=" << oldLatDist << ")\n";
3210 }
3211#endif
3212 }
3213 // take into account overriding traci sublane-request
3215 // @note: the influence is reset in MSAbstractLaneChangeModel::setOwnState at the end of the lane-changing code for this vehicle
3216 latDist = myVehicle.getInfluencer().getLatDist();
3217 maneuverDist = myVehicle.getInfluencer().getLatDist();
3218 state |= LCA_TRACI;
3219#ifdef DEBUG_KEEP_LATGAP
3220 if (gDebugFlag2) {
3221 std::cout << " traci influenced latDist=" << latDist << "\n";
3222 }
3223#endif
3224 }
3225 // if we cannot move in the desired direction, consider the maneuver blocked anyway
3226 const bool nonSublaneChange = (state & (LCA_STRATEGIC | LCA_COOPERATIVE | LCA_SPEEDGAIN | LCA_KEEPRIGHT)) != 0;
3227 const bool traciChange = (state & LCA_TRACI) != 0;
3228 if (nonSublaneChange && !traciChange) {
3229 if ((latDist < NUMERICAL_EPS * myVehicle.getActionStepLengthSecs()) && (oldLatDist > 0)) {
3230#ifdef DEBUG_KEEP_LATGAP
3231 if (gDebugFlag2) {
3232 std::cout << " wanted changeToLeft oldLatDist=" << oldLatDist << ", blocked latGap changeToRight\n";
3233 }
3234#endif
3235 latDist = oldLatDist; // restore old request for usage in decideDirection()
3237 } else if ((latDist > -NUMERICAL_EPS * myVehicle.getActionStepLengthSecs()) && (oldLatDist < 0)) {
3238#ifdef DEBUG_KEEP_LATGAP
3239 if (gDebugFlag2) {
3240 std::cout << " wanted changeToRight oldLatDist=" << oldLatDist << ", blocked latGap changeToLeft\n";
3241 }
3242#endif
3243 latDist = oldLatDist; // restore old request for usage in decideDirection()
3245 }
3246 }
3247 // if we move, even though we wish to stay, update the change reason (except for TraCI)
3248 if (fabs(latDist) > NUMERICAL_EPS * myVehicle.getActionStepLengthSecs() && oldLatDist == 0) {
3249 state &= (~(LCA_CHANGE_REASONS | LCA_STAY) | LCA_TRACI);
3250 }
3251 // update blocked status
3252 if (fabs(latDist - oldLatDist) > NUMERICAL_EPS * myVehicle.getActionStepLengthSecs()) {
3253#ifdef DEBUG_KEEP_LATGAP
3254 if (gDebugFlag2) {
3255 std::cout << " latDistUpdated=" << latDist << " oldLatDist=" << oldLatDist << "\n";
3256 }
3257#endif
3258 blocked = checkBlocking(neighLane, latDist, maneuverDist, laneOffset, leaders, followers, blockers, neighLeaders, neighFollowers, neighBlockers, nullptr, nullptr, nonSublaneChange);
3259 }
3260 if (fabs(latDist) > NUMERICAL_EPS * myVehicle.getActionStepLengthSecs()) {
3261 state = (state & ~LCA_STAY);
3262 if ((state & LCA_CHANGE_REASONS) == 0) {
3263 state |= LCA_SUBLANE;
3264 }
3265 } else {
3266 if ((state & LCA_SUBLANE) != 0) {
3267 state |= LCA_STAY;
3268 }
3269 // avoid setting blinker due to numerical issues
3270 latDist = 0;
3271 }
3272#if defined(DEBUG_KEEP_LATGAP) || defined(DEBUG_STATE)
3273 if (gDebugFlag2) {
3274 std::cout << " latDist2=" << latDist
3275 << " state2=" << toString((LaneChangeAction)state)
3276 << " lastGapLeft=" << myLastLateralGapLeft
3277 << " lastGapRight=" << myLastLateralGapRight
3278 << " blockedAfter=" << toString((LaneChangeAction)blocked)
3279 << "\n";
3280 }
3281#endif
3282 return state;
3283}
3284
3285
3286void
3287MSLCM_SL2015::updateGaps(const MSLeaderDistanceInfo& others, double foeOffset, double oldCenter, double gapFactor,
3288 double& surplusGapRight, double& surplusGapLeft,
3289 bool saveMinGap, double netOverlap,
3290 double latDist,
3291 std::vector<CLeaderDist>* collectBlockers) {
3292 if (others.hasVehicles()) {
3293 const double halfWidth = getWidth() * 0.5 + NUMERICAL_EPS;
3294 const double baseMinGap = myMinGapLat;
3295 for (int i = 0; i < others.numSublanes(); ++i) {
3296 if (others[i].first != 0 && others[i].second <= 0
3297 && myCFRelated.count(others[i].first) == 0
3298 && (netOverlap == 0 || others[i].second + others[i].first->getVehicleType().getMinGap() < netOverlap)) {
3300 const MSVehicle* foe = others[i].first;
3301 const double res = MSGlobals::gLateralResolution > 0 ? MSGlobals::gLateralResolution : others[i].first->getLane()->getWidth();
3302 double foeRight, foeLeft;
3303 others.getSublaneBorders(i, foeOffset, foeRight, foeLeft);
3304 const double foeCenter = foeRight + 0.5 * res;
3305 const double gap = MIN2(fabs(foeRight - oldCenter), fabs(foeLeft - oldCenter)) - halfWidth;
3307 const double desiredMinGap = baseMinGap * deltaV / LATGAP_SPEED_THRESHOLD;
3308 const double currentMinGap = desiredMinGap * gapFactor; // pushy vehicles may accept a lower lateral gap temporarily
3309 /*
3310 if (netOverlap != 0) {
3311 // foe vehicle is follower with its front ahead of the ego midpoint
3312 // scale gap requirements so it gets lower for foe which are further behind ego
3313 //
3314 // relOverlap approaches 0 as the foe gets closer to the midpoint and it equals 1 if the foe is driving head-to-head
3315 const double relOverlap = 1 - (others[i].second + others[i].first->getVehicleType().getMinGap()) / netOverlap;
3316 currentMinGap *= currOverlap * relOverlap;
3317 }
3318 */
3319#if defined(DEBUG_BLOCKING) || defined(DEBUG_KEEP_LATGAP)
3320 if (debugVehicle()) {
3321 std::cout << " updateGaps"
3322 << " i=" << i
3323 << " foe=" << foe->getID()
3324 << " foeRight=" << foeRight
3325 << " foeLeft=" << foeLeft
3326 << " oldCenter=" << oldCenter
3327 << " gap=" << others[i].second
3328 << " latgap=" << gap
3329 << " currentMinGap=" << currentMinGap
3330 << " surplusGapRight=" << surplusGapRight
3331 << " surplusGapLeft=" << surplusGapLeft
3332 << "\n";
3333 }
3334#endif
3335
3336 // If foe is maneuvering towards ego, reserve some additional distance.
3337 // But don't expect the foe to come closer than currentMinGap if it isn't already there.
3338 // (XXX: How can the ego know the foe's maneuver dist?)
3339 if (foeCenter < oldCenter) { // && foe->getLaneChangeModel().getSpeedLat() > 0) {
3340 const double foeManeuverDist = MAX2(0., foe->getLaneChangeModel().getManeuverDist());
3341 surplusGapRight = MIN3(surplusGapRight, gap - currentMinGap, MAX2(currentMinGap, gap - foeManeuverDist));
3342 } else { //if (foeCenter > oldCenter && foe->getLaneChangeModel().getSpeedLat() < 0) {
3343 const double foeManeuverDist = -MIN2(0., foe->getLaneChangeModel().getManeuverDist());
3344 surplusGapLeft = MIN3(surplusGapLeft, gap - currentMinGap, MAX2(currentMinGap, gap - foeManeuverDist));
3345 }
3346 if (saveMinGap) {
3347 if (foeCenter < oldCenter) {
3348#if defined(DEBUG_BLOCKING) || defined(DEBUG_KEEP_LATGAP)
3349 if (gDebugFlag2 && gap < myLastLateralGapRight) {
3350 std::cout << " new minimum rightGap=" << gap << "\n";
3351 }
3352#endif
3354 } else {
3355#if defined(DEBUG_BLOCKING) || defined(DEBUG_KEEP_LATGAP)
3356 if (gDebugFlag2 && gap < myLastLateralGapLeft) {
3357 std::cout << " new minimum leftGap=" << gap << "\n";
3358 }
3359#endif
3361 }
3362 }
3363 if (collectBlockers != nullptr) {
3364 // check if the vehicle is blocking a desire lane change
3365 if ((foeCenter < oldCenter && latDist < 0 && gap < (desiredMinGap - latDist))
3366 || (foeCenter > oldCenter && latDist > 0 && gap < (desiredMinGap + latDist))) {
3367 collectBlockers->push_back(others[i]);
3368 }
3369 }
3370 }
3371 }
3372 }
3373}
3374
3375
3376double
3378 return myVehicle.getVehicleType().getWidth() + NUMERICAL_EPS;
3379}
3380
3381
3382double
3383MSLCM_SL2015::computeSpeedLat(double latDist, double& maneuverDist, bool urgent) const {
3384 int currentDirection = mySpeedLat >= 0 ? 1 : -1;
3385 int directionWish = latDist >= 0 ? 1 : -1;
3386 double maxSpeedLat = myVehicle.getVehicleType().getMaxSpeedLat();
3387 double accelLat = myAccelLat;
3388 if (!urgent && (myLeftSpace > POSITION_EPS || myMaxSpeedLatFactor < 0)) {
3389 const double speedBound = myMaxSpeedLatStanding + myMaxSpeedLatFactor * myVehicle.getSpeed();
3390 if (myMaxSpeedLatFactor >= 0) {
3391 // speedbound increases with speed and needs an upper bound
3392 maxSpeedLat = MIN2(maxSpeedLat, speedBound);
3393 } else {
3394 // speedbound decreases with speed and needs a lower bound
3395 // (only useful if myMaxSpeedLatStanding > maxSpeedLat)
3396 maxSpeedLat = MAX2(maxSpeedLat, speedBound);
3397 // increase (never decrease) lateral acceleration in proportion
3398 accelLat *= MAX2(1.0, speedBound / myVehicle.getVehicleType().getMaxSpeedLat());
3399 }
3400 }
3401
3402#ifdef DEBUG_MANEUVER
3403 if (debugVehicle()) {
3404 std::cout << SIMTIME
3405 << " veh=" << myVehicle.getID()
3406 << " computeSpeedLat()"
3407 << " latDist=" << latDist
3408 << " maneuverDist=" << maneuverDist
3409 << " urgent=" << urgent
3410 << " speedLat=" << mySpeedLat
3411 << " currentDirection=" << currentDirection
3412 << " directionWish=" << directionWish
3413 << " myLeftSpace=" << myLeftSpace
3414 << " maxSpeedLat=" << maxSpeedLat
3415 << std::endl;
3416 }
3417#endif
3418 // reduced lateral speed (in the desired direction). Don't change direction against desired.
3419 double speedDecel;
3420 if (directionWish == 1) {
3421 speedDecel = MAX2(mySpeedLat - ACCEL2SPEED(accelLat), 0.);
3422 } else {
3423 speedDecel = MIN2(mySpeedLat + ACCEL2SPEED(accelLat), 0.);
3424 }
3425 // increased lateral speed (in the desired direction)
3426 double speedAccel = MAX2(MIN2(mySpeedLat + directionWish * ACCEL2SPEED(accelLat), maxSpeedLat), -maxSpeedLat);
3427
3428 // can we reach the target distance in a single step? (XXX: assumes "Euler" update)
3429 double speedBound = DIST2SPEED(latDist);
3430 // for lat-gap keeping maneuvres myOrigLatDist may be 0
3431 const double fullLatDist = latDist > 0 ? MIN2(mySafeLatDistLeft, MAX2(maneuverDist, latDist)) : MAX2(-mySafeLatDistRight, MIN2(maneuverDist, latDist));
3432
3433 // update maneuverDist, if safety constraints apply in its direction
3434 if (maneuverDist * latDist > 0) {
3435 maneuverDist = fullLatDist;
3436 }
3437
3438#ifdef DEBUG_MANEUVER
3439 if (debugVehicle()) {
3440 std::cout << " mySafeLatDistRight=" << mySafeLatDistRight
3441 << " mySafeLatDistLeft=" << mySafeLatDistLeft
3442 << " fullLatDist=" << fullLatDist
3443 << " speedAccel=" << speedAccel
3444 << " speedDecel=" << speedDecel
3445 << " speedBound=" << speedBound
3446 << std::endl;
3447 }
3448#endif
3449 if (speedDecel * speedAccel <= 0 && (
3450 // speedAccel and speedDecel bracket speed 0. This means we can end the maneuver
3451 (latDist >= 0 && speedAccel >= speedBound && speedBound >= speedDecel)
3452 || (latDist <= 0 && speedAccel <= speedBound && speedBound <= speedDecel))) {
3453 // we can reach the desired value in this step
3454#ifdef DEBUG_MANEUVER
3455 if (debugVehicle()) {
3456 std::cout << " computeSpeedLat a)\n";
3457 }
3458#endif
3459 return speedBound;
3460 }
3461 // are we currently moving in the wrong direction?
3462 if (latDist * mySpeedLat < 0) {
3463#ifdef DEBUG_MANEUVER
3464 if (debugVehicle()) {
3465 std::cout << " computeSpeedLat b)\n";
3466 }
3467#endif
3468 return speedAccel;
3469 }
3470 // check if the remaining distance allows to accelerate laterally
3471 double minDistAccel = SPEED2DIST(speedAccel) + currentDirection * MSCFModel::brakeGapEuler(fabs(speedAccel), accelLat, 0); // most we can move in the target direction
3472 if ((fabs(minDistAccel) < fabs(fullLatDist)) || (fabs(minDistAccel - fullLatDist) < NUMERICAL_EPS)) {
3473#ifdef DEBUG_MANEUVER
3474 if (debugVehicle()) {
3475 std::cout << " computeSpeedLat c)\n";
3476 }
3477#endif
3478 return speedAccel;
3479 } else {
3480#ifdef DEBUG_MANEUVER
3481 if (debugVehicle()) {
3482 std::cout << " minDistAccel=" << minDistAccel << "\n";
3483 }
3484#endif
3485 // check if the remaining distance allows to maintain current lateral speed
3486 double minDistCurrent = SPEED2DIST(mySpeedLat) + currentDirection * MSCFModel::brakeGapEuler(fabs(mySpeedLat), accelLat, 0);
3487 if ((fabs(minDistCurrent) < fabs(fullLatDist)) || (fabs(minDistCurrent - fullLatDist) < NUMERICAL_EPS)) {
3488#ifdef DEBUG_MANEUVER
3489 if (debugVehicle()) {
3490 std::cout << " computeSpeedLat d)\n";
3491 }
3492#endif
3493 return mySpeedLat;
3494 }
3495 }
3496 // reduce lateral speed
3497#ifdef DEBUG_MANEUVER
3498 if (debugVehicle()) {
3499 std::cout << " computeSpeedLat e)\n";
3500 }
3501#endif
3502 return speedDecel;
3503}
3504
3505
3506void
3507MSLCM_SL2015::commitManoeuvre(int blocked, int blockedFully,
3508 const MSLeaderDistanceInfo& leaders,
3509 const MSLeaderDistanceInfo& neighLeaders,
3510 const MSLane& neighLane,
3511 double maneuverDist) {
3512 if (!blocked && !blockedFully && !myCanChangeFully) {
3513 // round to full action steps
3514 double secondsToLeaveLane;
3516 secondsToLeaveLane = ceil(fabs(maneuverDist) / myVehicle.getVehicleType().getMaxSpeedLat() / myVehicle.getActionStepLengthSecs()) * myVehicle.getActionStepLengthSecs();
3517 // XXX myAccelLat must be taken into account (refs #3601, see ballistic case for solution)
3518
3519 // XXX This also causes probs: if the difference between the current speed and the committed is higher than the maximal decel,
3520 // the vehicle may pass myLeftSpace before completing the maneuver.
3521 myCommittedSpeed = MIN3(myLeftSpace / secondsToLeaveLane,
3524#if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3525 if (debugVehicle()) {
3526 std::cout << SIMTIME << " veh=" << myVehicle.getID() << " myCommittedSpeed=" << myCommittedSpeed << " leftSpace=" << myLeftSpace << " secondsToLeave=" << secondsToLeaveLane << "\n";
3527 }
3528#endif
3529 } else {
3530
3531 // Calculate seconds needed for leaving lane assuming start from lateral speed zero, and lat.accel == -lat.decel
3532 secondsToLeaveLane = MSCFModel::estimateArrivalTime(fabs(maneuverDist), 0., 0., myVehicle.getVehicleType().getMaxSpeedLat(), myAccelLat, myAccelLat);
3533 // round to full action steps
3534 secondsToLeaveLane = ceil(secondsToLeaveLane / myVehicle.getActionStepLengthSecs()) * myVehicle.getActionStepLengthSecs();
3535
3536 // committed speed will eventually be pushed into a drive item during the next planMove() step. This item
3537 // will not be read before the next action step at current time + actionStepLength-TS, so we need to schedule the corresponding speed.
3538 const double timeTillActionStep = myVehicle.getActionStepLengthSecs() - TS;
3539 const double nextActionStepSpeed = MAX2(0., myVehicle.getSpeed() + timeTillActionStep * myVehicle.getAcceleration());
3540 double nextLeftSpace;
3541 if (nextActionStepSpeed > 0.) {
3542 nextLeftSpace = myLeftSpace - timeTillActionStep * (myVehicle.getSpeed() + nextActionStepSpeed) * 0.5;
3543 } else if (myVehicle.getAcceleration() == 0) {
3544 nextLeftSpace = myLeftSpace;
3545 } else {
3546 assert(myVehicle.getAcceleration() < 0.);
3547 nextLeftSpace = myLeftSpace + (myVehicle.getSpeed() * myVehicle.getSpeed() / myVehicle.getAcceleration()) * 0.5;
3548 }
3549 const double avoidArrivalSpeed = nextActionStepSpeed + ACCEL2SPEED(MSCFModel::avoidArrivalAccel(
3550 nextLeftSpace, secondsToLeaveLane - timeTillActionStep, nextActionStepSpeed, myVehicle.getCarFollowModel().getEmergencyDecel()));
3551
3552 myCommittedSpeed = MIN3(avoidArrivalSpeed,
3555
3556#if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3557 if (gDebugFlag2) {
3558 std::cout << SIMTIME
3559 << " veh=" << myVehicle.getID()
3560 << " avoidArrivalSpeed=" << avoidArrivalSpeed
3561 << " currentSpeed=" << myVehicle.getSpeed()
3562 << " myLeftSpace=" << myLeftSpace
3563 << "\n nextLeftSpace=" << nextLeftSpace
3564 << " nextActionStepSpeed=" << nextActionStepSpeed
3565 << " nextActionStepRemainingSeconds=" << secondsToLeaveLane - timeTillActionStep
3566 << "\n";
3567 }
3568#endif
3569 }
3570 myCommittedSpeed = commitFollowSpeed(myCommittedSpeed, maneuverDist, secondsToLeaveLane, leaders, myVehicle.getLane()->getRightSideOnEdge());
3571 myCommittedSpeed = commitFollowSpeed(myCommittedSpeed, maneuverDist, secondsToLeaveLane, neighLeaders, neighLane.getRightSideOnEdge());
3573 myCommittedSpeed = 0;
3574 }
3575#if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3576 if (gDebugFlag2) {
3577 std::cout << SIMTIME
3578 << " veh=" << myVehicle.getID()
3579 << " secondsToLeave=" << secondsToLeaveLane
3581 << " committed=" << myCommittedSpeed
3582 << "\n";
3583 }
3584#endif
3585 }
3586}
3587
3588double
3589MSLCM_SL2015::commitFollowSpeed(double speed, double latDist, double secondsToLeaveLane, const MSLeaderDistanceInfo& leaders, double foeOffset) const {
3590 if (leaders.hasVehicles()) {
3591 // we distinguish 3 cases
3592 // - vehicles with lateral overlap at the end of the maneuver: try to follow safely
3593 // - vehicles with overlap at the start of the maneuver: avoid collision within secondsToLeaveLane
3594 // - vehicles without overlap: ignore
3595
3596 const double maxDecel = myVehicle.getCarFollowModel().getMaxDecel();
3597 // temporarily use another decel value
3598 MSCFModel& cfmodel = const_cast<MSCFModel&>(myVehicle.getCarFollowModel());
3599 cfmodel.setMaxDecel(maxDecel / getSafetyFactor());
3600
3601 const double vehWidth = getWidth();
3602 const double rightVehSide = myVehicle.getCenterOnEdge() - 0.5 * vehWidth;
3603 const double leftVehSide = rightVehSide + vehWidth;
3604 const double rightVehSideDest = rightVehSide + latDist;
3605 const double leftVehSideDest = leftVehSide + latDist;
3606#if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3607 if (gDebugFlag2) {
3608 std::cout << " commitFollowSpeed"
3609 << " latDist=" << latDist
3610 << " foeOffset=" << foeOffset
3611 << " vehRight=" << rightVehSide
3612 << " vehLeft=" << leftVehSide
3613 << " destRight=" << rightVehSideDest
3614 << " destLeft=" << leftVehSideDest
3615 << "\n";
3616 }
3617#endif
3618 for (int i = 0; i < leaders.numSublanes(); ++i) {
3619 CLeaderDist vehDist = leaders[i];
3620 if (vehDist.first != 0) {
3621 const MSVehicle* leader = vehDist.first;
3622 // only check the current stripe occuped by foe (transform into edge-coordinates)
3623 double foeRight, foeLeft;
3624 leaders.getSublaneBorders(i, foeOffset, foeRight, foeLeft);
3625#if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3626 if (gDebugFlag2) {
3627 std::cout << " foe=" << vehDist.first->getID()
3628 << " gap=" << vehDist.second
3629 << " secGap=" << myVehicle.getCarFollowModel().getSecureGap(&myVehicle, leader, myVehicle.getSpeed(), leader->getSpeed(), leader->getCarFollowModel().getMaxDecel())
3630 << " foeRight=" << foeRight
3631 << " foeLeft=" << foeLeft
3632 << " overlapBefore=" << overlap(rightVehSide, leftVehSide, foeRight, foeLeft)
3633 << " overlapDest=" << overlap(rightVehSideDest, leftVehSideDest, foeRight, foeLeft)
3634 << "\n";
3635 }
3636#endif
3637 if (overlap(rightVehSideDest, leftVehSideDest, foeRight, foeLeft)) {
3638 // case 1
3639 const double vSafe = myVehicle.getCarFollowModel().followSpeed(
3640 &myVehicle, speed, vehDist.second, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel());
3641 speed = MIN2(speed, vSafe);
3642#if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3643 if (gDebugFlag2) {
3644 std::cout << " case1 vsafe=" << vSafe << " speed=" << speed << "\n";
3645 }
3646#endif
3647 } else if (overlap(rightVehSide, leftVehSide, foeRight, foeLeft)) {
3648 // case 2
3649 const double vSafe = myVehicle.getCarFollowModel().followSpeedTransient(
3650 secondsToLeaveLane,
3651 &myVehicle, speed, vehDist.second, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel());
3652 speed = MIN2(speed, vSafe);
3653#if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3654 if (gDebugFlag2) {
3655 std::cout << " case2 vsafe=" << vSafe << " speed=" << speed << "\n";
3656 }
3657#endif
3658 }
3659 }
3660 }
3661 // restore original deceleration
3662 cfmodel.setMaxDecel(maxDecel);
3663
3664 }
3665 return speed;
3666}
3667
3668double
3670 return 1 / ((1 + 0.5 * myImpatience) * myAssertive);
3671}
3672
3673double
3675 return myOppositeParam <= 0 ? std::numeric_limits<double>::max() : 1 / myOppositeParam;
3676}
3677
3678
3679std::string
3680MSLCM_SL2015::getParameter(const std::string& key) const {
3682 return toString(myStrategicParam);
3683 } else if (key == toString(SUMO_ATTR_LCA_COOPERATIVE_PARAM)) {
3685 } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAIN_PARAM)) {
3686 return toString(mySpeedGainParam);
3687 } else if (key == toString(SUMO_ATTR_LCA_KEEPRIGHT_PARAM)) {
3688 return toString(myKeepRightParam);
3689 } else if (key == toString(SUMO_ATTR_LCA_OPPOSITE_PARAM)) {
3690 return toString(myOppositeParam);
3691 } else if (key == toString(SUMO_ATTR_LCA_SUBLANE_PARAM)) {
3692 return toString(mySublaneParam);
3693 } else if (key == toString(SUMO_ATTR_MINGAP_LAT)) {
3694 return toString(myMinGapLat);
3695 } else if (key == toString(SUMO_ATTR_LCA_PUSHY)) {
3696 return toString(myPushy);
3697 } else if (key == toString(SUMO_ATTR_LCA_PUSHYGAP)) {
3698 return toString((myPushy - 1) * myMinGapLat);
3699 } else if (key == toString(SUMO_ATTR_LCA_ASSERTIVE)) {
3700 return toString(myAssertive);
3701 } else if (key == toString(SUMO_ATTR_LCA_IMPATIENCE)) {
3702 return toString(myImpatience);
3703 } else if (key == toString(SUMO_ATTR_LCA_TIME_TO_IMPATIENCE)) {
3705 } else if (key == toString(SUMO_ATTR_LCA_ACCEL_LAT)) {
3706 return toString(myAccelLat);
3707 } else if (key == toString(SUMO_ATTR_LCA_LOOKAHEADLEFT)) {
3708 return toString(myLookaheadLeft);
3709 } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAINRIGHT)) {
3710 return toString(mySpeedGainRight);
3711 } else if (key == toString(SUMO_ATTR_LCA_LANE_DISCIPLINE)) {
3712 return toString(myLaneDiscipline);
3713 } else if (key == toString(SUMO_ATTR_LCA_SIGMA)) {
3714 return toString(mySigma);
3719 } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAIN_LOOKAHEAD)) {
3723 } else if (key == toString(SUMO_ATTR_LCA_COOPERATIVE_SPEED)) {
3725 } else if (key == toString(SUMO_ATTR_LCA_MAXSPEEDLATSTANDING)) {
3727 } else if (key == toString(SUMO_ATTR_LCA_MAXSPEEDLATFACTOR)) {
3729 } else if (key == toString(SUMO_ATTR_LCA_MAXDISTLATSTANDING)) {
3731 // access to internal state for debugging in sumo-gui (not documented since it may change at any time)
3732 } else if (key == "speedGainProbabilityRight") {
3734 } else if (key == "speedGainProbabilityLeft") {
3736 } else if (key == "keepRightProbability") {
3738 } else if (key == "lookAheadSpeed") {
3739 return toString(myLookAheadSpeed);
3740 } else if (key == "sigmaState") {
3741 return toString(mySigmaState);
3742 // motivation relative to threshold
3743 } else if (key == "speedGainRP") {
3745 } else if (key == "speedGainLP") {
3747 } else if (key == "keepRightP") {
3749 }
3750 throw InvalidArgument("Parameter '" + key + "' is not supported for laneChangeModel of type '" + toString(myModel) + "'");
3751}
3752
3753void
3754MSLCM_SL2015::setParameter(const std::string& key, const std::string& value) {
3755 double doubleValue;
3756 try {
3757 doubleValue = StringUtils::toDouble(value);
3758 } catch (NumberFormatException&) {
3759 throw InvalidArgument("Setting parameter '" + key + "' requires a number for laneChangeModel of type '" + toString(myModel) + "'");
3760 }
3762 myStrategicParam = doubleValue;
3763 } else if (key == toString(SUMO_ATTR_LCA_COOPERATIVE_PARAM)) {
3764 myCooperativeParam = doubleValue;
3765 } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAIN_PARAM)) {
3766 mySpeedGainParam = doubleValue;
3767 } else if (key == toString(SUMO_ATTR_LCA_KEEPRIGHT_PARAM)) {
3768 myKeepRightParam = doubleValue;
3769 } else if (key == toString(SUMO_ATTR_LCA_OPPOSITE_PARAM)) {
3770 myOppositeParam = doubleValue;
3771 } else if (key == toString(SUMO_ATTR_LCA_SUBLANE_PARAM)) {
3772 mySublaneParam = doubleValue;
3773 } else if (key == toString(SUMO_ATTR_MINGAP_LAT)) {
3774 myMinGapLat = doubleValue;
3775 } else if (key == toString(SUMO_ATTR_LCA_PUSHY)) {
3776 myPushy = doubleValue;
3777 } else if (key == toString(SUMO_ATTR_LCA_PUSHYGAP)) {
3778 myPushy = 1 - doubleValue / myMinGapLat;
3779 } else if (key == toString(SUMO_ATTR_LCA_ASSERTIVE)) {
3780 myAssertive = doubleValue;
3781 } else if (key == toString(SUMO_ATTR_LCA_IMPATIENCE)) {
3782 myImpatience = doubleValue;
3783 myMinImpatience = doubleValue;
3784 } else if (key == toString(SUMO_ATTR_LCA_TIME_TO_IMPATIENCE)) {
3785 myTimeToImpatience = doubleValue;
3786 } else if (key == toString(SUMO_ATTR_LCA_ACCEL_LAT)) {
3787 myAccelLat = doubleValue;
3789 myTurnAlignmentDist = doubleValue;
3790 } else if (key == toString(SUMO_ATTR_LCA_LOOKAHEADLEFT)) {
3791 myLookaheadLeft = doubleValue;
3792 } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAINRIGHT)) {
3793 mySpeedGainRight = doubleValue;
3794 } else if (key == toString(SUMO_ATTR_LCA_LANE_DISCIPLINE)) {
3795 myLaneDiscipline = doubleValue;
3796 } else if (key == toString(SUMO_ATTR_LCA_SIGMA)) {
3797 mySigma = doubleValue;
3799 myKeepRightAcceptanceTime = doubleValue;
3801 myOvertakeDeltaSpeedFactor = doubleValue;
3802 } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAIN_LOOKAHEAD)) {
3803 mySpeedGainLookahead = doubleValue;
3805 myRoundaboutBonus = doubleValue;
3806 } else if (key == toString(SUMO_ATTR_LCA_COOPERATIVE_SPEED)) {
3807 myCooperativeSpeed = doubleValue;
3808 } else if (key == toString(SUMO_ATTR_LCA_MAXSPEEDLATSTANDING)) {
3809 myMaxSpeedLatStanding = doubleValue;
3810 } else if (key == toString(SUMO_ATTR_LCA_MAXSPEEDLATFACTOR)) {
3811 myMaxSpeedLatFactor = doubleValue;
3812 } else if (key == toString(SUMO_ATTR_LCA_MAXDISTLATSTANDING)) {
3813 myMaxDistLatStanding = doubleValue;
3814 // access to internal state
3815 } else if (key == "speedGainProbabilityRight") {
3816 mySpeedGainProbabilityRight = doubleValue;
3817 } else if (key == "speedGainProbabilityLeft") {
3818 mySpeedGainProbabilityLeft = doubleValue;
3819 } else if (key == "keepRightProbability") {
3820 myKeepRightProbability = doubleValue;
3821 } else if (key == "lookAheadSpeed") {
3822 myLookAheadSpeed = doubleValue;
3823 } else if (key == "sigmaState") {
3824 mySigmaState = doubleValue;
3825 } else {
3826 throw InvalidArgument("Setting parameter '" + key + "' is not supported for laneChangeModel of type '" + toString(myModel) + "'");
3827 }
3829}
3830
3831
3832int
3834 int laneOffset,
3836 int blocked,
3837 const std::pair<MSVehicle*, double>& leader,
3838 const std::pair<MSVehicle*, double>& follower,
3839 const std::pair<MSVehicle*, double>& neighLead,
3840 const std::pair<MSVehicle*, double>& neighFollow,
3841 const MSLane& neighLane,
3842 const std::vector<MSVehicle::LaneQ>& preb,
3843 MSVehicle** lastBlocked,
3844 MSVehicle** firstBlocked) {
3845
3846 const LaneChangeAction alternatives = LCA_NONE; // @todo pas this data
3847
3848#ifdef DEBUG_WANTSCHANGE
3849 if (DEBUG_COND) {
3850 std::cout << "\nWANTS_CHANGE\n" << SIMTIME
3851 //<< std::setprecision(10)
3852 << " veh=" << myVehicle.getID()
3853 << " lane=" << myVehicle.getLane()->getID()
3854 << " neigh=" << neighLane.getID()
3855 << " pos=" << myVehicle.getPositionOnLane()
3856 << " posLat=" << myVehicle.getLateralPositionOnLane()
3857 << " speed=" << myVehicle.getSpeed()
3858 << " considerChangeTo=" << (laneOffset == -1 ? "right" : "left")
3859 << "\n";
3860 }
3861#endif
3862
3863 double latDist = 0;
3864 const double laneWidth = myVehicle.getLane()->getWidth();
3865 MSLeaderDistanceInfo leaders(leader, laneWidth);
3866 MSLeaderDistanceInfo followers(follower, laneWidth);
3867 MSLeaderDistanceInfo blockers(std::make_pair((MSVehicle*)nullptr, -1), laneWidth);
3868 MSLeaderDistanceInfo neighLeaders(neighLead, laneWidth);
3869 MSLeaderDistanceInfo neighFollowers(neighFollow, laneWidth);
3870 MSLeaderDistanceInfo neighBlockers(std::make_pair((MSVehicle*)nullptr, -1), laneWidth);
3871
3872 double maneuverDist;
3873 int result = _wantsChangeSublane(laneOffset,
3874 alternatives,
3875 leaders, followers, blockers,
3876 neighLeaders, neighFollowers, neighBlockers,
3877 neighLane, preb,
3878 lastBlocked, firstBlocked, latDist, maneuverDist, blocked);
3879
3880 myCanChangeFully = true;
3881 // ignore sublane motivation
3882 result &= ~LCA_SUBLANE;
3883 result |= getLCA(result, latDist);
3884
3885#if defined(DEBUG_WANTSCHANGE) || defined(DEBUG_STATE)
3886 if (DEBUG_COND) {
3887 if (result & LCA_WANTS_LANECHANGE) {
3888 std::cout << SIMTIME
3889 << " veh=" << myVehicle.getID()
3890 << " wantsChangeTo=" << (laneOffset == -1 ? "right" : "left")
3891 << ((result & LCA_URGENT) ? " (urgent)" : "")
3892 << ((result & LCA_CHANGE_TO_HELP) ? " (toHelp)" : "")
3893 << ((result & LCA_STRATEGIC) ? " (strat)" : "")
3894 << ((result & LCA_COOPERATIVE) ? " (coop)" : "")
3895 << ((result & LCA_SPEEDGAIN) ? " (speed)" : "")
3896 << ((result & LCA_KEEPRIGHT) ? " (keepright)" : "")
3897 << ((result & LCA_TRACI) ? " (traci)" : "")
3898 << ((blocked & LCA_BLOCKED) ? " (blocked)" : "")
3899 << ((blocked & LCA_OVERLAPPING) ? " (overlap)" : "")
3900 << "\n\n\n";
3901 }
3902 }
3903#endif
3904
3905 return result;
3906}
3907
3908
3909double
3910MSLCM_SL2015::getLeftBorder(bool checkOpposite) const {
3911 return (myVehicle.getLane()->getEdge().getWidth()
3912 + ((myVehicle.getLane()->getParallelOpposite() != nullptr && checkOpposite) ? myVehicle.getLane()->getParallelOpposite()->getEdge().getWidth() : 0));
3913}
3914
3915double
3917 if (isOpposite()) {
3919 } else {
3920 return myVehicle.getCenterOnEdge();
3921 }
3922}
3923
3924double
3925MSLCM_SL2015::getNeighRight(const MSLane& neighLane) const {
3926 if (isOpposite()) {
3928 } else if ((&myVehicle.getLane()->getEdge() != &neighLane.getEdge())) {
3930 } else {
3931 // the normal case
3932 return neighLane.getRightSideOnEdge();
3933 }
3934}
3935
3936
3937bool
3938MSLCM_SL2015::preventSliding(double maneuverDist) const {
3939 // prevent wide maneuvers with unsufficient forward space
3940 if (fabs(maneuverDist) > myMaxDistLatStanding) {
3941 // emergency vehicles should not be restricted (TODO solve this with LCA_URGENT)
3943 return false;
3944 }
3945 const double brakeGap = myVehicle.getCarFollowModel().brakeGap(myVehicle.getSpeed());
3946 const bool isSlide = fabs(maneuverDist) > myMaxDistLatStanding + brakeGap * fabs(myMaxSpeedLatFactor);
3947#ifdef DEBUG_SLIDING
3948 if (gDebugFlag2) {
3949 std::cout << SIMTIME << " veh=" << myVehicle.getID() << " bgap=" << brakeGap << " maneuverDist=" << maneuverDist
3950 << " mds=" << myMaxDistLatStanding << " isSlide=" << isSlide << "\n";
3951 }
3952#endif
3953 return isSlide;
3954 }
3955 return false;
3956}
3957
3958bool
3959MSLCM_SL2015::wantsKeepRight(double keepRightProb) const {
3961}
3962
3963
3964bool
3965MSLCM_SL2015::saveBlockerLength(double length, double foeLeftSpace) {
3966 const bool canReserve = MSLCHelper::canSaveBlockerLength(myVehicle, length, myLeftSpace);
3967 if (!isOpposite() && (canReserve || myLeftSpace > foeLeftSpace)) {
3969 if (myLeftSpace == 0 && foeLeftSpace < 0) {
3970 // called from opposite overtaking, myLeftSpace must be initialized
3972 }
3973 return true;
3974 } else {
3975 return false;
3976 }
3977}
3978
3979/****************************************************************************/
long long int SUMOTime
Definition: GUI.h:36
#define ARRIVALPOS_LAT_THRESHOLD
#define MAGIC_OFFSET
#define HELP_DECEL_FACTOR
#define SPEEDGAIN_MEMORY_FACTOR
#define LOOK_AHEAD_MIN_SPEED
#define LCA_RIGHT_IMPATIENCE
#define LOOK_FORWARD
#define HELP_OVERTAKE
#define REACT_TO_STOPPED_DISTANCE
#define KEEP_RIGHT_TIME
#define RELGAIN_NORMALIZATION_MIN_SPEED
#define CUT_IN_LEFT_SPEED_THRESHOLD
#define MAX_ONRAMP_LENGTH
#define SPEEDGAIN_DECAY_FACTOR
#define MIN_FALLBEHIND
#define LATGAP_SPEED_THRESHOLD
#define URGENCY
#define LOOK_AHEAD_SPEED_MEMORY
#define JAM_FACTOR
#define GAIN_PERCEPTION_THRESHOLD
#define DEBUG_COND
#define TURN_LANE_DIST
#define SPEED_GAIN_MIN_SECONDS
#define LATGAP_SPEED_THRESHOLD2
std::pair< const MSVehicle *, double > CLeaderDist
Definition: MSLeaderInfo.h:38
std::pair< const MSPerson *, double > PersonDist
Definition: MSPModel.h:41
#define STEPS2TIME(x)
Definition: SUMOTime.h:54
#define SPEED2DIST(x)
Definition: SUMOTime.h:44
#define ACCEL2SPEED(x)
Definition: SUMOTime.h:50
#define TS
Definition: SUMOTime.h:41
#define SIMTIME
Definition: SUMOTime.h:61
#define DIST2SPEED(x)
Definition: SUMOTime.h:46
#define SPEED2ACCEL(x)
Definition: SUMOTime.h:52
LatAlignmentDefinition
Possible ways to choose the lateral alignment, i.e., how vehicles align themselves within their lane.
@ RIGHT
drive on the right side
@ GIVEN
The alignment as offset is given.
@ DEFAULT
No information given; use default.
@ LEFT
drive on the left side
@ ARBITRARY
maintain the current alignment
@ NICE
align with the closest sublane border
@ COMPACT
align with the rightmost sublane that allows keeping the current speed
@ CENTER
drive in the middle
@ SVC_EMERGENCY
public emergency vehicles
@ RIGHT
At the rightmost side of the lane.
@ GIVEN
The position is given.
@ DEFAULT
No information given; use default.
@ LEFT
At the leftmost side of the lane.
@ CENTER
At the center of the lane.
@ PARTLEFT
The link is a partial left direction.
@ RIGHT
The link is a (hard) right direction.
@ TURN
The link is a 180 degree turn.
@ LEFT
The link is a (hard) left direction.
@ STRAIGHT
The link is a straight direction.
@ TURN_LEFTHAND
The link is a 180 degree turn (left-hand network)
@ PARTRIGHT
The link is a partial right direction.
@ NODIR
The link has no direction (is a dead end link)
LaneChangeAction
The state of a vehicle's lane-change behavior.
@ LCA_BLOCKED_LEFT
blocked left
@ LCA_KEEPRIGHT
The action is due to the default of keeping right "Rechtsfahrgebot".
@ LCA_CHANGE_TO_HELP
@ LCA_BLOCKED
blocked in all directions
@ LCA_NONE
@ LCA_URGENT
The action is urgent (to be defined by lc-model)
@ LCA_BLOCKED_BY_RIGHT_LEADER
The vehicle is blocked by right leader.
@ LCA_STAY
Needs to stay on the current lane.
@ LCA_SUBLANE
used by the sublane model
@ LCA_BLOCKED_BY_LEADER
blocked by leader
@ LCA_AMBACKBLOCKER
@ LCA_BLOCKED_BY_LEFT_FOLLOWER
The vehicle is blocked by left follower.
@ LCA_AMBLOCKINGLEADER
@ LCA_AMBLOCKINGFOLLOWER_DONTBRAKE
@ LCA_COOPERATIVE
The action is done to help someone else.
@ LCA_OVERLAPPING
The vehicle is blocked being overlapping.
@ LCA_LEFT
Wants go to the left.
@ LCA_MRIGHT
@ LCA_BLOCKED_RIGHT
blocked right
@ LCA_BLOCKED_BY_RIGHT_FOLLOWER
The vehicle is blocked by right follower.
@ LCA_STRATEGIC
The action is needed to follow the route (navigational lc)
@ LCA_AMBACKBLOCKER_STANDING
@ LCA_CHANGE_REASONS
reasons of lane change
@ LCA_TRACI
The action is due to a TraCI request.
@ LCA_SPEEDGAIN
The action is due to the wish to be faster (tactical lc)
@ LCA_WANTS_LANECHANGE
lane can change
@ LCA_RIGHT
Wants go to the right.
@ LCA_MLEFT
@ LCA_BLOCKED_BY_FOLLOWER
blocker by follower
@ LCA_BLOCKED_BY_LEFT_LEADER
@ LCA_AMBLOCKINGFOLLOWER
LaneChangeModel
@ SUMO_ATTR_LCA_PUSHY
@ SUMO_ATTR_LCA_COOPERATIVE_SPEED
@ SUMO_ATTR_LCA_ASSERTIVE
@ SUMO_ATTR_LCA_LANE_DISCIPLINE
@ SUMO_ATTR_LCA_TURN_ALIGNMENT_DISTANCE
@ SUMO_ATTR_LCA_PUSHYGAP
@ SUMO_ATTR_LCA_LOOKAHEADLEFT
@ SUMO_ATTR_LCA_SPEEDGAIN_PARAM
@ SUMO_ATTR_LCA_MAXDISTLATSTANDING
@ SUMO_ATTR_LCA_IMPATIENCE
@ SUMO_ATTR_LCA_COOPERATIVE_ROUNDABOUT
@ SUMO_ATTR_LCA_SPEEDGAIN_LOOKAHEAD
@ SUMO_ATTR_LCA_MAXSPEEDLATFACTOR
@ SUMO_ATTR_LCA_MAXSPEEDLATSTANDING
@ SUMO_ATTR_LCA_KEEPRIGHT_PARAM
@ SUMO_ATTR_LCA_COOPERATIVE_PARAM
@ SUMO_ATTR_LCA_OPPOSITE_PARAM
@ SUMO_ATTR_MINGAP_LAT
@ SUMO_ATTR_LCA_OVERTAKE_DELTASPEED_FACTOR
@ SUMO_ATTR_LCA_SUBLANE_PARAM
@ SUMO_ATTR_LCA_SIGMA
@ SUMO_ATTR_LCA_ACCEL_LAT
@ SUMO_ATTR_LCA_STRATEGIC_PARAM
@ SUMO_ATTR_LCA_KEEPRIGHT_ACCEPTANCE_TIME
@ SUMO_ATTR_LCA_TIME_TO_IMPATIENCE
@ SUMO_ATTR_LCA_SPEEDGAINRIGHT
int gPrecision
the precision for floating point outputs
Definition: StdDefs.cpp:25
bool gDebugFlag2
Definition: StdDefs.cpp:34
const double SUMO_const_laneWidth
Definition: StdDefs.h:48
#define UNUSED_PARAMETER(x)
Definition: StdDefs.h:30
T MIN3(T a, T b, T c)
Definition: StdDefs.h:84
T MIN2(T a, T b)
Definition: StdDefs.h:71
const double SUMO_const_haltingSpeed
the speed threshold at which vehicles are considered as halting
Definition: StdDefs.h:58
T MAX2(T a, T b)
Definition: StdDefs.h:77
T MAX3(T a, T b, T c)
Definition: StdDefs.h:91
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:46
A class responsible for exchanging messages between cars involved in lane-change interaction.
Interface for lane-change models.
double getForwardPos() const
get vehicle position relative to the forward direction lane
virtual void setOwnState(const int state)
int myPreviousState
lane changing state from the previous simulation step
double getManeuverDist() const
Returns the remaining unblocked distance for the current maneuver. (only used by sublane model)
int myOwnState
The current state of the vehicle.
MSLane * getShadowLane() const
Returns the lane the vehicle's shadow is on during continuous/sublane lane change.
double myCommittedSpeed
the speed when committing to a change maneuver
static bool myAllowOvertakingRight
whether overtaking on the right is permitted
const LaneChangeModel myModel
the type of this model
bool cancelRequest(int state, int laneOffset)
whether the influencer cancels the given request
double getMaxSpeedLat2() const
return the max of maxSpeedLat and lcMaxSpeedLatStanding
const MSCFModel & getCarFollowModel() const
The vehicle's car following model.
double mySpeedLat
the current lateral speed
MSVehicle & myVehicle
The vehicle this lane-changer belongs to.
double myLastLateralGapLeft
the minimum lateral gaps to other vehicles that were found when last changing to the left and right
virtual bool debugVehicle() const
whether the current vehicles shall be debugged
virtual double getArrivalPos() const
Returns this vehicle's desired arrivalPos for its current route (may change on reroute)
const SUMOVehicleParameter & getParameter() const
Returns the vehicle's parameter (including departure definition)
double getLength() const
Returns the vehicle's length.
const MSEdge * getEdge() const
Returns the edge the vehicle is currently at.
MSStop & getNextStop()
double getWaitingSeconds() const
Returns the number of seconds waited (speed was lesser than 0.1m/s)
SUMOVehicleClass getVClass() const
Returns the vehicle's access class.
const MSRoute & getRoute() const
Returns the current route.
const MSVehicleType & getVehicleType() const
Returns the vehicle's type definition.
The car-following model abstraction.
Definition: MSCFModel.h:55
virtual double maxNextSpeed(double speed, const MSVehicle *const veh) const
Returns the maximum speed given the current speed.
Definition: MSCFModel.cpp:270
virtual double getSecureGap(const MSVehicle *const, const MSVehicle *const, const double speed, const double leaderSpeed, const double leaderMaxDecel) const
Returns the minimum gap to reserve if the leader is braking at maximum (>=0)
Definition: MSCFModel.h:390
static double gapExtrapolation(const double duration, const double currentGap, double v1, double v2, double a1=0, double a2=0, const double maxV1=std::numeric_limits< double >::max(), const double maxV2=std::numeric_limits< double >::max())
return the resulting gap if, starting with gap currentGap, two vehicles continue with constant accele...
Definition: MSCFModel.cpp:542
virtual double minNextSpeedEmergency(double speed, const MSVehicle *const veh=0) const
Returns the minimum speed after emergency braking, given the current speed (depends on the numerical ...
Definition: MSCFModel.cpp:287
virtual double followSpeedTransient(double duration, const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel) const
Computes the vehicle's follow speed that avoids a collision for the given amount of time.
Definition: MSCFModel.cpp:331
double getEmergencyDecel() const
Get the vehicle type's maximal phisically possible deceleration [m/s^2].
Definition: MSCFModel.h:270
static double brakeGapEuler(const double speed, const double decel, const double headwayTime)
Definition: MSCFModel.cpp:90
static double avoidArrivalAccel(double dist, double time, double speed, double maxDecel)
Computes the acceleration needed to arrive not before the given time.
Definition: MSCFModel.cpp:499
virtual double minNextSpeed(double speed, const MSVehicle *const veh=0) const
Returns the minimum speed given the current speed (depends on the numerical update scheme and its ste...
Definition: MSCFModel.cpp:276
virtual void setMaxDecel(double decel)
Sets a new value for maximal comfortable deceleration [m/s^2].
Definition: MSCFModel.h:536
double getMaxAccel() const
Get the vehicle type's maximum acceleration [m/s^2].
Definition: MSCFModel.h:254
double brakeGap(const double speed) const
Returns the distance the vehicle needs to halt including driver's reaction time tau (i....
Definition: MSCFModel.h:373
double getMaxDecel() const
Get the vehicle type's maximal comfortable deceleration [m/s^2].
Definition: MSCFModel.h:262
static double estimateArrivalTime(double dist, double speed, double maxSpeed, double accel)
Computes the time needed to travel a distance dist given an initial speed and constant acceleration....
Definition: MSCFModel.cpp:428
virtual double followSpeed(const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle *const pred=0, const CalcReason usage=CalcReason::CURRENT) const =0
Computes the vehicle's follow speed (no dawdling)
double stopSpeed(const MSVehicle *const veh, const double speed, double gap, const CalcReason usage=CalcReason::CURRENT) const
Computes the vehicle's safe speed for approaching a non-moving obstacle (no dawdling)
Definition: MSCFModel.h:168
A road/street connecting two junctions.
Definition: MSEdge.h:77
const std::set< MSTransportable *, ComparatorNumericalIdLess > & getPersons() const
Returns this edge's persons set.
Definition: MSEdge.h:201
const std::vector< MSLane * > & getLanes() const
Returns this edge's lanes.
Definition: MSEdge.h:168
double getInternalFollowingLengthTo(const MSEdge *followerAfterInternal, SUMOVehicleClass vClass) const
returns the length of all internal edges on the junction until reaching the non-internal edge followe...
Definition: MSEdge.cpp:820
bool canChangeToOpposite() const
whether this edge allows changing to the opposite direction edge
Definition: MSEdge.cpp:1226
bool isInternal() const
return whether this edge is an internal edge
Definition: MSEdge.h:265
double getWidth() const
Returns the edges's width (sum over all lanes)
Definition: MSEdge.h:629
const std::vector< double > getSubLaneSides() const
Returns the right side offsets of this edge's sublanes.
Definition: MSEdge.h:634
static double gLateralResolution
Definition: MSGlobals.h:97
static bool gSemiImplicitEulerUpdate
Definition: MSGlobals.h:53
static bool gLefthand
Whether lefthand-drive is being simulated.
Definition: MSGlobals.h:168
static bool canSaveBlockerLength(const MSVehicle &veh, double requested, double leftSpace)
Definition: MSLCHelper.cpp:276
static double getRoundaboutDistBonus(const MSVehicle &veh, double bonusParam, const MSVehicle::LaneQ &curr, const MSVehicle::LaneQ &neigh, const MSVehicle::LaneQ &best)
Definition: MSLCHelper.cpp:43
static bool saveBlockerLength(const MSVehicle &veh, MSVehicle *blocker, int lcaCounter, double leftSpace, bool reliefConnection, double &leadingBlockerLength)
Definition: MSLCHelper.cpp:220
static bool divergentRoute(const MSVehicle &v1, const MSVehicle &v2)
return whether the vehicles are on the same junction but on divergent paths
Definition: MSLCHelper.cpp:288
std::vector< double > myLCAccelerationAdvices
vector of LC-related acceleration recommendations Filled in wantsChange() and applied in patchSpeed()
Definition: MSLCM_SL2015.h:397
double mySafeLatDistRight
the lateral distance the vehicle can safely move in the currently considered direction
Definition: MSLCM_SL2015.h:412
static bool overlap(double right, double left, double right2, double left2)
return whether the given intervals overlap
double _patchSpeed(double min, const double wanted, double max, const MSCFModel &cfModel)
double informLeaders(int blocked, int dir, const std::vector< CLeaderDist > &blockers, double remainingSeconds)
double myRoundaboutBonus
Definition: MSLCM_SL2015.h:451
void commitManoeuvre(int blocked, int blockedFully, const MSLeaderDistanceInfo &leaders, const MSLeaderDistanceInfo &neighLeaders, const MSLane &neighLane, double maneuverDist)
commit to lane change maneuvre potentially overriding safe speed
std::set< const MSVehicle * > myCFRelated
set of vehicles that are in a car-following relationship with ego (leader of followers)
Definition: MSLCM_SL2015.h:416
void prepareStep() override
double myKeepRightProbability
Definition: MSLCM_SL2015.h:386
double myMinImpatience
Definition: MSLCM_SL2015.h:435
double commitFollowSpeed(double speed, double latDist, double secondsToLeaveLane, const MSLeaderDistanceInfo &leaders, double foeOffset) const
compute speed when committing to an urgent change that is safe in regard to leading vehicles
double getLeftBorder(bool checkOpposite=true) const
return current edge width optionally extended by opposite direction lane width
double myChangeProbThresholdRight
Definition: MSLCM_SL2015.h:463
double informLeader(int blocked, int dir, const CLeaderDist &neighLead, double remainingSeconds)
double mySafeLatDistLeft
Definition: MSLCM_SL2015.h:413
MSLCM_SL2015(MSVehicle &v)
int computeSublaneShift(const MSEdge *prevEdge, const MSEdge *curEdge)
compute shift so that prevSublane + shift = newSublane
double patchSpeed(const double min, const double wanted, const double max, const MSCFModel &cfModel) override
Called to adapt the speed in order to allow a lane change. It uses information on LC-related desired ...
double getSafetyFactor() const override
return factor for modifying the safety constraints of the car-following model
double myCooperativeSpeed
Definition: MSLCM_SL2015.h:453
double computeSpeedLat(double latDist, double &maneuverDist, bool urgent) const override
decides the next lateral speed depending on the remaining lane change distance to be covered and upda...
std::vector< double > myExpectedSublaneSpeeds
expected travel speeds on all sublanes on the current edge(!)
Definition: MSLCM_SL2015.h:400
double getWidth() const
return the widht of this vehicle (padded for numerical stability)
bool myCanChangeFully
whether the current lane changing maneuver can be finished in a single step
Definition: MSLCM_SL2015.h:409
void addLCSpeedAdvice(const double vSafe)
Takes a vSafe (speed advice for speed in the next simulation step), converts it into an acceleration ...
void changed() override
int wantsChange(int laneOffset, MSAbstractLaneChangeModel::MSLCMessager &msgPass, int blocked, const std::pair< MSVehicle *, double > &leader, const std::pair< MSVehicle *, double > &follower, const std::pair< MSVehicle *, double > &neighLead, const std::pair< MSVehicle *, double > &neighFollow, const MSLane &neighLane, const std::vector< MSVehicle::LaneQ > &preb, MSVehicle **lastBlocked, MSVehicle **firstBlocked) override
Called to examine whether the vehicle wants to change using the given laneOffset (this is a wrapper a...
double myLaneDiscipline
Definition: MSLCM_SL2015.h:447
bool myDontBrake
flag to prevent speed adaptation by slowing down
Definition: MSLCM_SL2015.h:406
std::string getParameter(const std::string &key) const override
try to retrieve the given parameter from this device. Throw exception for unsupported key
bool wantsKeepRight(double keepRightProb) const
check against thresholds
double forecastAverageSpeed(double vSafe, double vMax, double gap, double vLeader) const
estimate average speed over mySpeedGainLookahead time
void updateCFRelated(const MSLeaderDistanceInfo &vehicles, double foeOffset, bool leaders)
find leaders/followers that are already in a car-following relationship with ego
bool debugVehicle() const override
whether the current vehicles shall be debugged
double myAccelLat
Definition: MSLCM_SL2015.h:439
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 &maneuverDist, int &blocked) override
Called to examine whether the vehicle wants to change with the given laneOffset (using the sublane mo...
double mySpeedGainProbabilityRight
a value for tracking the probability that a change to the right is beneficial
Definition: MSLCM_SL2015.h:379
double myLookAheadSpeed
Definition: MSLCM_SL2015.h:393
int slowDownForBlocked(MSVehicle **blocked, int state)
compute useful slowdowns for blocked vehicles
void initDerivedParameters()
init cached parameters derived directly from model parameters
int keepLatGap(int state, const MSLeaderDistanceInfo &leaders, const MSLeaderDistanceInfo &followers, const MSLeaderDistanceInfo &blockers, const MSLeaderDistanceInfo &neighLeaders, const MSLeaderDistanceInfo &neighFollowers, const MSLeaderDistanceInfo &neighBlockers, const MSLane &neighLane, int laneOffset, double &latDist, double &maneuverDist, int &blocked)
check whether lateral gap requirements are met override the current maneuver if necessary
bool tieBrakeLeader(const MSVehicle *veh) const
bool currentDistAllows(double dist, int laneOffset, double lookForwardDist)
Definition: MSLCM_SL2015.h:209
CLeaderDist getLongest(const MSLeaderDistanceInfo &ldi) const
get the longest vehicle in the given info
double myCooperativeParam
Definition: MSLCM_SL2015.h:422
double getNeighRight(const MSLane &neighLane) const
return the right offset of the neighboring lane relative to the current edge
double computeSpeedGain(double latDistSublane, double defaultNextSpeed) const
compute speedGain when moving by the given amount
double myKeepRightAcceptanceTime
Definition: MSLCM_SL2015.h:455
double mySigmaState
Definition: MSLCM_SL2015.h:470
void updateGaps(const MSLeaderDistanceInfo &others, double foeOffset, double oldCenter, double gapFactor, double &surplusGapRight, double &surplusGapLeft, bool saveMinGap=false, double netOverlap=0, double latDist=0, std::vector< CLeaderDist > *collectBlockers=0)
check remaining lateral gaps for the given foe vehicles and optionally update minimum lateral gaps
double mySpeedGainParam
Definition: MSLCM_SL2015.h:423
virtual void updateSafeLatDist(const double travelledLatDist) override
Updates the value of safe lateral distances (mySafeLatDistLeft and mySafeLatDistRight) during maneuve...
const MSEdge * myLastEdge
expected travel speeds on all sublanes on the current edge(!)
Definition: MSLCM_SL2015.h:403
double getOppositeSafetyFactor() const override
return factor for modifying the safety constraints for opposite-diretction overtaking of the car-foll...
int checkStrategicChange(int ret, const MSLane &neighLane, int laneOffset, const MSLeaderDistanceInfo &leaders, const MSLeaderDistanceInfo &neighLeaders, const MSVehicle::LaneQ &curr, const MSVehicle::LaneQ &neigh, const MSVehicle::LaneQ &best, int bestLaneOffset, bool changeToBest, double currentDist, double neighDist, double laDist, double roundaboutBonus, double latLaneDist, bool checkOpposite, double &latDist)
compute strategic lane change actions TODO: Better documentation, refs #2
StateAndDist decideDirection(StateAndDist sd1, StateAndDist sd2) const override
decide in which direction to move in case both directions are desirable
double myImpatience
Definition: MSLCM_SL2015.h:434
double myOppositeParam
Definition: MSLCM_SL2015.h:425
double myLeftSpace
Definition: MSLCM_SL2015.h:389
std::pair< double, int > Info
information regarding save velocity (unused) and state flags of the ego vehicle
Definition: MSLCM_SL2015.h:215
void msg(const CLeaderDist &cld, double speed, int state)
send a speed recommendation to the given vehicle
double myLookaheadLeft
Definition: MSLCM_SL2015.h:443
int checkBlocking(const MSLane &neighLane, double &latDist, double maneuverDist, int laneOffset, const MSLeaderDistanceInfo &leaders, const MSLeaderDistanceInfo &followers, const MSLeaderDistanceInfo &blockers, const MSLeaderDistanceInfo &neighLeaders, const MSLeaderDistanceInfo &neighFollowers, const MSLeaderDistanceInfo &neighBlockers, std::vector< CLeaderDist > *collectLeadBlockers=0, std::vector< CLeaderDist > *collectFollowBlockers=0, bool keepLatGapManeuver=false, double gapFactor=0, int *retBlockedFully=0)
restrict latDist to permissible speed and determine blocking state depending on that distance
double myStrategicParam
Definition: MSLCM_SL2015.h:421
double getVehicleCenter() const
return vehicle position relative to the current edge (extend by another virtual lane for opposite-dir...
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 &maneuverDist, int &blocked)
helper function for doing the actual work
double getLateralDrift()
get lateral drift for the current step
double computeGapFactor(int state) const
compute the gap factor for the given state
double getPosLat()
get lateral position of this vehicle
bool preventSliding(double maneuverDist) const
bool isBidi(const MSLane *lane) const
check whether lane is an upcoming bidi lane
bool mustOvertakeStopped(const MSLane &neighLane, const MSLeaderDistanceInfo &leaders, const MSLeaderDistanceInfo &neighLead, double posOnLane, double neighDist, bool right, double latLaneDist, double &currentDist, double &latDist)
void * inform(void *info, MSVehicle *sender) override
double myMinGapLat
Definition: MSLCM_SL2015.h:428
void informFollower(int blocked, int dir, const CLeaderDist &neighFollow, double remainingSeconds, double plannedSpeed)
decide whether we will try cut in before the follower or allow to be overtaken
void setParameter(const std::string &key, const std::string &value) override
try to set the given parameter for this laneChangeModel. Throw exception for unsupported key
bool saveBlockerLength(double length, double foeLeftSpace) override
reserve space at the end of the lane to avoid dead locks
double myOvertakeDeltaSpeedFactor
Definition: MSLCM_SL2015.h:457
double myTurnAlignmentDist
Definition: MSLCM_SL2015.h:441
double myLeadingBlockerLength
Definition: MSLCM_SL2015.h:388
void setOwnState(const int state) override
int checkBlockingVehicles(const MSVehicle *ego, const MSLeaderDistanceInfo &vehicles, int laneOffset, double latDist, double foeOffset, bool leaders, double &safeLatGapRight, double &safeLatGapLeft, std::vector< CLeaderDist > *collectBlockers=0) const
check whether any of the vehicles overlaps with ego
void informFollowers(int blocked, int dir, const std::vector< CLeaderDist > &blockers, double remainingSeconds, double plannedSpeed)
call informFollower for multiple followers
double mySpeedGainLookahead
Definition: MSLCM_SL2015.h:449
virtual ~MSLCM_SL2015()
double mySpeedLossProbThreshold
Definition: MSLCM_SL2015.h:467
void resetState() override
bool myCFRelatedReady
Definition: MSLCM_SL2015.h:417
double mySpeedGainProbabilityLeft
a value for tracking the probability that a change to the left is beneficial
Definition: MSLCM_SL2015.h:381
double myAssertive
Definition: MSLCM_SL2015.h:432
double mySpeedGainRight
Definition: MSLCM_SL2015.h:445
static LaneChangeAction getLCA(int state, double latDist)
compute lane change action from desired lateral distance
double myChangeProbThresholdLeft
Definition: MSLCM_SL2015.h:465
void updateExpectedSublaneSpeeds(const MSLeaderDistanceInfo &ahead, int sublaneOffset, int laneIndex) override
update expected speeds for each sublane of the current edge
bool currentDistDisallows(double dist, int laneOffset, double lookForwardDist)
Definition: MSLCM_SL2015.h:206
double getExtraReservation(int bestLaneOffset) const override
reserve extra space for unseen blockers when more tnan one lane change is required
double myTimeToImpatience
Definition: MSLCM_SL2015.h:437
static int lowest_bit(int changeReason)
return the most important change reason
static CLeaderDist getSlowest(const MSLeaderDistanceInfo &ldi)
get the slowest vehicle in the given info
double myPushy
Definition: MSLCM_SL2015.h:430
bool amBlockingFollowerPlusNB()
Definition: MSLCM_SL2015.h:203
double myKeepRightParam
Definition: MSLCM_SL2015.h:424
double mySublaneParam
Definition: MSLCM_SL2015.h:426
Representation of a lane in the micro simulation.
Definition: MSLane.h:84
std::pair< const MSPerson *, double > nextBlocking(double minPos, double minRight, double maxLeft, double stopTime=0, bool bidi=false) const
This is just a wrapper around MSPModel::nextBlocking. You should always check using hasPedestrians be...
Definition: MSLane.cpp:4178
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:2521
double getSpeedLimit() const
Returns the lane's maximum allowed speed.
Definition: MSLane.h:561
double getLength() const
Returns the lane's length.
Definition: MSLane.h:575
bool allowsVehicleClass(SUMOVehicleClass vclass) const
Definition: MSLane.h:856
double getVehicleMaxSpeed(const SUMOTrafficObject *const veh) const
Returns the lane's maximum speed, given a vehicle's speed limit adaptation.
Definition: MSLane.h:547
double getRightSideOnEdge() const
Definition: MSLane.h:1118
bool hasPedestrians() const
whether the lane has pedestrians on it
Definition: MSLane.cpp:4171
int getIndex() const
Returns the lane's index.
Definition: MSLane.h:597
MSLane * getOpposite() const
return the neighboring opposite direction lane for lane changing or nullptr
Definition: MSLane.cpp:4005
MSLane * getBidiLane() const
retrieve bidirectional lane or nullptr
Definition: MSLane.cpp:4252
MSLane * getParallelOpposite() const
return the opposite direction lane of this lanes edge or nullptr
Definition: MSLane.cpp:4011
MSEdge & getEdge() const
Returns the lane's edge.
Definition: MSLane.h:713
const MSLane * getNormalPredecessorLane() const
get normal lane leading to this internal lane, for normal lanes, the lane itself is returned
Definition: MSLane.cpp:2891
double getWidth() const
Returns the lane's width.
Definition: MSLane.h:590
const std::vector< MSLink * > & getLinkCont() const
returns the container with all links !!!
Definition: MSLane.h:675
int getRightmostSublane() const
Definition: MSLane.h:1122
saves leader/follower vehicles and their distances relative to an ego vehicle
Definition: MSLeaderInfo.h:144
virtual std::string toString() const
print a debugging representation
bool hasStoppedVehicle() const
whether a stopped vehicle is leader
void getSublaneBorders(int sublane, double latOffset, double &rightSide, double &leftSide) const
int numSublanes() const
Definition: MSLeaderInfo.h:86
bool hasVehicles() const
Definition: MSLeaderInfo.h:94
void getSubLanes(const MSVehicle *veh, double latOffset, int &rightmost, int &leftmost) const
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
Definition: MSNet.cpp:183
SUMOTime getCurrentTimeStep() const
Returns the current simulation step.
Definition: MSNet.h:321
const MSEdge * getLastEdge() const
returns the destination edge
Definition: MSRoute.cpp:92
const MSLane * lane
The lane to stop at (microsim only)
Definition: MSStop.h:50
double getLatDist() const
Definition: MSVehicle.h:1599
double changeRequestRemainingSeconds(const SUMOTime currentTime) const
Return the remaining number of seconds of the current laneTimeLine assuming one exists.
Definition: MSVehicle.cpp:762
bool ignoreOverlap() const
Definition: MSVehicle.h:1607
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:77
double getRightSideOnEdge(const MSLane *lane=0) const
Get the vehicle's lateral position on the edge of the given lane (or its current edge if lane == 0)
Definition: MSVehicle.cpp:6272
bool isActive() const
Returns whether the current simulation step is an action point for the vehicle.
Definition: MSVehicle.h:622
const std::pair< double, LinkDirection > & getNextTurn()
Get the distance and direction of the next upcoming turn for the vehicle (within its look-ahead range...
Definition: MSVehicle.h:830
MSAbstractLaneChangeModel & getLaneChangeModel()
Definition: MSVehicle.cpp:5367
double getActionStepLengthSecs() const
Returns the vehicle's action step length in secs, i.e. the interval between two action points.
Definition: MSVehicle.h:529
int influenceChangeDecision(int state)
allow TraCI to influence a lane change decision
Definition: MSVehicle.cpp:6808
double nextStopDist() const
return the distance to the next stop or doubleMax if there is none.
Definition: MSVehicle.h:1048
double getAcceleration() const
Returns the vehicle's acceleration in m/s (this is computed as the last step's mean acceleration in c...
Definition: MSVehicle.h:510
const std::vector< MSLane * > & getBestLanesContinuation() const
Returns the best sequence of lanes to continue the route starting at myLane.
Definition: MSVehicle.cpp:5848
int getBestLaneOffset() const
Definition: MSVehicle.cpp:6019
double lateralDistanceToLane(const int offset) const
Get the minimal lateral distance required to move fully onto the lane at given offset.
Definition: MSVehicle.cpp:6394
const MSLane * getLane() const
Returns the lane the vehicle is on.
Definition: MSVehicle.h:577
double getLastStepDist() const
Get the distance the vehicle covered in the previous timestep.
Definition: MSVehicle.h:384
Influencer & getInfluencer()
Definition: MSVehicle.cpp:6773
double getLateralPositionOnLane() const
Get the vehicle's lateral position on the lane.
Definition: MSVehicle.h:416
bool congested() const
Definition: MSVehicle.cpp:1393
double getSpeed() const
Returns the vehicle's current speed.
Definition: MSVehicle.h:486
const std::vector< LaneQ > & getBestLanes() const
Returns the description of best lanes to use in order to continue the route.
Definition: MSVehicle.cpp:5385
const MSCFModel & getCarFollowModel() const
Returns the vehicle's car following model definition.
Definition: MSVehicle.h:966
double getPositionOnLane() const
Get the vehicle's position along the lane.
Definition: MSVehicle.h:377
double getLateralOverlap() const
return the amount by which the vehicle extends laterally outside it's primary lane
Definition: MSVehicle.cpp:6450
bool hasInfluencer() const
whether the vehicle is individually influenced (via TraCI or special parameters)
Definition: MSVehicle.h:1690
double getCenterOnEdge(const MSLane *lane=0) const
Get the vehicle's lateral position on the edge of the given lane (or its current edge if lane == 0)
Definition: MSVehicle.cpp:6278
double getLengthWithGap() const
Get vehicle's length including the minimum gap [m].
double getWidth() const
Get the width which vehicles of this class shall have when being drawn.
SUMOVehicleClass getVehicleClass() const
Get this vehicle type's vehicle class.
const LatAlignmentDefinition & getPreferredLateralAlignment() const
Get vehicle's preferred lateral alignment procedure.
double getMinGap() const
Get the free space in front of vehicles of this class.
double getMaxSpeedLat() const
Get vehicle's maximum lateral speed [m/s].
double getLength() const
Get vehicle's length [m].
double getPreferredLateralAlignmentOffset() const
Get vehicle's preferred lateral alignment offset (in m from center line)
static std::string getIDSecure(const T *obj, const std::string &fallBack="NULL")
get an identifier for Named-like object which may be Null
Definition: Named.h:67
const std::string & getID() const
Returns the id.
Definition: Named.h:74
void step(double dt)
evolve for a time step of length dt.
double arrivalPosLat
(optional) The lateral position the vehicle shall arrive on
ArrivalPosLatDefinition arrivalPosLatProcedure
Information how the vehicle shall choose the lateral arrival position.
static double toDouble(const std::string &sData)
converts a string into the double value described by it by calling the char-type converter
Definition: json.hpp:4471
NLOHMANN_BASIC_JSON_TPL_DECLARATION void swap(nlohmann::NLOHMANN_BASIC_JSON_TPL &j1, nlohmann::NLOHMANN_BASIC_JSON_TPL &j2) noexcept(//NOLINT(readability-inconsistent-declaration-parameter-name) is_nothrow_move_constructible< nlohmann::NLOHMANN_BASIC_JSON_TPL >::value &&//NOLINT(misc-redundant-expression) is_nothrow_move_assignable< nlohmann::NLOHMANN_BASIC_JSON_TPL >::value)
exchanges the values of two JSON objects
Definition: json.hpp:21884
bool sameDirection(const StateAndDist &other) const
A structure representing the best lanes for continuing the current route starting at 'lane'.
Definition: MSVehicle.h:857
double length
The overall length which may be driven when using this lane without a lane change.
Definition: MSVehicle.h:861
std::vector< MSLane * > bestContinuations
Definition: MSVehicle.h:877
MSLane * lane
The described lane.
Definition: MSVehicle.h:859
int bestLaneOffset
The (signed) number of lanes to be crossed to get to the lane which allows to continue the drive.
Definition: MSVehicle.h:869
double occupation
The overall vehicle sum on consecutive lanes which can be passed without a lane change.
Definition: MSVehicle.h:865