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