47#define LOOK_FORWARD 10.
51#define LCA_RIGHT_IMPATIENCE -1.
52#define CUT_IN_LEFT_SPEED_THRESHOLD 27.
54#define LOOK_AHEAD_MIN_SPEED 0.0
55#define LOOK_AHEAD_SPEED_MEMORY 0.9
57#define HELP_DECEL_FACTOR 1.0
59#define HELP_OVERTAKE (10.0 / 3.6)
60#define MIN_FALLBEHIND (7.0 / 3.6)
62#define RELGAIN_NORMALIZATION_MIN_SPEED 10.0
64#define OPPOSITE_URGENCY 5.0
66#define KEEP_RIGHT_TIME 5.0
68#define KEEP_RIGHT_HEADWAY 2.0
69#define MAX_ONRAMP_LENGTH 200.
70#define TURN_LANE_DIST 200.0
72#define LC_RESOLUTION_SPEED_LAT 0.5
74#define REACT_TO_STOPPED_DISTANCE 100
89#define DEBUG_COND (myVehicle.isSelected())
97 mySpeedGainProbability(0),
98 myKeepRightProbability(0),
99 myLeadingBlockerLength(0),
117#ifdef DEBUG_CONSTRUCTOR
158 const std::pair<MSVehicle*, double>& leader,
159 const std::pair<MSVehicle*, double>& follower,
160 const std::pair<MSVehicle*, double>& neighLead,
161 const std::pair<MSVehicle*, double>& neighFollow,
163 const std::vector<MSVehicle::LaneQ>& preb,
167#ifdef DEBUG_WANTS_CHANGE
169 std::cout <<
"\nWANTS_CHANGE\n" <<
SIMTIME
176 <<
" considerChangeTo=" << (laneOffset == -1 ?
"right" :
"left")
181 const int result =
_wantsChange(laneOffset, msgPass, blocked, leader, follower, neighLead, neighFollow, neighLane, preb, lastBlocked, firstBlocked);
183#ifdef DEBUG_WANTS_CHANGE
196#ifdef DEBUG_PATCH_SPEED
198 std::cout <<
"\nPATCH_SPEED\n"
205 <<
" wanted=" << wanted
212 const double newSpeed =
_patchSpeed(
MAX2(min, 0.0), wanted, max, cfModel);
214#ifdef DEBUG_PATCH_SPEED
216 const std::string patched = (wanted != newSpeed ?
" patched=" +
toString(newSpeed) :
"");
229#ifdef DEBUG_PATCH_SPEED
241 double MAGIC_offset = 1.;
242 double nVSafe = wanted;
247#ifdef DEBUG_PATCH_SPEED
256 max =
MIN2(max,
MAX2(safe, vMinEmergency));
261 if (safe >= vMinEmergency) {
263 min =
MAX2(vMinEmergency, safe);
266#ifdef DEBUG_PATCH_SPEED
268 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" slowing down for leading blocker, safe=" << safe << (safe + NUMERICAL_EPS < min ?
" (not enough)" :
"") <<
"\n";
271 nVSafe =
MAX2(min, safe);
291 nVSafe =
MIN2(v, nVSafe);
293 nVSafe =
MIN2(v * coopWeight + (1 - coopWeight) * wanted, nVSafe);
296#ifdef DEBUG_PATCH_SPEED
303#ifdef DEBUG_PATCH_SPEED
305 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" ignoring low nVSafe=" << v <<
" min=" << min <<
"\n";
309#ifdef DEBUG_PATCH_SPEED
311 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" ignoring high nVSafe=" << v <<
" max=" << max <<
"\n";
319#ifdef DEBUG_PATCH_SPEED
332#ifdef DEBUG_PATCH_SPEED
337 return (max + wanted) / 2.0;
341#ifdef DEBUG_PATCH_SPEED
347 return (
MAX2(0., min) + wanted) / 2.0;
353#ifdef DEBUG_PATCH_SPEED
358 return (max + wanted) / 2.0;
399#ifdef DEBUG_PATCH_SPEED
404 return (max + wanted) / 2.0;
408#ifdef DEBUG_PATCH_SPEED
440 <<
" informedBy=" << sender->
getID()
441 <<
" info=" << pinfo->second
442 <<
" vSafe=" << pinfo->first
454 double overtakeDist = (gap
459 return MAX2(overtakeDist, 0.);
467 const std::pair<MSVehicle*, double>& neighLead,
468 double remainingSeconds) {
471 plannedSpeed =
MIN2(plannedSpeed,
475 const double a = i.first;
482 std::cout <<
"\nINFORM_LEADER"
487 const MSVehicle*
const nv = neighLead.first;
495 neighNextGap = neighLead.second +
SPEED2DIST(neighNextSpeed - plannedSpeed);
506 std::cout <<
" blocked by leader nv=" << nv->
getID() <<
" nvSpeed=" << nv->
getSpeed() <<
" needGap="
513 const double dv = plannedSpeed - nv->
getSpeed();
516 overtakeTime = overtakeDist / dv;
519 overtakeTime = remainingSeconds + 1;
525 <<
"\nnv = " << nv->
getID()
526 <<
"\nplannedSpeed = " << plannedSpeed
527 <<
"\nleaderSpeed = " << nv->
getSpeed()
529 <<
"\nremainingSeconds = " << remainingSeconds
530 <<
"\novertakeDist = " << overtakeDist
531 <<
"\novertakeTime = " << overtakeTime
546 && (!neighLead.first->isStopped() || (
isOpposite() && neighLead.second >= 0))) {
551 const double targetSpeed =
MAX2(
563 <<
" cannot overtake leader nv=" << nv->
getID()
567 <<
" overtakeDist=" << overtakeDist
568 <<
" overtakeTime=" << overtakeTime
569 <<
" remainingSeconds=" << remainingSeconds
570 <<
" currentGap=" << neighLead.second
572 <<
" neighNextSpeed=" << neighNextSpeed
573 <<
" neighNextGap=" << neighNextGap
574 <<
" targetSpeed=" << targetSpeed
575 <<
" nextSpeed=" << nextSpeed
586 <<
" cannot overtake fast leader nv=" << nv->
getID()
590 <<
" overtakeDist=" << overtakeDist
592 <<
" overtakeTime=" << overtakeTime
593 <<
" remainingSeconds=" << remainingSeconds
594 <<
" currentGap=" << neighLead.second
595 <<
" neighNextSpeed=" << neighNextSpeed
596 <<
" neighNextGap=" << neighNextGap
597 <<
" targetSpeed=" << targetSpeed
609 <<
" wants to overtake leader nv=" << nv->
getID()
611 <<
" overtakeDist=" << overtakeDist
612 <<
" remainingSeconds=" << remainingSeconds
613 <<
" overtakeTime=" << overtakeTime
614 <<
" currentGap=" << neighLead.second
624 const double targetSpeed =
MAX2(
630 std::cout <<
" not blocked by leader nv=" << nv->
getID()
632 <<
" gap=" << neighLead.second
633 <<
" neighNextSpeed=" << neighNextSpeed
634 <<
" neighNextGap=" << neighNextGap
636 <<
" targetSpeed=" << targetSpeed
640 return MIN2(targetSpeed, plannedSpeed);
648 const std::pair<MSVehicle*, double>& neighFollow,
649 double remainingSeconds,
650 double plannedSpeed) {
657 std::cout <<
"\nINFORM_FOLLOWER"
669 std::cout <<
" blocked by follower nv=" << nv->
getID() <<
" nvSpeed=" << nv->
getSpeed() <<
" needGap="
677 if ((neededGap - neighFollow.second) / remainingSeconds < (
MAX2(plannedSpeed, 0.) - nv->
getSpeed())) {
680 std::cout <<
" wants to cut in before nv=" << nv->
getID() <<
" without any help." <<
"\nneededGap = " << neededGap <<
"\n";
700 double neighNewSpeed;
702 double neighNewSpeed1s;
712 dv = plannedSpeed - neighNewSpeed1s;
719 decelGap = neighFollow.second + dv;
726 neighNewSpeed1s = nv->
getSpeed() - helpDecel;
742 <<
" plannedSpeed=" << plannedSpeed
743 <<
" threshold=" << onRampThreshold
744 <<
" neighNewSpeed=" << neighNewSpeed
745 <<
" neighNewSpeed1s=" << neighNewSpeed1s
747 <<
" gap=" << neighFollow.second
748 <<
" decelGap=" << decelGap
749 <<
" secureGap=" << secureGap
756 && neighNewSpeed1s < onRampThreshold) {
760 if (decelGap > 0 && decelGap >= secureGap) {
768 double vsafe, vsafe1;
790 std::cout <<
"nextGap=" << nextGap <<
" (without help decel) \n";
798 MAX2(0., plannedSpeed),
806 nv->
getSpeed(), plannedAccel, -decel2,
816 MAX2(0., plannedSpeed),
819 assert(vsafe >= vsafe1 - NUMERICAL_EPS);
823 std::cout <<
"nextGap=" << nextGap
824 <<
" (with vsafe1 and help decel) \nvsafe1=" << vsafe1
825 <<
" vsafe=" << vsafe
834 if (nextGap < nextSecureGap) {
836 vsafe = neighNewSpeed;
841 std::cout <<
"nextGap=" << nextGap
842 <<
" minNextSecureGap=" << nextSecureGap
843 <<
" vsafe=" << vsafe <<
"\n";
853 std::cout <<
" wants to cut in before nv=" << nv->
getID()
854 <<
" vsafe1=" << vsafe1 <<
" vsafe=" << vsafe
882 std::cout <<
" wants to cut in before nv=" << nv->
getID() <<
" (eventually)\n";
891 std::cout <<
" wants to cut in before nv=" << nv->
getID() <<
" (nv cannot overtake right)\n";
914 std::cout <<
" wants right follower to slow down a bit\n";
924 std::cout <<
" wants to cut in before right follower nv=" << nv->
getID() <<
" (eventually)\n";
946 if (gapAfterRemainingSecs >= secureGapAfterRemainingSecs) {
949 std::cout <<
" wants to cut in before follower nv=" << nv->
getID() <<
" (eventually)\n";
966 <<
" informs follower " << nv->
getID()
967 <<
" vhelp=" << vhelp
976 const double needDV = overtakeDist / remainingSeconds;
984 <<
" wants to be overtaken by=" << nv->
getID()
985 <<
" overtakeDist=" << overtakeDist
987 <<
" vhelp=" << vhelp
988 <<
" needDV=" << needDV
1001 std::cout <<
" wants to cut in before non-blocking follower nv=" << nv->
getID() <<
"\n";
1030 scaledDelta =
MIN2(overlap, maxDist);
1039 deltaPosLat =
MAX2(
MIN2(deltaPosLat, maxDist), -maxDist);
1052 if (bestLaneOffset < -1) {
1054 }
else if (bestLaneOffset > 1) {
1095 const std::pair<MSVehicle*, double>& leader,
1096 const std::pair<MSVehicle*, double>& follower,
1097 const std::pair<MSVehicle*, double>& neighLead,
1098 const std::pair<MSVehicle*, double>& neighFollow,
1100 const std::vector<MSVehicle::LaneQ>& preb,
1103 assert(laneOffset == 1 || laneOffset == -1);
1107 int bestLaneOffset = 0;
1112 double currentDist = 0;
1113 double neighDist = 0;
1126 const int prebOffset = laneOffset;
1127 for (
int p = 0; p < (int) preb.size(); ++p) {
1131 if (preb[p].lane == prebLane && p + laneOffset >= 0) {
1132 assert(p + prebOffset < (
int)preb.size());
1134 neigh = preb[p + prebOffset];
1135 currentDist = curr.
length;
1136 neighDist = neigh.
length;
1138 if (bestLaneOffset == 0 && preb[p + prebOffset].bestLaneOffset == 0 && !checkOpposite) {
1139#ifdef DEBUG_WANTS_CHANGE
1143 <<
" bestLaneOffsetOld=" << bestLaneOffset
1144 <<
" bestLaneOffsetNew=" << laneOffset
1148 bestLaneOffset = prebOffset;
1150 best = preb[p + bestLaneOffset];
1155 assert(curr.
lane !=
nullptr);
1156 assert(neigh.
lane !=
nullptr);
1157 assert(best.
lane !=
nullptr);
1159 const bool right = (laneOffset == -1);
1161 double driveToNextStop = -std::numeric_limits<double>::max();
1169#ifdef DEBUG_WANTS_CHANGE
1174 <<
" stopPos=" << stopPos
1175 <<
" currentDist=" << currentDist
1176 <<
" neighDist=" << neighDist
1180 currentDist =
MAX2(currentDist, stopPos);
1181 neighDist =
MAX2(neighDist, stopPos);
1186 bool changeToBest = (right && bestLaneOffset < 0) || (!right && bestLaneOffset > 0);
1192 if (lastBlocked != firstBlocked) {
1196#ifdef DEBUG_WANTS_CHANGE
1205 <<
" leaderGap=" << leader.second
1207 <<
" followerGap=" << follower.second
1209 <<
" neighLeadGap=" << neighLead.second
1211 <<
" neighFollowGap=" << neighFollow.second
1234 assert(memoryFactor > 0.);
1240 const bool hasStoppedLeader = leader.first != 0 && leader.first->isStopped() && leader.second < (currentDist - posOnLane);
1242 && leader.first !=
nullptr
1243 && leader.first->getLane()->
getBidiLane() !=
nullptr
1246 if (bestLaneOffset == 0 && hasBidiLeader) {
1249 }
else if (bestLaneOffset == 0 && hasStoppedLeader) {
1254 + leader.first->getVehicleType().getLengthWithGap()
1256 }
else if (bestLaneOffset == laneOffset && neighLead.first != 0 && neighLead.first->isStopped() && neighLead.second < (currentDist - posOnLane)) {
1261 + neighLead.first->getVehicleType().getLengthWithGap()
1262 + neighLead.second);
1263 }
else if (!hasStoppedLeader &&
1267 changeToBest =
false;
1286 currentDist += roundaboutBonus;
1287 neighDist += roundaboutBonus;
1291 const double maxJam =
MAX2(preb[currIdx + prebOffset].occupation, preb[currIdx].occupation);
1295 double thisLaneVSafe = vMax;
1298 double neighLeftPlace =
MAX2(0.0, neighDist - posOnLane - maxJam);
1299 if (neighLead.first != 0 && neighLead.first->isStopped()) {
1300 neighLeftPlace =
MIN2(neighLeftPlace, neighLead.second);
1303#ifdef DEBUG_WANTS_CHANGE
1308 <<
" laDist=" << laDist
1309 <<
" currentDist=" << currentDist
1310 <<
" usableDist=" << usableDist
1311 <<
" bestLaneOffset=" << bestLaneOffset
1313 <<
" best.length=" << best.
length
1314 <<
"\n roundaboutBonus=" << roundaboutBonus
1315 <<
" maxJam=" << maxJam
1316 <<
" neighDist=" << neighDist
1317 <<
" neighLeftPlace=" << neighLeftPlace
1322 bool changeLeftToAvoidOvertakeRight =
false;
1329 if (neighLead.first != 0 && checkOverTakeRight && !right) {
1341 if (vSafeFollow >= vMaxDecel) {
1342 vSafe = vSafeFollow;
1344 vSafe =
MAX2(vMaxDecel, vStayBehind);
1349 thisLaneVSafe =
MIN2(thisLaneVSafe, vSafe);
1352 const double deltaGapFuture = deltaV * 8;
1355 if (vSafeFuture < vSafe) {
1356 const double relativeGain = deltaV /
MAX2(vMax,
1359 changeLeftToAvoidOvertakeRight =
true;
1361#ifdef DEBUG_WANTS_CHANGE
1364 <<
" avoid overtaking on the right nv=" << nv->
getID()
1365 <<
" deltaV=" << deltaV
1374 const bool currFreeUntilNeighEnd = leader.first ==
nullptr || neighDist - posOnLane <= leader.second;
1375 const double overtakeDist = (leader.first == 0 ? -1 :
1379 &&
MIN2(neighDist, currentDist) - posOnLane > overtakeDist
1381 && (!checkOverTakeRight || !right)
1383 && (neighLead.first == 0 || !neighLead.first->isStopped()
1385 || neighLead.second > overtakeDist)) {
1388#ifdef DEBUG_WANTS_CHANGE
1390 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" overtake stopped leader=" << leader.first->getID()
1391 <<
" overtakeDist=" << overtakeDist
1392 <<
" remaining=" <<
MIN2(neighDist, currentDist) - posOnLane
1397 }
else if (!changeToBest && (
currentDistDisallows(neighLeftPlace, abs(bestLaneOffset) + 2, laDist))) {
1404#ifdef DEBUG_WANTS_CHANGE
1406 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" could not change back and forth in time (1) neighLeftPlace=" << neighLeftPlace <<
"\n";
1410 }
else if (bestLaneOffset == 0 && (neighLeftPlace * 2. < laDist)) {
1415#ifdef DEBUG_WANTS_CHANGE
1417 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" could not change back and forth in time (2) neighLeftPlace=" << neighLeftPlace <<
"\n";
1421 }
else if (bestLaneOffset == 0
1422 && (leader.first == 0 || !leader.first->isStopped())
1424 && roundaboutBonus == 0
1433#ifdef DEBUG_WANTS_CHANGE
1435 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" does not want to leave the bestLane (neighDist=" << neighDist <<
")\n";
1442#ifdef DEBUG_WANTS_CHANGE
1450 if ((ret & lcaCounter) != 0) {
1454#ifdef DEBUG_WANTS_CHANGE
1469 if (changeToBest && abs(bestLaneOffset) > 1) {
1472#ifdef DEBUG_WANTS_CHANGE
1474 std::cout <<
" reserving space for unseen blockers myLeadingBlockerLength=" <<
myLeadingBlockerLength <<
"\n";
1483 if (*firstBlocked != neighLead.first) {
1486#ifdef DEBUG_SAVE_BLOCKER_LENGTH
1488 std::cout <<
SIMTIME <<
" canReserve=" << canReserve <<
" canContinue=" << canContinue <<
"\n";
1497 const int remainingLanes =
MAX2(1, abs(bestLaneOffset));
1499 const double remainingSeconds = ((ret &
LCA_TRACI) == 0 ?
1503 const double plannedSpeed =
informLeader(msgPass, blocked, myLca, neighLead, remainingSeconds);
1508 informFollower(msgPass, blocked, myLca, neighFollow, remainingSeconds, plannedSpeed);
1511#ifdef DEBUG_WANTS_CHANGE
1516 <<
" remainingSeconds=" << remainingSeconds
1517 <<
" plannedSpeed=" << plannedSpeed
1529 const bool acceleratingLeader = (neighLead.first != 0 && neighLead.first->getAcceleration() > 0)
1530 || (leader.first != 0 && leader.first->getAcceleration() > 0);
1541 double inconvenience = laneOffset < 0
1545 const double relSpeedDiff = thisLaneVSafe == 0 ? 0 : (thisLaneVSafe - neighLaneVSafe) /
MAX2(thisLaneVSafe, neighLaneVSafe);
1546 inconvenience =
MAX2(relSpeedDiff, inconvenience);
1547 inconvenience =
MIN2(1.0, inconvenience);
1551#ifdef DEBUG_WANTS_CHANGE
1556 <<
" neighSpeedFactor=" << (thisLaneVSafe / neighLaneVSafe - 1)
1557 <<
" inconvenience=" << inconvenience
1558 <<
" speedInconv=" << speedGainInconvenient
1559 <<
" occInconv=" << neighOccupancyInconvenient
1565 if (roundaboutBonus > 0) {
1567#ifdef DEBUG_WANTS_CHANGE
1571 <<
" roundaboutBonus=" << roundaboutBonus
1601#ifdef DEBUG_WANTS_CHANGE
1603 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" does not want to get stranded on the on-ramp of a highway\n";
1623 && (!speedGainInconvenient)
1625 && (changeToBest ||
currentDistAllows(neighDist, abs(bestLaneOffset) + 1, laDist))) {
1628#ifdef DEBUG_COOPERATE
1632 <<
" wantsChangeToHelp=" << (right ?
"right" :
"left")
1634 << (((
myOwnState & myLca) == 0) ?
" (counter)" :
"")
1665 const double relativeGain = (neighLaneVSafe - thisLaneVSafe) /
MAX2(neighLaneVSafe,
1668#ifdef DEBUG_WANTS_CHANGE
1672 <<
" currentDist=" << currentDist
1673 <<
" neighDist=" << neighDist
1674 <<
" thisVSafe=" << thisLaneVSafe
1675 <<
" neighVSafe=" << neighLaneVSafe
1676 <<
" relGain=" <<
toString(relativeGain, 8)
1683 if (thisLaneVSafe - 5 / 3.6 > neighLaneVSafe) {
1700 if (mySpeedGainProbability < 0 || relativeGain > 0) {
1706 double acceptanceTime;
1713 if (follower.first !=
nullptr && follower.second < 2 * follower.first->getCarFollowModel().brakeGap(follower.first->getSpeed())) {
1717 const double fRSF = follower.first->getLane()->getVehicleMaxSpeed(follower.first) / follower.first->getLane()->getSpeedLimit();
1718 if (fRSF > roadSpeedFactor) {
1719 acceptanceTime /= fRSF;
1725 double fullSpeedDrivingSeconds =
MIN2(acceptanceTime, fullSpeedGap / vMax);
1726 if (neighLead.first != 0 && neighLead.first->getSpeed() < vMax) {
1727 fullSpeedGap =
MAX2(0.,
MIN2(fullSpeedGap,
1729 vMax, neighLead.first->
getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel())));
1730 fullSpeedDrivingSeconds =
MIN2(fullSpeedDrivingSeconds, fullSpeedGap / (vMax - neighLead.first->getSpeed()));
1733 if (checkOverTakeRight && leader.first != 0
1734 && leader.first->getLane()->getVehicleMaxSpeed(leader.first) < vMax) {
1735 fullSpeedGap =
MIN2(fullSpeedGap, leader.second);
1736 fullSpeedDrivingSeconds =
MIN2(fullSpeedDrivingSeconds, fullSpeedGap / (vMax - leader.first->getSpeed()));
1737 const double relGain = (vMax - leader.first->getLane()->getVehicleMaxSpeed(leader.first)) /
MAX2(vMax,
1754#ifdef DEBUG_WANTS_CHANGE
1759 <<
" neighDist=" << neighDist
1761 <<
" leaderSpeed=" << (neighLead.first == 0 ? -1 : neighLead.first->getSpeed())
1763 myVehicle.
getSpeed(), neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()))
1764 <<
" acceptanceTime=" << acceptanceTime
1765 <<
" fullSpeedGap=" << fullSpeedGap
1766 <<
" fullSpeedDrivingSeconds=" << fullSpeedDrivingSeconds
1767 <<
" dProb=" << deltaProb
1780#ifdef DEBUG_WANTS_CHANGE
1786 <<
" thisLaneVSafe=" << thisLaneVSafe
1787 <<
" neighLaneVSafe=" << neighLaneVSafe
1788 <<
" relativeGain=" << relativeGain
1789 <<
" blocked=" << blocked
1803 if (thisLaneVSafe > neighLaneVSafe) {
1808 }
else if (thisLaneVSafe == neighLaneVSafe) {
1826#ifdef DEBUG_WANTS_CHANGE
1832 <<
" thisLaneVSafe=" << thisLaneVSafe
1833 <<
" neighLaneVSafe=" << neighLaneVSafe
1834 <<
" relativeGain=" << relativeGain
1835 <<
" blocked=" << blocked
1841 && (relativeGain > NUMERICAL_EPS || changeLeftToAvoidOvertakeRight)
1852 && relativeGain >= 0
1853 && (right ? mySpeedGainProbability < 0 : mySpeedGainProbability > 0)) {
1860#ifdef DEBUG_WANTS_CHANGE
1866 <<
" thisLaneVSafe=" << thisLaneVSafe
1867 <<
" neighLaneVSafe=" << neighLaneVSafe
1878 const MSVehicle* leader = leaderDist.first;
1879 const double gap = leaderDist.second;
1881 if (acceleratingLeader) {
1885 if (leader ==
nullptr) {
1897 if (leader ==
nullptr) {
1908 futureSpeed =
MIN2(vMax, futureSpeed);
1911 const double deltaV = vMax - futureLeaderSpeed;
1912 if (deltaV > 0 && gap > 0) {
1914 const double fullSpeedGap = gap - secGap;
1918 const double gapClosingTime =
MAX2(0.0, fullSpeedGap / deltaV);
1921 futureSpeed =
MIN2(futureSpeed, (gapClosingTime * futureSpeed + (foreCastTime - gapClosingTime) * futureLeaderSpeed) / foreCastTime);
1933 if ((*blocked) !=
nullptr) {
1935#ifdef DEBUG_SLOW_DOWN
1944 if (gap > POSITION_EPS) {
1958 gap - POSITION_EPS, (*blocked)->getSpeed(),
1959 (*blocked)->getCarFollowModel().getMaxDecel()),
false);
1962#ifdef DEBUG_SLOW_DOWN
1966 <<
" slowing down for"
1989#ifdef DEBUG_WANTS_CHANGE
1991 std::cout <<
SIMTIME <<
" adapt to pedestrians on lane=" << lane->
getID() <<
"\n";
1997 if (leader.first != 0) {
1999 v =
MIN2(v, stopSpeed);
2000#ifdef DEBUG_WANTS_CHANGE
2002 std::cout <<
SIMTIME <<
" pedLeader=" << leader.first->getID() <<
" dist=" << leader.second <<
" v=" << v <<
"\n";
2013#ifdef DEBUG_WANTS_CHANGE
2015 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" myLeftSpace=" <<
myLeftSpace <<
" latDist=" << latDist <<
" maneuverDist=" << maneuverDist <<
" result=" << result <<
"\n";
2023 result =
MAX2(-speedBound,
MIN2(speedBound, result));
2036 return myOppositeParam <= 0 ? std::numeric_limits<double>::max() : 1 /
myOppositeParam;
2044#ifdef DEBUG_SAVE_BLOCKER_LENGTH
2046 std::cout <<
SIMTIME <<
" saveBlockerLength veh=" <<
myVehicle.
getID() <<
" canReserve=" << canReserve <<
" myLeftSpace=" <<
myLeftSpace <<
" foeLeftSpace=" << foeLeftSpace <<
"\n";
2098 }
else if (key ==
"speedGainProbabilityRight") {
2100 }
else if (key ==
"speedGainProbabilityLeft") {
2102 }
else if (key ==
"keepRightProbability") {
2104 }
else if (key ==
"lookAheadSpeed") {
2107 }
else if (key ==
"speedGainRP") {
2109 }
else if (key ==
"speedGainLP") {
2111 }
else if (key ==
"keepRightP") {
2163 }
else if (key ==
"speedGainProbabilityRight") {
2165 }
else if (key ==
"speedGainProbabilityLeft") {
2167 }
else if (key ==
"keepRightProbability") {
2169 }
else if (key ==
"lookAheadSpeed") {
#define HELP_DECEL_FACTOR
#define LOOK_AHEAD_MIN_SPEED
#define LCA_RIGHT_IMPATIENCE
#define REACT_TO_STOPPED_DISTANCE
#define RELGAIN_NORMALIZATION_MIN_SPEED
#define CUT_IN_LEFT_SPEED_THRESHOLD
#define MAX_ONRAMP_LENGTH
#define LOOK_AHEAD_SPEED_MEMORY
#define LC_RESOLUTION_SPEED_LAT
std::pair< const MSPerson *, double > PersonDist
LaneChangeAction
The state of a vehicle's lane-change behavior.
@ LCA_KEEPRIGHT
The action is due to the default of keeping right "Rechtsfahrgebot".
@ LCA_BLOCKED
blocked in all directions
@ LCA_URGENT
The action is urgent (to be defined by lc-model)
@ LCA_STAY
Needs to stay on the current lane.
@ LCA_BLOCKED_BY_LEADER
blocked by leader
@ LCA_AMBLOCKINGFOLLOWER_DONTBRAKE
@ LCA_COOPERATIVE
The action is done to help someone else.
@ LCA_LEFT
Wants go to the left.
@ LCA_STRATEGIC
The action is needed to follow the route (navigational lc)
@ LCA_AMBACKBLOCKER_STANDING
@ 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
@ SUMO_ATTR_LCA_COOPERATIVE_SPEED
@ SUMO_ATTR_LCA_ASSERTIVE
@ SUMO_ATTR_LCA_LOOKAHEADLEFT
@ SUMO_ATTR_LCA_SPEEDGAIN_PARAM
@ SUMO_ATTR_LCA_MAXDISTLATSTANDING
@ 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_OVERTAKE_DELTASPEED_FACTOR
@ SUMO_ATTR_LCA_OVERTAKE_RIGHT
@ SUMO_ATTR_LCA_STRATEGIC_PARAM
@ SUMO_ATTR_LCA_KEEPRIGHT_ACCEPTANCE_TIME
@ SUMO_ATTR_LCA_EXPERIMENTAL1
@ SUMO_ATTR_LCA_SPEEDGAINRIGHT
int gPrecision
the precision for floating point outputs
#define UNUSED_PARAMETER(x)
const double SUMO_const_haltingSpeed
the speed threshold at which vehicles are considered as halting
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
A class responsible for exchanging messages between cars involved in lane-change interaction.
void * informNeighFollower(void *info, MSVehicle *sender)
Informs the follower on the desired lane.
void * informNeighLeader(void *info, MSVehicle *sender)
Informs the leader on the desired lane.
Interface for lane-change models.
double getForwardPos() const
get vehicle position relative to the forward direction lane
bool hasBlueLight() const
virtual double computeSpeedLat(double latDist, double &maneuverDist, bool urgent) const
decides the next lateral speed depending on the remaining lane change distance to be covered and upda...
virtual void resetSpeedLat()
int myOwnState
The current state of the vehicle.
virtual void prepareStep()
double myMaxDistLatStanding
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 ...
int & getCanceledState(const int dir)
double myMaxSpeedLatFactor
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
void setSpeedLat(double speedLat)
set the lateral speed and update lateral acceleraton
double myOvertakeRightParam
const MSCFModel & getCarFollowModel() const
The vehicle's car following model.
double myMaxSpeedLatStanding
MSVehicle & myVehicle
The vehicle this lane-changer belongs to.
virtual bool avoidOvertakeRight() const
bool isChangingLanes() const
return true if the vehicle currently performs a lane change maneuver
double getImpatience() const
Returns this vehicles impatience.
double getWaitingSeconds() const
Returns the number of seconds waited (speed was lesser than 0.1m/s)
const MSVehicleType & getVehicleType() const
Returns the vehicle's type definition.
bool isStopped() const
Returns whether the vehicle is at a stop.
The car-following model abstraction.
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 ...
double maximumSafeStopSpeed(double gap, double decel, double currentSpeed, bool onInsertion=false, double headway=-1) const
Returns the maximum next velocity for stopping within gap.
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...
double maximumSafeFollowSpeed(double gap, double egoSpeed, double predSpeed, double predMaxDecel, bool onInsertion=false) const
Returns the maximum safe velocity for following the given leader.
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].
double brakeGap(const double speed) const
Returns the distance the vehicle needs to halt including driver's reaction time tau (i....
double getMaxDecel() const
Get the vehicle type's maximal comfortable deceleration [m/s^2].
virtual double getSpeedAfterMaxDecel(double v) const
Returns the velocity after maximum deceleration.
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)
const std::set< MSTransportable *, ComparatorNumericalIdLess > & getPersons() const
Returns this edge's persons set.
bool hasLaneChanger() const
bool isInternal() const
return whether this edge is an internal edge
static bool gSemiImplicitEulerUpdate
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 saveBlockerLength(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
bool currentDistAllows(double dist, int laneOffset, double lookForwardDist)
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 method gets th...
void informFollower(MSAbstractLaneChangeModel::MSLCMessager &msgPass, int blocked, int dir, const std::pair< MSVehicle *, double > &neighFollow, double remainingSeconds, double plannedSpeed)
decide whether we will try cut in before the follower or allow to be overtaken
double computeSpeedLat(double latDist, double &maneuverDist, bool urgent) const override
decides the next lateral speed (for continuous lane changing)
double myOvertakeDeltaSpeedFactor
double getExtraReservation(int bestLaneOffset) const override
reserve extra space for unseen blockers when more tnan one lane change is required
bool debugVehicle() const override
whether the current vehicles shall be debugged
double mySpeedGainLookahead
const double myExperimentalParam1
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 ...
void initDerivedParameters()
init cached parameters derived directly from model parameters
double myCooperativeParam
MSLCM_LC2013(MSVehicle &v)
double myChangeProbThresholdRight
double anticipateFollowSpeed(const std::pair< MSVehicle *, double > &leaderDist, double dist, double vMax, bool acceleratingLeader)
anticipate future follow speed for the given leader
std::string getParameter(const std::string &key) const override
try to retrieve the given parameter from this device. Throw exception for unsupported key
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
double myCooperativeSpeed
double informLeader(MSAbstractLaneChangeModel::MSLCMessager &msgPass, int blocked, int dir, const std::pair< MSVehicle *, double > &neighLead, double remainingSeconds)
double _patchSpeed(double min, const double wanted, double max, const MSCFModel &cfModel)
double myLeadingBlockerLength
std::pair< double, int > Info
information regarding save velocity (unused) and state flags of the ego vehicle
void prepareStep() override
double getSafetyFactor() const override
return factor for modifying the safety constraints of the car-following model
int slowDownForBlocked(MSVehicle **blocked, int state)
compute useful slowdowns for blocked vehicles
bool amBlockingFollowerPlusNB()
double myKeepRightProbability
bool currentDistDisallows(double dist, int laneOffset, double lookForwardDist)
void adaptSpeedToPedestrians(const MSLane *lane, double &v)
react to pedestrians on the given lane
double getOppositeSafetyFactor() const override
return factor for modifying the safety constraints for opposite-diretction overtaking of the car-foll...
void resetState() override
double myKeepRightAcceptanceTime
bool saveBlockerLength(double length, double foeLeftSpace) override
reserve space at the end of the lane to avoid dead locks
double mySpeedGainProbability
a value for tracking the probability that a change to the offset with the same sign is beneficial
double myChangeProbThresholdLeft
void * inform(void *info, MSVehicle *sender) 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)
helper function for doing the actual work
static double overtakeDistance(const MSVehicle *follower, const MSVehicle *leader, const double gap, double followerSpeed=INVALID_SPEED, double leaderSpeed=INVALID_SPEED)
Representation of a lane in the micro simulation.
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...
bool isAccelLane() const
return whether this lane is an acceleration lane
double getSpeedLimit() const
Returns the lane's maximum allowed speed.
double getLength() const
Returns the lane's length.
double getVehicleMaxSpeed(const SUMOTrafficObject *const veh) const
Returns the lane's maximum speed, given a vehicle's speed limit adaptation.
bool hasPedestrians() const
whether the lane has pedestrians on it
int getIndex() const
Returns the lane's index.
double getBruttoOccupancy() const
Returns the brutto (including minGaps) occupancy of this lane during the last step.
MSLane * getBidiLane() const
retrieve bidirectional lane or nullptr
MSEdge & getEdge() const
Returns the lane's edge.
const MSLane * getNormalPredecessorLane() const
get normal lane leading to this internal lane, for normal lanes, the lane itself is returned
const std::vector< MSLink * > & getLinkCont() const
returns the container with all links !!!
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
SUMOTime getCurrentTimeStep() const
Returns the current simulation step.
const MSLane * lane
The lane to stop at (microsim only)
double changeRequestRemainingSeconds(const SUMOTime currentTime) const
Return the remaining number of seconds of the current laneTimeLine assuming one exists.
Representation of a vehicle in the micro simulation.
double getActionStepLengthSecs() const
Returns the vehicle's action step length in secs, i.e. the interval between two action points.
int influenceChangeDecision(int state)
allow TraCI to influence a lane change decision
double getMaxSpeedOnLane() const
Returns the maximal speed for the vehicle on its current lane (including speed factor and deviation,...
double nextStopDist() const
return the distance to the next stop or doubleMax if there is none.
double getAcceleration() const
Returns the vehicle's acceleration in m/s (this is computed as the last step's mean acceleration in c...
int getBestLaneOffset() const
const MSLane * getLane() const
Returns the lane the vehicle is on.
double getLastStepDist() const
Get the distance the vehicle covered in the previous timestep.
Influencer & getInfluencer()
double getRightSideOnLane() const
Get the lateral position of the vehicles right side on the lane:
double getLateralPositionOnLane() const
Get the vehicle's lateral position on the lane.
double getSpeed() const
Returns the vehicle's current speed.
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.
double getPositionOnLane() const
Get the vehicle's position along the lane.
double getLateralOverlap() const
return the amount by which the vehicle extends laterally outside it's primary lane
void setLateralPositionOnLane(double posLat)
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.
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].
static std::string getIDSecure(const T *obj, const std::string &fallBack="NULL")
get an identifier for Named-like object which may be Null
const std::string & getID() const
Returns the id.
void step(double dt)
evolve for a time step of length dt.
static double toDouble(const std::string &sData)
converts a string into the double value described by it by calling the char-type converter
A structure representing the best lanes for continuing the current route starting at 'lane'.
double length
The overall length which may be driven when using this lane without a lane change.
std::vector< MSLane * > bestContinuations
MSLane * lane
The described lane.
int bestLaneOffset
The (signed) number of lanes to be crossed to get to the lane which allows to continue the drive.
double occupation
The overall vehicle sum on consecutive lanes which can be passed without a lane change.