47 #define MAGIC_OFFSET 1.
48 #define LOOK_FORWARD 10.
52 #define LCA_RIGHT_IMPATIENCE -1.
53 #define CUT_IN_LEFT_SPEED_THRESHOLD 27.
55 #define LOOK_AHEAD_MIN_SPEED 0.0
56 #define LOOK_AHEAD_SPEED_MEMORY 0.9
58 #define HELP_DECEL_FACTOR 1.0
60 #define HELP_OVERTAKE (10.0 / 3.6)
61 #define MIN_FALLBEHIND (7.0 / 3.6)
63 #define RELGAIN_NORMALIZATION_MIN_SPEED 10.0
65 #define OPPOSITE_URGENCY 5.0
67 #define KEEP_RIGHT_TIME 5.0
69 #define KEEP_RIGHT_HEADWAY 2.0
70 #define MAX_ONRAMP_LENGTH 200.
71 #define TURN_LANE_DIST 200.0
73 #define LC_RESOLUTION_SPEED_LAT 0.5
75 #define REACT_TO_STOPPED_DISTANCE 100
90 #define DEBUG_COND (myVehicle.isSelected())
98 mySpeedGainProbability(0),
99 myKeepRightProbability(0),
100 myLeadingBlockerLength(0),
118 #ifdef DEBUG_CONSTRUCTOR
159 const std::pair<MSVehicle*, double>& leader,
160 const std::pair<MSVehicle*, double>& follower,
161 const std::pair<MSVehicle*, double>& neighLead,
162 const std::pair<MSVehicle*, double>& neighFollow,
164 const std::vector<MSVehicle::LaneQ>& preb,
168 #ifdef DEBUG_WANTS_CHANGE
170 std::cout <<
"\nWANTS_CHANGE\n" <<
SIMTIME
177 <<
" considerChangeTo=" << (laneOffset == -1 ?
"right" :
"left")
182 const int result =
_wantsChange(laneOffset, msgPass, blocked, leader, follower, neighLead, neighFollow, neighLane, preb, lastBlocked, firstBlocked);
184 #ifdef DEBUG_WANTS_CHANGE
197 #ifdef DEBUG_PATCH_SPEED
199 std::cout <<
"\nPATCH_SPEED\n"
206 <<
" wanted=" << wanted
213 const double newSpeed =
_patchSpeed(
MAX2(min, 0.0), wanted, max, cfModel);
215 #ifdef DEBUG_PATCH_SPEED
217 const std::string patched = (wanted != newSpeed ?
" patched=" +
toString(newSpeed) :
"");
230 #ifdef DEBUG_PATCH_SPEED
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
436 #ifdef DEBUG_INFORMED
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;
480 #ifdef DEBUG_INFORMER
482 std::cout <<
"\nINFORM_LEADER"
487 const MSVehicle*
const nv = neighLead.first;
495 neighNextGap = neighLead.second +
SPEED2DIST(neighNextSpeed - plannedSpeed);
504 #ifdef DEBUG_INFORMER
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;
522 #ifdef DEBUG_INFORMER
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(
560 #ifdef DEBUG_INFORMER
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
583 #ifdef DEBUG_INFORMER
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
606 #ifdef DEBUG_INFORMER
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(
628 #ifdef DEBUG_INFORMER
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) {
655 #ifdef DEBUG_INFORMER
657 std::cout <<
"\nINFORM_FOLLOWER"
667 #ifdef DEBUG_INFORMER
669 std::cout <<
" blocked by follower nv=" << nv->
getID() <<
" nvSpeed=" << nv->
getSpeed() <<
" needGap="
677 if ((neededGap - neighFollow.second) / remainingSeconds < (
MAX2(plannedSpeed, 0.) - nv->
getSpeed())) {
678 #ifdef DEBUG_INFORMER
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;
738 #ifdef DEBUG_INFORMER
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;
788 #ifdef DEBUG_INFORMER
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);
821 #ifdef DEBUG_INFORMER
823 std::cout <<
"nextGap=" << nextGap
824 <<
" (with vsafe1 and help decel) \nvsafe1=" << vsafe1
825 <<
" vsafe=" << vsafe
834 if (nextGap < nextSecureGap) {
836 vsafe = neighNewSpeed;
839 #ifdef DEBUG_INFORMER
841 std::cout <<
"nextGap=" << nextGap
842 <<
" minNextSecureGap=" << nextSecureGap
843 <<
" vsafe=" << vsafe <<
"\n";
851 #ifdef DEBUG_INFORMER
853 std::cout <<
" wants to cut in before nv=" << nv->
getID()
854 <<
" vsafe1=" << vsafe1 <<
" vsafe=" << vsafe
880 #ifdef DEBUG_INFORMER
882 std::cout <<
" wants to cut in before nv=" << nv->
getID() <<
" (eventually)\n";
889 #ifdef DEBUG_INFORMER
891 std::cout <<
" wants to cut in before nv=" << nv->
getID() <<
" (nv cannot overtake right)\n";
910 #ifdef DEBUG_INFORMER
914 std::cout <<
" wants right follower to slow down a bit\n";
921 #ifdef DEBUG_INFORMER
924 std::cout <<
" wants to cut in before right follower nv=" << nv->
getID() <<
" (eventually)\n";
946 if (gapAfterRemainingSecs >= secureGapAfterRemainingSecs) {
947 #ifdef DEBUG_INFORMER
949 std::cout <<
" wants to cut in before follower nv=" << nv->
getID() <<
" (eventually)\n";
962 #ifdef DEBUG_INFORMER
966 <<
" informs follower " << nv->
getID()
967 <<
" vhelp=" << vhelp
976 const double needDV = overtakeDist / remainingSeconds;
980 #ifdef DEBUG_INFORMER
984 <<
" wants to be overtaken by=" << nv->
getID()
985 <<
" overtakeDist=" << overtakeDist
987 <<
" vhelp=" << vhelp
988 <<
" needDV=" << needDV
999 #ifdef DEBUG_INFORMER
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);
1244 if (bestLaneOffset == 0 && hasBidiLeader) {
1247 }
else if (bestLaneOffset == 0 && hasStoppedLeader) {
1252 + leader.first->getVehicleType().getLengthWithGap()
1254 }
else if (bestLaneOffset == laneOffset && neighLead.first != 0 && (neighLead.first->isStopped() || hasBidiNeighLeader) && neighLead.second < (currentDist - posOnLane)) {
1259 + neighLead.first->getVehicleType().getLengthWithGap()
1260 + neighLead.second);
1261 }
else if (!hasStoppedLeader &&
1263 || hasBidiNeighLeader)) {
1266 changeToBest =
false;
1285 currentDist += roundaboutBonus;
1286 neighDist += roundaboutBonus;
1290 const double maxJam =
MAX2(preb[currIdx + prebOffset].occupation, preb[currIdx].occupation);
1294 double thisLaneVSafe = vMax;
1297 double neighLeftPlace =
MAX2(0.0, neighDist - posOnLane - maxJam);
1298 if (neighLead.first != 0 && neighLead.first->isStopped()) {
1299 neighLeftPlace =
MIN2(neighLeftPlace, neighLead.second);
1302 #ifdef DEBUG_WANTS_CHANGE
1307 <<
" laDist=" << laDist
1308 <<
" currentDist=" << currentDist
1309 <<
" usableDist=" << usableDist
1310 <<
" bestLaneOffset=" << bestLaneOffset
1312 <<
" best.length=" << best.
length
1313 <<
"\n roundaboutBonus=" << roundaboutBonus
1314 <<
" maxJam=" << maxJam
1315 <<
" neighDist=" << neighDist
1316 <<
" neighLeftPlace=" << neighLeftPlace
1317 << (hasBidiLeader ?
" bidiLeader" :
"")
1318 << (hasBidiNeighLeader ?
" bidiNeighLeader" :
"")
1323 bool changeLeftToAvoidOvertakeRight =
false;
1330 if (neighLead.first != 0 && checkOverTakeRight && !right) {
1342 if (vSafeFollow >= vMaxDecel) {
1343 vSafe = vSafeFollow;
1345 vSafe =
MAX2(vMaxDecel, vStayBehind);
1350 thisLaneVSafe =
MIN2(thisLaneVSafe, vSafe);
1353 const double deltaGapFuture = deltaV * 8;
1356 if (vSafeFuture < vSafe) {
1357 const double relativeGain = deltaV /
MAX2(vMax,
1360 changeLeftToAvoidOvertakeRight =
true;
1362 #ifdef DEBUG_WANTS_CHANGE
1365 <<
" avoid overtaking on the right nv=" << nv->
getID()
1366 <<
" deltaV=" << deltaV
1375 const bool currFreeUntilNeighEnd = leader.first ==
nullptr || neighDist - posOnLane <= leader.second;
1376 const double overtakeDist = (leader.first == 0 || hasBidiLeader ? -1 :
1380 &&
MIN2(neighDist, currentDist) - posOnLane > overtakeDist
1382 && (!checkOverTakeRight || !right)
1384 && (neighLead.first == 0 || !neighLead.first->isStopped()
1386 || neighLead.second > overtakeDist)) {
1389 #ifdef DEBUG_WANTS_CHANGE
1391 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" overtake stopped leader=" << leader.first->getID()
1392 <<
" overtakeDist=" << overtakeDist
1393 <<
" remaining=" <<
MIN2(neighDist, currentDist) - posOnLane
1398 }
else if (!changeToBest &&
currentDistDisallows(neighLeftPlace, abs(bestLaneOffset) + 2, laDist) && !hasBidiLeader) {
1405 #ifdef DEBUG_WANTS_CHANGE
1407 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" could not change back and forth in time (1) neighLeftPlace=" << neighLeftPlace <<
"\n";
1411 }
else if (bestLaneOffset == 0 && (neighLeftPlace * 2. < laDist)) {
1416 #ifdef DEBUG_WANTS_CHANGE
1418 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" could not change back and forth in time (2) neighLeftPlace=" << neighLeftPlace <<
"\n";
1422 }
else if (bestLaneOffset == 0
1423 && (leader.first == 0 || !leader.first->isStopped())
1426 && roundaboutBonus == 0
1435 #ifdef DEBUG_WANTS_CHANGE
1437 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" does not want to leave the bestLane (neighDist=" << neighDist <<
")\n";
1444 #ifdef DEBUG_WANTS_CHANGE
1452 if ((ret & lcaCounter) != 0) {
1456 #ifdef DEBUG_WANTS_CHANGE
1471 if (changeToBest && abs(bestLaneOffset) > 1) {
1474 #ifdef DEBUG_WANTS_CHANGE
1476 std::cout <<
" reserving space for unseen blockers myLeadingBlockerLength=" <<
myLeadingBlockerLength <<
"\n";
1485 if (*firstBlocked != neighLead.first) {
1488 #ifdef DEBUG_SAVE_BLOCKER_LENGTH
1490 std::cout <<
SIMTIME <<
" canReserve=" << canReserve <<
" canContinue=" << canContinue <<
"\n";
1499 const int remainingLanes =
MAX2(1, abs(bestLaneOffset));
1501 const double remainingSeconds = ((ret &
LCA_TRACI) == 0 ?
1505 if (!hasBidiNeighLeader) {
1506 const double plannedSpeed =
informLeader(msgPass, blocked, myLca, neighLead, remainingSeconds);
1512 if (!hasBidiNeighFollower) {
1513 informFollower(msgPass, blocked, myLca, neighFollow, remainingSeconds, plannedSpeed);
1516 #ifdef DEBUG_WANTS_CHANGE
1521 <<
" remainingSeconds=" << remainingSeconds
1522 <<
" plannedSpeed=" << plannedSpeed
1527 #ifdef DEBUG_WANTS_CHANGE
1532 <<
" remainingSeconds=" << remainingSeconds
1533 <<
" hasBidiNeighLeader\n";
1546 const bool acceleratingLeader = (neighLead.first != 0 && neighLead.first->getAcceleration() > 0)
1547 || (leader.first != 0 && leader.first->getAcceleration() > 0);
1558 double inconvenience = laneOffset < 0
1562 const double relSpeedDiff = thisLaneVSafe == 0 ? 0 : (thisLaneVSafe - neighLaneVSafe) /
MAX2(thisLaneVSafe, neighLaneVSafe);
1563 inconvenience =
MAX2(relSpeedDiff, inconvenience);
1564 inconvenience =
MIN2(1.0, inconvenience);
1568 #ifdef DEBUG_WANTS_CHANGE
1573 <<
" neighSpeedFactor=" << (thisLaneVSafe / neighLaneVSafe - 1)
1574 <<
" inconvenience=" << inconvenience
1575 <<
" speedInconv=" << speedGainInconvenient
1576 <<
" occInconv=" << neighOccupancyInconvenient
1582 if (roundaboutBonus > 0) {
1584 #ifdef DEBUG_WANTS_CHANGE
1588 <<
" roundaboutBonus=" << roundaboutBonus
1618 #ifdef DEBUG_WANTS_CHANGE
1620 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" does not want to get stranded on the on-ramp of a highway\n";
1640 && (!speedGainInconvenient)
1642 && (changeToBest ||
currentDistAllows(neighDist, abs(bestLaneOffset) + 1, laDist))) {
1645 #ifdef DEBUG_COOPERATE
1649 <<
" wantsChangeToHelp=" << (right ?
"right" :
"left")
1651 << (((
myOwnState & myLca) == 0) ?
" (counter)" :
"")
1682 const double relativeGain = (neighLaneVSafe - thisLaneVSafe) /
MAX2(neighLaneVSafe,
1685 #ifdef DEBUG_WANTS_CHANGE
1689 <<
" currentDist=" << currentDist
1690 <<
" neighDist=" << neighDist
1691 <<
" thisVSafe=" << thisLaneVSafe
1692 <<
" neighVSafe=" << neighLaneVSafe
1693 <<
" relGain=" <<
toString(relativeGain, 8)
1700 if (thisLaneVSafe - 5 / 3.6 > neighLaneVSafe) {
1717 if (mySpeedGainProbability < 0 || relativeGain > 0) {
1723 double acceptanceTime;
1730 if (follower.first !=
nullptr && follower.second < 2 * follower.first->getCarFollowModel().brakeGap(follower.first->getSpeed())) {
1734 const double fRSF = follower.first->getLane()->getVehicleMaxSpeed(follower.first) / follower.first->getLane()->getSpeedLimit();
1735 if (fRSF > roadSpeedFactor) {
1736 acceptanceTime /= fRSF;
1742 double fullSpeedDrivingSeconds =
MIN2(acceptanceTime, fullSpeedGap / vMax);
1743 if (neighLead.first != 0 && neighLead.first->getSpeed() < vMax) {
1744 fullSpeedGap =
MAX2(0.,
MIN2(fullSpeedGap,
1746 vMax, neighLead.first->
getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel())));
1747 fullSpeedDrivingSeconds =
MIN2(fullSpeedDrivingSeconds, fullSpeedGap / (vMax - neighLead.first->getSpeed()));
1750 if (checkOverTakeRight && leader.first != 0
1751 && leader.first->getLane()->getVehicleMaxSpeed(leader.first) < vMax) {
1752 fullSpeedGap =
MIN2(fullSpeedGap, leader.second);
1753 fullSpeedDrivingSeconds =
MIN2(fullSpeedDrivingSeconds, fullSpeedGap / (vMax - leader.first->getSpeed()));
1754 const double relGain = (vMax - leader.first->getLane()->getVehicleMaxSpeed(leader.first)) /
MAX2(vMax,
1771 #ifdef DEBUG_WANTS_CHANGE
1776 <<
" neighDist=" << neighDist
1778 <<
" leaderSpeed=" << (neighLead.first == 0 ? -1 : neighLead.first->getSpeed())
1780 myVehicle.
getSpeed(), neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()))
1781 <<
" acceptanceTime=" << acceptanceTime
1782 <<
" fullSpeedGap=" << fullSpeedGap
1783 <<
" fullSpeedDrivingSeconds=" << fullSpeedDrivingSeconds
1784 <<
" dProb=" << deltaProb
1797 #ifdef DEBUG_WANTS_CHANGE
1803 <<
" thisLaneVSafe=" << thisLaneVSafe
1804 <<
" neighLaneVSafe=" << neighLaneVSafe
1805 <<
" relativeGain=" << relativeGain
1806 <<
" blocked=" << blocked
1820 if (thisLaneVSafe > neighLaneVSafe) {
1825 }
else if (thisLaneVSafe == neighLaneVSafe) {
1843 #ifdef DEBUG_WANTS_CHANGE
1849 <<
" thisLaneVSafe=" << thisLaneVSafe
1850 <<
" neighLaneVSafe=" << neighLaneVSafe
1851 <<
" relativeGain=" << relativeGain
1852 <<
" blocked=" << blocked
1858 && (relativeGain > NUMERICAL_EPS || changeLeftToAvoidOvertakeRight)
1869 && relativeGain >= 0
1870 && (right ? mySpeedGainProbability < 0 : mySpeedGainProbability > 0)) {
1877 #ifdef DEBUG_WANTS_CHANGE
1883 <<
" thisLaneVSafe=" << thisLaneVSafe
1884 <<
" neighLaneVSafe=" << neighLaneVSafe
1895 const MSVehicle* leader = leaderDist.first;
1896 const double gap = leaderDist.second;
1898 if (acceleratingLeader) {
1902 if (leader ==
nullptr) {
1914 if (leader ==
nullptr) {
1925 futureSpeed =
MIN2(vMax, futureSpeed);
1928 const double deltaV = vMax - futureLeaderSpeed;
1929 if (deltaV > 0 && gap > 0) {
1931 const double fullSpeedGap = gap - secGap;
1935 const double gapClosingTime =
MAX2(0.0, fullSpeedGap / deltaV);
1938 futureSpeed =
MIN2(futureSpeed, (gapClosingTime * futureSpeed + (foreCastTime - gapClosingTime) * futureLeaderSpeed) / foreCastTime);
1950 if ((*blocked) !=
nullptr) {
1952 #ifdef DEBUG_SLOW_DOWN
1961 if (gap > POSITION_EPS) {
1975 gap - POSITION_EPS, (*blocked)->getSpeed(),
1976 (*blocked)->getCarFollowModel().getMaxDecel()),
false);
1979 #ifdef DEBUG_SLOW_DOWN
1983 <<
" slowing down for"
2006 #ifdef DEBUG_WANTS_CHANGE
2008 std::cout <<
SIMTIME <<
" adapt to pedestrians on lane=" << lane->
getID() <<
"\n";
2014 if (leader.first != 0) {
2016 v =
MIN2(v, stopSpeed);
2017 #ifdef DEBUG_WANTS_CHANGE
2019 std::cout <<
SIMTIME <<
" pedLeader=" << leader.first->getID() <<
" dist=" << leader.second <<
" v=" << v <<
"\n";
2030 #ifdef DEBUG_WANTS_CHANGE
2032 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" myLeftSpace=" <<
myLeftSpace <<
" latDist=" << latDist <<
" maneuverDist=" << maneuverDist <<
" result=" << result <<
"\n";
2040 result =
MAX2(-speedBound,
MIN2(speedBound, result));
2053 return myOppositeParam <= 0 ? std::numeric_limits<double>::max() : 1 /
myOppositeParam;
2061 #ifdef DEBUG_SAVE_BLOCKER_LENGTH
2063 std::cout <<
SIMTIME <<
" saveBlockerLength veh=" <<
myVehicle.
getID() <<
" canReserve=" << canReserve <<
" myLeftSpace=" <<
myLeftSpace <<
" foeLeftSpace=" << foeLeftSpace <<
"\n";
2115 }
else if (key ==
"speedGainProbabilityRight") {
2117 }
else if (key ==
"speedGainProbabilityLeft") {
2119 }
else if (key ==
"keepRightProbability") {
2121 }
else if (key ==
"lookAheadSpeed") {
2124 }
else if (key ==
"speedGainRP") {
2126 }
else if (key ==
"speedGainLP") {
2128 }
else if (key ==
"keepRightP") {
2180 }
else if (key ==
"speedGainProbabilityRight") {
2182 }
else if (key ==
"speedGainProbabilityLeft") {
2184 }
else if (key ==
"keepRightProbability") {
2186 }
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
const MSCFModel & getCarFollowModel() const
The vehicle's car following model.
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
int & getCanceledState(const int dir)
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 ...
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
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 ...
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 maximumSafeStopSpeed(double gap, double decel, double currentSpeed, bool onInsertion=false, double headway=-1, bool relaxEmergency=true) const
Returns the maximum next velocity for stopping within gap.
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 isBidiFollower(const MSVehicle *ego, const MSVehicle *follower)
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 isBidiLeader(const MSVehicle *leader, const std::vector< MSLane * > &cont)
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
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.
const std::vector< MSLink * > & getLinkCont() const
returns the container with all links !!!
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.
MSEdge & getEdge() const
Returns the lane's edge.
MSLane * getBidiLane() const
retrieve bidirectional lane or nullptr
const MSLane * getNormalPredecessorLane() const
get normal lane leading to this internal lane, for normal lanes, the lane itself is returned
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
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.
double getPositionOnLane() const
Get the vehicle's position along the lane.
const MSLane * getLane() const
Returns the lane the vehicle is on.
const MSCFModel & getCarFollowModel() const
Returns the vehicle's car following model definition.
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.