40#define MAGIC_OFFSET 1.
41#define LOOK_FORWARD 10.
45#define LCA_RIGHT_IMPATIENCE -1.
46#define CUT_IN_LEFT_SPEED_THRESHOLD 27.
47#define MAX_ONRAMP_LENGTH 200.
49#define LOOK_AHEAD_MIN_SPEED 0.0
50#define LOOK_AHEAD_SPEED_MEMORY 0.9
52#define HELP_DECEL_FACTOR 1.0
54#define HELP_OVERTAKE (10.0 / 3.6)
55#define MIN_FALLBEHIND (7.0 / 3.6)
59#define KEEP_RIGHT_TIME 5.0
61#define RELGAIN_NORMALIZATION_MIN_SPEED 10.0
63#define TURN_LANE_DIST 200.0
64#define GAIN_PERCEPTION_THRESHOLD 0.05
66#define SPEED_GAIN_MIN_SECONDS 20.0
68#define ARRIVALPOS_LAT_THRESHOLD 100.0
71#define LATGAP_SPEED_THRESHOLD (50 / 3.6)
74#define LATGAP_SPEED_THRESHOLD2 (50 / 3.6)
77#define SPEEDGAIN_DECAY_FACTOR 0.5
79#define SPEEDGAIN_MEMORY_FACTOR 0.5
81#define REACT_TO_STOPPED_DISTANCE 100
106#define DEBUG_COND (myVehicle.isSelected())
118 mySpeedGainProbabilityRight(0),
119 mySpeedGainProbabilityLeft(0),
120 myKeepRightProbability(0),
121 myLeadingBlockerLength(0),
125 myCanChangeFully(true),
126 mySafeLatDistRight(0),
127 mySafeLatDistLeft(0),
135 myMinGapLat(v.getVehicleType().getMinGapLat()),
138 MAX2(NUMERICAL_EPS, myMinGapLat)) /
139 MAX2(NUMERICAL_EPS, myMinGapLat)))),
142 myMinImpatience(myImpatience),
193 const std::vector<MSVehicle::LaneQ>& preb,
196 double& latDist,
double& maneuverDist,
int& blocked) {
199 const std::string changeType = laneOffset == -1 ?
"right" : (laneOffset == 1 ?
"left" :
"current");
207 <<
" neigh=" << neighLane.
getID()
212 <<
" considerChangeTo=" << changeType
219 leaders, followers, blockers,
220 neighLeaders, neighFollowers, neighBlockers,
222 lastBlocked, firstBlocked, latDist, maneuverDist, blocked);
224 result =
keepLatGap(result, leaders, followers, blockers,
225 neighLeaders, neighFollowers, neighBlockers,
226 neighLane, laneOffset, latDist, maneuverDist, blocked);
228 result |=
getLCA(result, latDist);
230#if defined(DEBUG_MANEUVER) || defined(DEBUG_STATE)
231 double latDistTmp = latDist;
234#if defined(DEBUG_MANEUVER) || defined(DEBUG_STATE)
236 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" maneuverDist=" << maneuverDist <<
" latDist=" << latDistTmp <<
" mySpeedPrev=" <<
mySpeedLat <<
" speedLat=" <<
DIST2SPEED(latDist) <<
" latDist2=" << latDist <<
"\n";
243 <<
" wantsChangeTo=" << changeType
244 <<
" latDist=" << latDist
245 <<
" maneuverDist=" << maneuverDist
253 <<
" wantsNoChangeTo=" << changeType
310 const double newSpeed =
_patchSpeed(
MAX2(min, 0.0), wanted, max, cfModel);
311#ifdef DEBUG_PATCHSPEED
313 const std::string patched = (wanted != newSpeed ?
" patched=" +
toString(newSpeed) :
"");
320 <<
" wanted=" << wanted
339 double nVSafe = wanted;
345#ifdef DEBUG_PATCHSPEED
353 max =
MIN2(max, safe);
358 if (safe >= vMinEmergency) {
360 min =
MAX2(vMinEmergency, safe);
363#ifdef DEBUG_PATCHSPEED
365 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" slowing down for leading blocker, safe=" << safe << (safe + NUMERICAL_EPS < min ?
" (not enough)" :
"") <<
"\n";
368 nVSafe =
MAX2(min, safe);
375 double accel = i.first;
377 if (v >= min && v <= max) {
380 nVSafe =
MIN2(v, nVSafe);
382 nVSafe =
MIN2(v * coopWeight + (1 - coopWeight) * wanted, nVSafe);
385#ifdef DEBUG_PATCHSPEED
387 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" got accel=" << accel <<
" nVSafe=" << nVSafe <<
"\n";
391#ifdef DEBUG_PATCHSPEED
394 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" ignoring low nVSafe=" << v <<
" (accel=" << accel <<
") min=" << min <<
"\n";
398 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" ignoring high nVSafe=" << v <<
" (accel=" << accel <<
") max=" << max <<
"\n";
406#ifdef DEBUG_PATCHSPEED
419#if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
424 return (max + wanted) / 2.0;
428#if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
433 return (min + wanted) / 2.0;
436#if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
441 return (max + wanted) / 2.0;
482#if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
487 return (max + wanted) / 2.0;
491#if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
511 if (pinfo->first >= 0) {
520 <<
" informedBy=" << sender->
getID()
521 <<
" info=" << pinfo->second
522 <<
" vSafe=" << pinfo->first
535 assert(cld.first != 0);
544 double remainingSeconds) {
550 plannedSpeed =
MIN2(plannedSpeed, v);
555 std::cout <<
" informLeader speed=" <<
myVehicle.
getSpeed() <<
" planned=" << plannedSpeed <<
"\n";
566 if (
gDebugFlag2) std::cout <<
" blocked by leader nv=" << nv->
getID() <<
" nvSpeed=" << nv->
getSpeed() <<
" needGap="
570 const double dv = plannedSpeed - nv->
getSpeed();
571 const double overtakeDist = (neighLead.second
583 || dv * remainingSeconds < overtakeDist)
584 && (!neighLead.first->isStopped() || (
isOpposite() && neighLead.second >= 0))) {
599 <<
" cannot overtake leader nv=" << nv->
getID()
601 <<
" remainingSeconds=" << remainingSeconds
602 <<
" targetSpeed=" << targetSpeed
603 <<
" nextSpeed=" << nextSpeed
614 <<
" cannot overtake fast leader nv=" << nv->
getID()
616 <<
" remainingSeconds=" << remainingSeconds
617 <<
" targetSpeed=" << targetSpeed
628 <<
" wants to overtake leader nv=" << nv->
getID()
630 <<
" remainingSeconds=" << remainingSeconds
631 <<
" currentGap=" << neighLead.second
633 <<
" overtakeDist=" << overtakeDist
643 }
else if (neighLead.first != 0) {
646 double dv, nextNVSpeed;
666 std::cout <<
" not blocked by leader nv=" << nv->
getID()
668 <<
" gap=" << neighLead.second
669 <<
" nextGap=" << neighLead.second - dv
671 <<
" targetSpeed=" << targetSpeed
675 return MIN2(targetSpeed, plannedSpeed);
687 double remainingSeconds,
688 double plannedSpeed) {
696 if (
gDebugFlag2) std::cout <<
" blocked by follower nv=" << nv->
getID() <<
" nvSpeed=" << nv->
getSpeed() <<
" needGap="
703 if ((neededGap - neighFollow.second) / remainingSeconds < (plannedSpeed - nv->
getSpeed())) {
706 std::cout <<
" wants to cut in before nv=" << nv->
getID() <<
" without any help neededGap=" << neededGap <<
"\n";
725 const double neighNewSpeed1s =
MAX2(0., nv->
getSpeed() - helpDecel);
726 const double dv = plannedSpeed - neighNewSpeed1s;
728 const double decelGap = neighFollow.second + dv;
734 <<
" egoNV=" << plannedSpeed
735 <<
" nvNewSpeed=" << neighNewSpeed
736 <<
" nvNewSpeed1s=" << neighNewSpeed1s
737 <<
" deltaGap=" << dv
738 <<
" decelGap=" << decelGap
739 <<
" secGap=" << secureGap
743 if (decelGap > 0 && decelGap >= secureGap) {
758 std::cout <<
" wants to cut in before nv=" << nv->
getID()
759 <<
" vsafe1=" << vsafe1
760 <<
" vsafe=" << vsafe
765 }
else if (dv > 0 && dv * remainingSeconds > (secureGap - decelGap + POSITION_EPS)) {
770 std::cout <<
" wants to cut in before nv=" << nv->
getID() <<
" (eventually)\n";
778 std::cout <<
" wants to cut in before nv=" << nv->
getID() <<
" (nv cannot overtake right)\n";
794 std::cout <<
" wants right follower to slow down a bit\n";
800 std::cout <<
" wants to cut in before right follower nv=" << nv->
getID() <<
" (eventually)\n";
809 const double overtakeDist = (neighFollow.second
815 const double needDV = overtakeDist / remainingSeconds;
823 <<
" wants to be overtaken by=" << nv->
getID()
824 <<
" overtakeDist=" << overtakeDist
826 <<
" vhelp=" << vhelp
827 <<
" needDV=" << needDV
833 }
else if (neighFollow.first != 0) {
838 std::cout <<
" wants to cut in before non-blocking follower nv=" << neighFollow.first->getID() <<
"\n";
846 const std::vector<CLeaderDist>& blockers,
847 double remainingSeconds) {
859 plannedSpeed =
MIN2(plannedSpeed, safe);
861 for (std::vector<CLeaderDist>::const_iterator it = blockers.begin(); it != blockers.end(); ++it) {
862 plannedSpeed =
MIN2(plannedSpeed,
informLeader(blocked, dir, *it, remainingSeconds));
870 const std::vector<CLeaderDist>& blockers,
871 double remainingSeconds,
872 double plannedSpeed) {
874 for (std::vector<CLeaderDist>::const_iterator it = blockers.begin(); it != blockers.end(); ++it) {
875 informFollower(blocked, dir, *it, remainingSeconds, plannedSpeed);
900 const double halfWidth =
getWidth() * 0.5;
910 std::vector<double> newExpectedSpeeds;
919 const std::vector<MSLane*>& lanes = currEdge->
getLanes();
920 for (std::vector<MSLane*>::const_iterator it_lane = lanes.begin(); it_lane != lanes.end(); ++it_lane) {
922 for (
int i = 0; i < subLanes; ++i) {
923 newExpectedSpeeds.push_back((*it_lane)->getVehicleMaxSpeed(&
myVehicle));
929 for (
int i = 0; i < subLanes; ++i) {
930 newExpectedSpeeds.push_back(lanes.back()->getVehicleMaxSpeed(&
myVehicle));
938 if (subLaneShift < std::numeric_limits<int>::max()) {
940 const int newI = i + subLaneShift;
941 if (newI > 0 && newI < (
int)newExpectedSpeeds.size()) {
959 if (bestLaneOffset < -1) {
961 }
else if (bestLaneOffset > 1) {
988 for (
const MSLink*
const link : lane->getLinkCont()) {
989 if (&link->getLane()->getEdge() == curEdge) {
991 const MSLane* target = link->getLane();
992 const std::vector<MSLane*>& lanes2 = curEdge->
getLanes();
993 for (std::vector<MSLane*>::const_iterator it_lane2 = lanes2.begin(); it_lane2 != lanes2.end(); ++it_lane2) {
994 const MSLane* lane2 = *it_lane2;
995 if (lane2 == target) {
996 return prevShift + curShift;
1007 return std::numeric_limits<int>::max();
1041#if defined(DEBUG_MANEUVER) || defined(DEBUG_STATE)
1074 const std::vector<MSVehicle::LaneQ>& preb,
1077 double& latDist,
double& maneuverDist,
int& blocked) {
1082 int bestLaneOffset = 0;
1083 double currentDist = 0;
1084 double neighDist = 0;
1096 const int prebOffset = (checkOpposite ? 0 : laneOffset);
1097 for (
int p = 0; p < (int) preb.size(); ++p) {
1098 if (preb[p].lane == prebLane && p + laneOffset >= 0) {
1099 assert(p + prebOffset < (
int)preb.size());
1101 neigh = preb[p + prebOffset];
1102 currentDist = curr.
length;
1103 neighDist = neigh.
length;
1106 if (bestLaneOffset == 0 && preb[p + prebOffset].bestLaneOffset == 0 && !checkOpposite) {
1107#ifdef DEBUG_WANTSCHANGE
1111 <<
" bestLaneOffsetOld=" << bestLaneOffset
1112 <<
" bestLaneOffsetNew=" << laneOffset
1116 bestLaneOffset = prebOffset;
1118 best = preb[p + bestLaneOffset];
1122 assert(curr.
lane !=
nullptr);
1123 assert(neigh.
lane !=
nullptr);
1124 assert(best.
lane !=
nullptr);
1125 double driveToNextStop = -std::numeric_limits<double>::max();
1134#ifdef DEBUG_WANTS_CHANGE
1139 <<
" stopPos=" << stopPos
1140 <<
" currentDist=" << currentDist
1141 <<
" neighDist=" << neighDist
1145 currentDist =
MAX2(currentDist, stopPos);
1146 neighDist =
MAX2(neighDist, stopPos);
1149 const bool right = (laneOffset == -1);
1150 const bool left = (laneOffset == 1);
1153 const bool changeToBest = (right && bestLaneOffset < 0) || (left && bestLaneOffset > 0) || (laneOffset == 0 && bestLaneOffset == 0);
1179#ifdef DEBUG_WANTSCHANGE
1186 <<
"\n leaders=" << leaders.
toString()
1187 <<
"\n followers=" << followers.
toString()
1188 <<
"\n blockers=" << blockers.
toString()
1189 <<
"\n neighLeaders=" << neighLeaders.
toString()
1190 <<
"\n neighFollowers=" << neighFollowers.
toString()
1191 <<
"\n neighBlockers=" << neighBlockers.
toString()
1192 <<
"\n changeToBest=" << changeToBest
1193 <<
" latLaneDist=" << latLaneDist
1201 if (lastBlocked != firstBlocked) {
1252 for (
int i = 0; i < neighLeaders.
numSublanes(); ++i) {
1254 if (vehDist.first !=
nullptr && vehDist.first->isStopped()) {
1272 currentDist += roundaboutBonus;
1273 neighDist += roundaboutBonus;
1275 if (laneOffset != 0) {
1293 if ((ret &
LCA_STAY) != 0 && latDist == 0) {
1306 if (changeToBest && abs(bestLaneOffset) > 1
1312#ifdef DEBUG_WANTSCHANGE
1314 std::cout <<
" reserving space for unseen blockers myLeadingBlockerLength=" <<
myLeadingBlockerLength <<
"\n";
1323#ifdef DEBUG_WANTSCHANGE
1329 if (*firstBlocked != neighLeadLongest &&
tieBrakeLeader(*firstBlocked)) {
1338 std::vector<CLeaderDist> collectLeadBlockers;
1339 std::vector<CLeaderDist> collectFollowBlockers;
1340 int blockedFully = 0;
1341 maneuverDist = latDist;
1343 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1344 leaders, followers, blockers,
1345 neighLeaders, neighFollowers, neighBlockers, &collectLeadBlockers, &collectFollowBlockers,
1346 false, gapFactor, &blockedFully);
1348 const double absLaneOffset = fabs(bestLaneOffset != 0 ? bestLaneOffset : latDist /
SUMO_const_laneWidth);
1349 const double remainingSeconds = ((ret &
LCA_TRACI) == 0 ?
1352 const double plannedSpeed =
informLeaders(blocked, myLca, collectLeadBlockers, remainingSeconds);
1354 if (plannedSpeed >= 0) {
1356 informFollowers(blocked, myLca, collectFollowBlockers, remainingSeconds, plannedSpeed);
1358 if (plannedSpeed > 0) {
1359 commitManoeuvre(blocked, blockedFully, leaders, neighLeaders, neighLane, maneuverDist);
1361#if defined(DEBUG_WANTSCHANGE) || defined(DEBUG_STATE)
1368 <<
" remainingSeconds=" << remainingSeconds
1369 <<
" plannedSpeed=" << plannedSpeed
1380 if (roundaboutBonus > 0) {
1382#ifdef DEBUG_WANTS_CHANGE
1386 <<
" roundaboutBonus=" << roundaboutBonus
1397 latDist = latLaneDist;
1398 maneuverDist = latLaneDist;
1399 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1400 leaders, followers, blockers,
1401 neighLeaders, neighFollowers, neighBlockers);
1405 ret &= ~LCA_COOPERATIVE;
1421 const double inconvenience = (latLaneDist < 0
1424#ifdef DEBUG_COOPERATE
1432 <<
" inconvenience=" << inconvenience
1434 <<
" wantsChangeToHelp=" << (right ?
"right" :
"left")
1451 && (changeToBest ||
currentDistAllows(neighDist, abs(bestLaneOffset) + 1, laDist))) {
1454#ifdef DEBUG_COOPERATE
1456 std::cout <<
" wants cooperative change\n";
1463 maneuverDist = latDist;
1464 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1465 leaders, followers, blockers,
1466 neighLeaders, neighFollowers, neighBlockers);
1492 const double vehWidth =
getWidth();
1494 const double leftVehSide = rightVehSide + vehWidth;
1496 double defaultNextSpeed = std::numeric_limits<double>::max();
1498 int leftmostOnEdge = (int)sublaneSides.size() - 1;
1499 while (leftmostOnEdge > 0 && sublaneSides[leftmostOnEdge] > leftVehSide) {
1502 int rightmostOnEdge = leftmostOnEdge;
1503 while (rightmostOnEdge > 0 && sublaneSides[rightmostOnEdge] > rightVehSide + NUMERICAL_EPS) {
1505#ifdef DEBUG_WANTSCHANGE
1507 std::cout <<
" adapted to current sublane=" << rightmostOnEdge <<
" defaultNextSpeed=" << defaultNextSpeed <<
"\n";
1508 std::cout <<
" sublaneSides[rightmostOnEdge]=" << sublaneSides[rightmostOnEdge] <<
" rightVehSide=" << rightVehSide <<
"\n";
1514#ifdef DEBUG_WANTSCHANGE
1516 std::cout <<
" adapted to current sublane=" << rightmostOnEdge <<
" defaultNextSpeed=" << defaultNextSpeed <<
"\n";
1517 std::cout <<
" sublaneSides[rightmostOnEdge]=" << sublaneSides[rightmostOnEdge] <<
" rightVehSide=" << rightVehSide <<
"\n";
1520 double maxGain = -std::numeric_limits<double>::max();
1521 double maxGainRight = -std::numeric_limits<double>::max();
1522 double maxGainLeft = -std::numeric_limits<double>::max();
1523 double latDistNice = std::numeric_limits<double>::max();
1526 double leftMax =
MAX2(
1533 assert(leftMax <= edge.
getWidth());
1535 int sublaneCompact =
MAX2(iMin, rightmostOnEdge - 1);
1541 const double maxLatDist = leftMax - leftVehSide;
1542 const double minLatDist = rightMin - rightVehSide;
1543 const int iStart = laneOffset == 0 ? iMin : 0;
1544 const double rightEnd = laneOffset == 0 ? leftMax : (checkOpposite ?
getLeftBorder() : edge.
getWidth());
1545#ifdef DEBUG_WANTSCHANGE
1547 <<
" checking sublanes rightmostOnEdge=" << rightmostOnEdge
1548 <<
" rightEnd=" << rightEnd
1549 <<
" leftmostOnEdge=" << leftmostOnEdge
1550 <<
" iStart=" << iStart
1552 <<
" sublaneSides=" << sublaneSides.size()
1553 <<
" leftMax=" << leftMax
1554 <<
" minLatDist=" << minLatDist
1555 <<
" maxLatDist=" << maxLatDist
1556 <<
" sublaneCompact=" << sublaneCompact
1559 for (
int i = iStart; i < (int)sublaneSides.size(); ++i) {
1560 if (sublaneSides[i] + vehWidth < rightEnd) {
1566 while (vMin > 0 && j < (
int)sublaneSides.size() && sublaneSides[j] < sublaneSides[i] + vehWidth) {
1572 if (laneOffset != 0 &&
overlap(sublaneSides[i], sublaneSides[i] + vehWidth, laneBoundary, laneBoundary)) {
1576 const double currentLatDist =
MIN2(
MAX2(sublaneSides[i] - rightVehSide, minLatDist), maxLatDist);
1580 relativeGain *= 0.5;
1583 if (relativeGain > maxGain) {
1584 maxGain = relativeGain;
1587 latDist = currentLatDist;
1588#ifdef DEBUG_WANTSCHANGE
1590 std::cout <<
" i=" << i <<
" newLatDist=" << latDist <<
" relGain=" << relativeGain <<
"\n";
1596 if (currentLatDist > 0
1600 && maxGain - relativeGain < NUMERICAL_EPS) {
1601 latDist = currentLatDist;
1604#ifdef DEBUG_WANTSCHANGE
1606 std::cout <<
" i=" << i <<
" rightmostOnEdge=" << rightmostOnEdge <<
" vMin=" << vMin <<
" relGain=" << relativeGain <<
" sublaneCompact=" << sublaneCompact <<
" curLatDist=" << currentLatDist <<
"\n";
1610 maxGainRight =
MAX2(maxGainRight, relativeGain);
1612 maxGainLeft =
MAX2(maxGainLeft, relativeGain);
1614 const double subAlignDist = sublaneSides[i] - rightVehSide;
1615 if (fabs(subAlignDist) < fabs(latDistNice)) {
1616 latDistNice = subAlignDist;
1617#ifdef DEBUG_WANTSCHANGE
1619 <<
" nicest sublane=" << i
1620 <<
" side=" << sublaneSides[i]
1621 <<
" rightSide=" << rightVehSide
1622 <<
" latDistNice=" << latDistNice
1623 <<
" maxGainR=" << maxGainRight
1624 <<
" maxGainL=" << maxGainLeft
1631 if (maxGainRight != -std::numeric_limits<double>::max()) {
1632#ifdef DEBUG_WANTSCHANGE
1638#ifdef DEBUG_WANTSCHANGE
1644 if (maxGainLeft != -std::numeric_limits<double>::max()) {
1645#ifdef DEBUG_WANTSCHANGE
1651#ifdef DEBUG_WANTSCHANGE
1658 if ((fabs(maxGainRight) < NUMERICAL_EPS || maxGainRight == -std::numeric_limits<double>::max())
1659 && (right || (alternatives &
LCA_RIGHT) == 0)) {
1662 if ((fabs(maxGainLeft) < NUMERICAL_EPS || maxGainLeft == -std::numeric_limits<double>::max())
1663 && (left || (alternatives &
LCA_LEFT) == 0)) {
1668#ifdef DEBUG_WANTSCHANGE
1671 <<
" defaultNextSpeed=" << defaultNextSpeed
1672 <<
" maxGain=" << maxGain
1673 <<
" maxGainRight=" << maxGainRight
1674 <<
" maxGainLeft=" << maxGainLeft
1675 <<
" latDist=" << latDist
1676 <<
" latDistNice=" << latDistNice
1677 <<
" sublaneCompact=" << sublaneCompact
1690 double acceptanceTime;
1699 double minFactor = 1.0;
1700 for (
int i = 0; i < followers.
numSublanes(); ++i) {
1702 if (follower.first !=
nullptr && follower.second < 2 * follower.first->getCarFollowModel().brakeGap(follower.first->getSpeed())) {
1705 const double fRSF = follower.first->getLane()->getVehicleMaxSpeed(follower.first) / follower.first->getLane()->getSpeedLimit();
1706 if (fRSF > roadSpeedFactor) {
1709 if (factor < minFactor) {
1715 acceptanceTime *= minFactor;
1719 double fullSpeedDrivingSeconds =
MIN2(acceptanceTime, fullSpeedGap / vMax);
1721 if (neighLead.first != 0 && neighLead.first->getSpeed() < vMax) {
1722 fullSpeedGap =
MAX2(0.,
MIN2(fullSpeedGap,
1724 vMax, neighLead.first->
getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel())));
1725 fullSpeedDrivingSeconds =
MIN2(fullSpeedDrivingSeconds, fullSpeedGap / (vMax - neighLead.first->getSpeed()));
1734#ifdef DEBUG_WANTSCHANGE
1737 <<
" considering keepRight:"
1739 <<
" neighDist=" << neighDist
1741 <<
" leaderSpeed=" << (neighLead.first == 0 ? -1 : neighLead.first->getSpeed())
1743 myVehicle.
getSpeed(), neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()))
1744 <<
" acceptanceTime=" << acceptanceTime
1745 <<
" fullSpeedGap=" << fullSpeedGap
1746 <<
" fullSpeedDrivingSeconds=" << fullSpeedDrivingSeconds
1747 <<
" dProb=" << deltaProb
1748 <<
" isSlide=" << isSlide
1759 latDist = latLaneDist;
1760 maneuverDist = latLaneDist;
1761 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1762 leaders, followers, blockers,
1763 neighLeaders, neighFollowers, neighBlockers);
1766 ret &= ~LCA_KEEPRIGHT;
1772#ifdef DEBUG_WANTSCHANGE
1777 <<
" neighDist=" << neighDist
1781 <<
" latDist=" << latDist
1791 int blockedFully = 0;
1792 maneuverDist = latDist;
1793 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1794 leaders, followers, blockers,
1795 neighLeaders, neighFollowers, neighBlockers,
1796 nullptr,
nullptr,
false, 0, &blockedFully);
1802 ret &= ~LCA_SPEEDGAIN;
1809#ifdef DEBUG_WANTSCHANGE
1814 <<
" latDist=" << latDist
1815 <<
" neighDist=" << neighDist
1818 <<
" stayInLane=" << stayInLane
1829 int blockedFully = 0;
1830 maneuverDist = latDist;
1831 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1832 leaders, followers, blockers,
1833 neighLeaders, neighFollowers, neighBlockers,
1834 nullptr,
nullptr,
false, 0, &blockedFully);
1839 ret &= ~LCA_SPEEDGAIN;
1844 double latDistSublane = 0.;
1846 const double halfVehWidth =
getWidth() * 0.5;
1849 && bestLaneOffset == 0
1869#ifdef DEBUG_WANTSCHANGE
1880 latDistSublane = -halfLaneWidth + halfVehWidth -
getPosLat();
1883 latDistSublane = halfLaneWidth - halfVehWidth -
getPosLat();
1890 latDistSublane = latDistNice;
1893 latDistSublane = sublaneSides[sublaneCompact] - rightVehSide;
1911 latDistSublane * latDist > 0) {
1913#if defined(DEBUG_WANTSCHANGE) || defined(DEBUG_STATE) || defined(DEBUG_MANEUVER)
1918 <<
" latDist=" << latDist
1919 <<
" latDistSublane=" << latDistSublane
1920 <<
" relGainSublane=" <<
computeSpeedGain(latDistSublane, defaultNextSpeed)
1921 <<
" maneuverDist=" << maneuverDist
1933#if defined(DEBUG_WANTSCHANGE)
1935 <<
" speedGain=" <<
computeSpeedGain(latDistSublane, defaultNextSpeed) <<
")\n";
1943#if defined(DEBUG_WANTSCHANGE)
1945 std::cout <<
" aborting sublane change due to prior maneuver\n";
1950 latDist = latDistSublane * (
isOpposite() ? -1 : 1);
1955#ifdef DEBUG_WANTSCHANGE
1958 <<
" latDist=" << latDist
1966#ifdef DEBUG_WANTSCHANGE
1972 maneuverDist = latDist;
1973 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1974 leaders, followers, blockers,
1975 neighLeaders, neighFollowers, neighBlockers);
1978 ret &= ~LCA_SUBLANE;
2004#ifdef DEBUG_WANTSCHANGE
2021 if ((*blocked) !=
nullptr) {
2023#ifdef DEBUG_SLOWDOWN
2032 if (gap > POSITION_EPS) {
2046 (gap - POSITION_EPS), (*blocked)->getSpeed(),
2047 (*blocked)->getCarFollowModel().getMaxDecel()),
false);
2065 if (cand !=
nullptr && cand->getBidiLane() == lane) {
2079 assert(preb.size() == lanes.size() ||
isOpposite());
2080#ifdef DEBUG_EXPECTED_SLSPEED
2083 <<
" sublaneOffset=" << sublaneOffset <<
" laneIndex=" << laneIndex <<
" lane=" << lane->
getID() <<
" ahead=" << ahead.
toString() <<
"\n";
2087 for (
int sublane = 0; sublane < (int)ahead.
numSublanes(); ++sublane) {
2088 const int edgeSublane = sublane + sublaneOffset;
2096 const MSVehicle* leader = ahead[sublane].first;
2097 const double gap = ahead[sublane].second;
2099 if (leader ==
nullptr) {
2104 const int prebIndex =
isOpposite() ? (int)preb.size() - 1 : laneIndex;
2115#ifdef DEBUG_EXPECTED_SLSPEED
2117 std::cout <<
SIMTIME <<
" updateExpectedSublaneSpeeds sublane=" << sublane <<
" leader=" << leader->
getID() <<
" bidi=" << bidi->
getID() <<
" gap=" << gap <<
" vSafe=" << vSafe <<
"\n";
2127#ifdef DEBUG_EXPECTED_SLSPEED
2129 std::cout <<
" updateExpectedSublaneSpeeds edgeSublane=" << edgeSublane <<
" leader=" << leader->
getID() <<
" gap=" << gap <<
" vSafe=" << vSafe <<
"\n";
2138 double foeRight, foeLeft;
2142 if (pedLeader.first != 0) {
2147#ifdef DEBUG_EXPECTED_SLSPEED
2149 std::cout <<
" updateExpectedSublaneSpeeds edgeSublane=" << edgeSublane <<
" pedLeader=" << pedLeader.first->getID() <<
" gap=" << pedGap <<
" vSafe=" << vSafe <<
"\n";
2157 double foeRight, foeLeft;
2159 const double foeRightBidi = bidi->
getWidth() - foeLeft;
2160 const double foeLeftBidi = bidi->
getWidth() - foeRight;
2165 if (pedLeader.first != 0) {
2170#ifdef DEBUG_EXPECTED_SLSPEED
2172 std::cout <<
" updateExpectedSublaneSpeeds edgeSublane=" << edgeSublane <<
" pedLeader=" << pedLeader.first->getID() <<
" (bidi) gap=" << pedGap <<
" vSafe=" << vSafe <<
"\n";
2177 vSafe =
MIN2(vMax, vSafe);
2192 const double deltaV = vMax - vLeader;
2193 if (deltaV > 0 && gap / deltaV < mySpeedGainLookahead && mySpeedGainLookahead > 0) {
2197 const double gapClosingTime =
MAX2(0.0, gap / deltaV);
2198 const double vSafe2 = (gapClosingTime * vSafe + (foreCastTime - gapClosingTime) * vLeader) / foreCastTime;
2199#ifdef DEBUG_EXPECTED_SLSPEED
2201 std::cout <<
" foreCastTime=" << foreCastTime <<
" gapClosingTime=" << gapClosingTime <<
" extrapolated vSafe=" << vSafe2 <<
"\n";
2212 double result = std::numeric_limits<double>::max();
2214 const double vehWidth =
getWidth();
2216 const double leftVehSide = rightVehSide + vehWidth;
2217 for (
int i = 0; i < (int)sublaneSides.size(); ++i) {
2219 if (
overlap(rightVehSide, leftVehSide, sublaneSides[i], leftSide)) {
2224 return result - defaultNextSpeed;
2231 double maxLength = -1;
2242 return iMax >= 0 ? ldi[iMax] : std::make_pair(
nullptr, -1);
2259 double minSpeed = std::numeric_limits<double>::max();
2261 if (ldi[i].first != 0) {
2262 const double speed = ldi[i].first->getSpeed();
2263 if (speed < minSpeed) {
2281 std::vector<CLeaderDist>* collectLeadBlockers,
2282 std::vector<CLeaderDist>* collectFollowBlockers,
2283 bool keepLatGapManeuver,
2285 int* retBlockedFully) {
2288 latDist =
MAX2(
MIN2(latDist, maxDist), -maxDist);
2297 if (laneOffset != 0) {
2308 if (laneOffset != 0) {
2312#ifdef DEBUG_BLOCKING
2324 }
else if (!forcedTraCIChange) {
2330 }
else if (!forcedTraCIChange) {
2336#ifdef DEBUG_BLOCKING
2338 std::cout <<
" checkBlocking fully=" <<
myCanChangeFully <<
" latDist=" << latDist <<
" maneuverDist=" << maneuverDist <<
"\n";
2352 if (laneOffset != 0) {
2359 int blockedFully = 0;
2364 if (laneOffset != 0) {
2370 if (retBlockedFully !=
nullptr) {
2371 *retBlockedFully = blockedFully;
2378 blocked |= blockedFully;
2383 if (collectFollowBlockers !=
nullptr && collectLeadBlockers !=
nullptr) {
2385 for (std::vector<CLeaderDist>::const_iterator it2 = collectLeadBlockers->begin(); it2 != collectLeadBlockers->end(); ++it2) {
2386 for (std::vector<CLeaderDist>::iterator it = collectFollowBlockers->begin(); it != collectFollowBlockers->end();) {
2387 if ((*it2).first == (*it).first) {
2388#ifdef DEBUG_BLOCKING
2390 std::cout <<
" removed follower " << (*it).first->getID() <<
" because it is already a leader\n";
2393 it = collectFollowBlockers->erase(it);
2407 int laneOffset,
double latDist,
double foeOffset,
bool leaders,
2408 double& safeLatGapRight,
double& safeLatGapLeft,
2409 std::vector<CLeaderDist>* collectBlockers)
const {
2416 const double vehWidth =
getWidth();
2418 const double leftVehSide = rightVehSide + vehWidth;
2419 const double rightVehSideDest = rightVehSide + latDist;
2420 const double leftVehSideDest = leftVehSide + latDist;
2421 const double rightNoOverlap =
MIN2(rightVehSideDest, rightVehSide);
2422 const double leftNoOverlap =
MAX2(leftVehSideDest, leftVehSide);
2423#ifdef DEBUG_BLOCKING
2425 std::cout <<
" checkBlockingVehicles"
2426 <<
" laneOffset=" << laneOffset
2427 <<
" latDist=" << latDist
2428 <<
" foeOffset=" << foeOffset
2429 <<
" vehRight=" << rightVehSide
2430 <<
" vehLeft=" << leftVehSide
2431 <<
" rightNoOverlap=" << rightNoOverlap
2432 <<
" leftNoOverlap=" << leftNoOverlap
2433 <<
" destRight=" << rightVehSideDest
2434 <<
" destLeft=" << leftVehSideDest
2435 <<
" leaders=" << leaders
2441 for (
int i = 0; i < vehicles.
numSublanes(); ++i) {
2443 if (vehDist.first != 0 &&
myCFRelated.count(vehDist.first) == 0) {
2444 const MSVehicle* leader = vehDist.first;
2450 double foeRight, foeLeft;
2452 const bool overlapBefore =
overlap(rightVehSide, leftVehSide, foeRight, foeLeft);
2453 const bool overlapDest =
overlap(rightVehSideDest, leftVehSideDest, foeRight, foeLeft);
2454 const bool overlapAny =
overlap(rightNoOverlap, leftNoOverlap, foeRight, foeLeft);
2455#ifdef DEBUG_BLOCKING
2457 std::cout <<
" foe=" << vehDist.first->getID()
2458 <<
" gap=" << vehDist.second
2460 <<
" foeRight=" << foeRight
2461 <<
" foeLeft=" << foeLeft
2462 <<
" overlapBefore=" << overlapBefore
2463 <<
" overlap=" << overlapAny
2464 <<
" overlapDest=" << overlapDest
2469 if (vehDist.second < 0) {
2470 if (overlapBefore && !overlapDest && !
outsideEdge()) {
2471#ifdef DEBUG_BLOCKING
2473 std::cout <<
" ignoring current overlap to come clear\n";
2477#ifdef DEBUG_BLOCKING
2483 if (collectBlockers ==
nullptr) {
2486 collectBlockers->push_back(vehDist);
2502 const double expectedGap =
MSCFModel::gapExtrapolation(timeTillAction, vehDist.second, leader->
getSpeed(), follower->
getSpeed(), leaderAccel, followerAccel, std::numeric_limits<double>::max(), std::numeric_limits<double>::max());
2505 const double followerExpectedSpeed = follower->
getSpeed() + timeTillAction * followerAccel;
2506 const double leaderExpectedSpeed =
MAX2(0., leader->
getSpeed() + timeTillAction * leaderAccel);
2509#if defined(DEBUG_ACTIONSTEPS) && defined(DEBUG_BLOCKING)
2511 std::cout <<
" timeTillAction=" << timeTillAction
2512 <<
" followerAccel=" << followerAccel
2513 <<
" followerExpectedSpeed=" << followerExpectedSpeed
2514 <<
" leaderAccel=" << leaderAccel
2515 <<
" leaderExpectedSpeed=" << leaderExpectedSpeed
2516 <<
"\n gap=" << vehDist.second
2517 <<
" gapChange=" << (expectedGap - vehDist.second)
2518 <<
" expectedGap=" << expectedGap
2519 <<
" expectedSecureGap=" << expectedSecureGap
2520 <<
" safeLatGapLeft=" << safeLatGapLeft
2521 <<
" safeLatGapRight=" << safeLatGapRight
2528 if (expectedGap < secureGap2) {
2530 if (foeRight > leftVehSide) {
2531 safeLatGapLeft =
MIN2(safeLatGapLeft, foeRight - leftVehSide);
2532 }
else if (foeLeft < rightVehSide) {
2533 safeLatGapRight =
MIN2(safeLatGapRight, rightVehSide - foeLeft);
2536#ifdef DEBUG_BLOCKING
2538 std::cout <<
" blocked by " << vehDist.first->getID() <<
" gap=" << vehDist.second <<
" expectedGap=" << expectedGap
2539 <<
" expectedSecureGap=" << expectedSecureGap <<
" secGap2=" << secureGap2 <<
" safetyFactor=" <<
getSafetyFactor()
2540 <<
" safeLatGapLeft=" << safeLatGapLeft <<
" safeLatGapRight=" << safeLatGapRight
2544 result |= blockType;
2545 if (collectBlockers ==
nullptr) {
2548#ifdef DEBUG_BLOCKING
2549 }
else if (
gDebugFlag2 && expectedGap < expectedSecureGap) {
2550 std::cout <<
" ignore blocker " << vehDist.first->getID() <<
" gap=" << vehDist.second <<
" expectedGap=" << expectedGap
2551 <<
" expectedSecureGap=" << expectedSecureGap <<
" secGap2=" << secureGap2 <<
" safetyFactor=" <<
getSafetyFactor() <<
"\n";
2554 if (collectBlockers !=
nullptr) {
2557 collectBlockers->push_back(vehDist);
2574 const double leftVehSide = rightVehSide + vehWidth;
2575#ifdef DEBUG_BLOCKING
2577 std::cout <<
" updateCFRelated foeOffset=" << foeOffset <<
" vehicles=" << vehicles.
toString() <<
"\n";
2580 for (
int i = 0; i < vehicles.
numSublanes(); ++i) {
2582 if (vehDist.first != 0 && (
myCFRelated.count(vehDist.first) == 0 || vehDist.second < 0)) {
2583 double foeRight, foeLeft;
2585#ifdef DEBUG_BLOCKING
2587 std::cout <<
" foe=" << vehDist.first->getID() <<
" gap=" << vehDist.second
2589 <<
" foeOffset=" << foeOffset
2590 <<
" egoR=" << rightVehSide <<
" egoL=" << leftVehSide
2591 <<
" iR=" << foeRight <<
" iL=" << foeLeft
2597 if (
overlap(rightVehSide, leftVehSide, foeRight, foeLeft) && !
outsideEdge() && (vehDist.second >= 0
2603 && -vehDist.second < vehDist.first->getVehicleType().getMinGap()
2606#ifdef DEBUG_BLOCKING
2608 std::cout <<
" ignoring cfrelated foe=" << vehDist.first->getID() <<
"\n";
2613 const int erased = (int)
myCFRelated.erase(vehDist.first);
2614#ifdef DEBUG_BLOCKING
2616 std::cout <<
" restoring cfrelated foe=" << vehDist.first->getID() <<
"\n";
2629 assert(right <= left);
2630 assert(right2 <= left2);
2631 return left2 >= right + NUMERICAL_EPS && left >= right2 + NUMERICAL_EPS;
2652 return changeReason;
2659 if (sd1.
state == 0) {
2661 }
else if (sd2.
state == 0) {
2671#ifdef DEBUG_WANTSCHANGE
2677 <<
" dir1=" << sd1.
dir
2681 <<
" dir2=" << sd2.
dir
2697 if (reason1 < reason2) {
2699 return (!can1 && can2 && sd1.
sameDirection(sd2)) ? sd2 : sd1;
2701 }
else if (reason1 > reason2) {
2703 return (!can2 && can1 && sd1.
sameDirection(sd2)) ? sd1 : sd2;
2711 }
else if (sd2.
dir == 0) {
2716 assert(sd1.
dir == -1);
2717 assert(sd2.
dir == 1);
2720 }
else if (sd2.
latDist >= 0) {
2769 double roundaboutBonus,
2774 const bool right = (laneOffset == -1);
2775 const bool left = (laneOffset == 1);
2782 const double neighLeftPlace =
MAX2(0., neighDist - forwardPos - maxJam);
2785#ifdef DEBUG_STRATEGIC_CHANGE
2789 <<
" forwardPos=" << forwardPos
2791 <<
" laDist=" << laDist
2792 <<
" currentDist=" << currentDist
2793 <<
" usableDist=" << usableDist
2794 <<
" bestLaneOffset=" << bestLaneOffset
2795 <<
" best.length=" << best.
length
2796 <<
" maxJam=" << maxJam
2797 <<
" neighLeftPlace=" << neighLeftPlace
2803 if (laneOffset != 0 && changeToBest && bestLaneOffset == curr.
bestLaneOffset
2806 latDist = latLaneDist;
2823#ifdef DEBUG_STRATEGIC_CHANGE
2826 <<
" avoid overtaking on the right nv=" << nv->
getID()
2838 if (laneOffset != 0 &&
myStrategicParam >= 0 && noOpposites &&
mustOvertakeStopped(neighLane, leaders, neighLeaders, forwardPos, neighDist, right, latLaneDist, currentDist, latDist)) {
2845 }
else if (!changeToBest && (
currentDistDisallows(neighLeftPlace, abs(bestLaneOffset) + 2, laDist))) {
2852#ifdef DEBUG_STRATEGIC_CHANGE
2854 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" could not change back and forth in time (1) neighLeftPlace=" << neighLeftPlace <<
"\n";
2860 && bestLaneOffset == 0
2863 && roundaboutBonus == 0
2870#ifdef DEBUG_STRATEGIC_CHANGE
2872 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" does not want to leave the bestLane (neighDist=" << neighDist <<
")\n";
2877 && bestLaneOffset == 0
2883#ifdef DEBUG_STRATEGIC_CHANGE
2885 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" does not want to get stranded on the on-ramp of a highway\n";
2899 MSLane* shadowPrev =
nullptr;
2901 if (*it ==
nullptr) {
2905 if (shadow ==
nullptr || currentShadowDist >= requiredDist) {
2908 if (shadowPrev !=
nullptr) {
2911 currentShadowDist += shadow->
getLength();
2912 shadowPrev = shadow;
2913#ifdef DEBUG_STRATEGIC_CHANGE
2915 std::cout <<
" shadow=" << shadow->
getID() <<
" currentShadowDist=" << currentShadowDist <<
"\n";
2919#ifdef DEBUG_STRATEGIC_CHANGE
2924 if (currentShadowDist < requiredDist && currentShadowDist < usableDist) {
2927#ifdef DEBUG_STRATEGIC_CHANGE
2929 std::cout <<
" must change for shadowLane end latDist=" << latDist <<
" myLeftSpace=" <<
myLeftSpace <<
"\n";
2937#if defined(DEBUG_STRATEGIC_CHANGE) || defined(DEBUG_TRACI)
2949 }
else if (((retTraCI &
LCA_RIGHT) != 0 && laneOffset < 0)
2950 || ((retTraCI &
LCA_LEFT) != 0 && laneOffset > 0)) {
2952 latDist = latLaneDist;
2955#if defined(DEBUG_STRATEGIC_CHANGE) || defined(DEBUG_TRACI)
2966 double posOnLane,
double neighDist,
bool right,
double latLaneDist,
double& currentDist,
double& latDist) {
2967 bool mustOvertake =
false;
2974 if (curHasStopped) {
2976 for (
int i = rightmost; i <= leftmost; i++) {
2981 MIN2(neighDist, currentDist) - posOnLane > overtakeDist
2983 && (!checkOverTakeRight || !right)
2991 latDist = latLaneDist;
2992 mustOvertake =
true;
2993#ifdef DEBUG_WANTS_CHANGE
2995 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" overtake stopped leader=" << leader.first->getID()
2996 <<
" overtakeDist=" << overtakeDist
2997 <<
" remaining=" <<
MIN2(neighDist, currentDist) - posOnLane
3010 for (
int i = 0; i < neighLead.
numSublanes(); i++) {
3013 mustOvertake =
true;
3014 if (i >= rightmost && i <= leftmost) {
3021 return mustOvertake;
3042 double& maneuverDist,
3078 const double oldLatDist = latDist;
3079 const double oldManeuverDist = maneuverDist;
3084 const double halfWidth =
getWidth() * 0.5;
3090 double surplusGapRight = oldCenter - halfWidth;
3091 double surplusGapLeft =
getLeftBorder(laneOffset != 0) - oldCenter - halfWidth;
3092 const bool stayInLane = (laneOffset == 0
3096 && (surplusGapLeft >= 0 && surplusGapRight >= 0)));
3099 std::swap(surplusGapLeft, surplusGapRight);
3101#ifdef DEBUG_KEEP_LATGAP
3103 std::cout <<
"\n " <<
SIMTIME <<
" keepLatGap() laneOffset=" << laneOffset
3104 <<
" latDist=" << latDist
3105 <<
" maneuverDist=" << maneuverDist
3109 <<
" gapFactor=" << gapFactor
3110 <<
" stayInLane=" << stayInLane <<
"\n"
3111 <<
" stayInEdge: surplusGapRight=" << surplusGapRight <<
" surplusGapLeft=" << surplusGapLeft <<
"\n";
3115 if (surplusGapLeft < 0 || surplusGapRight < 0) {
3125 if (laneOffset != 0) {
3128 updateGaps(neighLeaders, neighRight, oldCenter, gapFactor, surplusGapRight, surplusGapLeft,
true);
3129 updateGaps(neighFollowers, neighRight, oldCenter, gapFactor, surplusGapRight, surplusGapLeft,
true, netOverlap);
3131#ifdef DEBUG_KEEP_LATGAP
3133 std::cout <<
" minGapLat: surplusGapRight=" << surplusGapRight <<
" surplusGapLeft=" << surplusGapLeft <<
"\n"
3143 if (stayInLane || laneOffset == 1) {
3146 surplusGapRight =
MIN2(surplusGapRight,
MAX2(0.0, halfLaneWidth + posLat - halfWidth));
3147 physicalGapRight =
MIN2(physicalGapRight,
MAX2(0.0, halfLaneWidth + posLat - halfWidth));
3149 if (stayInLane || laneOffset == -1) {
3152 surplusGapLeft =
MIN2(surplusGapLeft,
MAX2(0.0, halfLaneWidth - posLat - halfWidth));
3153 physicalGapLeft =
MIN2(physicalGapLeft,
MAX2(0.0, halfLaneWidth - posLat - halfWidth));
3155#ifdef DEBUG_KEEP_LATGAP
3157 std::cout <<
" stayInLane: surplusGapRight=" << surplusGapRight <<
" surplusGapLeft=" << surplusGapLeft <<
"\n";
3161 if (surplusGapRight + surplusGapLeft < 0) {
3166 const double equalDeficit = 0.5 * (surplusGapLeft + surplusGapRight);
3167 if (surplusGapRight < surplusGapLeft) {
3169 const double delta =
MIN2(equalDeficit - surplusGapRight, physicalGapLeft);
3171 maneuverDist = delta;
3172#ifdef DEBUG_KEEP_LATGAP
3174 std::cout <<
" insufficient latSpace, move left: delta=" << delta <<
"\n";
3179 const double delta =
MIN2(equalDeficit - surplusGapLeft, physicalGapRight);
3181 maneuverDist = -delta;
3182#ifdef DEBUG_KEEP_LATGAP
3184 std::cout <<
" insufficient latSpace, move right: delta=" << delta <<
"\n";
3190 latDist =
MAX2(
MIN2(latDist, surplusGapLeft), -surplusGapRight);
3191 maneuverDist =
MAX2(
MIN2(maneuverDist, surplusGapLeft), -surplusGapRight);
3192 if ((state &
LCA_KEEPRIGHT) != 0 && maneuverDist != oldManeuverDist) {
3194 latDist = oldLatDist;
3195 maneuverDist = oldManeuverDist;
3197#ifdef DEBUG_KEEP_LATGAP
3199 std::cout <<
" adapted latDist=" << latDist <<
" maneuverDist=" << maneuverDist <<
" (old=" << oldLatDist <<
")\n";
3209#ifdef DEBUG_KEEP_LATGAP
3211 std::cout <<
" traci influenced latDist=" << latDist <<
"\n";
3217 const bool traciChange = ((state | traciState) &
LCA_TRACI) != 0;
3218 if (nonSublaneChange && !traciChange) {
3220#ifdef DEBUG_KEEP_LATGAP
3222 std::cout <<
" wanted changeToLeft oldLatDist=" << oldLatDist <<
", blocked latGap changeToRight\n";
3225 latDist = oldLatDist;
3228#ifdef DEBUG_KEEP_LATGAP
3230 std::cout <<
" wanted changeToRight oldLatDist=" << oldLatDist <<
", blocked latGap changeToLeft\n";
3233 latDist = oldLatDist;
3243#ifdef DEBUG_KEEP_LATGAP
3245 std::cout <<
" latDistUpdated=" << latDist <<
" oldLatDist=" << oldLatDist <<
"\n";
3248 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset, leaders, followers, blockers, neighLeaders, neighFollowers, neighBlockers,
nullptr,
nullptr, nonSublaneChange);
3251 state = (state & ~LCA_STAY);
3262#if defined(DEBUG_KEEP_LATGAP) || defined(DEBUG_STATE)
3264 std::cout <<
" latDist2=" << latDist
3278 double& surplusGapRight,
double& surplusGapLeft,
3279 bool saveMinGap,
double netOverlap,
3281 std::vector<CLeaderDist>* collectBlockers) {
3283 const double halfWidth =
getWidth() * 0.5 + NUMERICAL_EPS;
3286 if (others[i].first != 0 && others[i].second <= 0
3288 && (netOverlap == 0 || others[i].second + others[i].first->getVehicleType().getMinGap() < netOverlap)) {
3292 double foeRight, foeLeft;
3294 const double foeCenter = foeRight + 0.5 * res;
3295 const double gap =
MIN2(fabs(foeRight - oldCenter), fabs(foeLeft - oldCenter)) - halfWidth;
3298 const double currentMinGap = desiredMinGap * gapFactor;
3309#if defined(DEBUG_BLOCKING) || defined(DEBUG_KEEP_LATGAP)
3311 std::cout <<
" updateGaps"
3313 <<
" foe=" << foe->
getID()
3314 <<
" foeRight=" << foeRight
3315 <<
" foeLeft=" << foeLeft
3316 <<
" oldCenter=" << oldCenter
3317 <<
" gap=" << others[i].second
3318 <<
" latgap=" << gap
3319 <<
" currentMinGap=" << currentMinGap
3320 <<
" surplusGapRight=" << surplusGapRight
3321 <<
" surplusGapLeft=" << surplusGapLeft
3329 if (foeCenter < oldCenter) {
3331 surplusGapRight =
MIN3(surplusGapRight, gap - currentMinGap,
MAX2(currentMinGap, gap - foeManeuverDist));
3334 surplusGapLeft =
MIN3(surplusGapLeft, gap - currentMinGap,
MAX2(currentMinGap, gap - foeManeuverDist));
3337 if (foeCenter < oldCenter) {
3338#if defined(DEBUG_BLOCKING) || defined(DEBUG_KEEP_LATGAP)
3340 std::cout <<
" new minimum rightGap=" << gap <<
"\n";
3345#if defined(DEBUG_BLOCKING) || defined(DEBUG_KEEP_LATGAP)
3347 std::cout <<
" new minimum leftGap=" << gap <<
"\n";
3353 if (collectBlockers !=
nullptr) {
3355 if ((foeCenter < oldCenter && latDist < 0 && gap < (desiredMinGap - latDist))
3356 || (foeCenter > oldCenter && latDist > 0 && gap < (desiredMinGap + latDist))) {
3357 collectBlockers->push_back(others[i]);
3374 int currentDirection =
mySpeedLat >= 0 ? 1 : -1;
3375 int directionWish = latDist >= 0 ? 1 : -1;
3382 maxSpeedLat =
MIN2(maxSpeedLat, speedBound);
3386 maxSpeedLat =
MAX2(maxSpeedLat, speedBound);
3392#ifdef DEBUG_MANEUVER
3396 <<
" computeSpeedLat()"
3397 <<
" latDist=" << latDist
3398 <<
" maneuverDist=" << maneuverDist
3399 <<
" urgent=" << urgent
3401 <<
" currentDirection=" << currentDirection
3402 <<
" directionWish=" << directionWish
3404 <<
" maxSpeedLat=" << maxSpeedLat
3410 if (directionWish == 1) {
3424 if (maneuverDist * latDist > 0) {
3425 maneuverDist = fullLatDist;
3428#ifdef DEBUG_MANEUVER
3432 <<
" fullLatDist=" << fullLatDist
3433 <<
" speedAccel=" << speedAccel
3434 <<
" speedDecel=" << speedDecel
3435 <<
" speedBound=" << speedBound
3439 if (speedDecel * speedAccel <= 0 && (
3441 (latDist >= 0 && speedAccel >= speedBound && speedBound >= speedDecel)
3442 || (latDist <= 0 && speedAccel <= speedBound && speedBound <= speedDecel))) {
3444#ifdef DEBUG_MANEUVER
3446 std::cout <<
" computeSpeedLat a)\n";
3453#ifdef DEBUG_MANEUVER
3455 std::cout <<
" computeSpeedLat b)\n";
3462 if ((fabs(minDistAccel) < fabs(fullLatDist)) || (fabs(minDistAccel - fullLatDist) < NUMERICAL_EPS)) {
3463#ifdef DEBUG_MANEUVER
3465 std::cout <<
" computeSpeedLat c)\n";
3470#ifdef DEBUG_MANEUVER
3472 std::cout <<
" minDistAccel=" << minDistAccel <<
"\n";
3477 if ((fabs(minDistCurrent) < fabs(fullLatDist)) || (fabs(minDistCurrent - fullLatDist) < NUMERICAL_EPS)) {
3478#ifdef DEBUG_MANEUVER
3480 std::cout <<
" computeSpeedLat d)\n";
3487#ifdef DEBUG_MANEUVER
3489 std::cout <<
" computeSpeedLat e)\n";
3503 const bool indirect = turnInfo.second ==
nullptr ? false : turnInfo.second->isIndirect();
3536 double maneuverDist) {
3539 double secondsToLeaveLane;
3549#if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3565 double nextLeftSpace;
3566 if (nextActionStepSpeed > 0.) {
3581#if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3585 <<
" avoidArrivalSpeed=" << avoidArrivalSpeed
3588 <<
"\n nextLeftSpace=" << nextLeftSpace
3589 <<
" nextActionStepSpeed=" << nextActionStepSpeed
3590 <<
" nextActionStepRemainingSeconds=" << secondsToLeaveLane - timeTillActionStep
3600#if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3604 <<
" secondsToLeave=" << secondsToLeaveLane
3626 const double vehWidth =
getWidth();
3628 const double leftVehSide = rightVehSide + vehWidth;
3629 const double rightVehSideDest = rightVehSide + latDist;
3630 const double leftVehSideDest = leftVehSide + latDist;
3631#if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3633 std::cout <<
" commitFollowSpeed"
3634 <<
" latDist=" << latDist
3635 <<
" foeOffset=" << foeOffset
3636 <<
" vehRight=" << rightVehSide
3637 <<
" vehLeft=" << leftVehSide
3638 <<
" destRight=" << rightVehSideDest
3639 <<
" destLeft=" << leftVehSideDest
3645 if (vehDist.first != 0) {
3646 const MSVehicle* leader = vehDist.first;
3648 double foeRight, foeLeft;
3650#if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3652 std::cout <<
" foe=" << vehDist.first->getID()
3653 <<
" gap=" << vehDist.second
3655 <<
" foeRight=" << foeRight
3656 <<
" foeLeft=" << foeLeft
3657 <<
" overlapBefore=" <<
overlap(rightVehSide, leftVehSide, foeRight, foeLeft)
3658 <<
" overlapDest=" <<
overlap(rightVehSideDest, leftVehSideDest, foeRight, foeLeft)
3662 if (
overlap(rightVehSideDest, leftVehSideDest, foeRight, foeLeft)) {
3666 speed =
MIN2(speed, vSafe);
3667#if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3669 std::cout <<
" case1 vsafe=" << vSafe <<
" speed=" << speed <<
"\n";
3672 }
else if (
overlap(rightVehSide, leftVehSide, foeRight, foeLeft)) {
3677 speed =
MIN2(speed, vSafe);
3678#if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3680 std::cout <<
" case2 vsafe=" << vSafe <<
" speed=" << speed <<
"\n";
3700 return myOppositeParam <= 0 ? std::numeric_limits<double>::max() : 1 /
myOppositeParam;
3757 }
else if (key ==
"speedGainProbabilityRight") {
3759 }
else if (key ==
"speedGainProbabilityLeft") {
3761 }
else if (key ==
"keepRightProbability") {
3763 }
else if (key ==
"lookAheadSpeed") {
3765 }
else if (key ==
"sigmaState") {
3768 }
else if (key ==
"speedGainRP") {
3770 }
else if (key ==
"speedGainLP") {
3772 }
else if (key ==
"keepRightP") {
3840 }
else if (key ==
"speedGainProbabilityRight") {
3842 }
else if (key ==
"speedGainProbabilityLeft") {
3844 }
else if (key ==
"keepRightProbability") {
3846 }
else if (key ==
"lookAheadSpeed") {
3848 }
else if (key ==
"sigmaState") {
3862 const std::pair<MSVehicle*, double>& leader,
3863 const std::pair<MSVehicle*, double>& follower,
3864 const std::pair<MSVehicle*, double>& neighLead,
3865 const std::pair<MSVehicle*, double>& neighFollow,
3867 const std::vector<MSVehicle::LaneQ>& preb,
3873#ifdef DEBUG_WANTSCHANGE
3875 std::cout <<
"\nWANTS_CHANGE\n" <<
SIMTIME
3879 <<
" neigh=" << neighLane.
getID()
3883 <<
" considerChangeTo=" << (laneOffset == -1 ?
"right" :
"left")
3897 double maneuverDist;
3900 leaders, followers, blockers,
3901 neighLeaders, neighFollowers, neighBlockers,
3903 lastBlocked, firstBlocked, latDist, maneuverDist, blocked);
3907 result &= ~LCA_SUBLANE;
3908 result |=
getLCA(result, latDist);
3910#if defined(DEBUG_WANTSCHANGE) || defined(DEBUG_STATE)
3915 <<
" wantsChangeTo=" << (laneOffset == -1 ?
"right" :
"left")
3916 << ((result &
LCA_URGENT) ?
" (urgent)" :
"")
3922 << ((result &
LCA_TRACI) ?
" (traci)" :
"")
3974 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" bgap=" << brakeGap <<
" maneuverDist=" << maneuverDist
#define ARRIVALPOS_LAT_THRESHOLD
#define HELP_DECEL_FACTOR
#define SPEEDGAIN_MEMORY_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 SPEEDGAIN_DECAY_FACTOR
#define LATGAP_SPEED_THRESHOLD
#define LOOK_AHEAD_SPEED_MEMORY
#define GAIN_PERCEPTION_THRESHOLD
#define SPEED_GAIN_MIN_SECONDS
#define LATGAP_SPEED_THRESHOLD2
std::pair< const MSVehicle *, double > CLeaderDist
std::pair< const MSPerson *, double > PersonDist
LatAlignmentDefinition
Possible ways to choose the lateral alignment, i.e., how vehicles align themselves within their lane.
@ RIGHT
drive on the right side
@ GIVEN
The alignment as offset is given.
@ DEFAULT
No information given; use default.
@ LEFT
drive on the left side
@ ARBITRARY
maintain the current alignment
@ NICE
align with the closest sublane border
@ COMPACT
align with the rightmost sublane that allows keeping the current speed
@ CENTER
drive in the middle
@ SVC_EMERGENCY
public emergency vehicles
@ RIGHT
At the rightmost side of the lane.
@ GIVEN
The position is given.
@ DEFAULT
No information given; use default.
@ LEFT
At the leftmost side of the lane.
@ CENTER
At the center of the lane.
LinkDirection
The different directions a link between two lanes may take (or a stream between two edges)....
@ PARTLEFT
The link is a partial left direction.
@ RIGHT
The link is a (hard) right direction.
@ TURN
The link is a 180 degree turn.
@ LEFT
The link is a (hard) left direction.
@ STRAIGHT
The link is a straight direction.
@ TURN_LEFTHAND
The link is a 180 degree turn (left-hand network)
@ PARTRIGHT
The link is a partial right direction.
@ NODIR
The link has no direction (is a dead end link)
LaneChangeAction
The state of a vehicle's lane-change behavior.
@ LCA_BLOCKED_LEFT
blocked left
@ LCA_KEEPRIGHT
The action is due to the default of keeping right "Rechtsfahrgebot".
@ LCA_BLOCKED
blocked in all directions
@ LCA_URGENT
The action is urgent (to be defined by lc-model)
@ LCA_BLOCKED_BY_RIGHT_LEADER
The vehicle is blocked by right leader.
@ LCA_STAY
Needs to stay on the current lane.
@ LCA_SUBLANE
used by the sublane model
@ LCA_BLOCKED_BY_LEADER
blocked by leader
@ LCA_BLOCKED_BY_LEFT_FOLLOWER
The vehicle is blocked by left follower.
@ LCA_AMBLOCKINGFOLLOWER_DONTBRAKE
@ LCA_COOPERATIVE
The action is done to help someone else.
@ LCA_OVERLAPPING
The vehicle is blocked being overlapping.
@ LCA_LEFT
Wants go to the left.
@ LCA_BLOCKED_RIGHT
blocked right
@ LCA_BLOCKED_BY_RIGHT_FOLLOWER
The vehicle is blocked by right follower.
@ LCA_STRATEGIC
The action is needed to follow the route (navigational lc)
@ LCA_AMBACKBLOCKER_STANDING
@ LCA_CHANGE_REASONS
reasons of lane change
@ LCA_TRACI
The action is due to a TraCI request.
@ LCA_SPEEDGAIN
The action is due to the wish to be faster (tactical lc)
@ LCA_WANTS_LANECHANGE
lane can change
@ LCA_RIGHT
Wants go to the right.
@ LCA_BLOCKED_BY_FOLLOWER
blocker by follower
@ LCA_BLOCKED_BY_LEFT_LEADER
@ SUMO_ATTR_LCA_COOPERATIVE_SPEED
@ SUMO_ATTR_LCA_ASSERTIVE
@ SUMO_ATTR_LCA_LANE_DISCIPLINE
@ SUMO_ATTR_LCA_TURN_ALIGNMENT_DISTANCE
@ SUMO_ATTR_LCA_LOOKAHEADLEFT
@ SUMO_ATTR_LCA_SPEEDGAIN_PARAM
@ SUMO_ATTR_LCA_MAXDISTLATSTANDING
@ SUMO_ATTR_LCA_IMPATIENCE
@ SUMO_ATTR_LCA_COOPERATIVE_ROUNDABOUT
@ SUMO_ATTR_LCA_SPEEDGAIN_LOOKAHEAD
@ SUMO_ATTR_LCA_MAXSPEEDLATFACTOR
@ SUMO_ATTR_LCA_MAXSPEEDLATSTANDING
@ SUMO_ATTR_LCA_KEEPRIGHT_PARAM
@ SUMO_ATTR_LCA_COOPERATIVE_PARAM
@ SUMO_ATTR_LCA_OPPOSITE_PARAM
@ SUMO_ATTR_LCA_OVERTAKE_DELTASPEED_FACTOR
@ SUMO_ATTR_LCA_SUBLANE_PARAM
@ SUMO_ATTR_LCA_ACCEL_LAT
@ SUMO_ATTR_LCA_STRATEGIC_PARAM
@ SUMO_ATTR_LCA_KEEPRIGHT_ACCEPTANCE_TIME
@ SUMO_ATTR_LCA_TIME_TO_IMPATIENCE
@ SUMO_ATTR_LCA_SPEEDGAINRIGHT
int gPrecision
the precision for floating point outputs
const double SUMO_const_laneWidth
#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.
Interface for lane-change models.
double getForwardPos() const
get vehicle position relative to the forward direction lane
bool hasBlueLight() const
double getPreviousManeuverDist() const
virtual void setOwnState(const int state)
int myPreviousState
lane changing state from the previous simulation step
double getManeuverDist() const
Returns the remaining unblocked distance for the current maneuver. (only used by sublane model)
int myOwnState
The current state of the vehicle.
virtual void prepareStep()
MSLane * getShadowLane() const
Returns the lane the vehicle's shadow is on during continuous/sublane lane change.
double myLastLateralGapRight
double myCommittedSpeed
the speed when committing to a change maneuver
virtual LatAlignmentDefinition getDesiredAlignment() const
static const double NO_NEIGHBOR
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
double getMaxSpeedLat2() const
return the max of maxSpeedLat and lcMaxSpeedLatStanding
const MSCFModel & getCarFollowModel() const
The vehicle's car following model.
double mySpeedLat
the current lateral speed
double myMaxSpeedLatStanding
MSVehicle & myVehicle
The vehicle this lane-changer belongs to.
double myLastLateralGapLeft
the minimum lateral gaps to other vehicles that were found when last changing to the left and right
virtual bool avoidOvertakeRight() const
virtual bool debugVehicle() const
whether the current vehicles shall be debugged
virtual double getArrivalPos() const
Returns this vehicle's desired arrivalPos for its current route (may change on reroute)
const SUMOVehicleParameter & getParameter() const
Returns the vehicle's parameter (including departure definition)
double getLength() const
Returns the vehicle's length.
const MSEdge * getEdge() const
Returns the edge the vehicle is currently at.
double getWaitingSeconds() const
Returns the number of seconds waited (speed was lesser than 0.1m/s)
SUMOVehicleClass getVClass() const
Returns the vehicle's access class.
const MSRoute & getRoute() const
Returns the current route.
const MSVehicleType & getVehicleType() const
Returns the vehicle's type definition.
The car-following model abstraction.
virtual double maxNextSpeed(double speed, const MSVehicle *const veh) const
Returns the maximum speed given the current speed.
static double gapExtrapolation(const double duration, const double currentGap, double v1, double v2, double a1=0, double a2=0, const double maxV1=std::numeric_limits< double >::max(), const double maxV2=std::numeric_limits< double >::max())
return the resulting gap if, starting with gap currentGap, two vehicles continue with constant accele...
virtual double minNextSpeedEmergency(double speed, const MSVehicle *const veh=0) const
Returns the minimum speed after emergency braking, given the current speed (depends on the numerical ...
virtual double followSpeedTransient(double duration, const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel) const
Computes the vehicle's follow speed that avoids a collision for the given amount of time.
double getEmergencyDecel() const
Get the vehicle type's maximal phisically possible deceleration [m/s^2].
static double brakeGapEuler(const double speed, const double decel, const double headwayTime)
static double avoidArrivalAccel(double dist, double time, double speed, double maxDecel)
Computes the acceleration needed to arrive not before the given time.
virtual double minNextSpeed(double speed, const MSVehicle *const veh=0) const
Returns the minimum speed given the current speed (depends on the numerical update scheme and its ste...
virtual void setMaxDecel(double decel)
Sets a new value for maximal comfortable deceleration [m/s^2].
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].
static double estimateArrivalTime(double dist, double speed, double maxSpeed, double accel)
Computes the time needed to travel a distance dist given an initial speed and constant acceleration....
virtual double followSpeed(const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle *const pred=0, const CalcReason usage=CalcReason::CURRENT) const =0
Computes the vehicle's follow speed (no dawdling)
double stopSpeed(const MSVehicle *const veh, const double speed, double gap, const CalcReason usage=CalcReason::CURRENT) const
Computes the vehicle's safe speed for approaching a non-moving obstacle (no dawdling)
A road/street connecting two junctions.
const std::set< MSTransportable *, ComparatorNumericalIdLess > & getPersons() const
Returns this edge's persons set.
const std::vector< MSLane * > & getLanes() const
Returns this edge's lanes.
double getInternalFollowingLengthTo(const MSEdge *followerAfterInternal, SUMOVehicleClass vClass) const
returns the length of all internal edges on the junction until reaching the non-internal edge followe...
bool canChangeToOpposite() const
whether this edge allows changing to the opposite direction edge
bool isInternal() const
return whether this edge is an internal edge
double getWidth() const
Returns the edges's width (sum over all lanes)
const std::vector< double > getSubLaneSides() const
Returns the right side offsets of this edge's sublanes.
static double gLateralResolution
static bool gSemiImplicitEulerUpdate
static bool gLefthand
Whether lefthand-drive is being simulated.
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
double mySafeLatDistRight
the lateral distance the vehicle can safely move in the currently considered direction
static bool overlap(double right, double left, double right2, double left2)
return whether the given intervals overlap
double _patchSpeed(double min, const double wanted, double max, const MSCFModel &cfModel)
double informLeaders(int blocked, int dir, const std::vector< CLeaderDist > &blockers, double remainingSeconds)
void commitManoeuvre(int blocked, int blockedFully, const MSLeaderDistanceInfo &leaders, const MSLeaderDistanceInfo &neighLeaders, const MSLane &neighLane, double maneuverDist)
commit to lane change maneuvre potentially overriding safe speed
std::set< const MSVehicle * > myCFRelated
set of vehicles that are in a car-following relationship with ego (leader of followers)
void prepareStep() override
double myKeepRightProbability
double commitFollowSpeed(double speed, double latDist, double secondsToLeaveLane, const MSLeaderDistanceInfo &leaders, double foeOffset) const
compute speed when committing to an urgent change that is safe in regard to leading vehicles
double getLeftBorder(bool checkOpposite=true) const
return current edge width optionally extended by opposite direction lane width
double myChangeProbThresholdRight
double informLeader(int blocked, int dir, const CLeaderDist &neighLead, double remainingSeconds)
MSLCM_SL2015(MSVehicle &v)
int computeSublaneShift(const MSEdge *prevEdge, const MSEdge *curEdge)
compute shift so that prevSublane + shift = newSublane
double patchSpeed(const double min, const double wanted, const double max, const MSCFModel &cfModel) override
Called to adapt the speed in order to allow a lane change. It uses information on LC-related desired ...
double getSafetyFactor() const override
return factor for modifying the safety constraints of the car-following model
double myCooperativeSpeed
double computeSpeedLat(double latDist, double &maneuverDist, bool urgent) const override
decides the next lateral speed depending on the remaining lane change distance to be covered and upda...
std::vector< double > myExpectedSublaneSpeeds
expected travel speeds on all sublanes on the current edge(!)
double getWidth() const
return the widht of this vehicle (padded for numerical stability)
bool myCanChangeFully
whether the current lane changing maneuver can be finished in a single step
int wantsChange(int laneOffset, MSAbstractLaneChangeModel::MSLCMessager &msgPass, int blocked, const std::pair< MSVehicle *, double > &leader, const std::pair< MSVehicle *, double > &follower, const std::pair< MSVehicle *, double > &neighLead, const std::pair< MSVehicle *, double > &neighFollow, const MSLane &neighLane, const std::vector< MSVehicle::LaneQ > &preb, MSVehicle **lastBlocked, MSVehicle **firstBlocked) override
Called to examine whether the vehicle wants to change using the given laneOffset (this is a wrapper a...
bool outsideEdge() const
whether the ego vehicle is driving outside edgebounds
bool myDontBrake
flag to prevent speed adaptation by slowing down
std::string getParameter(const std::string &key) const override
try to retrieve the given parameter from this device. Throw exception for unsupported key
bool wantsKeepRight(double keepRightProb) const
check against thresholds
double forecastAverageSpeed(double vSafe, double vMax, double gap, double vLeader) const
estimate average speed over mySpeedGainLookahead time
void updateCFRelated(const MSLeaderDistanceInfo &vehicles, double foeOffset, bool leaders)
find leaders/followers that are already in a car-following relationship with ego
bool debugVehicle() const override
whether the current vehicles shall be debugged
int wantsChangeSublane(int laneOffset, LaneChangeAction alternatives, const MSLeaderDistanceInfo &leaders, const MSLeaderDistanceInfo &followers, const MSLeaderDistanceInfo &blockers, const MSLeaderDistanceInfo &neighLeaders, const MSLeaderDistanceInfo &neighFollowers, const MSLeaderDistanceInfo &neighBlockers, const MSLane &neighLane, const std::vector< MSVehicle::LaneQ > &preb, MSVehicle **lastBlocked, MSVehicle **firstBlocked, double &latDist, double &maneuverDist, int &blocked) override
Called to examine whether the vehicle wants to change with the given laneOffset (using the sublane mo...
LatAlignmentDefinition getDesiredAlignment() const override
double mySpeedGainProbabilityRight
a value for tracking the probability that a change to the right is beneficial
int slowDownForBlocked(MSVehicle **blocked, int state)
compute useful slowdowns for blocked vehicles
void initDerivedParameters()
init cached parameters derived directly from model parameters
int keepLatGap(int state, const MSLeaderDistanceInfo &leaders, const MSLeaderDistanceInfo &followers, const MSLeaderDistanceInfo &blockers, const MSLeaderDistanceInfo &neighLeaders, const MSLeaderDistanceInfo &neighFollowers, const MSLeaderDistanceInfo &neighBlockers, const MSLane &neighLane, int laneOffset, double &latDist, double &maneuverDist, int &blocked)
check whether lateral gap requirements are met override the current maneuver if necessary
bool tieBrakeLeader(const MSVehicle *veh) const
bool currentDistAllows(double dist, int laneOffset, double lookForwardDist)
CLeaderDist getLongest(const MSLeaderDistanceInfo &ldi) const
get the longest vehicle in the given info
double myCooperativeParam
double getNeighRight(const MSLane &neighLane) const
return the right offset of the neighboring lane relative to the current edge
double computeSpeedGain(double latDistSublane, double defaultNextSpeed) const
compute speedGain when moving by the given amount
double myKeepRightAcceptanceTime
void updateGaps(const MSLeaderDistanceInfo &others, double foeOffset, double oldCenter, double gapFactor, double &surplusGapRight, double &surplusGapLeft, bool saveMinGap=false, double netOverlap=0, double latDist=0, std::vector< CLeaderDist > *collectBlockers=0)
check remaining lateral gaps for the given foe vehicles and optionally update minimum lateral gaps
virtual void updateSafeLatDist(const double travelledLatDist) override
Updates the value of safe lateral distances (mySafeLatDistLeft and mySafeLatDistRight) during maneuve...
const MSEdge * myLastEdge
expected travel speeds on all sublanes on the current edge(!)
double getOppositeSafetyFactor() const override
return factor for modifying the safety constraints for opposite-diretction overtaking of the car-foll...
int checkStrategicChange(int ret, const MSLane &neighLane, int laneOffset, const MSLeaderDistanceInfo &leaders, const MSLeaderDistanceInfo &neighLeaders, const MSVehicle::LaneQ &curr, const MSVehicle::LaneQ &neigh, const MSVehicle::LaneQ &best, int bestLaneOffset, bool changeToBest, double currentDist, double neighDist, double laDist, double roundaboutBonus, double latLaneDist, bool checkOpposite, double &latDist)
compute strategic lane change actions TODO: Better documentation, refs #2
StateAndDist decideDirection(StateAndDist sd1, StateAndDist sd2) const override
decide in which direction to move in case both directions are desirable
std::pair< double, int > Info
information regarding save velocity (unused) and state flags of the ego vehicle
void msg(const CLeaderDist &cld, double speed, int state)
send a speed recommendation to the given vehicle
int checkBlocking(const MSLane &neighLane, double &latDist, double maneuverDist, int laneOffset, const MSLeaderDistanceInfo &leaders, const MSLeaderDistanceInfo &followers, const MSLeaderDistanceInfo &blockers, const MSLeaderDistanceInfo &neighLeaders, const MSLeaderDistanceInfo &neighFollowers, const MSLeaderDistanceInfo &neighBlockers, std::vector< CLeaderDist > *collectLeadBlockers=0, std::vector< CLeaderDist > *collectFollowBlockers=0, bool keepLatGapManeuver=false, double gapFactor=0, int *retBlockedFully=0)
restrict latDist to permissible speed and determine blocking state depending on that distance
double getVehicleCenter() const
return vehicle position relative to the current edge (extend by another virtual lane for opposite-dir...
int _wantsChangeSublane(int laneOffset, LaneChangeAction alternatives, const MSLeaderDistanceInfo &leaders, const MSLeaderDistanceInfo &followers, const MSLeaderDistanceInfo &blockers, const MSLeaderDistanceInfo &neighLeaders, const MSLeaderDistanceInfo &neighFollowers, const MSLeaderDistanceInfo &neighBlockers, const MSLane &neighLane, const std::vector< MSVehicle::LaneQ > &preb, MSVehicle **lastBlocked, MSVehicle **firstBlocked, double &latDist, double &maneuverDist, int &blocked)
helper function for doing the actual work
double getLateralDrift()
get lateral drift for the current step
double computeGapFactor(int state) const
compute the gap factor for the given state
double getPosLat()
get lateral position of this vehicle
bool preventSliding(double maneuverDist) const
bool isBidi(const MSLane *lane) const
check whether lane is an upcoming bidi lane
bool mustOvertakeStopped(const MSLane &neighLane, const MSLeaderDistanceInfo &leaders, const MSLeaderDistanceInfo &neighLead, double posOnLane, double neighDist, bool right, double latLaneDist, double ¤tDist, double &latDist)
void * inform(void *info, MSVehicle *sender) override
void informFollower(int blocked, int dir, const CLeaderDist &neighFollow, double remainingSeconds, double plannedSpeed)
decide whether we will try cut in before the follower or allow to be overtaken
void setParameter(const std::string &key, const std::string &value) override
try to set the given parameter for this laneChangeModel. Throw exception for unsupported key
bool saveBlockerLength(double length, double foeLeftSpace) override
reserve space at the end of the lane to avoid dead locks
double myOvertakeDeltaSpeedFactor
double myTurnAlignmentDist
double myLeadingBlockerLength
void setOwnState(const int state) override
int checkBlockingVehicles(const MSVehicle *ego, const MSLeaderDistanceInfo &vehicles, int laneOffset, double latDist, double foeOffset, bool leaders, double &safeLatGapRight, double &safeLatGapLeft, std::vector< CLeaderDist > *collectBlockers=0) const
check whether any of the vehicles overlaps with ego
void informFollowers(int blocked, int dir, const std::vector< CLeaderDist > &blockers, double remainingSeconds, double plannedSpeed)
call informFollower for multiple followers
double mySpeedGainLookahead
double mySpeedLossProbThreshold
void resetState() override
double mySpeedGainProbabilityLeft
a value for tracking the probability that a change to the left is beneficial
static LaneChangeAction getLCA(int state, double latDist)
compute lane change action from desired lateral distance
double myChangeProbThresholdLeft
void updateExpectedSublaneSpeeds(const MSLeaderDistanceInfo &ahead, int sublaneOffset, int laneIndex) override
update expected speeds for each sublane of the current edge
bool currentDistDisallows(double dist, int laneOffset, double lookForwardDist)
double getExtraReservation(int bestLaneOffset) const override
reserve extra space for unseen blockers when more tnan one lane change is required
double myTimeToImpatience
static int lowest_bit(int changeReason)
return the most important change reason
static CLeaderDist getSlowest(const MSLeaderDistanceInfo &ldi)
get the slowest vehicle in the given info
bool amBlockingFollowerPlusNB()
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...
MSLane * getParallelLane(int offset, bool includeOpposite=true) const
Returns the lane with the given offset parallel to this one or 0 if it does not exist.
double getSpeedLimit() const
Returns the lane's maximum allowed speed.
double getLength() const
Returns the lane's length.
bool allowsVehicleClass(SUMOVehicleClass vclass) const
double getVehicleMaxSpeed(const SUMOTrafficObject *const veh) const
Returns the lane's maximum speed, given a vehicle's speed limit adaptation.
double getRightSideOnEdge() const
bool hasPedestrians() const
whether the lane has pedestrians on it
int getIndex() const
Returns the lane's index.
MSLane * getOpposite() const
return the neighboring opposite direction lane for lane changing or nullptr
MSLane * getBidiLane() const
retrieve bidirectional lane or nullptr
MSLane * getParallelOpposite() const
return the opposite direction lane of this lanes edge 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
double getWidth() const
Returns the lane's width.
const std::vector< MSLink * > & getLinkCont() const
returns the container with all links !!!
int getRightmostSublane() const
saves leader/follower vehicles and their distances relative to an ego vehicle
virtual std::string toString() const
print a debugging representation
bool hasStoppedVehicle() const
whether a stopped vehicle is leader
void getSublaneBorders(int sublane, double latOffset, double &rightSide, double &leftSide) const
void getSubLanes(const MSVehicle *veh, double latOffset, int &rightmost, int &leftmost) const
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
SUMOTime getCurrentTimeStep() const
Returns the current simulation step.
const MSEdge * getLastEdge() const
returns the destination edge
const MSLane * lane
The lane to stop at (microsim only)
double getLatDist() const
double changeRequestRemainingSeconds(const SUMOTime currentTime) const
Return the remaining number of seconds of the current laneTimeLine assuming one exists.
bool ignoreOverlap() const
Representation of a vehicle in the micro simulation.
double getRightSideOnEdge(const MSLane *lane=0) const
Get the vehicle's lateral position on the edge of the given lane (or its current edge if lane == 0)
double getLeftSideOnEdge(const MSLane *lane=0) const
Get the vehicle's lateral position on the edge of the given lane (or its current edge if lane == 0)
bool isActive() const
Returns whether the current simulation step is an action point for the vehicle.
const std::pair< double, const MSLink * > & getNextTurn()
Get the distance and direction of the next upcoming turn for the vehicle (within its look-ahead range...
MSAbstractLaneChangeModel & getLaneChangeModel()
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 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...
const std::vector< MSLane * > & getBestLanesContinuation() const
Returns the best sequence of lanes to continue the route starting at myLane.
int getBestLaneOffset() const
double lateralDistanceToLane(const int offset) const
Get the minimal lateral distance required to move fully onto the lane at given offset.
const MSLane * getLane() const
Returns the lane the vehicle is on.
double getLastStepDist() const
Get the distance the vehicle covered in the previous timestep.
Influencer & getInfluencer()
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
bool hasInfluencer() const
whether the vehicle is individually influenced (via TraCI or special parameters)
double getCenterOnEdge(const MSLane *lane=0) const
Get the vehicle's lateral position on the edge of the given lane (or its current edge if lane == 0)
double getLengthWithGap() const
Get vehicle's length including the minimum gap [m].
double getWidth() const
Get the width which vehicles of this class shall have when being drawn.
SUMOVehicleClass getVehicleClass() const
Get this vehicle type's vehicle class.
const LatAlignmentDefinition & getPreferredLateralAlignment() const
Get vehicle's preferred lateral alignment procedure.
double getMinGap() const
Get the free space in front of vehicles of this class.
double getMaxSpeedLat() const
Get vehicle's maximum lateral speed [m/s].
double getLength() const
Get vehicle's length [m].
double getPreferredLateralAlignmentOffset() const
Get vehicle's preferred lateral alignment offset (in m from center line)
static std::string getIDSecure(const T *obj, const std::string &fallBack="NULL")
get an identifier for Named-like object which may be Null
const std::string & getID() const
Returns the id.
void step(double dt)
evolve for a time step of length dt.
double arrivalPosLat
(optional) The lateral position the vehicle shall arrive on
ArrivalPosLatDefinition arrivalPosLatProcedure
Information how the vehicle shall choose the lateral arrival position.
static double toDouble(const std::string &sData)
converts a string into the double value described by it by calling the char-type converter
NLOHMANN_BASIC_JSON_TPL_DECLARATION void swap(nlohmann::NLOHMANN_BASIC_JSON_TPL &j1, nlohmann::NLOHMANN_BASIC_JSON_TPL &j2) noexcept(//NOLINT(readability-inconsistent-declaration-parameter-name) is_nothrow_move_constructible< nlohmann::NLOHMANN_BASIC_JSON_TPL >::value &&//NOLINT(misc-redundant-expression) is_nothrow_move_assignable< nlohmann::NLOHMANN_BASIC_JSON_TPL >::value)
exchanges the values of two JSON objects
bool sameDirection(const StateAndDist &other) const
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.