/
MSLCM_LC2013.cpp
2195 lines (2027 loc) · 107 KB
/
MSLCM_LC2013.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
/****************************************************************************/
// Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
// Copyright (C) 2001-2024 German Aerospace Center (DLR) and others.
// This program and the accompanying materials are made available under the
// terms of the Eclipse Public License 2.0 which is available at
// https://www.eclipse.org/legal/epl-2.0/
// This Source Code may also be made available under the following Secondary
// Licenses when the conditions for such availability set forth in the Eclipse
// Public License 2.0 are satisfied: GNU General Public License, version 2
// or later which is available at
// https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
// SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
/****************************************************************************/
/// @file MSLCM_LC2013.cpp
/// @author Daniel Krajzewicz
/// @author Jakob Erdmann
/// @author Friedemann Wesner
/// @author Sascha Krieg
/// @author Michael Behrisch
/// @author Laura Bieker
/// @author Leonhard Luecken
/// @date Fri, 08.10.2013
///
// A lane change model developed by J. Erdmann
// based on the model of D. Krajzewicz developed between 2004 and 2011 (MSLCM_DK2004)
/****************************************************************************/
#include <config.h>
#include <iostream>
#include <utils/common/RandHelper.h>
#include <utils/common/StringUtils.h>
#include <microsim/transportables/MSPModel.h>
#include <microsim/transportables/MSTransportableControl.h>
#include <microsim/MSEdge.h>
#include <microsim/MSLane.h>
#include <microsim/MSLink.h>
#include <microsim/MSDriverState.h>
#include <microsim/MSNet.h>
#include <microsim/MSStop.h>
#include "MSLCHelper.h"
#include "MSLCM_LC2013.h"
// ===========================================================================
// variable definitions
// ===========================================================================
#define MAGIC_OFFSET 1.
#define LOOK_FORWARD 10.
#define JAM_FACTOR 1.
#define LCA_RIGHT_IMPATIENCE -1.
#define CUT_IN_LEFT_SPEED_THRESHOLD 27.
#define LOOK_AHEAD_MIN_SPEED 0.0
#define LOOK_AHEAD_SPEED_MEMORY 0.9
#define HELP_DECEL_FACTOR 1.0
#define HELP_OVERTAKE (10.0 / 3.6)
#define MIN_FALLBEHIND (7.0 / 3.6)
#define RELGAIN_NORMALIZATION_MIN_SPEED 10.0
#define URGENCY 2.0
#define OPPOSITE_URGENCY 5.0
#define KEEP_RIGHT_TIME 5.0 // the number of seconds after which a vehicle should move to the right lane
#define KEEP_RIGHT_HEADWAY 2.0
#define MAX_ONRAMP_LENGTH 200.
#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
#define LC_RESOLUTION_SPEED_LAT 0.5 // the lateral speed (in m/s) for a standing vehicle which was unable to finish a continuous LC in time (in case mySpeedLatStanding==0), see #3771
#define REACT_TO_STOPPED_DISTANCE 100
// ===========================================================================
// debug defines
// ===========================================================================
//#define DEBUG_CONSTRUCTOR
//#define DEBUG_PATCH_SPEED
//#define DEBUG_INFORMED
//#define DEBUG_INFORMER
//#define DEBUG_WANTS_CHANGE
//#define DEBUG_SLOW_DOWN
//#define DEBUG_COOPERATE
//#define DEBUG_SAVE_BLOCKER_LENGTH
//#define DEBUG_COND (myVehicle.getID() == "disabled")
#define DEBUG_COND (myVehicle.isSelected())
//#define DEBUG_COND (false)
// ===========================================================================
// member method definitions
// ===========================================================================
MSLCM_LC2013::MSLCM_LC2013(MSVehicle& v) :
MSAbstractLaneChangeModel(v, LaneChangeModel::LC2013),
mySpeedGainProbability(0),
myKeepRightProbability(0),
myLeadingBlockerLength(0),
myLeftSpace(0),
myLookAheadSpeed(LOOK_AHEAD_MIN_SPEED),
myStrategicParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_STRATEGIC_PARAM, 1)),
myCooperativeParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_COOPERATIVE_PARAM, 1)),
mySpeedGainParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SPEEDGAIN_PARAM, 1)),
myKeepRightParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_KEEPRIGHT_PARAM, 1)),
myOppositeParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_OPPOSITE_PARAM, 1)),
myLookaheadLeft(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_LOOKAHEADLEFT, 2.0)),
mySpeedGainRight(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SPEEDGAINRIGHT, 0.1)),
myAssertive(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_ASSERTIVE, 1)),
mySpeedGainLookahead(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SPEEDGAIN_LOOKAHEAD, 0)),
myRoundaboutBonus(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_COOPERATIVE_ROUNDABOUT, myCooperativeParam)),
myCooperativeSpeed(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_COOPERATIVE_SPEED, myCooperativeParam)),
myKeepRightAcceptanceTime(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_KEEPRIGHT_ACCEPTANCE_TIME, -1)),
myOvertakeDeltaSpeedFactor(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_OVERTAKE_DELTASPEED_FACTOR, 0)),
myExperimentalParam1(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_EXPERIMENTAL1, 0)) {
initDerivedParameters();
#ifdef DEBUG_CONSTRUCTOR
if (DEBUG_COND) {
std::cout << SIMTIME
<< " create lcModel veh=" << myVehicle.getID()
<< " lcStrategic=" << myStrategicParam
<< " lcCooperative=" << myCooperativeParam
<< " lcSpeedGain=" << mySpeedGainParam
<< " lcKeepRight=" << myKeepRightParam
<< "\n";
}
#endif
}
MSLCM_LC2013::~MSLCM_LC2013() {
changed();
}
void
MSLCM_LC2013::initDerivedParameters() {
if (mySpeedGainParam <= 0) {
myChangeProbThresholdRight = std::numeric_limits<double>::max();
myChangeProbThresholdLeft = std::numeric_limits<double>::max();
} else {
myChangeProbThresholdRight = (0.2 / mySpeedGainRight) / mySpeedGainParam;
myChangeProbThresholdLeft = 0.2 / mySpeedGainParam;
}
}
bool
MSLCM_LC2013::debugVehicle() const {
return DEBUG_COND;
}
int
MSLCM_LC2013::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) {
#ifdef DEBUG_WANTS_CHANGE
if (DEBUG_COND) {
std::cout << "\nWANTS_CHANGE\n" << SIMTIME
<< std::setprecision(gPrecision)
<< " veh=" << myVehicle.getID()
<< " lane=" << myVehicle.getLane()->getID()
<< " pos=" << myVehicle.getPositionOnLane()
<< " posLat=" << myVehicle.getLateralPositionOnLane()
<< " speed=" << myVehicle.getSpeed()
<< " considerChangeTo=" << (laneOffset == -1 ? "right" : "left")
<< "\n";
}
#endif
const int result = _wantsChange(laneOffset, msgPass, blocked, leader, follower, neighLead, neighFollow, neighLane, preb, lastBlocked, firstBlocked);
#ifdef DEBUG_WANTS_CHANGE
if (DEBUG_COND) {
std::cout << SIMTIME << " veh=" << myVehicle.getID() << " result=" << toString((LaneChangeAction)result) << " blocked=" << toString((LaneChangeAction)blocked) << "\n\n\n";
}
#endif
return result;
}
double
MSLCM_LC2013::patchSpeed(const double min, const double wanted, const double max, const MSCFModel& cfModel) {
#ifdef DEBUG_PATCH_SPEED
if (DEBUG_COND) {
std::cout << "\nPATCH_SPEED\n"
<< SIMTIME
<< " veh=" << myVehicle.getID()
<< " lane=" << myVehicle.getLane()->getID()
<< " pos=" << myVehicle.getPositionOnLane()
<< " v=" << myVehicle.getSpeed()
<< " min=" << min
<< " wanted=" << wanted
<< " max=" << max
<< "\n";
}
#endif
// negative min speed may be passed when using ballistic updated
const double newSpeed = _patchSpeed(MAX2(min, 0.0), wanted, max, cfModel);
#ifdef DEBUG_PATCH_SPEED
if (DEBUG_COND) {
const std::string patched = (wanted != newSpeed ? " patched=" + toString(newSpeed) : "");
std::cout << patched
<< "\n";
}
#endif
return newSpeed;
}
double
MSLCM_LC2013::_patchSpeed(double min, const double wanted, double max, const MSCFModel& cfModel) {
int state = myOwnState;
#ifdef DEBUG_PATCH_SPEED
if (DEBUG_COND) {
std::cout
<< "\n" << SIMTIME << std::setprecision(gPrecision)
<< " patchSpeed state=" << toString((LaneChangeAction)state) << " myLCAccelerationAdvices=" << toString(myLCAccelerationAdvices)
<< "\n speed=" << myVehicle.getSpeed() << " min=" << min << " wanted=" << wanted
<< "\n myLeadingBlockerLength=" << myLeadingBlockerLength
<< "\n";
}
#endif
// letting vehicles merge in at the end of the lane in case of counter-lane change, step#2
double nVSafe = wanted;
bool gotOne = false;
// if we want to change and have a blocking leader and there is enough room for him in front of us
if (myLeadingBlockerLength != 0) {
double space = myLeftSpace - myLeadingBlockerLength - MAGIC_OFFSET - myVehicle.getVehicleType().getMinGap();
#ifdef DEBUG_PATCH_SPEED
if (DEBUG_COND) {
std::cout << SIMTIME << " veh=" << myVehicle.getID() << " myLeftSpace=" << myLeftSpace << " myLeadingBlockerLength=" << myLeadingBlockerLength << " space=" << space << "\n";
}
#endif
if (space > 0) { // XXX space > -MAGIC_OFFSET
// compute speed for decelerating towards a place which allows the blocking leader to merge in in front
const double vMinEmergency = myVehicle.getCarFollowModel().minNextSpeedEmergency(myVehicle.getSpeed(), &myVehicle);
double safe = cfModel.stopSpeed(&myVehicle, myVehicle.getSpeed(), space, MSCFModel::CalcReason::LANE_CHANGE);
max = MIN2(max, MAX2(safe, vMinEmergency));
// if we are approaching this place
if (safe < wanted) {
// return this speed as the speed to use
if (safe < min) {
if (safe >= vMinEmergency) {
// permit harder braking if needed and helpful
min = MAX2(vMinEmergency, safe);
}
}
#ifdef DEBUG_PATCH_SPEED
if (DEBUG_COND) {
std::cout << SIMTIME << " veh=" << myVehicle.getID() << " slowing down for leading blocker, safe=" << safe << (safe + NUMERICAL_EPS < min ? " (not enough)" : "") << "\n";
}
#endif
nVSafe = MAX2(min, safe);
gotOne = true;
}
}
}
const double coopWeight = MAX2(0.0, MIN2(1.0, myCooperativeSpeed));
for (auto i : myLCAccelerationAdvices) {
double a = i.first;
double v = myVehicle.getSpeed() + ACCEL2SPEED(a);
if (v >= min && v <= max && (MSGlobals::gSemiImplicitEulerUpdate
// ballistic update: (negative speeds may appear, e.g. min<0, v<0), BUT:
// XXX: LaneChanging returns -1 to indicate no restrictions, which leads to probs here (Leo), refs. #2577
// As a quick fix, we just dismiss cases where v=-1
// VERY rarely (whenever a requested help-acceleration is really indicated by v=-1)
// this can lead to failing lane-change attempts, though)
|| v != -1)) {
if (i.second) {
// own advice, no scaling needed
nVSafe = MIN2(v, nVSafe);
} else {
nVSafe = MIN2(v * coopWeight + (1 - coopWeight) * wanted, nVSafe);
}
gotOne = true;
#ifdef DEBUG_PATCH_SPEED
if (DEBUG_COND) {
std::cout << SIMTIME << " veh=" << myVehicle.getID() << " got nVSafe=" << nVSafe << "\n";
}
#endif
} else {
if (v < min) {
#ifdef DEBUG_PATCH_SPEED
if (DEBUG_COND) {
std::cout << SIMTIME << " veh=" << myVehicle.getID() << " ignoring low nVSafe=" << v << " min=" << min << "\n";
}
#endif
} else {
#ifdef DEBUG_PATCH_SPEED
if (DEBUG_COND) {
std::cout << SIMTIME << " veh=" << myVehicle.getID() << " ignoring high nVSafe=" << v << " max=" << max << "\n";
}
#endif
}
}
}
// myDontBrake is used in counter-lane-change situations with relief connection
if (gotOne && !myDontBrake) {
#ifdef DEBUG_PATCH_SPEED
if (DEBUG_COND) {
std::cout << SIMTIME << " veh=" << myVehicle.getID() << " got vSafe\n";
}
#endif
return nVSafe;
}
// check whether the vehicle is blocked
if ((state & LCA_WANTS_LANECHANGE) != 0 && (state & LCA_BLOCKED) != 0) {
if ((state & LCA_STRATEGIC) != 0) {
// necessary decelerations are controlled via vSafe. If there are
// none it means we should speed up
#ifdef DEBUG_PATCH_SPEED
if (DEBUG_COND) {
std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_WANTS_LANECHANGE (strat, no vSafe)\n";
}
#endif
return (max + wanted) / 2.0;
} else if ((state & LCA_COOPERATIVE) != 0) {
// only minor adjustments in speed should be done
if ((state & LCA_BLOCKED_BY_LEADER) != 0) {
#ifdef DEBUG_PATCH_SPEED
if (DEBUG_COND) {
std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_BLOCKED_BY_LEADER (coop)\n";
}
#endif
if (wanted >= 0.) {
return (MAX2(0., min) + wanted) / 2.0;
} else {
return wanted;
}
}
if ((state & LCA_BLOCKED_BY_FOLLOWER) != 0) {
#ifdef DEBUG_PATCH_SPEED
if (DEBUG_COND) {
std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_BLOCKED_BY_FOLLOWER (coop)\n";
}
#endif
return (max + wanted) / 2.0;
}
//} else { // VARIANT_16
// // only accelerations should be performed
// if ((state & LCA_BLOCKED_BY_FOLLOWER) != 0) {
// if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_BLOCKED_BY_FOLLOWER\n";
// return (max + wanted) / 2.0;
// }
}
}
/*
// decelerate if being a blocking follower
// (and does not have to change lanes)
if ((state & LCA_AMBLOCKINGFOLLOWER) != 0) {
if (fabs(max - myVehicle.getCarFollowModel().maxNextSpeed(myVehicle.getSpeed(), &myVehicle)) < 0.001 && min == 0) { // !!! was standing
if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBLOCKINGFOLLOWER (standing)\n";
return 0;
}
if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBLOCKINGFOLLOWER\n";
//return min; // VARIANT_3 (brakeStrong)
return (min + wanted) / 2.0;
}
if ((state & LCA_AMBACKBLOCKER) != 0) {
if (max <= myVehicle.getCarFollowModel().maxNextSpeed(myVehicle.getSpeed(), &myVehicle) && min == 0) { // !!! was standing
if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBACKBLOCKER (standing)\n";
//return min; VARIANT_9 (backBlockVSafe)
return nVSafe;
}
}
if ((state & LCA_AMBACKBLOCKER_STANDING) != 0) {
if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBACKBLOCKER_STANDING\n";
//return min;
return nVSafe;
}
*/
// accelerate if being a blocking leader or blocking follower not able to brake
// (and does not have to change lanes)
if ((state & LCA_AMBLOCKINGLEADER) != 0) {
#ifdef DEBUG_PATCH_SPEED
if (DEBUG_COND) {
std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBLOCKINGLEADER\n";
}
#endif
return (max + wanted) / 2.0;
}
if ((state & LCA_AMBLOCKINGFOLLOWER_DONTBRAKE) != 0) {
#ifdef DEBUG_PATCH_SPEED
if (DEBUG_COND) {
std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBLOCKINGFOLLOWER_DONTBRAKE\n";
}
#endif
/*
// VARIANT_4 (dontbrake)
if (max <= myVehicle.getCarFollowModel().maxNextSpeed(myVehicle.getSpeed(), &myVehicle) && min == 0) { // !!! was standing
return wanted;
}
return (min + wanted) / 2.0;
*/
}
if (!myVehicle.getLane()->getEdge().hasLaneChanger()) {
// remove chaning information if on a road with a single lane
changed();
}
return wanted;
}
void*
MSLCM_LC2013::inform(void* info, MSVehicle* sender) {
UNUSED_PARAMETER(sender);
Info* pinfo = (Info*)info;
assert(pinfo->first >= 0 || !MSGlobals::gSemiImplicitEulerUpdate);
addLCSpeedAdvice(pinfo->first, false);
myOwnState |= pinfo->second;
#ifdef DEBUG_INFORMED
if (DEBUG_COND) {
std::cout << SIMTIME
<< " veh=" << myVehicle.getID()
<< " informedBy=" << sender->getID()
<< " info=" << pinfo->second
<< " vSafe=" << pinfo->first
<< "\n";
}
#endif
delete pinfo;
return (void*) true;
}
double
MSLCM_LC2013::overtakeDistance(const MSVehicle* follower, const MSVehicle* leader, const double gap, double followerSpeed, double leaderSpeed) {
followerSpeed = followerSpeed == INVALID_SPEED ? follower->getSpeed() : followerSpeed;
leaderSpeed = leaderSpeed == INVALID_SPEED ? leader->getSpeed() : leaderSpeed;
double overtakeDist = (gap // drive to back of leader
+ leader->getVehicleType().getLengthWithGap() // drive to front of leader
+ follower->getVehicleType().getLength() // follower back reaches leader front
+ leader->getCarFollowModel().getSecureGap( // save gap to leader
leader, follower, leaderSpeed, followerSpeed, follower->getCarFollowModel().getMaxDecel()));
return MAX2(overtakeDist, 0.);
}
double
MSLCM_LC2013::informLeader(MSAbstractLaneChangeModel::MSLCMessager& msgPass,
int blocked,
int dir,
const std::pair<MSVehicle*, double>& neighLead,
double remainingSeconds) {
double plannedSpeed = myVehicle.getSpeed();
if (!isOpposite()) {
plannedSpeed = MIN2(plannedSpeed,
myVehicle.getCarFollowModel().stopSpeed(&myVehicle, myVehicle.getSpeed(), myLeftSpace - myLeadingBlockerLength));
}
for (auto i : myLCAccelerationAdvices) {
const double a = i.first;
if (a >= -myVehicle.getCarFollowModel().getMaxDecel()) {
plannedSpeed = MIN2(plannedSpeed, myVehicle.getSpeed() + ACCEL2SPEED(a));
}
}
#ifdef DEBUG_INFORMER
if (DEBUG_COND) {
std::cout << "\nINFORM_LEADER"
<< "\nspeed=" << myVehicle.getSpeed() << " planned=" << plannedSpeed << "\n";
}
#endif
const MSVehicle* const nv = neighLead.first;
if (nv == nullptr) {
// not overtaking
return plannedSpeed;
}
const double neighNextSpeed = nv->getSpeed() - ACCEL2SPEED(MAX2(1.0, -nv->getAcceleration()));
double neighNextGap;
if (MSGlobals::gSemiImplicitEulerUpdate) {
neighNextGap = neighLead.second + SPEED2DIST(neighNextSpeed - plannedSpeed);
} else {
neighNextGap = neighLead.second + SPEED2DIST((nv->getSpeed() + neighNextSpeed) / 2) - SPEED2DIST((myVehicle.getSpeed() + plannedSpeed) / 2);
}
if ((blocked & LCA_BLOCKED_BY_LEADER) != 0) {
if (MSLCHelper::divergentRoute(myVehicle, *nv)) {
//std::cout << SIMTIME << " ego=" << myVehicle.getID() << " ignoresDivergentBlockingLeader=" << nv->getID() << "\n";
return plannedSpeed;
}
#ifdef DEBUG_INFORMER
if (DEBUG_COND) {
std::cout << " blocked by leader nv=" << nv->getID() << " nvSpeed=" << nv->getSpeed() << " needGap="
<< myVehicle.getCarFollowModel().getSecureGap(&myVehicle, nv, myVehicle.getSpeed(), nv->getSpeed(), nv->getCarFollowModel().getMaxDecel()) << "\n";
}
#endif
// decide whether we want to overtake the leader or follow it
double overtakeTime;
const double overtakeDist = overtakeDistance(&myVehicle, nv, neighLead.second);
const double dv = plannedSpeed - nv->getSpeed();
if (dv > myOvertakeDeltaSpeedFactor * myVehicle.getLane()->getSpeedLimit()) {
overtakeTime = overtakeDist / dv;
} else {
// -> set overtakeTime to something indicating impossibility of overtaking
overtakeTime = remainingSeconds + 1;
}
#ifdef DEBUG_INFORMER
if (DEBUG_COND) {
std::cout << SIMTIME << " informLeader() of " << myVehicle.getID()
<< "\nnv = " << nv->getID()
<< "\nplannedSpeed = " << plannedSpeed
<< "\nleaderSpeed = " << nv->getSpeed()
<< "\nmyLeftSpace = " << myLeftSpace
<< "\nremainingSeconds = " << remainingSeconds
<< "\novertakeDist = " << overtakeDist
<< "\novertakeTime = " << overtakeTime
<< std::endl;
}
#endif
if ((dv < myOvertakeDeltaSpeedFactor * myVehicle.getLane()->getSpeedLimit()
// overtaking on the right on an uncongested highway is forbidden (noOvertakeLCLeft)
|| (dir == LCA_MLEFT && !myVehicle.congested() && !myAllowOvertakingRight)
// not enough space to overtake?
|| (MSGlobals::gSemiImplicitEulerUpdate && myLeftSpace - myLeadingBlockerLength - myVehicle.getCarFollowModel().brakeGap(myVehicle.getSpeed()) < overtakeDist)
// using brakeGap() without headway seems adequate in a situation where the obstacle (the lane end) is not moving [XXX implemented in branch ticket860, can be used in general if desired, refs. #2575] (Leo).
|| (!MSGlobals::gSemiImplicitEulerUpdate && myLeftSpace - myLeadingBlockerLength - myVehicle.getCarFollowModel().brakeGap(myVehicle.getSpeed(), getCarFollowModel().getMaxDecel(), 0.) < overtakeDist)
// not enough time to overtake? (skipped for a stopped leader [currently only for ballistic update XXX: check if appropriate for euler, too, refs. #2575] to ensure that it can be overtaken if only enough space is exists) (Leo)
|| (remainingSeconds < overtakeTime && (MSGlobals::gSemiImplicitEulerUpdate || !nv->isStopped())))
// opposite driving and must overtake
&& (!neighLead.first->isStopped() || (isOpposite() && neighLead.second >= 0))) {
// cannot overtake
msgPass.informNeighLeader(new Info(std::numeric_limits<double>::max(), dir | LCA_AMBLOCKINGLEADER), &myVehicle);
// slow down smoothly to follow leader
// account for minor decelerations by the leader (dawdling)
const double targetSpeed = MAX2(
myVehicle.getCarFollowModel().minNextSpeed(myVehicle.getSpeed(), &myVehicle),
getCarFollowModel().followSpeed(&myVehicle, myVehicle.getSpeed(), neighNextGap, neighNextSpeed, nv->getCarFollowModel().getMaxDecel()));
if (targetSpeed < myVehicle.getSpeed()) {
// slow down smoothly to follow leader
const double decel = remainingSeconds == 0. ? myVehicle.getCarFollowModel().getMaxDecel() :
MIN2(myVehicle.getCarFollowModel().getMaxDecel(),
MAX2(MIN_FALLBEHIND, (myVehicle.getSpeed() - targetSpeed) / remainingSeconds));
const double nextSpeed = MIN2(plannedSpeed, MAX2(0.0, myVehicle.getSpeed() - ACCEL2SPEED(decel)));
#ifdef DEBUG_INFORMER
if (DEBUG_COND) {
std::cout << SIMTIME
<< " cannot overtake leader nv=" << nv->getID()
<< " dv=" << dv
<< " myLookAheadSpeed=" << myLookAheadSpeed
<< " myLeftSpace=" << myLeftSpace
<< " overtakeDist=" << overtakeDist
<< " overtakeTime=" << overtakeTime
<< " remainingSeconds=" << remainingSeconds
<< " currentGap=" << neighLead.second
<< " brakeGap=" << myVehicle.getCarFollowModel().brakeGap(myVehicle.getSpeed(), getCarFollowModel().getMaxDecel(), 0.)
<< " neighNextSpeed=" << neighNextSpeed
<< " neighNextGap=" << neighNextGap
<< " targetSpeed=" << targetSpeed
<< " nextSpeed=" << nextSpeed
<< "\n";
}
#endif
addLCSpeedAdvice(nextSpeed);
return nextSpeed;
} else {
// leader is fast enough anyway
#ifdef DEBUG_INFORMER
if (DEBUG_COND) {
std::cout << SIMTIME
<< " cannot overtake fast leader nv=" << nv->getID()
<< " dv=" << dv
<< " myLookAheadSpeed=" << myLookAheadSpeed
<< " myLeftSpace=" << myLeftSpace
<< " overtakeDist=" << overtakeDist
<< " myLeadingBlockerLength=" << myLeadingBlockerLength
<< " overtakeTime=" << overtakeTime
<< " remainingSeconds=" << remainingSeconds
<< " currentGap=" << neighLead.second
<< " neighNextSpeed=" << neighNextSpeed
<< " neighNextGap=" << neighNextGap
<< " targetSpeed=" << targetSpeed
<< "\n";
}
#endif
addLCSpeedAdvice(targetSpeed);
return plannedSpeed;
}
} else {
// overtaking, leader should not accelerate
#ifdef DEBUG_INFORMER
if (DEBUG_COND) {
std::cout << SIMTIME
<< " wants to overtake leader nv=" << nv->getID()
<< " dv=" << dv
<< " overtakeDist=" << overtakeDist
<< " remainingSeconds=" << remainingSeconds
<< " overtakeTime=" << overtakeTime
<< " currentGap=" << neighLead.second
<< " secureGap=" << nv->getCarFollowModel().getSecureGap(nv, &myVehicle, nv->getSpeed(), myVehicle.getSpeed(), myVehicle.getCarFollowModel().getMaxDecel())
<< "\n";
}
#endif
msgPass.informNeighLeader(new Info(nv->getSpeed(), dir | LCA_AMBLOCKINGLEADER), &myVehicle);
return -1; // XXX: using -1 is ambiguous for the ballistic update! Currently this is being catched in patchSpeed() (Leo), consider returning INVALID_SPEED, refs. #2577
}
} else { // (remainUnblocked)
// we are not blocked now. make sure we stay far enough from the leader
const double targetSpeed = MAX2(
myVehicle.getCarFollowModel().minNextSpeed(myVehicle.getSpeed(), &myVehicle),
getCarFollowModel().followSpeed(&myVehicle, myVehicle.getSpeed(), neighNextGap, neighNextSpeed, nv->getCarFollowModel().getMaxDecel()));
addLCSpeedAdvice(targetSpeed);
#ifdef DEBUG_INFORMER
if (DEBUG_COND) {
std::cout << " not blocked by leader nv=" << nv->getID()
<< " nvSpeed=" << nv->getSpeed()
<< " gap=" << neighLead.second
<< " neighNextSpeed=" << neighNextSpeed
<< " neighNextGap=" << neighNextGap
<< " needGap=" << myVehicle.getCarFollowModel().getSecureGap(&myVehicle, nv, myVehicle.getSpeed(), nv->getSpeed(), nv->getCarFollowModel().getMaxDecel())
<< " targetSpeed=" << targetSpeed
<< "\n";
}
#endif
return MIN2(targetSpeed, plannedSpeed);
}
}
void
MSLCM_LC2013::informFollower(MSAbstractLaneChangeModel::MSLCMessager& msgPass,
int blocked,
int dir,
const std::pair<MSVehicle*, double>& neighFollow,
double remainingSeconds,
double plannedSpeed) {
MSVehicle* nv = neighFollow.first;
const double plannedAccel = SPEED2ACCEL(MAX2(MIN2(getCarFollowModel().getMaxAccel(), plannedSpeed - myVehicle.getSpeed()), -getCarFollowModel().getMaxDecel()));
#ifdef DEBUG_INFORMER
if (DEBUG_COND) {
std::cout << "\nINFORM_FOLLOWER"
<< "\nspeed=" << myVehicle.getSpeed() << " planned=" << plannedSpeed << "\n";
}
#endif
if ((blocked & LCA_BLOCKED_BY_FOLLOWER) != 0 && nv != nullptr) {
if (MSLCHelper::divergentRoute(myVehicle, *nv)) {
//std::cout << SIMTIME << " ego=" << myVehicle.getID() << " ignoresDivergentBlockingFollower=" << nv->getID() << "\n";
return;
}
#ifdef DEBUG_INFORMER
if (DEBUG_COND) {
std::cout << " blocked by follower nv=" << nv->getID() << " nvSpeed=" << nv->getSpeed() << " needGap="
<< nv->getCarFollowModel().getSecureGap(nv, &myVehicle, nv->getSpeed(), myVehicle.getSpeed(), myVehicle.getCarFollowModel().getMaxDecel()) << " planned=" << plannedSpeed << "\n";
}
#endif
// are we fast enough to cut in without any help?
if (MAX2(plannedSpeed, 0.) - nv->getSpeed() >= HELP_OVERTAKE) {
const double neededGap = nv->getCarFollowModel().getSecureGap(nv, &myVehicle, nv->getSpeed(), plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel());
if ((neededGap - neighFollow.second) / remainingSeconds < (MAX2(plannedSpeed, 0.) - nv->getSpeed())) {
#ifdef DEBUG_INFORMER
if (DEBUG_COND) {
std::cout << " wants to cut in before nv=" << nv->getID() << " without any help." << "\nneededGap = " << neededGap << "\n";
}
#endif
// follower might even accelerate but not to much
// XXX: I don't understand this. The needed gap was determined for nv->getSpeed(), not for (plannedSpeed - HELP_OVERTAKE)?! (Leo), refs. #2578
msgPass.informNeighFollower(new Info(MAX2(plannedSpeed, 0.) - HELP_OVERTAKE, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
return;
}
}
// decide whether we will request help to cut in before the follower or allow to be overtaken
// PARAMETERS
// assume other vehicle will assume the equivalent of 1 second of
// maximum deceleration to help us (will probably be spread over
// multiple seconds)
// -----------
const double helpDecel = nv->getCarFollowModel().getMaxDecel() * HELP_DECEL_FACTOR;
// follower's new speed in next step
double neighNewSpeed;
// follower's new speed after 1s.
double neighNewSpeed1s;
// velocity difference, gap after follower-deceleration
double dv, decelGap;
if (MSGlobals::gSemiImplicitEulerUpdate) {
// euler
neighNewSpeed = MAX2(0., nv->getSpeed() - ACCEL2SPEED(helpDecel));
neighNewSpeed1s = MAX2(0., nv->getSpeed() - helpDecel); // TODO: consider introduction of a configurable anticipationTime here (see far below in the !blocked part). Refs. #2578
// change in the gap between ego and blocker over 1 second (not STEP!)
// XXX: though here it is calculated as if it were one step!? (Leo) Refs. #2578
dv = plannedSpeed - neighNewSpeed1s; // XXX: what is this quantity (if TS!=1)?
// new gap between follower and self in case the follower does brake for 1s
// XXX: if the step-length is not 1s., this is not the gap after 1s. deceleration!
// And this formula overestimates the real gap. Isn't that problematic? (Leo)
// Below, it seems that decelGap > secureGap is taken to indicate the possibility
// to cut in within the next time-step. However, this is not the case, if TS<1s.,
// since decelGap is (not exactly, though!) the gap after 1s. Refs. #2578
decelGap = neighFollow.second + dv;
} else {
// ballistic
// negative newSpeed-extrapolation possible, if stop lies within the next time-step
// XXX: this code should work for the euler case as well, since gapExtrapolation() takes
// care of this, but for TS!=1 we will have different behavior (see previous remark) Refs. #2578
neighNewSpeed = nv->getSpeed() - ACCEL2SPEED(helpDecel);
neighNewSpeed1s = nv->getSpeed() - helpDecel;
dv = myVehicle.getSpeed() - nv->getSpeed(); // current velocity difference
decelGap = getCarFollowModel().gapExtrapolation(1., neighFollow.second, myVehicle.getSpeed(),
nv->getSpeed(), plannedAccel, -helpDecel, myVehicle.getMaxSpeedOnLane(), nv->getMaxSpeedOnLane());
}
const double secureGap = nv->getCarFollowModel().getSecureGap(nv, &myVehicle, MAX2(neighNewSpeed1s, 0.),
MAX2(plannedSpeed, 0.), myVehicle.getCarFollowModel().getMaxDecel());
const double onRampThreshold = myVehicle.getLane()->getSpeedLimit() * 0.8 * myExperimentalParam1 * (1 - myVehicle.getImpatience());
#ifdef DEBUG_INFORMER
if (DEBUG_COND) {
std::cout << SIMTIME
<< " speed=" << myVehicle.getSpeed()
<< " plannedSpeed=" << plannedSpeed
<< " threshold=" << onRampThreshold
<< " neighNewSpeed=" << neighNewSpeed
<< " neighNewSpeed1s=" << neighNewSpeed1s
<< " dv=" << dv
<< " gap=" << neighFollow.second
<< " decelGap=" << decelGap
<< " secureGap=" << secureGap
<< "\n";
}
#endif
// prevent vehicles on an on ramp stopping the main flow
if (dir == LCA_MLEFT
&& myVehicle.getLane()->isAccelLane()
&& neighNewSpeed1s < onRampThreshold) {
return;
}
if (decelGap > 0 && decelGap >= secureGap) {
// XXX: This does not assure that the leader can cut in in the next step if TS < 1 (see above)
// this seems to be supposed in the following (euler code)...?! (Leo) Refs. #2578
// if the blocking follower brakes it could help
// how hard does it actually need to be?
// to be safe in the next step the following equation has to hold for the follower's vsafe:
// vsafe <= followSpeed(gap=currentGap - SPEED2DIST(vsafe), ...)
double vsafe, vsafe1;
if (MSGlobals::gSemiImplicitEulerUpdate) {
// euler
// we compute an upper bound on vsafe by doing the computation twice
vsafe1 = MAX2(neighNewSpeed, nv->getCarFollowModel().followSpeed(
nv, nv->getSpeed(), neighFollow.second + SPEED2DIST(plannedSpeed), plannedSpeed, getCarFollowModel().getMaxDecel()));
vsafe = MAX2(neighNewSpeed, nv->getCarFollowModel().followSpeed(
nv, nv->getSpeed(), neighFollow.second + SPEED2DIST(plannedSpeed - vsafe1), plannedSpeed, getCarFollowModel().getMaxDecel()));
//assert(vsafe <= vsafe1); assertion does not hold for models with randomness in followSpeed (W99)
} else {
// ballistic
// XXX: This block should actually do as well for euler update (TODO: test!), refs #2575
// we compute an upper bound on vsafe
// next step's gap without help deceleration (nv's speed assumed constant)
double nextGap = getCarFollowModel().gapExtrapolation(TS,
neighFollow.second, myVehicle.getSpeed(),
nv->getSpeed(), plannedAccel, 0,
myVehicle.getMaxSpeedOnLane(), nv->getMaxSpeedOnLane());
#ifdef DEBUG_INFORMER
if (DEBUG_COND) {
std::cout << "nextGap=" << nextGap << " (without help decel) \n";
}
#endif
// NOTE: the second argument of MIN2() can get larger than nv->getSpeed()
vsafe1 = MIN2(nv->getSpeed(), MAX2(neighNewSpeed,
nv->getCarFollowModel().followSpeed(nv,
nv->getSpeed(), nextGap,
MAX2(0., plannedSpeed),
getCarFollowModel().getMaxDecel())));
// next step's gap with possibly less than maximal help deceleration (in case vsafe1 > neighNewSpeed)
double decel2 = SPEED2ACCEL(nv->getSpeed() - vsafe1);
nextGap = getCarFollowModel().gapExtrapolation(TS,
neighFollow.second, myVehicle.getSpeed(),
nv->getSpeed(), plannedAccel, -decel2,
myVehicle.getMaxSpeedOnLane(), nv->getMaxSpeedOnLane());
// vsafe = MAX(neighNewSpeed, safe speed assuming next_gap)
// Thus, the gap resulting from vsafe is larger or equal to next_gap
// in contrast to the euler case, where nv's follow speed doesn't depend on the actual speed,
// we need to assure, that nv doesn't accelerate
vsafe = MIN2(nv->getSpeed(), MAX2(neighNewSpeed,
nv->getCarFollowModel().followSpeed(nv,
nv->getSpeed(), nextGap,
MAX2(0., plannedSpeed),
getCarFollowModel().getMaxDecel())));
assert(vsafe >= vsafe1 - NUMERICAL_EPS);
#ifdef DEBUG_INFORMER
if (DEBUG_COND) {
std::cout << "nextGap=" << nextGap
<< " (with vsafe1 and help decel) \nvsafe1=" << vsafe1
<< " vsafe=" << vsafe
<< "\n";
}
#endif
// For subsecond simulation, this might not lead to secure gaps for a long time,
// we seek to establish a secure gap as soon as possible
double nextSecureGap = nv->getCarFollowModel().getSecureGap(nv, &myVehicle, vsafe, plannedSpeed, getCarFollowModel().getMaxDecel());
if (nextGap < nextSecureGap) {
// establish a secureGap as soon as possible
vsafe = neighNewSpeed;
}
#ifdef DEBUG_INFORMER
if (DEBUG_COND) {
std::cout << "nextGap=" << nextGap
<< " minNextSecureGap=" << nextSecureGap
<< " vsafe=" << vsafe << "\n";
}
#endif
}
msgPass.informNeighFollower(
new Info(vsafe, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
#ifdef DEBUG_INFORMER
if (DEBUG_COND) {
std::cout << " wants to cut in before nv=" << nv->getID()
<< " vsafe1=" << vsafe1 << " vsafe=" << vsafe
<< " newSecGap="
<< nv->getCarFollowModel().getSecureGap(nv, &myVehicle, vsafe,
plannedSpeed,
myVehicle.getCarFollowModel().getMaxDecel())
<< "\n";
}
#endif
} else if ((MSGlobals::gSemiImplicitEulerUpdate && dv > 0 && dv * remainingSeconds > (secureGap - decelGap + POSITION_EPS))
|| (!MSGlobals::gSemiImplicitEulerUpdate && dv > 0 && dv * (remainingSeconds - 1) > secureGap - decelGap + POSITION_EPS)
) {
// XXX: Alternative formulation (encapsulating differences of euler and ballistic) TODO: test, refs. #2575
// double eventualGap = getCarFollowModel().gapExtrapolation(remainingSeconds - 1., decelGap, plannedSpeed, neighNewSpeed1s);
// } else if (eventualGap > secureGap + POSITION_EPS) {
// NOTE: This case corresponds to the situation, where some time is left to perform the lc
// For the ballistic case this is interpreted as follows:
// If the follower breaks with helpDecel for one second, this vehicle maintains the plannedSpeed,
// and both continue with their speeds for remainingSeconds seconds the gap will suffice for a laneChange
// For the euler case we had the following comment:
// 'decelerating once is sufficient to open up a large enough gap in time', but:
// XXX: 1) Decelerating *once* does not necessarily lead to the gap decelGap! (if TS<1s.) (Leo)
// 2) Probably, the if() for euler should test for dv * (remainingSeconds-1) > ..., too ?!, refs. #2578
msgPass.informNeighFollower(new Info(neighNewSpeed, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
#ifdef DEBUG_INFORMER
if (DEBUG_COND) {
std::cout << " wants to cut in before nv=" << nv->getID() << " (eventually)\n";
}
#endif
} else if (dir == LCA_MRIGHT && !myAllowOvertakingRight && !nv->congested()) {
// XXX: check if this requires a special treatment for the ballistic update, refs. #2575
const double vhelp = MAX2(neighNewSpeed, HELP_OVERTAKE);
msgPass.informNeighFollower(new Info(vhelp, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
#ifdef DEBUG_INFORMER
if (DEBUG_COND) {
std::cout << " wants to cut in before nv=" << nv->getID() << " (nv cannot overtake right)\n";
}
#endif
} else {
double vhelp = MAX2(nv->getSpeed(), myVehicle.getSpeed() + HELP_OVERTAKE);
//if (dir == LCA_MRIGHT && myVehicle.getWaitingSeconds() > LCA_RIGHT_IMPATIENCE &&
// nv->getSpeed() > myVehicle.getSpeed()) {
if (nv->getSpeed() > myVehicle.getSpeed() &&
((dir == LCA_MRIGHT && myVehicle.getWaitingSeconds() > LCA_RIGHT_IMPATIENCE) // NOTE: it might be considered to use myVehicle.getAccumulatedWaitingSeconds() > LCA_RIGHT_IMPATIENCE instead (Leo). Refs. #2578
|| (dir == LCA_MLEFT && plannedSpeed > CUT_IN_LEFT_SPEED_THRESHOLD) // VARIANT_22 (slowDownLeft)
// XXX this is a hack to determine whether the vehicles is on an on-ramp. This information should be retrieved from the network itself
|| (dir == LCA_MLEFT && myVehicle.getLane()->getLength() > MAX_ONRAMP_LENGTH)
)) {
// let the follower slow down to increase the likelihood that later vehicles will be slow enough to help
// follower should still be fast enough to open a gap
// XXX: The probability for that success would be larger if the slow down of the appropriate following vehicle
// would take place without the immediate follower slowing down. We might consider to model reactions of
// vehicles that are not immediate followers. (Leo) -> see ticket #2532
vhelp = MAX2(neighNewSpeed, myVehicle.getSpeed() + HELP_OVERTAKE);
#ifdef DEBUG_INFORMER
if (DEBUG_COND) {
// NOTE: the condition labeled "VARIANT_22" seems to imply that this could as well concern the *left* follower?! (Leo)
// Further, vhelp might be larger than nv->getSpeed(), so the request issued below is not to slow down!? (see below) Refs. #2578
std::cout << " wants right follower to slow down a bit\n";
}
#endif
if (MSGlobals::gSemiImplicitEulerUpdate) {
// euler
if ((nv->getSpeed() - myVehicle.getSpeed()) / helpDecel < remainingSeconds) {
#ifdef DEBUG_INFORMER
if (DEBUG_COND) {
// NOTE: the condition labeled "VARIANT_22" seems to imply that this could as well concern the *left* follower?! Refs. #2578
std::cout << " wants to cut in before right follower nv=" << nv->getID() << " (eventually)\n";
}
#endif
// XXX: I don't understand. This vhelp might be larger than nv->getSpeed() but the above condition seems to rely
// on the reasoning that if nv breaks with helpDecel for remaining Seconds, nv will be so slow, that this
// vehicle will be able to cut in. But nv might have overtaken this vehicle already (or am I missing sth?). (Leo)
// Ad: To my impression, the intention behind allowing larger speeds for the blocking follower is to prevent a
// situation, where an overlapping follower keeps blocking the ego vehicle. Refs. #2578
msgPass.informNeighFollower(new Info(vhelp, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
return;
}
} else {
// ballistic (this block is a bit different to the logic in the euler part, but in general suited to work on euler as well.. must be tested <- TODO, refs. #2575)
// estimate gap after remainingSeconds.
// Assumptions:
// (A1) leader continues with currentSpeed. (XXX: That might be wrong: Think of accelerating on an on-ramp or of a congested region ahead!)
// (A2) follower breaks with helpDecel.
const double gapAfterRemainingSecs = getCarFollowModel().gapExtrapolation(
remainingSeconds, neighFollow.second, myVehicle.getSpeed(), nv->getSpeed(), 0, -helpDecel, myVehicle.getMaxSpeedOnLane(), nv->getMaxSpeedOnLane());
const double secureGapAfterRemainingSecs = nv->getCarFollowModel().getSecureGap(nv, &myVehicle,
MAX2(nv->getSpeed() - remainingSeconds * helpDecel, 0.), myVehicle.getSpeed(), myVehicle.getCarFollowModel().getMaxDecel());
if (gapAfterRemainingSecs >= secureGapAfterRemainingSecs) { // XXX: here it would be wise to check whether there is enough space for eventual braking if the maneuver doesn't succeed
#ifdef DEBUG_INFORMER
if (DEBUG_COND) {
std::cout << " wants to cut in before follower nv=" << nv->getID() << " (eventually)\n";
}
#endif
// NOTE: ballistic uses neighNewSpeed instead of vhelp, see my note above. (Leo)
// TODO: recheck if this might cause suboptimal behaviour in some LC-situations. Refs. #2578
msgPass.informNeighFollower(new Info(neighNewSpeed, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
return;
}
}
}
#ifdef DEBUG_INFORMER
if (DEBUG_COND) {
std::cout << SIMTIME
<< " veh=" << myVehicle.getID()
<< " informs follower " << nv->getID()
<< " vhelp=" << vhelp
<< "\n";
}
#endif
msgPass.informNeighFollower(new Info(vhelp, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
// This follower is supposed to overtake us. Slow down smoothly to allow this.
const double overtakeDist = overtakeDistance(nv, &myVehicle, neighFollow.second, vhelp, plannedSpeed);
// speed difference to create a sufficiently large gap
const double needDV = overtakeDist / remainingSeconds;
// make sure the deceleration is not to strong (XXX: should be assured in finalizeSpeed -> TODO: remove the MAX2 if agreed) -> prob with possibly non-existing maximal deceleration for som CF Models(?) Refs. #2578
addLCSpeedAdvice(MAX2(vhelp - needDV, myVehicle.getSpeed() - ACCEL2SPEED(myVehicle.getCarFollowModel().getMaxDecel())));
#ifdef DEBUG_INFORMER
if (DEBUG_COND) {
std::cout << SIMTIME
<< " veh=" << myVehicle.getID()
<< " wants to be overtaken by=" << nv->getID()
<< " overtakeDist=" << overtakeDist
<< " vneigh=" << nv->getSpeed()
<< " vhelp=" << vhelp
<< " needDV=" << needDV
<< " vsafe=" << myLCAccelerationAdvices.back().first
<< "\n";
}
#endif
}
} else if (neighFollow.first != nullptr && (blocked & LCA_BLOCKED_BY_LEADER)) {
// we are not blocked by the follower now, make sure it remains that way
const double vsafe = MSLCHelper::getSpeedPreservingSecureGap(myVehicle, *neighFollow.first, neighFollow.second, plannedSpeed);
msgPass.informNeighFollower(new Info(vsafe, dir), &myVehicle);
#ifdef DEBUG_INFORMER
if (DEBUG_COND) {