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
73#define LC_ASSUMED_DECEL 1.0
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 MAGIC_offset = 1.;
243 double nVSafe = wanted;
248#ifdef DEBUG_PATCH_SPEED
257 max =
MIN2(max,
MAX2(safe, vMinEmergency));
262 if (safe >= vMinEmergency) {
264 min =
MAX2(vMinEmergency, safe);
267#ifdef DEBUG_PATCH_SPEED
269 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" slowing down for leading blocker, safe=" << safe << (safe + NUMERICAL_EPS < min ?
" (not enough)" :
"") <<
"\n";
272 nVSafe =
MAX2(min, safe);
290 nVSafe =
MIN2(v * coopWeight + (1 - coopWeight) * wanted, nVSafe);
292#ifdef DEBUG_PATCH_SPEED
299#ifdef DEBUG_PATCH_SPEED
301 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" ignoring low nVSafe=" << v <<
" min=" << min <<
"\n";
305#ifdef DEBUG_PATCH_SPEED
307 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" ignoring high nVSafe=" << v <<
" max=" << max <<
"\n";
315#ifdef DEBUG_PATCH_SPEED
328#ifdef DEBUG_PATCH_SPEED
333 return (max + wanted) / 2.0;
337#ifdef DEBUG_PATCH_SPEED
343 return (
MAX2(0., min) + wanted) / 2.0;
349#ifdef DEBUG_PATCH_SPEED
354 return (max + wanted) / 2.0;
395#ifdef DEBUG_PATCH_SPEED
400 return (max + wanted) / 2.0;
404#ifdef DEBUG_PATCH_SPEED
436 <<
" informedBy=" << sender->
getID()
437 <<
" info=" << pinfo->second
438 <<
" vSafe=" << pinfo->first
450 double overtakeDist = (gap
455 return MAX2(overtakeDist, 0.);
463 const std::pair<MSVehicle*, double>& neighLead,
464 double remainingSeconds) {
467 plannedSpeed =
MIN2(plannedSpeed,
478 std::cout <<
"\nINFORM_LEADER"
483 const MSVehicle*
const nv = neighLead.first;
491 neighNextGap = neighLead.second +
SPEED2DIST(neighNextSpeed - plannedSpeed);
502 std::cout <<
" blocked by leader nv=" << nv->
getID() <<
" nvSpeed=" << nv->
getSpeed() <<
" needGap="
509 const double dv = plannedSpeed - nv->
getSpeed();
512 overtakeTime = overtakeDist / dv;
515 overtakeTime = remainingSeconds + 1;
521 <<
"\nnv = " << nv->
getID()
522 <<
"\nplannedSpeed = " << plannedSpeed
523 <<
"\nleaderSpeed = " << nv->
getSpeed()
525 <<
"\nremainingSeconds = " << remainingSeconds
526 <<
"\novertakeDist = " << overtakeDist
527 <<
"\novertakeTime = " << overtakeTime
542 && (!neighLead.first->isStopped() || (
isOpposite() && neighLead.second >= 0))) {
547 const double targetSpeed =
MAX2(
559 <<
" cannot overtake leader nv=" << nv->
getID()
563 <<
" overtakeDist=" << overtakeDist
564 <<
" overtakeTime=" << overtakeTime
565 <<
" remainingSeconds=" << remainingSeconds
566 <<
" currentGap=" << neighLead.second
568 <<
" neighNextSpeed=" << neighNextSpeed
569 <<
" neighNextGap=" << neighNextGap
570 <<
" targetSpeed=" << targetSpeed
571 <<
" nextSpeed=" << nextSpeed
582 <<
" cannot overtake fast leader nv=" << nv->
getID()
586 <<
" overtakeDist=" << overtakeDist
588 <<
" overtakeTime=" << overtakeTime
589 <<
" remainingSeconds=" << remainingSeconds
590 <<
" currentGap=" << neighLead.second
591 <<
" neighNextSpeed=" << neighNextSpeed
592 <<
" neighNextGap=" << neighNextGap
593 <<
" targetSpeed=" << targetSpeed
605 <<
" wants to overtake leader nv=" << nv->
getID()
607 <<
" overtakeDist=" << overtakeDist
608 <<
" remainingSeconds=" << remainingSeconds
609 <<
" overtakeTime=" << overtakeTime
610 <<
" currentGap=" << neighLead.second
620 const double targetSpeed =
MAX2(
626 std::cout <<
" not blocked by leader nv=" << nv->
getID()
628 <<
" gap=" << neighLead.second
629 <<
" neighNextSpeed=" << neighNextSpeed
630 <<
" neighNextGap=" << neighNextGap
632 <<
" targetSpeed=" << targetSpeed
636 return MIN2(targetSpeed, plannedSpeed);
644 const std::pair<MSVehicle*, double>& neighFollow,
645 double remainingSeconds,
646 double plannedSpeed) {
653 std::cout <<
"\nINFORM_FOLLOWER"
665 std::cout <<
" blocked by follower nv=" << nv->
getID() <<
" nvSpeed=" << nv->
getSpeed() <<
" needGap="
673 if ((neededGap - neighFollow.second) / remainingSeconds < (
MAX2(plannedSpeed, 0.) - nv->
getSpeed())) {
676 std::cout <<
" wants to cut in before nv=" << nv->
getID() <<
" without any help." <<
"\nneededGap = " << neededGap <<
"\n";
696 double neighNewSpeed;
698 double neighNewSpeed1s;
708 dv = plannedSpeed - neighNewSpeed1s;
715 decelGap = neighFollow.second + dv;
722 neighNewSpeed1s = nv->
getSpeed() - helpDecel;
738 <<
" plannedSpeed=" << plannedSpeed
739 <<
" threshold=" << onRampThreshold
740 <<
" neighNewSpeed=" << neighNewSpeed
741 <<
" neighNewSpeed1s=" << neighNewSpeed1s
743 <<
" gap=" << neighFollow.second
744 <<
" decelGap=" << decelGap
745 <<
" secureGap=" << secureGap
752 && neighNewSpeed1s < onRampThreshold) {
756 if (decelGap > 0 && decelGap >= secureGap) {
764 double vsafe, vsafe1;
786 std::cout <<
"nextGap=" << nextGap <<
" (without help decel) \n";
794 MAX2(0., plannedSpeed),
802 nv->
getSpeed(), plannedAccel, -decel2,
812 MAX2(0., plannedSpeed),
815 assert(vsafe >= vsafe1 - NUMERICAL_EPS);
819 std::cout <<
"nextGap=" << nextGap
820 <<
" (with vsafe1 and help decel) \nvsafe1=" << vsafe1
821 <<
" vsafe=" << vsafe
830 if (nextGap < nextSecureGap) {
832 vsafe = neighNewSpeed;
837 std::cout <<
"nextGap=" << nextGap
838 <<
" minNextSecureGap=" << nextSecureGap
839 <<
" vsafe=" << vsafe <<
"\n";
849 std::cout <<
" wants to cut in before nv=" << nv->
getID()
850 <<
" vsafe1=" << vsafe1 <<
" vsafe=" << vsafe
878 std::cout <<
" wants to cut in before nv=" << nv->
getID() <<
" (eventually)\n";
887 std::cout <<
" wants to cut in before nv=" << nv->
getID() <<
" (nv cannot overtake right)\n";
910 std::cout <<
" wants right follower to slow down a bit\n";
920 std::cout <<
" wants to cut in before right follower nv=" << nv->
getID() <<
" (eventually)\n";
942 if (gapAfterRemainingSecs >= secureGapAfterRemainingSecs) {
945 std::cout <<
" wants to cut in before follower nv=" << nv->
getID() <<
" (eventually)\n";
962 <<
" informs follower " << nv->
getID()
963 <<
" vhelp=" << vhelp
972 const double needDV = overtakeDist / remainingSeconds;
980 <<
" wants to be overtaken by=" << nv->
getID()
981 <<
" overtakeDist=" << overtakeDist
983 <<
" vhelp=" << vhelp
984 <<
" needDV=" << needDV
993 double vsafe, vsafe1;
996 MSVehicle*
const nfv = neighFollow.first;
1006 double anticipationTime = 1.;
1017 if (anticipatedGap > secureGap) {
1024 if (anticipatedGap < secureGap) {
1035#ifdef DEBUG_INFORMER
1037 std::cout <<
" wants to cut in before non-blocking follower nv=" << nv->
getID() <<
"\n";
1066 scaledDelta =
MIN2(overlap, maxDist);
1075 deltaPosLat =
MAX2(
MIN2(deltaPosLat, maxDist), -maxDist);
1085 if (bestLaneOffset < -1) {
1087 }
else if (bestLaneOffset > 1) {
1128 const std::pair<MSVehicle*, double>& leader,
1129 const std::pair<MSVehicle*, double>& follower,
1130 const std::pair<MSVehicle*, double>& neighLead,
1131 const std::pair<MSVehicle*, double>& neighFollow,
1133 const std::vector<MSVehicle::LaneQ>& preb,
1136 assert(laneOffset == 1 || laneOffset == -1);
1140 int bestLaneOffset = 0;
1145 double currentDist = 0;
1146 double neighDist = 0;
1159 const int prebOffset = laneOffset;
1160 for (
int p = 0; p < (int) preb.size(); ++p) {
1164 if (preb[p].lane == prebLane && p + laneOffset >= 0) {
1165 assert(p + prebOffset < (
int)preb.size());
1167 neigh = preb[p + prebOffset];
1168 currentDist = curr.
length;
1169 neighDist = neigh.
length;
1171 if (bestLaneOffset == 0 && preb[p + prebOffset].bestLaneOffset == 0 && !checkOpposite) {
1172#ifdef DEBUG_WANTS_CHANGE
1176 <<
" bestLaneOffsetOld=" << bestLaneOffset
1177 <<
" bestLaneOffsetNew=" << laneOffset
1181 bestLaneOffset = prebOffset;
1183 best = preb[p + bestLaneOffset];
1188 assert(curr.
lane !=
nullptr);
1189 assert(neigh.
lane !=
nullptr);
1190 assert(best.
lane !=
nullptr);
1192 const bool right = (laneOffset == -1);
1194 double driveToNextStop = -std::numeric_limits<double>::max();
1202#ifdef DEBUG_WANTS_CHANGE
1207 <<
" stopPos=" << stopPos
1208 <<
" currentDist=" << currentDist
1209 <<
" neighDist=" << neighDist
1213 currentDist =
MAX2(currentDist, stopPos);
1214 neighDist =
MAX2(neighDist, stopPos);
1219 const bool changeToBest = (right && bestLaneOffset < 0) || (!right && bestLaneOffset > 0);
1225 if (lastBlocked != firstBlocked) {
1229#ifdef DEBUG_WANTS_CHANGE
1238 <<
" leaderGap=" << leader.second
1240 <<
" followerGap=" << follower.second
1242 <<
" neighLeadGap=" << neighLead.second
1244 <<
" neighFollowGap=" << neighFollow.second
1267 assert(memoryFactor > 0.);
1275 if (bestLaneOffset == 0 && leader.first != 0 && leader.first->isStopped() && leader.second < (currentDist - posOnLane)) {
1280 + leader.first->getVehicleType().getLengthWithGap()
1282 }
else if (bestLaneOffset == laneOffset && neighLead.first != 0 && neighLead.first->isStopped() && neighLead.second < (currentDist - posOnLane)) {
1285 + neighLead.first->getVehicleType().getLengthWithGap()
1286 + neighLead.second);
1304 currentDist += roundaboutBonus;
1305 neighDist += roundaboutBonus;
1309 const double maxJam =
MAX2(preb[currIdx + prebOffset].occupation, preb[currIdx].occupation);
1310 const double neighLeftPlace =
MAX2(0.0, neighDist - posOnLane - maxJam);
1314 double thisLaneVSafe = vMax;
1317#ifdef DEBUG_WANTS_CHANGE
1322 <<
" laDist=" << laDist
1323 <<
" currentDist=" << currentDist
1324 <<
" usableDist=" << usableDist
1325 <<
" bestLaneOffset=" << bestLaneOffset
1327 <<
" best.length=" << best.
length
1328 <<
"\n roundaboutBonus=" << roundaboutBonus
1329 <<
" maxJam=" << maxJam
1330 <<
" neighDist=" << neighDist
1331 <<
" neighLeftPlace=" << neighLeftPlace
1336 bool changeLeftToAvoidOvertakeRight =
false;
1343 if (neighLead.first != 0 && checkOverTakeRight && !right) {
1355 if (vSafeFollow >= vMaxDecel) {
1356 vSafe = vSafeFollow;
1358 vSafe =
MAX2(vMaxDecel, vStayBehind);
1363 thisLaneVSafe =
MIN2(thisLaneVSafe, vSafe);
1366 const double deltaGapFuture = deltaV * 8;
1369 if (vSafeFuture < vSafe) {
1370 const double relativeGain = deltaV /
MAX2(vMax,
1373 changeLeftToAvoidOvertakeRight =
true;
1375#ifdef DEBUG_WANTS_CHANGE
1378 <<
" avoid overtaking on the right nv=" << nv->
getID()
1379 <<
" deltaV=" << deltaV
1388 const bool currFreeUntilNeighEnd = leader.first ==
nullptr || neighDist - posOnLane <= leader.second;
1389 const double overtakeDist = (leader.first == 0 ? -1 :
1393 &&
MIN2(neighDist, currentDist) - posOnLane > overtakeDist
1395 && (!checkOverTakeRight || !right)
1397 && (neighLead.first == 0 || !neighLead.first->isStopped()
1399 || neighLead.second > overtakeDist)) {
1402#ifdef DEBUG_WANTS_CHANGE
1404 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" overtake stopped leader=" << leader.first->getID()
1405 <<
" overtakeDist=" << overtakeDist
1406 <<
" remaining=" <<
MIN2(neighDist, currentDist) - posOnLane
1411 }
else if (!changeToBest && (
currentDistDisallows(neighLeftPlace, abs(bestLaneOffset) + 2, laDist))) {
1418#ifdef DEBUG_WANTS_CHANGE
1420 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" could not change back and forth in time (1) neighLeftPlace=" << neighLeftPlace <<
"\n";
1424 }
else if (bestLaneOffset == 0 && (neighLeftPlace * 2. < laDist)) {
1429#ifdef DEBUG_WANTS_CHANGE
1431 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" could not change back and forth in time (2) neighLeftPlace=" << neighLeftPlace <<
"\n";
1435 }
else if (bestLaneOffset == 0
1436 && (leader.first == 0 || !leader.first->isStopped())
1438 && roundaboutBonus == 0
1447#ifdef DEBUG_WANTS_CHANGE
1449 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" does not want to leave the bestLane (neighDist=" << neighDist <<
")\n";
1456#ifdef DEBUG_WANTS_CHANGE
1464 if ((ret & lcaCounter) != 0) {
1468#ifdef DEBUG_WANTS_CHANGE
1483 if (changeToBest && abs(bestLaneOffset) > 1) {
1486#ifdef DEBUG_WANTS_CHANGE
1488 std::cout <<
" reserving space for unseen blockers myLeadingBlockerLength=" <<
myLeadingBlockerLength <<
"\n";
1497 if (*firstBlocked != neighLead.first) {
1500#ifdef DEBUG_SAVE_BLOCKER_LENGTH
1502 std::cout <<
SIMTIME <<
" canReserve=" << canReserve <<
" canContinue=" << canContinue <<
"\n";
1511 const int remainingLanes =
MAX2(1, abs(bestLaneOffset));
1513 const double remainingSeconds = ((ret &
LCA_TRACI) == 0 ?
1517 const double plannedSpeed =
informLeader(msgPass, blocked, myLca, neighLead, remainingSeconds);
1522 informFollower(msgPass, blocked, myLca, neighFollow, remainingSeconds, plannedSpeed);
1525#ifdef DEBUG_WANTS_CHANGE
1530 <<
" remainingSeconds=" << remainingSeconds
1531 <<
" plannedSpeed=" << plannedSpeed
1543 const bool acceleratingLeader = (neighLead.first != 0 && neighLead.first->getAcceleration() > 0)
1544 || (leader.first != 0 && leader.first->getAcceleration() > 0);
1555 double inconvenience = laneOffset < 0
1559 const double relSpeedDiff = thisLaneVSafe == 0 ? 0 : (thisLaneVSafe - neighLaneVSafe) /
MAX2(thisLaneVSafe, neighLaneVSafe);
1560 inconvenience =
MAX2(relSpeedDiff, inconvenience);
1561 inconvenience =
MIN2(1.0, inconvenience);
1565#ifdef DEBUG_WANTS_CHANGE
1570 <<
" neighSpeedFactor=" << (thisLaneVSafe / neighLaneVSafe - 1)
1571 <<
" inconvenience=" << inconvenience
1572 <<
" speedInconv=" << speedGainInconvenient
1573 <<
" occInconv=" << neighOccupancyInconvenient
1579 if (roundaboutBonus > 0) {
1581#ifdef DEBUG_WANTS_CHANGE
1585 <<
" roundaboutBonus=" << roundaboutBonus
1615#ifdef DEBUG_WANTS_CHANGE
1617 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" does not want to get stranded on the on-ramp of a highway\n";
1637 && (!speedGainInconvenient)
1639 && (changeToBest ||
currentDistAllows(neighDist, abs(bestLaneOffset) + 1, laDist))) {
1642#ifdef DEBUG_COOPERATE
1646 <<
" wantsChangeToHelp=" << (right ?
"right" :
"left")
1648 << (((
myOwnState & myLca) == 0) ?
" (counter)" :
"")
1679 const double relativeGain = (neighLaneVSafe - thisLaneVSafe) /
MAX2(neighLaneVSafe,
1682#ifdef DEBUG_WANTS_CHANGE
1686 <<
" currentDist=" << currentDist
1687 <<
" neighDist=" << neighDist
1688 <<
" thisVSafe=" << thisLaneVSafe
1689 <<
" neighVSafe=" << neighLaneVSafe
1690 <<
" relGain=" <<
toString(relativeGain, 8)
1697 if (thisLaneVSafe - 5 / 3.6 > neighLaneVSafe) {
1714 if (mySpeedGainProbability < 0 || relativeGain > 0) {
1720 double acceptanceTime;
1727 if (follower.first !=
nullptr && follower.second < 2 * follower.first->getCarFollowModel().brakeGap(follower.first->getSpeed())) {
1731 const double fRSF = follower.first->getLane()->getVehicleMaxSpeed(follower.first) / follower.first->getLane()->getSpeedLimit();
1732 if (fRSF > roadSpeedFactor) {
1733 acceptanceTime /= fRSF;
1739 double fullSpeedDrivingSeconds =
MIN2(acceptanceTime, fullSpeedGap / vMax);
1740 if (neighLead.first != 0 && neighLead.first->getSpeed() < vMax) {
1741 fullSpeedGap =
MAX2(0.,
MIN2(fullSpeedGap,
1743 vMax, neighLead.first->
getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel())));
1744 fullSpeedDrivingSeconds =
MIN2(fullSpeedDrivingSeconds, fullSpeedGap / (vMax - neighLead.first->getSpeed()));
1747 if (checkOverTakeRight && leader.first != 0
1748 && leader.first->getLane()->getVehicleMaxSpeed(leader.first) < vMax) {
1749 fullSpeedGap =
MIN2(fullSpeedGap, leader.second);
1750 fullSpeedDrivingSeconds =
MIN2(fullSpeedDrivingSeconds, fullSpeedGap / (vMax - leader.first->getSpeed()));
1751 const double relGain = (vMax - leader.first->getLane()->getVehicleMaxSpeed(leader.first)) /
MAX2(vMax,
1768#ifdef DEBUG_WANTS_CHANGE
1773 <<
" neighDist=" << neighDist
1775 <<
" leaderSpeed=" << (neighLead.first == 0 ? -1 : neighLead.first->getSpeed())
1777 myVehicle.
getSpeed(), neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()))
1778 <<
" acceptanceTime=" << acceptanceTime
1779 <<
" fullSpeedGap=" << fullSpeedGap
1780 <<
" fullSpeedDrivingSeconds=" << fullSpeedDrivingSeconds
1781 <<
" dProb=" << deltaProb
1794#ifdef DEBUG_WANTS_CHANGE
1800 <<
" thisLaneVSafe=" << thisLaneVSafe
1801 <<
" neighLaneVSafe=" << neighLaneVSafe
1802 <<
" relativeGain=" << relativeGain
1803 <<
" blocked=" << blocked
1817 if (thisLaneVSafe > neighLaneVSafe) {
1822 }
else if (thisLaneVSafe == neighLaneVSafe) {
1840#ifdef DEBUG_WANTS_CHANGE
1846 <<
" thisLaneVSafe=" << thisLaneVSafe
1847 <<
" neighLaneVSafe=" << neighLaneVSafe
1848 <<
" relativeGain=" << relativeGain
1849 <<
" blocked=" << blocked
1855 && (relativeGain > NUMERICAL_EPS || changeLeftToAvoidOvertakeRight)
1866 && relativeGain >= 0
1867 && (right ? mySpeedGainProbability < 0 : mySpeedGainProbability > 0)) {
1874#ifdef DEBUG_WANTS_CHANGE
1880 <<
" thisLaneVSafe=" << thisLaneVSafe
1881 <<
" neighLaneVSafe=" << neighLaneVSafe
1892 const MSVehicle* leader = leaderDist.first;
1893 const double gap = leaderDist.second;
1895 if (acceleratingLeader) {
1899 if (leader ==
nullptr) {
1911 if (leader ==
nullptr) {
1922 futureSpeed =
MIN2(vMax, futureSpeed);
1925 const double deltaV = vMax - futureLeaderSpeed;
1926 if (deltaV > 0 && gap > 0) {
1928 const double fullSpeedGap = gap - secGap;
1932 const double gapClosingTime =
MAX2(0.0, fullSpeedGap / deltaV);
1935 futureSpeed =
MIN2(futureSpeed, (gapClosingTime * futureSpeed + (foreCastTime - gapClosingTime) * futureLeaderSpeed) / foreCastTime);
1947 if ((*blocked) !=
nullptr) {
1949#ifdef DEBUG_SLOW_DOWN
1958 if (gap > POSITION_EPS) {
1972 gap - POSITION_EPS, (*blocked)->getSpeed(),
1973 (*blocked)->getCarFollowModel().getMaxDecel()));
1976#ifdef DEBUG_SLOW_DOWN
1980 <<
" slowing down for"
2003#ifdef DEBUG_WANTS_CHANGE
2005 std::cout <<
SIMTIME <<
" adapt to pedestrians on lane=" << lane->
getID() <<
"\n";
2011 if (leader.first != 0) {
2013 v =
MIN2(v, stopSpeed);
2014#ifdef DEBUG_WANTS_CHANGE
2016 std::cout <<
SIMTIME <<
" pedLeader=" << leader.first->getID() <<
" dist=" << leader.second <<
" v=" << v <<
"\n";
2033#ifdef DEBUG_WANTS_CHANGE
2035 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" myLeftSpace=" <<
myLeftSpace <<
" latDist=" << latDist <<
" maneuverDist=" << maneuverDist <<
" result=" << result <<
"\n";
2043 result =
MAX2(-speedBound,
MIN2(speedBound, result));
2060 return myOppositeParam <= 0 ? std::numeric_limits<double>::max() : 1 /
myOppositeParam;
2068#ifdef DEBUG_SAVE_BLOCKER_LENGTH
2070 std::cout <<
SIMTIME <<
" saveBlockerLength veh=" <<
myVehicle.
getID() <<
" canReserve=" << canReserve <<
" myLeftSpace=" <<
myLeftSpace <<
" foeLeftSpace=" << foeLeftSpace <<
"\n";
2122 }
else if (key ==
"speedGainProbabilityRight") {
2124 }
else if (key ==
"speedGainProbabilityLeft") {
2126 }
else if (key ==
"keepRightProbability") {
2128 }
else if (key ==
"lookAheadSpeed") {
2131 }
else if (key ==
"speedGainRP") {
2133 }
else if (key ==
"speedGainLP") {
2135 }
else if (key ==
"keepRightP") {
2187 }
else if (key ==
"speedGainProbabilityRight") {
2189 }
else if (key ==
"speedGainProbabilityLeft") {
2191 }
else if (key ==
"keepRightProbability") {
2193 }
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...
int myOwnState
The current state of the vehicle.
virtual void prepareStep()
double myMaxDistLatStanding
static bool myAllowOvertakingRight
whether overtaking on the right is permitted
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
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.
virtual double getSecureGap(const MSVehicle *const, 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)
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.
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 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 getAssumedDecelForLaneChangeDuration() const override
Returns a deceleration value which is used for the estimation of the duration of a lane change.
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
std::vector< double > myLCAccelerationAdvices
vector of LC-related acceleration recommendations Filled in wantsChange() and applied in patchSpeed()
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
void addLCSpeedAdvice(const double vSafe)
Takes a vSafe (speed advice for speed in the next simulation step), converts it into an acceleration ...
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.
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.