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);
376 if (v >= min && v <= max) {
377 nVSafe =
MIN2(v * coopWeight + (1 - coopWeight) * wanted, nVSafe);
379#ifdef DEBUG_PATCHSPEED
381 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" got accel=" << (*i) <<
" nVSafe=" << nVSafe <<
"\n";
385#ifdef DEBUG_PATCHSPEED
388 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" ignoring low nVSafe=" << v <<
" min=" << min <<
"\n";
392 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" ignoring high nVSafe=" << v <<
" max=" << max <<
"\n";
400#ifdef DEBUG_PATCHSPEED
413#if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
418 return (max + wanted) / 2.0;
422#if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
427 return (min + wanted) / 2.0;
430#if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
435 return (max + wanted) / 2.0;
476#if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
481 return (max + wanted) / 2.0;
485#if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
505 if (pinfo->first >= 0) {
514 <<
" informedBy=" << sender->
getID()
515 <<
" info=" << pinfo->second
516 <<
" vSafe=" << pinfo->first
529 assert(cld.first != 0);
538 double remainingSeconds) {
544 plannedSpeed =
MIN2(plannedSpeed, v);
549 std::cout <<
" informLeader speed=" <<
myVehicle.
getSpeed() <<
" planned=" << plannedSpeed <<
"\n";
560 if (
gDebugFlag2) std::cout <<
" blocked by leader nv=" << nv->
getID() <<
" nvSpeed=" << nv->
getSpeed() <<
" needGap="
564 const double dv = plannedSpeed - nv->
getSpeed();
565 const double overtakeDist = (neighLead.second
577 || dv * remainingSeconds < overtakeDist)
578 && (!neighLead.first->isStopped() || (
isOpposite() && neighLead.second >= 0))) {
593 <<
" cannot overtake leader nv=" << nv->
getID()
595 <<
" remainingSeconds=" << remainingSeconds
596 <<
" targetSpeed=" << targetSpeed
597 <<
" nextSpeed=" << nextSpeed
608 <<
" cannot overtake fast leader nv=" << nv->
getID()
610 <<
" remainingSeconds=" << remainingSeconds
611 <<
" targetSpeed=" << targetSpeed
622 <<
" wants to overtake leader nv=" << nv->
getID()
624 <<
" remainingSeconds=" << remainingSeconds
625 <<
" currentGap=" << neighLead.second
627 <<
" overtakeDist=" << overtakeDist
637 }
else if (neighLead.first != 0) {
640 double dv, nextNVSpeed;
660 std::cout <<
" not blocked by leader nv=" << nv->
getID()
662 <<
" gap=" << neighLead.second
663 <<
" nextGap=" << neighLead.second - dv
665 <<
" targetSpeed=" << targetSpeed
669 return MIN2(targetSpeed, plannedSpeed);
681 double remainingSeconds,
682 double plannedSpeed) {
690 if (
gDebugFlag2) std::cout <<
" blocked by follower nv=" << nv->
getID() <<
" nvSpeed=" << nv->
getSpeed() <<
" needGap="
697 if ((neededGap - neighFollow.second) / remainingSeconds < (plannedSpeed - nv->
getSpeed())) {
700 std::cout <<
" wants to cut in before nv=" << nv->
getID() <<
" without any help neededGap=" << neededGap <<
"\n";
719 const double neighNewSpeed1s =
MAX2(0., nv->
getSpeed() - helpDecel);
720 const double dv = plannedSpeed - neighNewSpeed1s;
722 const double decelGap = neighFollow.second + dv;
728 <<
" egoNV=" << plannedSpeed
729 <<
" nvNewSpeed=" << neighNewSpeed
730 <<
" nvNewSpeed1s=" << neighNewSpeed1s
731 <<
" deltaGap=" << dv
732 <<
" decelGap=" << decelGap
733 <<
" secGap=" << secureGap
737 if (decelGap > 0 && decelGap >= secureGap) {
752 std::cout <<
" wants to cut in before nv=" << nv->
getID()
753 <<
" vsafe1=" << vsafe1
754 <<
" vsafe=" << vsafe
759 }
else if (dv > 0 && dv * remainingSeconds > (secureGap - decelGap + POSITION_EPS)) {
764 std::cout <<
" wants to cut in before nv=" << nv->
getID() <<
" (eventually)\n";
772 std::cout <<
" wants to cut in before nv=" << nv->
getID() <<
" (nv cannot overtake right)\n";
788 std::cout <<
" wants right follower to slow down a bit\n";
794 std::cout <<
" wants to cut in before right follower nv=" << nv->
getID() <<
" (eventually)\n";
803 const double overtakeDist = (neighFollow.second
809 const double needDV = overtakeDist / remainingSeconds;
817 <<
" wants to be overtaken by=" << nv->
getID()
818 <<
" overtakeDist=" << overtakeDist
820 <<
" vhelp=" << vhelp
821 <<
" needDV=" << needDV
827 }
else if (neighFollow.first != 0) {
837 std::cout <<
" wants to cut in before non-blocking follower nv=" << nv->
getID() <<
"\n";
845 const std::vector<CLeaderDist>& blockers,
846 double remainingSeconds) {
858 plannedSpeed =
MIN2(plannedSpeed, safe);
860 for (std::vector<CLeaderDist>::const_iterator it = blockers.begin(); it != blockers.end(); ++it) {
861 plannedSpeed =
MIN2(plannedSpeed,
informLeader(blocked, dir, *it, remainingSeconds));
869 const std::vector<CLeaderDist>& blockers,
870 double remainingSeconds,
871 double plannedSpeed) {
873 for (std::vector<CLeaderDist>::const_iterator it = blockers.begin(); it != blockers.end(); ++it) {
874 informFollower(blocked, dir, *it, remainingSeconds, plannedSpeed);
899 const double halfWidth =
getWidth() * 0.5;
909 std::vector<double> newExpectedSpeeds;
918 const std::vector<MSLane*>& lanes = currEdge->
getLanes();
919 for (std::vector<MSLane*>::const_iterator it_lane = lanes.begin(); it_lane != lanes.end(); ++it_lane) {
921 for (
int i = 0; i < subLanes; ++i) {
922 newExpectedSpeeds.push_back((*it_lane)->getVehicleMaxSpeed(&
myVehicle));
928 for (
int i = 0; i < subLanes; ++i) {
929 newExpectedSpeeds.push_back(lanes.back()->getVehicleMaxSpeed(&
myVehicle));
937 if (subLaneShift < std::numeric_limits<int>::max()) {
939 const int newI = i + subLaneShift;
940 if (newI > 0 && newI < (
int)newExpectedSpeeds.size()) {
958 if (bestLaneOffset < -1) {
960 }
else if (bestLaneOffset > 1) {
987 for (
const MSLink*
const link : lane->getLinkCont()) {
988 if (&link->getLane()->getEdge() == curEdge) {
990 const MSLane* target = link->getLane();
991 const std::vector<MSLane*>& lanes2 = curEdge->
getLanes();
992 for (std::vector<MSLane*>::const_iterator it_lane2 = lanes2.begin(); it_lane2 != lanes2.end(); ++it_lane2) {
993 const MSLane* lane2 = *it_lane2;
994 if (lane2 == target) {
995 return prevShift + curShift;
1006 return std::numeric_limits<int>::max();
1040#if defined(DEBUG_MANEUVER) || defined(DEBUG_STATE)
1073 const std::vector<MSVehicle::LaneQ>& preb,
1076 double& latDist,
double& maneuverDist,
int& blocked) {
1081 int bestLaneOffset = 0;
1082 double currentDist = 0;
1083 double neighDist = 0;
1095 const int prebOffset = (checkOpposite ? 0 : laneOffset);
1096 for (
int p = 0; p < (int) preb.size(); ++p) {
1097 if (preb[p].lane == prebLane && p + laneOffset >= 0) {
1098 assert(p + prebOffset < (
int)preb.size());
1100 neigh = preb[p + prebOffset];
1101 currentDist = curr.
length;
1102 neighDist = neigh.
length;
1105 if (bestLaneOffset == 0 && preb[p + prebOffset].bestLaneOffset == 0 && !checkOpposite) {
1106#ifdef DEBUG_WANTSCHANGE
1110 <<
" bestLaneOffsetOld=" << bestLaneOffset
1111 <<
" bestLaneOffsetNew=" << laneOffset
1115 bestLaneOffset = prebOffset;
1117 best = preb[p + bestLaneOffset];
1121 assert(curr.
lane !=
nullptr);
1122 assert(neigh.
lane !=
nullptr);
1123 assert(best.
lane !=
nullptr);
1124 double driveToNextStop = -std::numeric_limits<double>::max();
1133#ifdef DEBUG_WANTS_CHANGE
1138 <<
" stopPos=" << stopPos
1139 <<
" currentDist=" << currentDist
1140 <<
" neighDist=" << neighDist
1144 currentDist =
MAX2(currentDist, stopPos);
1145 neighDist =
MAX2(neighDist, stopPos);
1148 const bool right = (laneOffset == -1);
1149 const bool left = (laneOffset == 1);
1152 const bool changeToBest = (right && bestLaneOffset < 0) || (left && bestLaneOffset > 0) || (laneOffset == 0 && bestLaneOffset == 0);
1178#ifdef DEBUG_WANTSCHANGE
1185 <<
"\n leaders=" << leaders.
toString()
1186 <<
"\n followers=" << followers.
toString()
1187 <<
"\n blockers=" << blockers.
toString()
1188 <<
"\n neighLeaders=" << neighLeaders.
toString()
1189 <<
"\n neighFollowers=" << neighFollowers.
toString()
1190 <<
"\n neighBlockers=" << neighBlockers.
toString()
1191 <<
"\n changeToBest=" << changeToBest
1192 <<
" latLaneDist=" << latLaneDist
1200 if (lastBlocked != firstBlocked) {
1251 for (
int i = 0; i < neighLeaders.
numSublanes(); ++i) {
1253 if (vehDist.first !=
nullptr && vehDist.first->isStopped()) {
1271 currentDist += roundaboutBonus;
1272 neighDist += roundaboutBonus;
1274 if (laneOffset != 0) {
1292 if ((ret &
LCA_STAY) != 0 && latDist == 0) {
1305 if (changeToBest && abs(bestLaneOffset) > 1
1311#ifdef DEBUG_WANTSCHANGE
1313 std::cout <<
" reserving space for unseen blockers myLeadingBlockerLength=" <<
myLeadingBlockerLength <<
"\n";
1322#ifdef DEBUG_WANTSCHANGE
1328 if (*firstBlocked != neighLeadLongest &&
tieBrakeLeader(*firstBlocked)) {
1337 std::vector<CLeaderDist> collectLeadBlockers;
1338 std::vector<CLeaderDist> collectFollowBlockers;
1339 int blockedFully = 0;
1340 maneuverDist = latDist;
1342 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1343 leaders, followers, blockers,
1344 neighLeaders, neighFollowers, neighBlockers, &collectLeadBlockers, &collectFollowBlockers,
1345 false, gapFactor, &blockedFully);
1347 const double absLaneOffset = fabs(bestLaneOffset != 0 ? bestLaneOffset : latDist /
SUMO_const_laneWidth);
1348 const double remainingSeconds = ((ret &
LCA_TRACI) == 0 ?
1351 const double plannedSpeed =
informLeaders(blocked, myLca, collectLeadBlockers, remainingSeconds);
1353 if (plannedSpeed >= 0) {
1355 informFollowers(blocked, myLca, collectFollowBlockers, remainingSeconds, plannedSpeed);
1357 if (plannedSpeed > 0) {
1358 commitManoeuvre(blocked, blockedFully, leaders, neighLeaders, neighLane, maneuverDist);
1360#if defined(DEBUG_WANTSCHANGE) || defined(DEBUG_STATE)
1367 <<
" remainingSeconds=" << remainingSeconds
1368 <<
" plannedSpeed=" << plannedSpeed
1379 if (roundaboutBonus > 0) {
1381#ifdef DEBUG_WANTS_CHANGE
1385 <<
" roundaboutBonus=" << roundaboutBonus
1396 latDist = latLaneDist;
1397 maneuverDist = latLaneDist;
1398 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1399 leaders, followers, blockers,
1400 neighLeaders, neighFollowers, neighBlockers);
1404 ret &= ~LCA_COOPERATIVE;
1420 const double inconvenience = (latLaneDist < 0
1423#ifdef DEBUG_COOPERATE
1431 <<
" inconvenience=" << inconvenience
1433 <<
" wantsChangeToHelp=" << (right ?
"right" :
"left")
1450 && (changeToBest ||
currentDistAllows(neighDist, abs(bestLaneOffset) + 1, laDist))) {
1453#ifdef DEBUG_COOPERATE
1455 std::cout <<
" wants cooperative change\n";
1462 maneuverDist = latDist;
1463 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1464 leaders, followers, blockers,
1465 neighLeaders, neighFollowers, neighBlockers);
1491 const double vehWidth =
getWidth();
1493 const double leftVehSide = rightVehSide + vehWidth;
1495 double defaultNextSpeed = std::numeric_limits<double>::max();
1497 int leftmostOnEdge = (int)sublaneSides.size() - 1;
1498 while (leftmostOnEdge > 0 && sublaneSides[leftmostOnEdge] > leftVehSide) {
1501 int rightmostOnEdge = leftmostOnEdge;
1502 while (rightmostOnEdge > 0 && sublaneSides[rightmostOnEdge] > rightVehSide + NUMERICAL_EPS) {
1504#ifdef DEBUG_WANTSCHANGE
1506 std::cout <<
" adapted to current sublane=" << rightmostOnEdge <<
" defaultNextSpeed=" << defaultNextSpeed <<
"\n";
1507 std::cout <<
" sublaneSides[rightmostOnEdge]=" << sublaneSides[rightmostOnEdge] <<
" rightVehSide=" << rightVehSide <<
"\n";
1513#ifdef DEBUG_WANTSCHANGE
1515 std::cout <<
" adapted to current sublane=" << rightmostOnEdge <<
" defaultNextSpeed=" << defaultNextSpeed <<
"\n";
1516 std::cout <<
" sublaneSides[rightmostOnEdge]=" << sublaneSides[rightmostOnEdge] <<
" rightVehSide=" << rightVehSide <<
"\n";
1519 double maxGain = -std::numeric_limits<double>::max();
1520 double maxGainRight = -std::numeric_limits<double>::max();
1521 double maxGainLeft = -std::numeric_limits<double>::max();
1522 double latDistNice = std::numeric_limits<double>::max();
1525 double leftMax =
MAX2(
1532 assert(leftMax <= edge.
getWidth());
1534 int sublaneCompact =
MAX2(iMin, rightmostOnEdge - 1);
1540 const double maxLatDist = leftMax - leftVehSide;
1541 const double minLatDist = rightMin - rightVehSide;
1542 const int iStart = laneOffset == 0 ? iMin : 0;
1543 const double rightEnd = laneOffset == 0 ? leftMax : (checkOpposite ?
getLeftBorder() : edge.
getWidth());
1544#ifdef DEBUG_WANTSCHANGE
1546 <<
" checking sublanes rightmostOnEdge=" << rightmostOnEdge
1547 <<
" rightEnd=" << rightEnd
1548 <<
" leftmostOnEdge=" << leftmostOnEdge
1549 <<
" iStart=" << iStart
1551 <<
" sublaneSides=" << sublaneSides.size()
1552 <<
" leftMax=" << leftMax
1553 <<
" minLatDist=" << minLatDist
1554 <<
" maxLatDist=" << maxLatDist
1555 <<
" sublaneCompact=" << sublaneCompact
1558 for (
int i = iStart; i < (int)sublaneSides.size(); ++i) {
1559 if (sublaneSides[i] + vehWidth < rightEnd) {
1565 while (vMin > 0 && j < (
int)sublaneSides.size() && sublaneSides[j] < sublaneSides[i] + vehWidth) {
1571 if (laneOffset != 0 &&
overlap(sublaneSides[i], sublaneSides[i] + vehWidth, laneBoundary, laneBoundary)) {
1575 const double currentLatDist =
MIN2(
MAX2(sublaneSides[i] - rightVehSide, minLatDist), maxLatDist);
1579 relativeGain *= 0.5;
1582 if (relativeGain > maxGain) {
1583 maxGain = relativeGain;
1586 latDist = currentLatDist;
1587#ifdef DEBUG_WANTSCHANGE
1589 std::cout <<
" i=" << i <<
" newLatDist=" << latDist <<
" relGain=" << relativeGain <<
"\n";
1595 if (currentLatDist > 0
1599 && maxGain - relativeGain < NUMERICAL_EPS) {
1600 latDist = currentLatDist;
1603#ifdef DEBUG_WANTSCHANGE
1605 std::cout <<
" i=" << i <<
" rightmostOnEdge=" << rightmostOnEdge <<
" vMin=" << vMin <<
" relGain=" << relativeGain <<
" sublaneCompact=" << sublaneCompact <<
" curLatDist=" << currentLatDist <<
"\n";
1609 maxGainRight =
MAX2(maxGainRight, relativeGain);
1611 maxGainLeft =
MAX2(maxGainLeft, relativeGain);
1613 const double subAlignDist = sublaneSides[i] - rightVehSide;
1614 if (fabs(subAlignDist) < fabs(latDistNice)) {
1615 latDistNice = subAlignDist;
1616#ifdef DEBUG_WANTSCHANGE
1618 <<
" nicest sublane=" << i
1619 <<
" side=" << sublaneSides[i]
1620 <<
" rightSide=" << rightVehSide
1621 <<
" latDistNice=" << latDistNice
1622 <<
" maxGainR=" << maxGainRight
1623 <<
" maxGainL=" << maxGainLeft
1630 if (maxGainRight != -std::numeric_limits<double>::max()) {
1631#ifdef DEBUG_WANTSCHANGE
1637#ifdef DEBUG_WANTSCHANGE
1643 if (maxGainLeft != -std::numeric_limits<double>::max()) {
1644#ifdef DEBUG_WANTSCHANGE
1650#ifdef DEBUG_WANTSCHANGE
1657 if ((fabs(maxGainRight) < NUMERICAL_EPS || maxGainRight == -std::numeric_limits<double>::max())
1658 && (right || (alternatives &
LCA_RIGHT) == 0)) {
1661 if ((fabs(maxGainLeft) < NUMERICAL_EPS || maxGainLeft == -std::numeric_limits<double>::max())
1662 && (left || (alternatives &
LCA_LEFT) == 0)) {
1667#ifdef DEBUG_WANTSCHANGE
1670 <<
" defaultNextSpeed=" << defaultNextSpeed
1671 <<
" maxGain=" << maxGain
1672 <<
" maxGainRight=" << maxGainRight
1673 <<
" maxGainLeft=" << maxGainLeft
1674 <<
" latDist=" << latDist
1675 <<
" latDistNice=" << latDistNice
1676 <<
" sublaneCompact=" << sublaneCompact
1689 double acceptanceTime;
1698 double minFactor = 1.0;
1699 for (
int i = 0; i < followers.
numSublanes(); ++i) {
1701 if (follower.first !=
nullptr && follower.second < 2 * follower.first->getCarFollowModel().brakeGap(follower.first->getSpeed())) {
1704 const double fRSF = follower.first->getLane()->getVehicleMaxSpeed(follower.first) / follower.first->getLane()->getSpeedLimit();
1705 if (fRSF > roadSpeedFactor) {
1708 if (factor < minFactor) {
1714 acceptanceTime *= minFactor;
1718 double fullSpeedDrivingSeconds =
MIN2(acceptanceTime, fullSpeedGap / vMax);
1720 if (neighLead.first != 0 && neighLead.first->getSpeed() < vMax) {
1721 fullSpeedGap =
MAX2(0.,
MIN2(fullSpeedGap,
1723 vMax, neighLead.first->
getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel())));
1724 fullSpeedDrivingSeconds =
MIN2(fullSpeedDrivingSeconds, fullSpeedGap / (vMax - neighLead.first->getSpeed()));
1733#ifdef DEBUG_WANTSCHANGE
1736 <<
" considering keepRight:"
1738 <<
" neighDist=" << neighDist
1740 <<
" leaderSpeed=" << (neighLead.first == 0 ? -1 : neighLead.first->getSpeed())
1742 myVehicle.
getSpeed(), neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()))
1743 <<
" acceptanceTime=" << acceptanceTime
1744 <<
" fullSpeedGap=" << fullSpeedGap
1745 <<
" fullSpeedDrivingSeconds=" << fullSpeedDrivingSeconds
1746 <<
" dProb=" << deltaProb
1747 <<
" isSlide=" << isSlide
1758 latDist = latLaneDist;
1759 maneuverDist = latLaneDist;
1760 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1761 leaders, followers, blockers,
1762 neighLeaders, neighFollowers, neighBlockers);
1765 ret &= ~LCA_KEEPRIGHT;
1771#ifdef DEBUG_WANTSCHANGE
1776 <<
" neighDist=" << neighDist
1780 <<
" latDist=" << latDist
1790 int blockedFully = 0;
1791 maneuverDist = latDist;
1792 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1793 leaders, followers, blockers,
1794 neighLeaders, neighFollowers, neighBlockers,
1795 nullptr,
nullptr,
false, 0, &blockedFully);
1801 ret &= ~LCA_SPEEDGAIN;
1808#ifdef DEBUG_WANTSCHANGE
1813 <<
" latDist=" << latDist
1814 <<
" neighDist=" << neighDist
1817 <<
" stayInLane=" << stayInLane
1828 int blockedFully = 0;
1829 maneuverDist = latDist;
1830 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1831 leaders, followers, blockers,
1832 neighLeaders, neighFollowers, neighBlockers,
1833 nullptr,
nullptr,
false, 0, &blockedFully);
1838 ret &= ~LCA_SPEEDGAIN;
1843 double latDistSublane = 0.;
1845 const double halfVehWidth =
getWidth() * 0.5;
1848 && bestLaneOffset == 0
1868#ifdef DEBUG_WANTSCHANGE
1882 switch (turnInfo.second) {
1902 latDistSublane = -halfLaneWidth + halfVehWidth -
getPosLat();
1905 latDistSublane = halfLaneWidth - halfVehWidth -
getPosLat();
1912 latDistSublane = latDistNice;
1915 latDistSublane = sublaneSides[sublaneCompact] - rightVehSide;
1933 latDistSublane * latDist > 0) {
1935#if defined(DEBUG_WANTSCHANGE) || defined(DEBUG_STATE) || defined(DEBUG_MANEUVER)
1940 <<
" latDist=" << latDist
1941 <<
" latDistSublane=" << latDistSublane
1942 <<
" relGainSublane=" <<
computeSpeedGain(latDistSublane, defaultNextSpeed)
1943 <<
" maneuverDist=" << maneuverDist
1955#if defined(DEBUG_WANTSCHANGE)
1957 <<
" speedGain=" <<
computeSpeedGain(latDistSublane, defaultNextSpeed) <<
")\n";
1965#if defined(DEBUG_WANTSCHANGE)
1967 std::cout <<
" aborting sublane change due to prior maneuver\n";
1972 latDist = latDistSublane * (
isOpposite() ? -1 : 1);
1977#ifdef DEBUG_WANTSCHANGE
1980 <<
" latDist=" << latDist
1988#ifdef DEBUG_WANTSCHANGE
1994 maneuverDist = latDist;
1995 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1996 leaders, followers, blockers,
1997 neighLeaders, neighFollowers, neighBlockers);
2000 ret &= ~LCA_SUBLANE;
2026#ifdef DEBUG_WANTSCHANGE
2043 if ((*blocked) !=
nullptr) {
2045#ifdef DEBUG_SLOWDOWN
2054 if (gap > POSITION_EPS) {
2068 (gap - POSITION_EPS), (*blocked)->getSpeed(),
2069 (*blocked)->getCarFollowModel().getMaxDecel()));
2084 <<
"vSafe=" << vSafe <<
" -> accel=" << accel <<
"\n";
2095 if (cand !=
nullptr && cand->getBidiLane() == lane) {
2109 assert(preb.size() == lanes.size() ||
isOpposite());
2110#ifdef DEBUG_EXPECTED_SLSPEED
2113 <<
" sublaneOffset=" << sublaneOffset <<
" laneIndex=" << laneIndex <<
" lane=" << lane->
getID() <<
" ahead=" << ahead.
toString() <<
"\n";
2117 for (
int sublane = 0; sublane < (int)ahead.
numSublanes(); ++sublane) {
2118 const int edgeSublane = sublane + sublaneOffset;
2126 const MSVehicle* leader = ahead[sublane].first;
2127 const double gap = ahead[sublane].second;
2129 if (leader ==
nullptr) {
2134 const int prebIndex =
isOpposite() ? (int)preb.size() - 1 : laneIndex;
2145#ifdef DEBUG_EXPECTED_SLSPEED
2147 std::cout <<
SIMTIME <<
" updateExpectedSublaneSpeeds sublane=" << sublane <<
" leader=" << leader->
getID() <<
" bidi=" << bidi->
getID() <<
" gap=" << gap <<
" vSafe=" << vSafe <<
"\n";
2157#ifdef DEBUG_EXPECTED_SLSPEED
2159 std::cout <<
" updateExpectedSublaneSpeeds edgeSublane=" << edgeSublane <<
" leader=" << leader->
getID() <<
" gap=" << gap <<
" vSafe=" << vSafe <<
"\n";
2168 double foeRight, foeLeft;
2172 if (pedLeader.first != 0) {
2177#ifdef DEBUG_EXPECTED_SLSPEED
2179 std::cout <<
" updateExpectedSublaneSpeeds edgeSublane=" << edgeSublane <<
" pedLeader=" << pedLeader.first->getID() <<
" gap=" << pedGap <<
" vSafe=" << vSafe <<
"\n";
2187 double foeRight, foeLeft;
2189 const double foeRightBidi = bidi->
getWidth() - foeLeft;
2190 const double foeLeftBidi = bidi->
getWidth() - foeRight;
2195 if (pedLeader.first != 0) {
2200#ifdef DEBUG_EXPECTED_SLSPEED
2202 std::cout <<
" updateExpectedSublaneSpeeds edgeSublane=" << edgeSublane <<
" pedLeader=" << pedLeader.first->getID() <<
" (bidi) gap=" << pedGap <<
" vSafe=" << vSafe <<
"\n";
2207 vSafe =
MIN2(vMax, vSafe);
2222 const double deltaV = vMax - vLeader;
2223 if (deltaV > 0 && gap / deltaV < mySpeedGainLookahead && mySpeedGainLookahead > 0) {
2227 const double gapClosingTime =
MAX2(0.0, gap / deltaV);
2228 const double vSafe2 = (gapClosingTime * vSafe + (foreCastTime - gapClosingTime) * vLeader) / foreCastTime;
2229#ifdef DEBUG_EXPECTED_SLSPEED
2231 std::cout <<
" foreCastTime=" << foreCastTime <<
" gapClosingTime=" << gapClosingTime <<
" extrapolated vSafe=" << vSafe2 <<
"\n";
2242 double result = std::numeric_limits<double>::max();
2244 const double vehWidth =
getWidth();
2246 const double leftVehSide = rightVehSide + vehWidth;
2247 for (
int i = 0; i < (int)sublaneSides.size(); ++i) {
2249 if (
overlap(rightVehSide, leftVehSide, sublaneSides[i], leftSide)) {
2254 return result - defaultNextSpeed;
2261 double maxLength = -1;
2272 return iMax >= 0 ? ldi[iMax] : std::make_pair(
nullptr, -1);
2289 double minSpeed = std::numeric_limits<double>::max();
2291 if (ldi[i].first != 0) {
2292 const double speed = ldi[i].first->getSpeed();
2293 if (speed < minSpeed) {
2311 std::vector<CLeaderDist>* collectLeadBlockers,
2312 std::vector<CLeaderDist>* collectFollowBlockers,
2313 bool keepLatGapManeuver,
2315 int* retBlockedFully) {
2318 latDist =
MAX2(
MIN2(latDist, maxDist), -maxDist);
2327 if (laneOffset != 0) {
2338 if (laneOffset != 0) {
2342#ifdef DEBUG_BLOCKING
2354 }
else if (!forcedTraCIChange) {
2360 }
else if (!forcedTraCIChange) {
2366#ifdef DEBUG_BLOCKING
2368 std::cout <<
" checkBlocking fully=" <<
myCanChangeFully <<
" latDist=" << latDist <<
" maneuverDist=" << maneuverDist <<
"\n";
2382 if (laneOffset != 0) {
2389 int blockedFully = 0;
2394 if (laneOffset != 0) {
2400 if (retBlockedFully !=
nullptr) {
2401 *retBlockedFully = blockedFully;
2408 blocked |= blockedFully;
2413 if (collectFollowBlockers !=
nullptr && collectLeadBlockers !=
nullptr) {
2415 for (std::vector<CLeaderDist>::const_iterator it2 = collectLeadBlockers->begin(); it2 != collectLeadBlockers->end(); ++it2) {
2416 for (std::vector<CLeaderDist>::iterator it = collectFollowBlockers->begin(); it != collectFollowBlockers->end();) {
2417 if ((*it2).first == (*it).first) {
2418#ifdef DEBUG_BLOCKING
2420 std::cout <<
" removed follower " << (*it).first->getID() <<
" because it is already a leader\n";
2423 it = collectFollowBlockers->erase(it);
2437 int laneOffset,
double latDist,
double foeOffset,
bool leaders,
2438 double& safeLatGapRight,
double& safeLatGapLeft,
2439 std::vector<CLeaderDist>* collectBlockers)
const {
2446 const double vehWidth =
getWidth();
2448 const double leftVehSide = rightVehSide + vehWidth;
2449 const double rightVehSideDest = rightVehSide + latDist;
2450 const double leftVehSideDest = leftVehSide + latDist;
2451 const double rightNoOverlap =
MIN2(rightVehSideDest, rightVehSide);
2452 const double leftNoOverlap =
MAX2(leftVehSideDest, leftVehSide);
2453#ifdef DEBUG_BLOCKING
2455 std::cout <<
" checkBlockingVehicles"
2456 <<
" laneOffset=" << laneOffset
2457 <<
" latDist=" << latDist
2458 <<
" foeOffset=" << foeOffset
2459 <<
" vehRight=" << rightVehSide
2460 <<
" vehLeft=" << leftVehSide
2461 <<
" rightNoOverlap=" << rightNoOverlap
2462 <<
" leftNoOverlap=" << leftNoOverlap
2463 <<
" destRight=" << rightVehSideDest
2464 <<
" destLeft=" << leftVehSideDest
2465 <<
" leaders=" << leaders
2471 for (
int i = 0; i < vehicles.
numSublanes(); ++i) {
2473 if (vehDist.first != 0 &&
myCFRelated.count(vehDist.first) == 0) {
2474 const MSVehicle* leader = vehDist.first;
2480 double foeRight, foeLeft;
2482 const bool overlapBefore =
overlap(rightVehSide, leftVehSide, foeRight, foeLeft);
2483 const bool overlapDest =
overlap(rightVehSideDest, leftVehSideDest, foeRight, foeLeft);
2484 const bool overlapAny =
overlap(rightNoOverlap, leftNoOverlap, foeRight, foeLeft);
2485#ifdef DEBUG_BLOCKING
2487 std::cout <<
" foe=" << vehDist.first->getID()
2488 <<
" gap=" << vehDist.second
2490 <<
" foeRight=" << foeRight
2491 <<
" foeLeft=" << foeLeft
2492 <<
" overlapBefore=" << overlapBefore
2493 <<
" overlap=" << overlapAny
2494 <<
" overlapDest=" << overlapDest
2499 if (vehDist.second < 0) {
2500 if (overlapBefore && !overlapDest) {
2501#ifdef DEBUG_BLOCKING
2503 std::cout <<
" ignoring current overlap to come clear\n";
2507#ifdef DEBUG_BLOCKING
2513 if (collectBlockers ==
nullptr) {
2516 collectBlockers->push_back(vehDist);
2532 const double expectedGap =
MSCFModel::gapExtrapolation(timeTillAction, vehDist.second, leader->
getSpeed(), follower->
getSpeed(), leaderAccel, followerAccel, std::numeric_limits<double>::max(), std::numeric_limits<double>::max());
2535 const double followerExpectedSpeed = follower->
getSpeed() + timeTillAction * followerAccel;
2536 const double leaderExpectedSpeed =
MAX2(0., leader->
getSpeed() + timeTillAction * leaderAccel);
2539#if defined(DEBUG_ACTIONSTEPS) && defined(DEBUG_BLOCKING)
2541 std::cout <<
" timeTillAction=" << timeTillAction
2542 <<
" followerAccel=" << followerAccel
2543 <<
" followerExpectedSpeed=" << followerExpectedSpeed
2544 <<
" leaderAccel=" << leaderAccel
2545 <<
" leaderExpectedSpeed=" << leaderExpectedSpeed
2546 <<
"\n gap=" << vehDist.second
2547 <<
" gapChange=" << (expectedGap - vehDist.second)
2548 <<
" expectedGap=" << expectedGap
2549 <<
" expectedSecureGap=" << expectedSecureGap
2550 <<
" safeLatGapLeft=" << safeLatGapLeft
2551 <<
" safeLatGapRight=" << safeLatGapRight
2558 if (expectedGap < secureGap2) {
2560 if (foeRight > leftVehSide) {
2561 safeLatGapLeft =
MIN2(safeLatGapLeft, foeRight - leftVehSide);
2562 }
else if (foeLeft < rightVehSide) {
2563 safeLatGapRight =
MIN2(safeLatGapRight, rightVehSide - foeLeft);
2566#ifdef DEBUG_BLOCKING
2568 std::cout <<
" blocked by " << vehDist.first->getID() <<
" gap=" << vehDist.second <<
" expectedGap=" << expectedGap
2569 <<
" expectedSecureGap=" << expectedSecureGap <<
" secGap2=" << secureGap2 <<
" safetyFactor=" <<
getSafetyFactor()
2570 <<
" safeLatGapLeft=" << safeLatGapLeft <<
" safeLatGapRight=" << safeLatGapRight
2574 result |= blockType;
2575 if (collectBlockers ==
nullptr) {
2578#ifdef DEBUG_BLOCKING
2579 }
else if (
gDebugFlag2 && expectedGap < expectedSecureGap) {
2580 std::cout <<
" ignore blocker " << vehDist.first->getID() <<
" gap=" << vehDist.second <<
" expectedGap=" << expectedGap
2581 <<
" expectedSecureGap=" << expectedSecureGap <<
" secGap2=" << secureGap2 <<
" safetyFactor=" <<
getSafetyFactor() <<
"\n";
2584 if (collectBlockers !=
nullptr) {
2587 collectBlockers->push_back(vehDist);
2604 const double leftVehSide = rightVehSide + vehWidth;
2605#ifdef DEBUG_BLOCKING
2607 std::cout <<
" updateCFRelated foeOffset=" << foeOffset <<
" vehicles=" << vehicles.
toString() <<
"\n";
2610 for (
int i = 0; i < vehicles.
numSublanes(); ++i) {
2612 if (vehDist.first != 0 &&
myCFRelated.count(vehDist.first) == 0) {
2613 double foeRight, foeLeft;
2615#ifdef DEBUG_BLOCKING
2617 std::cout <<
" foe=" << vehDist.first->getID() <<
" gap=" << vehDist.second
2619 <<
" foeOffset=" << foeOffset
2620 <<
" egoR=" << rightVehSide <<
" egoL=" << leftVehSide
2621 <<
" iR=" << foeRight <<
" iL=" << foeLeft
2627 if (
overlap(rightVehSide, leftVehSide, foeRight, foeLeft) && (vehDist.second >= 0
2633 && -vehDist.second < vehDist.first->getVehicleType().getMinGap()
2636#ifdef DEBUG_BLOCKING
2638 std::cout <<
" ignoring cfrelated foe=" << vehDist.first->getID() <<
"\n";
2650 assert(right <= left);
2651 assert(right2 <= left2);
2652 return left2 >= right + NUMERICAL_EPS && left >= right2 + NUMERICAL_EPS;
2673 return changeReason;
2680 if (sd1.
state == 0) {
2682 }
else if (sd2.
state == 0) {
2692#ifdef DEBUG_WANTSCHANGE
2698 <<
" dir1=" << sd1.
dir
2702 <<
" dir2=" << sd2.
dir
2710 if (reason1 < reason2) {
2712 return (!can1 && can2 && sd1.
sameDirection(sd2)) ? sd2 : sd1;
2714 }
else if (reason1 > reason2) {
2716 return (!can2 && can1 && sd1.
sameDirection(sd2)) ? sd1 : sd2;
2724 }
else if (sd2.
dir == 0) {
2729 assert(sd1.
dir == -1);
2730 assert(sd2.
dir == 1);
2733 }
else if (sd2.
latDist >= 0) {
2782 double roundaboutBonus,
2787 const bool right = (laneOffset == -1);
2788 const bool left = (laneOffset == 1);
2795 const double neighLeftPlace =
MAX2(0., neighDist - forwardPos - maxJam);
2798#ifdef DEBUG_STRATEGIC_CHANGE
2802 <<
" forwardPos=" << forwardPos
2804 <<
" laDist=" << laDist
2805 <<
" currentDist=" << currentDist
2806 <<
" usableDist=" << usableDist
2807 <<
" bestLaneOffset=" << bestLaneOffset
2808 <<
" best.length=" << best.
length
2809 <<
" maxJam=" << maxJam
2810 <<
" neighLeftPlace=" << neighLeftPlace
2816 if (laneOffset != 0 && changeToBest && bestLaneOffset == curr.
bestLaneOffset
2819 latDist = latLaneDist;
2836#ifdef DEBUG_STRATEGIC_CHANGE
2839 <<
" avoid overtaking on the right nv=" << nv->
getID()
2851 if (laneOffset != 0 &&
myStrategicParam >= 0 && noOpposites &&
mustOvertakeStopped(neighLane, leaders, neighLeaders, forwardPos, neighDist, right, latLaneDist, currentDist, latDist)) {
2858 }
else if (!changeToBest && (
currentDistDisallows(neighLeftPlace, abs(bestLaneOffset) + 2, laDist))) {
2865#ifdef DEBUG_STRATEGIC_CHANGE
2867 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" could not change back and forth in time (1) neighLeftPlace=" << neighLeftPlace <<
"\n";
2873 && bestLaneOffset == 0
2876 && roundaboutBonus == 0
2883#ifdef DEBUG_STRATEGIC_CHANGE
2885 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" does not want to leave the bestLane (neighDist=" << neighDist <<
")\n";
2890 && bestLaneOffset == 0
2896#ifdef DEBUG_STRATEGIC_CHANGE
2898 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" does not want to get stranded on the on-ramp of a highway\n";
2912 MSLane* shadowPrev =
nullptr;
2914 if (*it ==
nullptr) {
2918 if (shadow ==
nullptr || currentShadowDist >= requiredDist) {
2921 if (shadowPrev !=
nullptr) {
2924 currentShadowDist += shadow->
getLength();
2925 shadowPrev = shadow;
2926#ifdef DEBUG_STRATEGIC_CHANGE
2928 std::cout <<
" shadow=" << shadow->
getID() <<
" currentShadowDist=" << currentShadowDist <<
"\n";
2932#ifdef DEBUG_STRATEGIC_CHANGE
2937 if (currentShadowDist < requiredDist && currentShadowDist < usableDist) {
2940#ifdef DEBUG_STRATEGIC_CHANGE
2942 std::cout <<
" must change for shadowLane end latDist=" << latDist <<
" myLeftSpace=" <<
myLeftSpace <<
"\n";
2950#if defined(DEBUG_STRATEGIC_CHANGE) || defined(DEBUG_TRACI)
2962 }
else if (((retTraCI &
LCA_RIGHT) != 0 && laneOffset < 0)
2963 || ((retTraCI &
LCA_LEFT) != 0 && laneOffset > 0)) {
2965 latDist = latLaneDist;
2968#if defined(DEBUG_STRATEGIC_CHANGE) || defined(DEBUG_TRACI)
2970 std::cout <<
" reqAfterInfluence=" << ret <<
" ret=" << ret <<
"\n";
2979 double posOnLane,
double neighDist,
bool right,
double latLaneDist,
double& currentDist,
double& latDist) {
2980 bool mustOvertake =
false;
2987 if (curHasStopped) {
2989 for (
int i = rightmost; i <= leftmost; i++) {
2994 MIN2(neighDist, currentDist) - posOnLane > overtakeDist
2996 && (!checkOverTakeRight || !right)
3004 latDist = latLaneDist;
3005 mustOvertake =
true;
3006#ifdef DEBUG_WANTS_CHANGE
3008 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" overtake stopped leader=" << leader.first->getID()
3009 <<
" overtakeDist=" << overtakeDist
3010 <<
" remaining=" <<
MIN2(neighDist, currentDist) - posOnLane
3023 for (
int i = 0; i < neighLead.
numSublanes(); i++) {
3026 mustOvertake =
true;
3027 if (i >= rightmost && i <= leftmost) {
3034 return mustOvertake;
3055 double& maneuverDist,
3091 const double oldLatDist = latDist;
3092 const double oldManeuverDist = maneuverDist;
3095 const double halfWidth =
getWidth() * 0.5;
3101 double surplusGapRight = oldCenter - halfWidth;
3102 double surplusGapLeft =
getLeftBorder(laneOffset != 0) - oldCenter - halfWidth;
3103 const bool stayInLane = (laneOffset == 0
3107 && (surplusGapLeft >= 0 && surplusGapRight >= 0)));
3110 std::swap(surplusGapLeft, surplusGapRight);
3112#ifdef DEBUG_KEEP_LATGAP
3114 std::cout <<
"\n " <<
SIMTIME <<
" keepLatGap() laneOffset=" << laneOffset
3115 <<
" latDist=" << latDist
3116 <<
" maneuverDist=" << maneuverDist
3119 <<
" gapFactor=" << gapFactor
3120 <<
" stayInLane=" << stayInLane <<
"\n"
3121 <<
" stayInEdge: surplusGapRight=" << surplusGapRight <<
" surplusGapLeft=" << surplusGapLeft <<
"\n";
3125 if (surplusGapLeft < 0 || surplusGapRight < 0) {
3135 if (laneOffset != 0) {
3138 updateGaps(neighLeaders, neighRight, oldCenter, gapFactor, surplusGapRight, surplusGapLeft,
true);
3139 updateGaps(neighFollowers, neighRight, oldCenter, gapFactor, surplusGapRight, surplusGapLeft,
true, netOverlap);
3141#ifdef DEBUG_KEEP_LATGAP
3143 std::cout <<
" minGapLat: surplusGapRight=" << surplusGapRight <<
" surplusGapLeft=" << surplusGapLeft <<
"\n"
3153 if (stayInLane || laneOffset == 1) {
3156 surplusGapRight =
MIN2(surplusGapRight,
MAX2(0.0, halfLaneWidth + posLat - halfWidth));
3157 physicalGapRight =
MIN2(physicalGapRight,
MAX2(0.0, halfLaneWidth + posLat - halfWidth));
3159 if (stayInLane || laneOffset == -1) {
3162 surplusGapLeft =
MIN2(surplusGapLeft,
MAX2(0.0, halfLaneWidth - posLat - halfWidth));
3163 physicalGapLeft =
MIN2(physicalGapLeft,
MAX2(0.0, halfLaneWidth - posLat - halfWidth));
3165#ifdef DEBUG_KEEP_LATGAP
3167 std::cout <<
" stayInLane: surplusGapRight=" << surplusGapRight <<
" surplusGapLeft=" << surplusGapLeft <<
"\n";
3171 if (surplusGapRight + surplusGapLeft < 0) {
3176 const double equalDeficit = 0.5 * (surplusGapLeft + surplusGapRight);
3177 if (surplusGapRight < surplusGapLeft) {
3179 const double delta =
MIN2(equalDeficit - surplusGapRight, physicalGapLeft);
3181 maneuverDist = delta;
3182#ifdef DEBUG_KEEP_LATGAP
3184 std::cout <<
" insufficient latSpace, move left: delta=" << delta <<
"\n";
3189 const double delta =
MIN2(equalDeficit - surplusGapLeft, physicalGapRight);
3191 maneuverDist = -delta;
3192#ifdef DEBUG_KEEP_LATGAP
3194 std::cout <<
" insufficient latSpace, move right: delta=" << delta <<
"\n";
3200 latDist =
MAX2(
MIN2(latDist, surplusGapLeft), -surplusGapRight);
3201 maneuverDist =
MAX2(
MIN2(maneuverDist, surplusGapLeft), -surplusGapRight);
3202 if ((state &
LCA_KEEPRIGHT) != 0 && maneuverDist != oldManeuverDist) {
3204 latDist = oldLatDist;
3205 maneuverDist = oldManeuverDist;
3207#ifdef DEBUG_KEEP_LATGAP
3209 std::cout <<
" adapted latDist=" << latDist <<
" maneuverDist=" << maneuverDist <<
" (old=" << oldLatDist <<
")\n";
3219#ifdef DEBUG_KEEP_LATGAP
3221 std::cout <<
" traci influenced latDist=" << latDist <<
"\n";
3227 const bool traciChange = (state &
LCA_TRACI) != 0;
3228 if (nonSublaneChange && !traciChange) {
3230#ifdef DEBUG_KEEP_LATGAP
3232 std::cout <<
" wanted changeToLeft oldLatDist=" << oldLatDist <<
", blocked latGap changeToRight\n";
3235 latDist = oldLatDist;
3238#ifdef DEBUG_KEEP_LATGAP
3240 std::cout <<
" wanted changeToRight oldLatDist=" << oldLatDist <<
", blocked latGap changeToLeft\n";
3243 latDist = oldLatDist;
3253#ifdef DEBUG_KEEP_LATGAP
3255 std::cout <<
" latDistUpdated=" << latDist <<
" oldLatDist=" << oldLatDist <<
"\n";
3258 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset, leaders, followers, blockers, neighLeaders, neighFollowers, neighBlockers,
nullptr,
nullptr, nonSublaneChange);
3261 state = (state & ~LCA_STAY);
3272#if defined(DEBUG_KEEP_LATGAP) || defined(DEBUG_STATE)
3274 std::cout <<
" latDist2=" << latDist
3288 double& surplusGapRight,
double& surplusGapLeft,
3289 bool saveMinGap,
double netOverlap,
3291 std::vector<CLeaderDist>* collectBlockers) {
3293 const double halfWidth =
getWidth() * 0.5 + NUMERICAL_EPS;
3296 if (others[i].first != 0 && others[i].second <= 0
3298 && (netOverlap == 0 || others[i].second + others[i].first->getVehicleType().getMinGap() < netOverlap)) {
3302 double foeRight, foeLeft;
3304 const double foeCenter = foeRight + 0.5 * res;
3305 const double gap =
MIN2(fabs(foeRight - oldCenter), fabs(foeLeft - oldCenter)) - halfWidth;
3308 const double currentMinGap = desiredMinGap * gapFactor;
3319#if defined(DEBUG_BLOCKING) || defined(DEBUG_KEEP_LATGAP)
3321 std::cout <<
" updateGaps"
3323 <<
" foe=" << foe->
getID()
3324 <<
" foeRight=" << foeRight
3325 <<
" foeLeft=" << foeLeft
3326 <<
" oldCenter=" << oldCenter
3327 <<
" gap=" << others[i].second
3328 <<
" latgap=" << gap
3329 <<
" currentMinGap=" << currentMinGap
3330 <<
" surplusGapRight=" << surplusGapRight
3331 <<
" surplusGapLeft=" << surplusGapLeft
3339 if (foeCenter < oldCenter) {
3341 surplusGapRight =
MIN3(surplusGapRight, gap - currentMinGap,
MAX2(currentMinGap, gap - foeManeuverDist));
3344 surplusGapLeft =
MIN3(surplusGapLeft, gap - currentMinGap,
MAX2(currentMinGap, gap - foeManeuverDist));
3347 if (foeCenter < oldCenter) {
3348#if defined(DEBUG_BLOCKING) || defined(DEBUG_KEEP_LATGAP)
3350 std::cout <<
" new minimum rightGap=" << gap <<
"\n";
3355#if defined(DEBUG_BLOCKING) || defined(DEBUG_KEEP_LATGAP)
3357 std::cout <<
" new minimum leftGap=" << gap <<
"\n";
3363 if (collectBlockers !=
nullptr) {
3365 if ((foeCenter < oldCenter && latDist < 0 && gap < (desiredMinGap - latDist))
3366 || (foeCenter > oldCenter && latDist > 0 && gap < (desiredMinGap + latDist))) {
3367 collectBlockers->push_back(others[i]);
3384 int currentDirection =
mySpeedLat >= 0 ? 1 : -1;
3385 int directionWish = latDist >= 0 ? 1 : -1;
3392 maxSpeedLat =
MIN2(maxSpeedLat, speedBound);
3396 maxSpeedLat =
MAX2(maxSpeedLat, speedBound);
3402#ifdef DEBUG_MANEUVER
3406 <<
" computeSpeedLat()"
3407 <<
" latDist=" << latDist
3408 <<
" maneuverDist=" << maneuverDist
3409 <<
" urgent=" << urgent
3411 <<
" currentDirection=" << currentDirection
3412 <<
" directionWish=" << directionWish
3414 <<
" maxSpeedLat=" << maxSpeedLat
3420 if (directionWish == 1) {
3434 if (maneuverDist * latDist > 0) {
3435 maneuverDist = fullLatDist;
3438#ifdef DEBUG_MANEUVER
3442 <<
" fullLatDist=" << fullLatDist
3443 <<
" speedAccel=" << speedAccel
3444 <<
" speedDecel=" << speedDecel
3445 <<
" speedBound=" << speedBound
3449 if (speedDecel * speedAccel <= 0 && (
3451 (latDist >= 0 && speedAccel >= speedBound && speedBound >= speedDecel)
3452 || (latDist <= 0 && speedAccel <= speedBound && speedBound <= speedDecel))) {
3454#ifdef DEBUG_MANEUVER
3456 std::cout <<
" computeSpeedLat a)\n";
3463#ifdef DEBUG_MANEUVER
3465 std::cout <<
" computeSpeedLat b)\n";
3472 if ((fabs(minDistAccel) < fabs(fullLatDist)) || (fabs(minDistAccel - fullLatDist) < NUMERICAL_EPS)) {
3473#ifdef DEBUG_MANEUVER
3475 std::cout <<
" computeSpeedLat c)\n";
3480#ifdef DEBUG_MANEUVER
3482 std::cout <<
" minDistAccel=" << minDistAccel <<
"\n";
3487 if ((fabs(minDistCurrent) < fabs(fullLatDist)) || (fabs(minDistCurrent - fullLatDist) < NUMERICAL_EPS)) {
3488#ifdef DEBUG_MANEUVER
3490 std::cout <<
" computeSpeedLat d)\n";
3497#ifdef DEBUG_MANEUVER
3499 std::cout <<
" computeSpeedLat e)\n";
3511 double maneuverDist) {
3514 double secondsToLeaveLane;
3524#if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3540 double nextLeftSpace;
3541 if (nextActionStepSpeed > 0.) {
3556#if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3560 <<
" avoidArrivalSpeed=" << avoidArrivalSpeed
3563 <<
"\n nextLeftSpace=" << nextLeftSpace
3564 <<
" nextActionStepSpeed=" << nextActionStepSpeed
3565 <<
" nextActionStepRemainingSeconds=" << secondsToLeaveLane - timeTillActionStep
3575#if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3579 <<
" secondsToLeave=" << secondsToLeaveLane
3601 const double vehWidth =
getWidth();
3603 const double leftVehSide = rightVehSide + vehWidth;
3604 const double rightVehSideDest = rightVehSide + latDist;
3605 const double leftVehSideDest = leftVehSide + latDist;
3606#if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3608 std::cout <<
" commitFollowSpeed"
3609 <<
" latDist=" << latDist
3610 <<
" foeOffset=" << foeOffset
3611 <<
" vehRight=" << rightVehSide
3612 <<
" vehLeft=" << leftVehSide
3613 <<
" destRight=" << rightVehSideDest
3614 <<
" destLeft=" << leftVehSideDest
3620 if (vehDist.first != 0) {
3621 const MSVehicle* leader = vehDist.first;
3623 double foeRight, foeLeft;
3625#if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3627 std::cout <<
" foe=" << vehDist.first->getID()
3628 <<
" gap=" << vehDist.second
3630 <<
" foeRight=" << foeRight
3631 <<
" foeLeft=" << foeLeft
3632 <<
" overlapBefore=" <<
overlap(rightVehSide, leftVehSide, foeRight, foeLeft)
3633 <<
" overlapDest=" <<
overlap(rightVehSideDest, leftVehSideDest, foeRight, foeLeft)
3637 if (
overlap(rightVehSideDest, leftVehSideDest, foeRight, foeLeft)) {
3641 speed =
MIN2(speed, vSafe);
3642#if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3644 std::cout <<
" case1 vsafe=" << vSafe <<
" speed=" << speed <<
"\n";
3647 }
else if (
overlap(rightVehSide, leftVehSide, foeRight, foeLeft)) {
3652 speed =
MIN2(speed, vSafe);
3653#if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3655 std::cout <<
" case2 vsafe=" << vSafe <<
" speed=" << speed <<
"\n";
3675 return myOppositeParam <= 0 ? std::numeric_limits<double>::max() : 1 /
myOppositeParam;
3732 }
else if (key ==
"speedGainProbabilityRight") {
3734 }
else if (key ==
"speedGainProbabilityLeft") {
3736 }
else if (key ==
"keepRightProbability") {
3738 }
else if (key ==
"lookAheadSpeed") {
3740 }
else if (key ==
"sigmaState") {
3743 }
else if (key ==
"speedGainRP") {
3745 }
else if (key ==
"speedGainLP") {
3747 }
else if (key ==
"keepRightP") {
3815 }
else if (key ==
"speedGainProbabilityRight") {
3817 }
else if (key ==
"speedGainProbabilityLeft") {
3819 }
else if (key ==
"keepRightProbability") {
3821 }
else if (key ==
"lookAheadSpeed") {
3823 }
else if (key ==
"sigmaState") {
3837 const std::pair<MSVehicle*, double>& leader,
3838 const std::pair<MSVehicle*, double>& follower,
3839 const std::pair<MSVehicle*, double>& neighLead,
3840 const std::pair<MSVehicle*, double>& neighFollow,
3842 const std::vector<MSVehicle::LaneQ>& preb,
3848#ifdef DEBUG_WANTSCHANGE
3850 std::cout <<
"\nWANTS_CHANGE\n" <<
SIMTIME
3854 <<
" neigh=" << neighLane.
getID()
3858 <<
" considerChangeTo=" << (laneOffset == -1 ?
"right" :
"left")
3872 double maneuverDist;
3875 leaders, followers, blockers,
3876 neighLeaders, neighFollowers, neighBlockers,
3878 lastBlocked, firstBlocked, latDist, maneuverDist, blocked);
3882 result &= ~LCA_SUBLANE;
3883 result |=
getLCA(result, latDist);
3885#if defined(DEBUG_WANTSCHANGE) || defined(DEBUG_STATE)
3890 <<
" wantsChangeTo=" << (laneOffset == -1 ?
"right" :
"left")
3891 << ((result &
LCA_URGENT) ?
" (urgent)" :
"")
3897 << ((result &
LCA_TRACI) ?
" (traci)" :
"")
3949 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.
@ 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
static const double NO_NEIGHBOR
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 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.
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 ...
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].
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 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
std::vector< double > myLCAccelerationAdvices
vector of LC-related acceleration recommendations Filled in wantsChange() and applied in patchSpeed()
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
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) override
Called to examine whether the vehicle wants to change using the given laneOffset (this is a wrapper a...
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...
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)
bool isActive() const
Returns whether the current simulation step is an action point for the vehicle.
const std::pair< double, LinkDirection > & 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.