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