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
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),
187 const std::vector<MSVehicle::LaneQ>& preb,
190 double& latDist,
double& maneuverDist,
int& blocked) {
193 const std::string changeType = laneOffset == -1 ?
"right" : (laneOffset == 1 ?
"left" :
"current");
195 #ifdef DEBUG_MANEUVER
201 <<
" neigh=" << neighLane.
getID()
206 <<
" considerChangeTo=" << changeType
213 leaders, followers, blockers,
214 neighLeaders, neighFollowers, neighBlockers,
216 lastBlocked, firstBlocked, latDist, maneuverDist, blocked);
218 result =
keepLatGap(result, leaders, followers, blockers,
219 neighLeaders, neighFollowers, neighBlockers,
220 neighLane, laneOffset, latDist, maneuverDist, blocked);
222 result |=
getLCA(result, latDist);
224 #if defined(DEBUG_MANEUVER) || defined(DEBUG_STATE)
225 double latDistTmp = latDist;
228 #if defined(DEBUG_MANEUVER) || defined(DEBUG_STATE)
230 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" maneuverDist=" << maneuverDist <<
" latDist=" << latDistTmp <<
" mySpeedPrev=" <<
mySpeedLat <<
" speedLat=" <<
DIST2SPEED(latDist) <<
" latDist2=" << latDist <<
"\n";
237 <<
" wantsChangeTo=" << changeType
238 <<
" latDist=" << latDist
239 <<
" maneuverDist=" << maneuverDist
247 <<
" wantsNoChangeTo=" << changeType
304 const double newSpeed =
_patchSpeed(
MAX2(min, 0.0), wanted, max, cfModel);
305 #ifdef DEBUG_PATCHSPEED
307 const std::string patched = (wanted != newSpeed ?
" patched=" +
toString(newSpeed) :
"");
314 <<
" wanted=" << wanted
333 double nVSafe = wanted;
339 #ifdef DEBUG_PATCHSPEED
349 #ifdef DEBUG_PATCHSPEED
351 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" slowing down for leading blocker, safe=" << safe << (safe + NUMERICAL_EPS < min ?
" (not enough)" :
"") <<
"\n";
354 nVSafe =
MAX2(min, safe);
362 if (v >= min && v <= max) {
363 nVSafe =
MIN2(v * coopWeight + (1 - coopWeight) * wanted, nVSafe);
365 #ifdef DEBUG_PATCHSPEED
367 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" got accel=" << (*i) <<
" nVSafe=" << nVSafe <<
"\n";
371 #ifdef DEBUG_PATCHSPEED
374 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" ignoring low nVSafe=" << v <<
" min=" << min <<
"\n";
378 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" ignoring high nVSafe=" << v <<
" max=" << max <<
"\n";
386 #ifdef DEBUG_PATCHSPEED
399 #if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
404 return (max + wanted) / 2.0;
408 #if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
413 return (min + wanted) / 2.0;
416 #if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
421 return (max + wanted) / 2.0;
462 #if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
467 return (max + wanted) / 2.0;
471 #if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
491 if (pinfo->first >= 0) {
500 <<
" informedBy=" << sender->
getID()
501 <<
" info=" << pinfo->second
502 <<
" vSafe=" << pinfo->first
515 assert(cld.first != 0);
524 double remainingSeconds) {
530 plannedSpeed =
MIN2(plannedSpeed, v);
535 std::cout <<
" informLeader speed=" <<
myVehicle.
getSpeed() <<
" planned=" << plannedSpeed <<
"\n";
542 if (
gDebugFlag2) std::cout <<
" blocked by leader nv=" << nv->
getID() <<
" nvSpeed=" << nv->
getSpeed() <<
" needGap="
546 const double dv = plannedSpeed - nv->
getSpeed();
547 const double overtakeDist = (neighLead.second
553 if (dv < NUMERICAL_EPS
559 || dv * remainingSeconds < overtakeDist) {
574 <<
" cannot overtake leader nv=" << nv->
getID()
576 <<
" remainingSeconds=" << remainingSeconds
577 <<
" targetSpeed=" << targetSpeed
578 <<
" nextSpeed=" << nextSpeed
589 <<
" cannot overtake fast leader nv=" << nv->
getID()
591 <<
" remainingSeconds=" << remainingSeconds
592 <<
" targetSpeed=" << targetSpeed
603 <<
" wants to overtake leader nv=" << nv->
getID()
605 <<
" remainingSeconds=" << remainingSeconds
606 <<
" currentGap=" << neighLead.second
608 <<
" overtakeDist=" << overtakeDist
618 }
else if (neighLead.first != 0) {
621 double dv, nextNVSpeed;
641 std::cout <<
" not blocked by leader nv=" << nv->
getID()
643 <<
" gap=" << neighLead.second
644 <<
" nextGap=" << neighLead.second - dv
646 <<
" targetSpeed=" << targetSpeed
650 return MIN2(targetSpeed, plannedSpeed);
662 double remainingSeconds,
663 double plannedSpeed) {
667 if (
gDebugFlag2) std::cout <<
" blocked by follower nv=" << nv->
getID() <<
" nvSpeed=" << nv->
getSpeed() <<
" needGap="
674 if ((neededGap - neighFollow.second) / remainingSeconds < (plannedSpeed - nv->
getSpeed())) {
677 std::cout <<
" wants to cut in before nv=" << nv->
getID() <<
" without any help neededGap=" << neededGap <<
"\n";
696 const double neighNewSpeed1s =
MAX2(0., nv->
getSpeed() - helpDecel);
697 const double dv = plannedSpeed - neighNewSpeed1s;
699 const double decelGap = neighFollow.second + dv;
705 <<
" egoNV=" << plannedSpeed
706 <<
" nvNewSpeed=" << neighNewSpeed
707 <<
" nvNewSpeed1s=" << neighNewSpeed1s
708 <<
" deltaGap=" << dv
709 <<
" decelGap=" << decelGap
710 <<
" secGap=" << secureGap
714 if (decelGap > 0 && decelGap >= secureGap) {
729 std::cout <<
" wants to cut in before nv=" << nv->
getID()
730 <<
" vsafe1=" << vsafe1
731 <<
" vsafe=" << vsafe
736 }
else if (dv > 0 && dv * remainingSeconds > (secureGap - decelGap + POSITION_EPS)) {
741 std::cout <<
" wants to cut in before nv=" << nv->
getID() <<
" (eventually)\n";
749 std::cout <<
" wants to cut in before nv=" << nv->
getID() <<
" (nv cannot overtake right)\n";
765 std::cout <<
" wants right follower to slow down a bit\n";
771 std::cout <<
" wants to cut in before right follower nv=" << nv->
getID() <<
" (eventually)\n";
780 const double overtakeDist = (neighFollow.second
786 const double needDV = overtakeDist / remainingSeconds;
794 <<
" wants to be overtaken by=" << nv->
getID()
795 <<
" overtakeDist=" << overtakeDist
797 <<
" vhelp=" << vhelp
798 <<
" needDV=" << needDV
804 }
else if (neighFollow.first != 0) {
814 std::cout <<
" wants to cut in before non-blocking follower nv=" << nv->
getID() <<
"\n";
822 const std::vector<CLeaderDist>& blockers,
823 double remainingSeconds) {
835 plannedSpeed =
MIN2(plannedSpeed, safe);
837 for (std::vector<CLeaderDist>::const_iterator it = blockers.begin(); it != blockers.end(); ++it) {
838 plannedSpeed =
MIN2(plannedSpeed,
informLeader(blocked, dir, *it, remainingSeconds));
846 const std::vector<CLeaderDist>& blockers,
847 double remainingSeconds,
848 double plannedSpeed) {
850 for (std::vector<CLeaderDist>::const_iterator it = blockers.begin(); it != blockers.end(); ++it) {
851 informFollower(blocked, dir, *it, remainingSeconds, plannedSpeed);
876 const double halfWidth =
getWidth() * 0.5;
886 std::vector<double> newExpectedSpeeds;
895 const std::vector<MSLane*>& lanes = currEdge->
getLanes();
896 for (std::vector<MSLane*>::const_iterator it_lane = lanes.begin(); it_lane != lanes.end(); ++it_lane) {
898 for (
int i = 0; i < subLanes; ++i) {
899 newExpectedSpeeds.push_back((*it_lane)->getVehicleMaxSpeed(&
myVehicle));
905 for (
int i = 0; i < subLanes; ++i) {
906 newExpectedSpeeds.push_back(lanes.back()->getVehicleMaxSpeed(&
myVehicle));
914 if (subLaneShift < std::numeric_limits<int>::max()) {
916 const int newI = i + subLaneShift;
917 if (newI > 0 && newI < (
int)newExpectedSpeeds.size()) {
953 for (
const MSLink*
const link : lane->getLinkCont()) {
954 if (&link->getLane()->getEdge() == curEdge) {
956 const MSLane* target = link->getLane();
957 const std::vector<MSLane*>& lanes2 = curEdge->
getLanes();
958 for (std::vector<MSLane*>::const_iterator it_lane2 = lanes2.begin(); it_lane2 != lanes2.end(); ++it_lane2) {
959 const MSLane* lane2 = *it_lane2;
960 if (lane2 == target) {
961 return prevShift + curShift;
972 return std::numeric_limits<int>::max();
1006 #if defined(DEBUG_MANEUVER) || defined(DEBUG_STATE)
1025 const std::vector<MSVehicle::LaneQ>& preb,
1028 double& latDist,
double& maneuverDist,
int& blocked) {
1033 int bestLaneOffset = 0;
1034 double currentDist = 0;
1035 double neighDist = 0;
1047 const int prebOffset = (checkOpposite ? 0 : laneOffset);
1048 for (
int p = 0; p < (int) preb.size(); ++p) {
1049 if (preb[p].lane == prebLane && p + laneOffset >= 0) {
1050 assert(p + prebOffset < (
int)preb.size());
1052 neigh = preb[p + prebOffset];
1053 currentDist = curr.
length;
1054 neighDist = neigh.
length;
1057 if (bestLaneOffset == 0 && preb[p + prebOffset].bestLaneOffset == 0 && !checkOpposite) {
1058 #ifdef DEBUG_WANTSCHANGE
1062 <<
" bestLaneOffsetOld=" << bestLaneOffset
1063 <<
" bestLaneOffsetNew=" << laneOffset
1067 bestLaneOffset = prebOffset;
1069 best = preb[p + bestLaneOffset];
1073 assert(curr.
lane !=
nullptr);
1074 assert(neigh.
lane !=
nullptr);
1075 assert(best.
lane !=
nullptr);
1076 double driveToNextStop = -std::numeric_limits<double>::max();
1085 #ifdef DEBUG_WANTS_CHANGE
1090 <<
" stopPos=" << stopPos
1091 <<
" currentDist=" << currentDist
1092 <<
" neighDist=" << neighDist
1096 currentDist =
MAX2(currentDist, stopPos);
1097 neighDist =
MAX2(neighDist, stopPos);
1100 const bool right = (laneOffset == -1);
1101 const bool left = (laneOffset == 1);
1104 const bool changeToBest = (right && bestLaneOffset < 0) || (left && bestLaneOffset > 0) || (laneOffset == 0 && bestLaneOffset == 0);
1130 #ifdef DEBUG_WANTSCHANGE
1137 <<
"\n leaders=" << leaders.
toString()
1138 <<
"\n followers=" << followers.
toString()
1139 <<
"\n blockers=" << blockers.
toString()
1140 <<
"\n neighLeaders=" << neighLeaders.
toString()
1141 <<
"\n neighFollowers=" << neighFollowers.
toString()
1142 <<
"\n neighBlockers=" << neighBlockers.
toString()
1143 <<
"\n changeToBest=" << changeToBest
1144 <<
" latLaneDist=" << latLaneDist
1152 if (lastBlocked != firstBlocked) {
1203 for (
int i = 0; i < neighLeaders.
numSublanes(); ++i) {
1205 if (vehDist.first !=
nullptr && vehDist.first->isStopped()) {
1223 currentDist += roundaboutBonus;
1224 neighDist += roundaboutBonus;
1226 if (laneOffset != 0) {
1243 if ((ret &
LCA_STAY) != 0 && latDist == 0) {
1256 if (changeToBest && abs(bestLaneOffset) > 1
1262 #ifdef DEBUG_WANTSCHANGE
1264 std::cout <<
" reserving space for unseen blockers myLeadingBlockerLength=" <<
myLeadingBlockerLength <<
"\n";
1273 if (*firstBlocked != neighLeadLongest) {
1276 std::vector<CLeaderDist> collectLeadBlockers;
1277 std::vector<CLeaderDist> collectFollowBlockers;
1278 int blockedFully = 0;
1279 maneuverDist = latDist;
1281 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1282 leaders, followers, blockers,
1283 neighLeaders, neighFollowers, neighBlockers, &collectLeadBlockers, &collectFollowBlockers,
1284 false, gapFactor, &blockedFully);
1286 const double absLaneOffset = fabs(bestLaneOffset != 0 ? bestLaneOffset : latDist /
SUMO_const_laneWidth);
1287 const double remainingSeconds = ((ret &
LCA_TRACI) == 0 ?
1290 const double plannedSpeed =
informLeaders(blocked, myLca, collectLeadBlockers, remainingSeconds);
1292 if (plannedSpeed >= 0) {
1294 informFollowers(blocked, myLca, collectFollowBlockers, remainingSeconds, plannedSpeed);
1296 if (plannedSpeed > 0) {
1297 commitManoeuvre(blocked, blockedFully, leaders, neighLeaders, neighLane, maneuverDist);
1299 #if defined(DEBUG_WANTSCHANGE) || defined(DEBUG_STATE)
1306 <<
" remainingSeconds=" << remainingSeconds
1307 <<
" plannedSpeed=" << plannedSpeed
1318 if (roundaboutBonus > 0) {
1320 #ifdef DEBUG_WANTS_CHANGE
1324 <<
" roundaboutBonus=" << roundaboutBonus
1335 latDist = latLaneDist;
1336 maneuverDist = latLaneDist;
1337 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1338 leaders, followers, blockers,
1339 neighLeaders, neighFollowers, neighBlockers);
1359 const double inconvenience = (latLaneDist < 0
1373 && (changeToBest ||
currentDistAllows(neighDist, abs(bestLaneOffset) + 1, laDist))) {
1376 #ifdef DEBUG_COOPERATE
1383 <<
" wantsChangeToHelp=" << (right ?
"right" :
"left")
1393 maneuverDist = latDist;
1394 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1395 leaders, followers, blockers,
1396 neighLeaders, neighFollowers, neighBlockers);
1422 const double vehWidth =
getWidth();
1424 const double leftVehSide = rightVehSide + vehWidth;
1426 double defaultNextSpeed = std::numeric_limits<double>::max();
1428 int leftmostOnEdge = (int)sublaneSides.size() - 1;
1429 while (leftmostOnEdge > 0 && sublaneSides[leftmostOnEdge] > leftVehSide) {
1432 int rightmostOnEdge = leftmostOnEdge;
1433 while (rightmostOnEdge > 0 && sublaneSides[rightmostOnEdge] > rightVehSide + NUMERICAL_EPS) {
1435 #ifdef DEBUG_WANTSCHANGE
1437 std::cout <<
" adapted to current sublane=" << rightmostOnEdge <<
" defaultNextSpeed=" << defaultNextSpeed <<
"\n";
1438 std::cout <<
" sublaneSides[rightmostOnEdge]=" << sublaneSides[rightmostOnEdge] <<
" rightVehSide=" << rightVehSide <<
"\n";
1444 #ifdef DEBUG_WANTSCHANGE
1446 std::cout <<
" adapted to current sublane=" << rightmostOnEdge <<
" defaultNextSpeed=" << defaultNextSpeed <<
"\n";
1447 std::cout <<
" sublaneSides[rightmostOnEdge]=" << sublaneSides[rightmostOnEdge] <<
" rightVehSide=" << rightVehSide <<
"\n";
1450 double maxGain = -std::numeric_limits<double>::max();
1451 double maxGainRight = -std::numeric_limits<double>::max();
1452 double maxGainLeft = -std::numeric_limits<double>::max();
1453 double latDistNice = std::numeric_limits<double>::max();
1456 double leftMax =
MAX2(
1463 assert(leftMax <= edge.
getWidth());
1465 int sublaneCompact =
MAX2(iMin, rightmostOnEdge - 1);
1471 const double maxLatDist = leftMax - leftVehSide;
1472 const double minLatDist = rightMin - rightVehSide;
1473 const int iStart = laneOffset == 0 ? iMin : 0;
1474 const double rightEnd = laneOffset == 0 ? leftMax : (checkOpposite ?
getLeftBorder() : edge.
getWidth());
1475 #ifdef DEBUG_WANTSCHANGE
1477 <<
" checking sublanes rightmostOnEdge=" << rightmostOnEdge
1478 <<
" rightEnd=" << rightEnd
1479 <<
" leftmostOnEdge=" << leftmostOnEdge
1480 <<
" iStart=" << iStart
1482 <<
" sublaneSides=" << sublaneSides.size()
1483 <<
" leftMax=" << leftMax
1484 <<
" minLatDist=" << minLatDist
1485 <<
" maxLatDist=" << maxLatDist
1486 <<
" sublaneCompact=" << sublaneCompact
1489 for (
int i = iStart; i < (int)sublaneSides.size(); ++i) {
1490 if (sublaneSides[i] + vehWidth < rightEnd) {
1496 while (vMin > 0 && j < (
int)sublaneSides.size() && sublaneSides[j] < sublaneSides[i] + vehWidth) {
1502 if (laneOffset != 0 &&
overlap(sublaneSides[i], sublaneSides[i] + vehWidth, laneBoundary, laneBoundary)) {
1506 const double currentLatDist =
MIN2(
MAX2(sublaneSides[i] - rightVehSide, minLatDist), maxLatDist);
1508 if (relativeGain > maxGain) {
1509 maxGain = relativeGain;
1512 latDist = currentLatDist;
1513 #ifdef DEBUG_WANTSCHANGE
1515 std::cout <<
" i=" << i <<
" newLatDist=" << latDist <<
" relGain=" << relativeGain <<
"\n";
1521 if (currentLatDist > 0
1525 && maxGain - relativeGain < NUMERICAL_EPS) {
1526 latDist = currentLatDist;
1529 #ifdef DEBUG_WANTSCHANGE
1531 std::cout <<
" i=" << i <<
" rightmostOnEdge=" << rightmostOnEdge <<
" vMin=" << vMin <<
" relGain=" << relativeGain <<
" sublaneCompact=" << sublaneCompact <<
" curLatDist=" << currentLatDist <<
"\n";
1535 maxGainRight =
MAX2(maxGainRight, relativeGain);
1537 maxGainLeft =
MAX2(maxGainLeft, relativeGain);
1539 const double subAlignDist = sublaneSides[i] - rightVehSide;
1540 if (fabs(subAlignDist) < fabs(latDistNice)) {
1541 latDistNice = subAlignDist;
1542 #ifdef DEBUG_WANTSCHANGE
1544 <<
" nicest sublane=" << i
1545 <<
" side=" << sublaneSides[i]
1546 <<
" rightSide=" << rightVehSide
1547 <<
" latDistNice=" << latDistNice
1548 <<
" maxGainR=" << maxGainRight
1549 <<
" maxGainL=" << maxGainLeft
1556 if (maxGainRight != -std::numeric_limits<double>::max()) {
1557 #ifdef DEBUG_WANTSCHANGE
1563 #ifdef DEBUG_WANTSCHANGE
1569 if (maxGainLeft != -std::numeric_limits<double>::max()) {
1570 #ifdef DEBUG_WANTSCHANGE
1576 #ifdef DEBUG_WANTSCHANGE
1583 if ((fabs(maxGainRight) < NUMERICAL_EPS || maxGainRight == -std::numeric_limits<double>::max())
1584 && (right || (alternatives &
LCA_RIGHT) == 0)) {
1587 if ((fabs(maxGainLeft) < NUMERICAL_EPS || maxGainLeft == -std::numeric_limits<double>::max())
1588 && (left || (alternatives &
LCA_LEFT) == 0)) {
1593 #ifdef DEBUG_WANTSCHANGE
1596 <<
" defaultNextSpeed=" << defaultNextSpeed
1597 <<
" maxGain=" << maxGain
1598 <<
" maxGainRight=" << maxGainRight
1599 <<
" maxGainLeft=" << maxGainLeft
1600 <<
" latDist=" << latDist
1601 <<
" latDistNice=" << latDistNice
1602 <<
" sublaneCompact=" << sublaneCompact
1615 double acceptanceTime;
1624 double minFactor = 1.0;
1625 for (
int i = 0; i < followers.
numSublanes(); ++i) {
1627 if (follower.first !=
nullptr && follower.second < 2 * follower.first->getCarFollowModel().brakeGap(follower.first->getSpeed())) {
1630 const double fRSF = follower.first->getLane()->getVehicleMaxSpeed(follower.first) / follower.first->getLane()->getSpeedLimit();
1631 if (fRSF > roadSpeedFactor) {
1634 if (factor < minFactor) {
1640 acceptanceTime *= minFactor;
1644 double fullSpeedDrivingSeconds =
MIN2(acceptanceTime, fullSpeedGap / vMax);
1646 if (neighLead.first != 0 && neighLead.first->getSpeed() < vMax) {
1647 fullSpeedGap =
MAX2(0.,
MIN2(fullSpeedGap,
1649 vMax, neighLead.first->
getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel())));
1650 fullSpeedDrivingSeconds =
MIN2(fullSpeedDrivingSeconds, fullSpeedGap / (vMax - neighLead.first->getSpeed()));
1659 #ifdef DEBUG_WANTSCHANGE
1662 <<
" considering keepRight:"
1664 <<
" neighDist=" << neighDist
1666 <<
" leaderSpeed=" << (neighLead.first == 0 ? -1 : neighLead.first->getSpeed())
1668 myVehicle.
getSpeed(), neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()))
1669 <<
" acceptanceTime=" << acceptanceTime
1670 <<
" fullSpeedGap=" << fullSpeedGap
1671 <<
" fullSpeedDrivingSeconds=" << fullSpeedDrivingSeconds
1672 <<
" dProb=" << deltaProb
1673 <<
" isSlide=" << isSlide
1684 latDist = latLaneDist;
1685 maneuverDist = latLaneDist;
1686 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1687 leaders, followers, blockers,
1688 neighLeaders, neighFollowers, neighBlockers);
1696 #ifdef DEBUG_WANTSCHANGE
1701 <<
" neighDist=" << neighDist
1704 <<
" latDist=" << latDist
1713 int blockedFully = 0;
1714 maneuverDist = latDist;
1715 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1716 leaders, followers, blockers,
1717 neighLeaders, neighFollowers, neighBlockers,
1718 nullptr,
nullptr,
false, 0, &blockedFully);
1731 #ifdef DEBUG_WANTSCHANGE
1736 <<
" latDist=" << latDist
1737 <<
" neighDist=" << neighDist
1740 <<
" stayInLane=" << stayInLane
1751 int blockedFully = 0;
1752 maneuverDist = latDist;
1753 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1754 leaders, followers, blockers,
1755 neighLeaders, neighFollowers, neighBlockers,
1756 nullptr,
nullptr,
false, 0, &blockedFully);
1766 double latDistSublane = 0.;
1768 const double halfVehWidth =
getWidth() * 0.5;
1771 && bestLaneOffset == 0
1791 #ifdef DEBUG_WANTSCHANGE
1805 switch (turnInfo.second) {
1825 latDistSublane = -halfLaneWidth + halfVehWidth -
getPosLat();
1828 latDistSublane = halfLaneWidth - halfVehWidth -
getPosLat();
1835 latDistSublane = latDistNice;
1838 latDistSublane = sublaneSides[sublaneCompact] - rightVehSide;
1856 latDistSublane * latDist > 0) {
1858 #if defined(DEBUG_WANTSCHANGE) || defined(DEBUG_STATE) || defined(DEBUG_MANEUVER)
1863 <<
" latDist=" << latDist
1864 <<
" latDistSublane=" << latDistSublane
1865 <<
" relGainSublane=" <<
computeSpeedGain(latDistSublane, defaultNextSpeed)
1866 <<
" maneuverDist=" << maneuverDist
1878 #if defined(DEBUG_WANTSCHANGE)
1880 <<
" speedGain=" <<
computeSpeedGain(latDistSublane, defaultNextSpeed) <<
")\n";
1888 #if defined(DEBUG_WANTSCHANGE)
1890 std::cout <<
" aborting sublane change due to prior maneuver\n";
1895 latDist = latDistSublane * (
isOpposite() ? -1 : 1);
1900 #ifdef DEBUG_WANTSCHANGE
1903 <<
" latDist=" << latDist
1911 #ifdef DEBUG_WANTSCHANGE
1917 maneuverDist = latDist;
1918 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1919 leaders, followers, blockers,
1920 neighLeaders, neighFollowers, neighBlockers);
1949 #ifdef DEBUG_WANTSCHANGE
1966 if ((*blocked) !=
nullptr) {
1968 #ifdef DEBUG_SLOWDOWN
1977 if (gap > POSITION_EPS) {
1991 (gap - POSITION_EPS), (*blocked)->getSpeed(),
1992 (*blocked)->getCarFollowModel().getMaxDecel()));
2003 #ifdef DEBUG_SAVE_BLOCKER_LENGTH
2019 #ifdef DEBUG_SAVE_BLOCKER_LENGTH
2027 #ifdef DEBUG_SAVE_BLOCKER_LENGTH
2044 <<
"vSafe=" << vSafe <<
" -> accel=" << accel <<
"\n";
2056 assert(preb.size() == lanes.size() ||
isOpposite());
2057 #ifdef DEBUG_EXPECTED_SLSPEED
2060 <<
" sublaneOffset=" << sublaneOffset <<
" laneIndex=" << laneIndex <<
" lane=" << lane->
getID() <<
" ahead=" << ahead.
toString() <<
"\n";
2064 for (
int sublane = 0; sublane < (int)ahead.
numSublanes(); ++sublane) {
2065 const int edgeSublane = sublane + sublaneOffset;
2073 const MSVehicle* leader = ahead[sublane].first;
2074 const double gap = ahead[sublane].second;
2076 if (leader ==
nullptr) {
2081 const int prebIndex =
isOpposite() ? (int)preb.size() - 1 : laneIndex;
2092 #ifdef DEBUG_EXPECTED_SLSPEED
2094 std::cout <<
" updateExpectedSublaneSpeeds edgeSublane=" << edgeSublane <<
" leader=" << leader->
getID() <<
" gap=" << gap <<
" vSafe=" << vSafe <<
"\n";
2103 double foeRight, foeLeft;
2107 if (pedLeader.first != 0) {
2112 vSafe =
MIN2(vMax, vSafe);
2127 const double deltaV = vMax - vLeader;
2128 if (deltaV > 0 && gap / deltaV < mySpeedGainLookahead && mySpeedGainLookahead > 0) {
2132 const double gapClosingTime =
MAX2(0.0, gap / deltaV);
2133 const double vSafe2 = (gapClosingTime * vSafe + (foreCastTime - gapClosingTime) * vLeader) / foreCastTime;
2134 #ifdef DEBUG_EXPECTED_SLSPEED
2136 std::cout <<
" foreCastTime=" << foreCastTime <<
" gapClosingTime=" << gapClosingTime <<
" extrapolated vSafe=" << vSafe2 <<
"\n";
2147 double result = std::numeric_limits<double>::max();
2149 const double vehWidth =
getWidth();
2151 const double leftVehSide = rightVehSide + vehWidth;
2152 for (
int i = 0; i < (int)sublaneSides.size(); ++i) {
2154 if (
overlap(rightVehSide, leftVehSide, sublaneSides[i], leftSide)) {
2159 return result - defaultNextSpeed;
2166 double maxLength = -1;
2168 if (ldi[i].first != 0) {
2169 const double length = ldi[i].first->getVehicleType().getLength();
2170 if (length > maxLength) {
2183 double minSpeed = std::numeric_limits<double>::max();
2185 if (ldi[i].first != 0) {
2186 const double speed = ldi[i].first->getSpeed();
2187 if (speed < minSpeed) {
2205 std::vector<CLeaderDist>* collectLeadBlockers,
2206 std::vector<CLeaderDist>* collectFollowBlockers,
2207 bool keepLatGapManeuver,
2209 int* retBlockedFully) {
2212 latDist =
MAX2(
MIN2(latDist, maxDist), -maxDist);
2221 if (laneOffset != 0) {
2232 if (laneOffset != 0) {
2236 #ifdef DEBUG_BLOCKING
2248 }
else if (!forcedTraCIChange) {
2254 }
else if (!forcedTraCIChange) {
2260 #ifdef DEBUG_BLOCKING
2262 std::cout <<
" checkBlocking fully=" <<
myCanChangeFully <<
" latDist=" << latDist <<
" maneuverDist=" << maneuverDist <<
"\n";
2276 if (laneOffset != 0) {
2285 int blockedFully = 0;
2290 if (laneOffset != 0) {
2298 if (retBlockedFully !=
nullptr) {
2299 *retBlockedFully = blockedFully;
2306 blocked |= blockedFully;
2311 if (collectFollowBlockers !=
nullptr && collectLeadBlockers !=
nullptr) {
2313 for (std::vector<CLeaderDist>::const_iterator it2 = collectLeadBlockers->begin(); it2 != collectLeadBlockers->end(); ++it2) {
2314 for (std::vector<CLeaderDist>::iterator it = collectFollowBlockers->begin(); it != collectFollowBlockers->end();) {
2315 if ((*it2).first == (*it).first) {
2316 #ifdef DEBUG_BLOCKING
2318 std::cout <<
" removed follower " << (*it).first->getID() <<
" because it is already a leader\n";
2321 it = collectFollowBlockers->erase(it);
2335 double latDist,
double foeOffset,
bool leaders,
LaneChangeAction blockType,
2336 double& safeLatGapRight,
double& safeLatGapLeft,
2337 std::vector<CLeaderDist>* collectBlockers)
const {
2339 const double vehWidth =
getWidth();
2341 const double leftVehSide = rightVehSide + vehWidth;
2342 const double rightVehSideDest = rightVehSide + latDist;
2343 const double leftVehSideDest = leftVehSide + latDist;
2344 const double rightNoOverlap =
MIN2(rightVehSideDest, rightVehSide);
2345 const double leftNoOverlap =
MAX2(leftVehSideDest, leftVehSide);
2346 #ifdef DEBUG_BLOCKING
2348 std::cout <<
" checkBlockingVehicles"
2349 <<
" latDist=" << latDist
2350 <<
" foeOffset=" << foeOffset
2351 <<
" vehRight=" << rightVehSide
2352 <<
" vehLeft=" << leftVehSide
2353 <<
" rightNoOverlap=" << rightNoOverlap
2354 <<
" leftNoOverlap=" << leftNoOverlap
2355 <<
" destRight=" << rightVehSideDest
2356 <<
" destLeft=" << leftVehSideDest
2357 <<
" leaders=" << leaders
2363 for (
int i = 0; i < vehicles.
numSublanes(); ++i) {
2365 if (vehDist.first != 0 &&
myCFRelated.count(vehDist.first) == 0) {
2366 const MSVehicle* leader = vehDist.first;
2369 std::swap(leader, follower);
2372 double foeRight, foeLeft;
2374 const bool overlapBefore =
overlap(rightVehSide, leftVehSide, foeRight, foeLeft);
2375 const bool overlapDest =
overlap(rightVehSideDest, leftVehSideDest, foeRight, foeLeft);
2376 const bool overlapAny =
overlap(rightNoOverlap, leftNoOverlap, foeRight, foeLeft);
2377 #ifdef DEBUG_BLOCKING
2379 std::cout <<
" foe=" << vehDist.first->getID()
2380 <<
" gap=" << vehDist.second
2382 <<
" foeRight=" << foeRight
2383 <<
" foeLeft=" << foeLeft
2384 <<
" overlapBefore=" << overlapBefore
2385 <<
" overlap=" << overlapAny
2386 <<
" overlapDest=" << overlapDest
2391 if (vehDist.second < 0) {
2392 if (overlapBefore && !overlapDest) {
2393 #ifdef DEBUG_BLOCKING
2395 std::cout <<
" ignoring current overlap to come clear\n";
2399 #ifdef DEBUG_BLOCKING
2405 if (collectBlockers ==
nullptr) {
2408 collectBlockers->push_back(vehDist);
2424 const double expectedGap =
MSCFModel::gapExtrapolation(timeTillAction, vehDist.second, leader->
getSpeed(), follower->
getSpeed(), leaderAccel, followerAccel, std::numeric_limits<double>::max(), std::numeric_limits<double>::max());
2427 const double followerExpectedSpeed = follower->
getSpeed() + timeTillAction * followerAccel;
2428 const double leaderExpectedSpeed =
MAX2(0., leader->
getSpeed() + timeTillAction * leaderAccel);
2431 #if defined(DEBUG_ACTIONSTEPS) && defined(DEBUG_BLOCKING)
2433 std::cout <<
" timeTillAction=" << timeTillAction
2434 <<
" followerAccel=" << followerAccel
2435 <<
" followerExpectedSpeed=" << followerExpectedSpeed
2436 <<
" leaderAccel=" << leaderAccel
2437 <<
" leaderExpectedSpeed=" << leaderExpectedSpeed
2438 <<
"\n gap=" << vehDist.second
2439 <<
" gapChange=" << (expectedGap - vehDist.second)
2440 <<
" expectedGap=" << expectedGap
2441 <<
" expectedSecureGap=" << expectedSecureGap
2442 <<
" safeLatGapLeft=" << safeLatGapLeft
2443 <<
" safeLatGapRight=" << safeLatGapRight
2450 if (expectedGap < secureGap2) {
2452 if (foeRight > leftVehSide) {
2453 safeLatGapLeft =
MIN2(safeLatGapLeft, foeRight - leftVehSide);
2454 }
else if (foeLeft < rightVehSide) {
2455 safeLatGapRight =
MIN2(safeLatGapRight, rightVehSide - foeLeft);
2458 #ifdef DEBUG_BLOCKING
2460 std::cout <<
" blocked by " << vehDist.first->getID() <<
" gap=" << vehDist.second <<
" expectedGap=" << expectedGap
2461 <<
" expectedSecureGap=" << expectedSecureGap <<
" secGap2=" << secureGap2 <<
" safetyFactor=" <<
getSafetyFactor()
2462 <<
" safeLatGapLeft=" << safeLatGapLeft <<
" safeLatGapRight=" << safeLatGapRight
2466 result |= blockType;
2467 if (collectBlockers ==
nullptr) {
2470 #ifdef DEBUG_BLOCKING
2471 }
else if (
gDebugFlag2 && expectedGap < expectedSecureGap) {
2472 std::cout <<
" ignore blocker " << vehDist.first->getID() <<
" gap=" << vehDist.second <<
" expectedGap=" << expectedGap
2473 <<
" expectedSecureGap=" << expectedSecureGap <<
" secGap2=" << secureGap2 <<
" safetyFactor=" <<
getSafetyFactor() <<
"\n";
2476 if (collectBlockers !=
nullptr) {
2479 collectBlockers->push_back(vehDist);
2496 const double leftVehSide = rightVehSide + vehWidth;
2497 #ifdef DEBUG_BLOCKING
2499 std::cout <<
" updateCFRelated foeOffset=" << foeOffset <<
" vehicles=" << vehicles.
toString() <<
"\n";
2502 for (
int i = 0; i < vehicles.
numSublanes(); ++i) {
2504 if (vehDist.first != 0 &&
myCFRelated.count(vehDist.first) == 0) {
2505 double foeRight, foeLeft;
2507 if (
overlap(rightVehSide, leftVehSide, foeRight, foeLeft) && (vehDist.second >= 0
2513 && -vehDist.second < vehDist.first->getVehicleType().getMinGap()
2516 #ifdef DEBUG_BLOCKING
2518 std::cout <<
" ignoring cfrelated foe=" << vehDist.first->getID() <<
" gap=" << vehDist.second
2520 <<
" foeOffset=" << foeOffset
2521 <<
" egoR=" << rightVehSide <<
" egoL=" << leftVehSide
2522 <<
" iR=" << foeRight <<
" iL=" << foeLeft
2537 assert(right <= left);
2538 assert(right2 <= left2);
2539 return left2 >= right + NUMERICAL_EPS && left >= right2 + NUMERICAL_EPS;
2560 return changeReason;
2567 if (sd1.
state == 0) {
2569 }
else if (sd2.
state == 0) {
2579 #ifdef DEBUG_WANTSCHANGE
2585 <<
" dir1=" << sd1.
dir
2589 <<
" dir2=" << sd2.
dir
2597 if (reason1 < reason2) {
2599 return (!can1 && can2 && sd1.
sameDirection(sd2)) ? sd2 : sd1;
2601 }
else if (reason1 > reason2) {
2603 return (!can2 && can1 && sd1.
sameDirection(sd2)) ? sd1 : sd2;
2611 }
else if (sd2.
dir == 0) {
2616 assert(sd1.
dir == -1);
2617 assert(sd2.
dir == 1);
2620 }
else if (sd2.
latDist >= 0) {
2668 double roundaboutBonus,
2673 const bool right = (laneOffset == -1);
2674 const bool left = (laneOffset == 1);
2681 const double neighLeftPlace =
MAX2(0., neighDist - forwardPos - maxJam);
2684 #ifdef DEBUG_STRATEGIC_CHANGE
2688 <<
" forwardPos=" << forwardPos
2690 <<
" laDist=" << laDist
2691 <<
" currentDist=" << currentDist
2692 <<
" usableDist=" << usableDist
2693 <<
" bestLaneOffset=" << bestLaneOffset
2694 <<
" best.length=" << best.
length
2695 <<
" maxJam=" << maxJam
2696 <<
" neighLeftPlace=" << neighLeftPlace
2702 if (laneOffset != 0 && changeToBest && bestLaneOffset == curr.
bestLaneOffset
2705 latDist = latLaneDist;
2722 #ifdef DEBUG_STRATEGIC_CHANGE
2725 <<
" avoid overtaking on the right nv=" << nv->
getID()
2735 if (!changeToBest && (
currentDistDisallows(neighLeftPlace, abs(bestLaneOffset) + 2, laDist))) {
2742 #ifdef DEBUG_STRATEGIC_CHANGE
2744 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" could not change back and forth in time (1) neighLeftPlace=" << neighLeftPlace <<
"\n";
2750 && bestLaneOffset == 0
2753 && roundaboutBonus == 0
2760 #ifdef DEBUG_STRATEGIC_CHANGE
2762 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" does not want to leave the bestLane (neighDist=" << neighDist <<
")\n";
2767 && bestLaneOffset == 0
2773 #ifdef DEBUG_STRATEGIC_CHANGE
2775 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" does not want to get stranded on the on-ramp of a highway\n";
2789 MSLane* shadowPrev =
nullptr;
2791 if (*it ==
nullptr) {
2795 if (shadow ==
nullptr || currentShadowDist >= requiredDist) {
2798 if (shadowPrev !=
nullptr) {
2801 currentShadowDist += shadow->
getLength();
2802 shadowPrev = shadow;
2803 #ifdef DEBUG_STRATEGIC_CHANGE
2805 std::cout <<
" shadow=" << shadow->
getID() <<
" currentShadowDist=" << currentShadowDist <<
"\n";
2809 #ifdef DEBUG_STRATEGIC_CHANGE
2814 if (currentShadowDist < requiredDist && currentShadowDist < usableDist) {
2817 #ifdef DEBUG_STRATEGIC_CHANGE
2819 std::cout <<
" must change for shadowLane end latDist=" << latDist <<
" myLeftSpace=" <<
myLeftSpace <<
"\n";
2827 #if defined(DEBUG_STRATEGIC_CHANGE) || defined(DEBUG_TRACI)
2839 }
else if (((retTraCI &
LCA_RIGHT) != 0 && laneOffset < 0)
2840 || ((retTraCI &
LCA_LEFT) != 0 && laneOffset > 0)) {
2842 latDist = latLaneDist;
2845 #if defined(DEBUG_STRATEGIC_CHANGE) || defined(DEBUG_TRACI)
2847 std::cout <<
" reqAfterInfluence=" << ret <<
" ret=" << ret <<
"\n";
2871 double& maneuverDist,
2907 const bool stayInLane = laneOffset == 0 || ((state &
LCA_STRATEGIC) != 0 && (state &
LCA_STAY) != 0);
2908 const double oldLatDist = latDist;
2909 const double oldManeuverDist = maneuverDist;
2912 const double halfWidth =
getWidth() * 0.5;
2918 double surplusGapRight = oldCenter - halfWidth;
2919 double surplusGapLeft =
getLeftBorder(laneOffset != 0) - oldCenter - halfWidth;
2921 std::swap(surplusGapLeft, surplusGapRight);
2923 #ifdef DEBUG_KEEP_LATGAP
2925 std::cout <<
"\n " <<
SIMTIME <<
" keepLatGap() laneOffset=" << laneOffset
2926 <<
" latDist=" << latDist
2927 <<
" maneuverDist=" << maneuverDist
2930 <<
" gapFactor=" << gapFactor
2931 <<
" stayInLane=" << stayInLane <<
"\n"
2932 <<
" stayInEdge: surplusGapRight=" << surplusGapRight <<
" surplusGapLeft=" << surplusGapLeft <<
"\n";
2936 if (surplusGapLeft < 0 || surplusGapRight < 0) {
2946 if (laneOffset != 0) {
2949 updateGaps(neighLeaders, neighRight, oldCenter, gapFactor, surplusGapRight, surplusGapLeft,
true);
2950 updateGaps(neighFollowers, neighRight, oldCenter, gapFactor, surplusGapRight, surplusGapLeft,
true, netOverlap);
2952 #ifdef DEBUG_KEEP_LATGAP
2954 std::cout <<
" minGapLat: surplusGapRight=" << surplusGapRight <<
" surplusGapLeft=" << surplusGapLeft <<
"\n"
2964 if (stayInLane || laneOffset == 1) {
2967 surplusGapRight =
MIN2(surplusGapRight,
MAX2(0.0, halfLaneWidth + posLat - halfWidth));
2968 physicalGapRight =
MIN2(physicalGapRight,
MAX2(0.0, halfLaneWidth + posLat - halfWidth));
2970 if (stayInLane || laneOffset == -1) {
2973 surplusGapLeft =
MIN2(surplusGapLeft,
MAX2(0.0, halfLaneWidth - posLat - halfWidth));
2974 physicalGapLeft =
MIN2(physicalGapLeft,
MAX2(0.0, halfLaneWidth - posLat - halfWidth));
2976 #ifdef DEBUG_KEEP_LATGAP
2978 std::cout <<
" stayInLane: surplusGapRight=" << surplusGapRight <<
" surplusGapLeft=" << surplusGapLeft <<
"\n";
2982 if (surplusGapRight + surplusGapLeft < 0) {
2987 const double equalDeficit = 0.5 * (surplusGapLeft + surplusGapRight);
2988 if (surplusGapRight < surplusGapLeft) {
2990 const double delta =
MIN2(equalDeficit - surplusGapRight, physicalGapLeft);
2992 maneuverDist = delta;
2993 #ifdef DEBUG_KEEP_LATGAP
2995 std::cout <<
" insufficient latSpace, move left: delta=" << delta <<
"\n";
3000 const double delta =
MIN2(equalDeficit - surplusGapLeft, physicalGapRight);
3002 maneuverDist = -delta;
3003 #ifdef DEBUG_KEEP_LATGAP
3005 std::cout <<
" insufficient latSpace, move right: delta=" << delta <<
"\n";
3011 latDist =
MAX2(
MIN2(latDist, surplusGapLeft), -surplusGapRight);
3012 maneuverDist =
MAX2(
MIN2(maneuverDist, surplusGapLeft), -surplusGapRight);
3013 if ((state &
LCA_KEEPRIGHT) != 0 && maneuverDist != oldManeuverDist) {
3015 latDist = oldLatDist;
3016 maneuverDist = oldManeuverDist;
3018 #ifdef DEBUG_KEEP_LATGAP
3020 std::cout <<
" adapted latDist=" << latDist <<
" maneuverDist=" << maneuverDist <<
" (old=" << oldLatDist <<
")\n";
3030 #ifdef DEBUG_KEEP_LATGAP
3032 std::cout <<
" traci influenced latDist=" << latDist <<
"\n";
3038 const bool traciChange = (state &
LCA_TRACI) != 0;
3039 if (nonSublaneChange && !traciChange) {
3041 #ifdef DEBUG_KEEP_LATGAP
3043 std::cout <<
" wanted changeToLeft oldLatDist=" << oldLatDist <<
", blocked latGap changeToRight\n";
3046 latDist = oldLatDist;
3049 #ifdef DEBUG_KEEP_LATGAP
3051 std::cout <<
" wanted changeToRight oldLatDist=" << oldLatDist <<
", blocked latGap changeToLeft\n";
3054 latDist = oldLatDist;
3064 #ifdef DEBUG_KEEP_LATGAP
3066 std::cout <<
" latDistUpdated=" << latDist <<
" oldLatDist=" << oldLatDist <<
"\n";
3069 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset, leaders, followers, blockers, neighLeaders, neighFollowers, neighBlockers,
nullptr,
nullptr, nonSublaneChange);
3083 #if defined(DEBUG_KEEP_LATGAP) || defined(DEBUG_STATE)
3085 std::cout <<
" latDist2=" << latDist
3099 double& surplusGapRight,
double& surplusGapLeft,
3100 bool saveMinGap,
double netOverlap,
3102 std::vector<CLeaderDist>* collectBlockers) {
3104 const double halfWidth =
getWidth() * 0.5 + NUMERICAL_EPS;
3107 if (others[i].first != 0 && others[i].second <= 0
3109 && (netOverlap == 0 || others[i].second + others[i].first->getVehicleType().getMinGap() < netOverlap)) {
3113 double foeRight, foeLeft;
3115 const double foeCenter = foeRight + 0.5 * res;
3116 const double gap =
MIN2(fabs(foeRight - oldCenter), fabs(foeLeft - oldCenter)) - halfWidth;
3119 const double currentMinGap = desiredMinGap * gapFactor;
3130 #if defined(DEBUG_BLOCKING) || defined(DEBUG_KEEP_LATGAP)
3132 std::cout <<
" updateGaps"
3134 <<
" foe=" << foe->
getID()
3135 <<
" foeRight=" << foeRight
3136 <<
" foeLeft=" << foeLeft
3137 <<
" oldCenter=" << oldCenter
3138 <<
" gap=" << others[i].second
3139 <<
" latgap=" << gap
3140 <<
" currentMinGap=" << currentMinGap
3141 <<
" surplusGapRight=" << surplusGapRight
3142 <<
" surplusGapLeft=" << surplusGapLeft
3150 if (foeCenter < oldCenter) {
3152 surplusGapRight =
MIN3(surplusGapRight, gap - currentMinGap,
MAX2(currentMinGap, gap - foeManeuverDist));
3155 surplusGapLeft =
MIN3(surplusGapLeft, gap - currentMinGap,
MAX2(currentMinGap, gap - foeManeuverDist));
3158 if (foeCenter < oldCenter) {
3159 #if defined(DEBUG_BLOCKING) || defined(DEBUG_KEEP_LATGAP)
3161 std::cout <<
" new minimum rightGap=" << gap <<
"\n";
3166 #if defined(DEBUG_BLOCKING) || defined(DEBUG_KEEP_LATGAP)
3168 std::cout <<
" new minimum leftGap=" << gap <<
"\n";
3174 if (collectBlockers !=
nullptr) {
3176 if ((foeCenter < oldCenter && latDist < 0 && gap < (desiredMinGap - latDist))
3177 || (foeCenter > oldCenter && latDist > 0 && gap < (desiredMinGap + latDist))) {
3178 collectBlockers->push_back(others[i]);
3195 int currentDirection =
mySpeedLat >= 0 ? 1 : -1;
3196 int directionWish = latDist >= 0 ? 1 : -1;
3203 maxSpeedLat =
MIN2(maxSpeedLat, speedBound);
3207 maxSpeedLat =
MAX2(maxSpeedLat, speedBound);
3213 #ifdef DEBUG_MANEUVER
3217 <<
" computeSpeedLat()"
3218 <<
" latDist=" << latDist
3219 <<
" maneuverDist=" << maneuverDist
3220 <<
" urgent=" << urgent
3222 <<
" currentDirection=" << currentDirection
3223 <<
" directionWish=" << directionWish
3225 <<
" maxSpeedLat=" << maxSpeedLat
3231 if (directionWish == 1) {
3245 if (maneuverDist * latDist > 0) {
3246 maneuverDist = fullLatDist;
3249 #ifdef DEBUG_MANEUVER
3253 <<
" fullLatDist=" << fullLatDist
3254 <<
" speedAccel=" << speedAccel
3255 <<
" speedDecel=" << speedDecel
3256 <<
" speedBound=" << speedBound
3260 if (speedDecel * speedAccel <= 0 && (
3262 (latDist >= 0 && speedAccel >= speedBound && speedBound >= speedDecel)
3263 || (latDist <= 0 && speedAccel <= speedBound && speedBound <= speedDecel))) {
3265 #ifdef DEBUG_MANEUVER
3267 std::cout <<
" computeSpeedLat a)\n";
3274 #ifdef DEBUG_MANEUVER
3276 std::cout <<
" computeSpeedLat b)\n";
3283 if ((fabs(minDistAccel) < fabs(fullLatDist)) || (fabs(minDistAccel - fullLatDist) < NUMERICAL_EPS)) {
3284 #ifdef DEBUG_MANEUVER
3286 std::cout <<
" computeSpeedLat c)\n";
3291 #ifdef DEBUG_MANEUVER
3293 std::cout <<
" minDistAccel=" << minDistAccel <<
"\n";
3298 if ((fabs(minDistCurrent) < fabs(fullLatDist)) || (fabs(minDistCurrent - fullLatDist) < NUMERICAL_EPS)) {
3299 #ifdef DEBUG_MANEUVER
3301 std::cout <<
" computeSpeedLat d)\n";
3308 #ifdef DEBUG_MANEUVER
3310 std::cout <<
" computeSpeedLat e)\n";
3322 double maneuverDist) {
3325 double secondsToLeaveLane;
3335 #if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3351 double nextLeftSpace;
3352 if (nextActionStepSpeed > 0.) {
3367 #if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3371 <<
" avoidArrivalSpeed=" << avoidArrivalSpeed
3374 <<
"\n nextLeftSpace=" << nextLeftSpace
3375 <<
" nextActionStepSpeed=" << nextActionStepSpeed
3376 <<
" nextActionStepRemainingSeconds=" << secondsToLeaveLane - timeTillActionStep
3386 #if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3390 <<
" secondsToLeave=" << secondsToLeaveLane
3412 const double vehWidth =
getWidth();
3414 const double leftVehSide = rightVehSide + vehWidth;
3415 const double rightVehSideDest = rightVehSide + latDist;
3416 const double leftVehSideDest = leftVehSide + latDist;
3417 #if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3419 std::cout <<
" commitFollowSpeed"
3420 <<
" latDist=" << latDist
3421 <<
" foeOffset=" << foeOffset
3422 <<
" vehRight=" << rightVehSide
3423 <<
" vehLeft=" << leftVehSide
3424 <<
" destRight=" << rightVehSideDest
3425 <<
" destLeft=" << leftVehSideDest
3431 if (vehDist.first != 0) {
3432 const MSVehicle* leader = vehDist.first;
3434 double foeRight, foeLeft;
3436 #if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3438 std::cout <<
" foe=" << vehDist.first->getID()
3439 <<
" gap=" << vehDist.second
3441 <<
" foeRight=" << foeRight
3442 <<
" foeLeft=" << foeLeft
3443 <<
" overlapBefore=" <<
overlap(rightVehSide, leftVehSide, foeRight, foeLeft)
3444 <<
" overlapDest=" <<
overlap(rightVehSideDest, leftVehSideDest, foeRight, foeLeft)
3448 if (
overlap(rightVehSideDest, leftVehSideDest, foeRight, foeLeft)) {
3452 speed =
MIN2(speed, vSafe);
3453 #if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3455 std::cout <<
" case1 vsafe=" << vSafe <<
" speed=" << speed <<
"\n";
3458 }
else if (
overlap(rightVehSide, leftVehSide, foeRight, foeLeft)) {
3463 speed =
MIN2(speed, vSafe);
3464 #if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3466 std::cout <<
" case2 vsafe=" << vSafe <<
" speed=" << speed <<
"\n";
3486 return myOppositeParam <= 0 ? std::numeric_limits<double>::max() : 1 /
myOppositeParam;
3541 }
else if (key ==
"speedGainProbabilityRight") {
3543 }
else if (key ==
"speedGainProbabilityLeft") {
3545 }
else if (key ==
"keepRightProbability") {
3547 }
else if (key ==
"lookAheadSpeed") {
3549 }
else if (key ==
"sigmaState") {
3552 }
else if (key ==
"speedGainRP") {
3554 }
else if (key ==
"speedGainLP") {
3556 }
else if (key ==
"keepRightP") {
3633 const std::pair<MSVehicle*, double>& leader,
3634 const std::pair<MSVehicle*, double>& follower,
3635 const std::pair<MSVehicle*, double>& neighLead,
3636 const std::pair<MSVehicle*, double>& neighFollow,
3638 const std::vector<MSVehicle::LaneQ>& preb,
3644 #ifdef DEBUG_WANTSCHANGE
3646 std::cout <<
"\nWANTS_CHANGE\n" <<
SIMTIME
3650 <<
" neigh=" << neighLane.
getID()
3654 <<
" considerChangeTo=" << (laneOffset == -1 ?
"right" :
"left")
3668 double maneuverDist;
3671 leaders, followers, blockers,
3672 neighLeaders, neighFollowers, neighBlockers,
3674 lastBlocked, firstBlocked, latDist, maneuverDist, blocked);
3679 result |=
getLCA(result, latDist);
3681 #if defined(DEBUG_WANTSCHANGE) || defined(DEBUG_STATE)
3686 <<
" wantsChangeTo=" << (laneOffset == -1 ?
"right" :
"left")
3687 << ((result &
LCA_URGENT) ?
" (urgent)" :
"")
3693 << ((result &
LCA_TRACI) ?
" (traci)" :
"")
3743 #ifdef DEBUG_SLIDING
3745 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 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_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
const MSCFModel & getCarFollowModel() const
The vehicle's car following model.
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()
double myLastLateralGapRight
double myCommittedSpeed
the speed when committing to a change maneuver
MSLane * getShadowLane() const
Returns the lane the vehicle's shadow is on during continuous/sublane lane change.
static const double NO_NEIGHBOR
double myMaxDistLatStanding
int & getCanceledState(const int dir)
static bool myAllowOvertakingRight
whether overtaking on the right is permitted
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
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 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)
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)
const MSVehicleType & getVehicleType() const
Returns the vehicle's type definition.
const MSRoute & getRoute() const
Returns the current route.
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)
double stopSpeed(const MSVehicle *const veh, const double speed, double gap) const
Computes the vehicle's safe speed for approaching a non-moving obstacle (no dawdling)
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 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 double brakeGap(const double speed) const
Returns the distance the vehicle needs to halt including driver's reaction time tau (i....
virtual void setMaxDecel(double decel)
Sets a new value for maximal comfortable deceleration [m/s^2].
virtual double followSpeed(const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle *const pred=0) const =0
Computes the vehicle's follow speed (no dawdling)
double getMaxAccel() const
Get the vehicle type's maximum acceleration [m/s^2].
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....
A road/street connecting two junctions.
const std::set< MSTransportable * > & getPersons() const
Returns this edge's persons set.
const std::vector< MSLane * > & getLanes() const
Returns this edge's lanes.
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)
double getInternalFollowingLengthTo(const MSEdge *followerAfterInternal) const
returns the length of all internal edges on the junction until reaching the non-internal edge followe...
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 double getRoundaboutDistBonus(const MSVehicle &veh, double bonusParam, const MSVehicle::LaneQ &curr, const MSVehicle::LaneQ &neigh, const MSVehicle::LaneQ &best)
Computes the artificial bonus distance for roundabout lanes this additional distance reduces the sens...
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 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 checkStrategicChange(int ret, 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
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
void saveBlockerLength(const MSVehicle *blocker, int lcaCounter)
save space for vehicles which need to counter-lane-change
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 currentDistAllows(double dist, int laneOffset, double lookForwardDist)
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...
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
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
double myTurnAlignmentDist
double myLeadingBlockerLength
void setOwnState(const int state) override
void informFollowers(int blocked, int dir, const std::vector< CLeaderDist > &blockers, double remainingSeconds, double plannedSpeed)
call informFollower for multiple followers
double mySpeedGainLookahead
double _patchSpeed(const double min, const double wanted, const double max, const MSCFModel &cfModel)
double mySpeedLossProbThreshold
double mySpeedGainProbabilityLeft
a value for tracking the probability that a change to the left is beneficial
int checkBlockingVehicles(const MSVehicle *ego, const MSLeaderDistanceInfo &vehicles, double latDist, double foeOffset, bool leaders, LaneChangeAction blockType, double &safeLatGapRight, double &safeLatGapLeft, std::vector< CLeaderDist > *collectBlockers=0) const
check whether any of the vehicles overlaps with ego
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
static CLeaderDist getLongest(const MSLeaderDistanceInfo &ldi)
get the longest vehicle in the given info
bool currentDistDisallows(double dist, int laneOffset, double lookForwardDist)
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.
const std::vector< MSLink * > & getLinkCont() const
returns the container with all links !!!
std::pair< const MSPerson *, double > nextBlocking(double minPos, double minRight, double maxLeft, double stopTime=0) const
This is just a wrapper around MSPModel::nextBlocking. You should always check using hasPedestrians be...
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.
MSEdge & getEdge() const
Returns the lane's edge.
MSLane * getOpposite() const
return the neighboring opposite direction lane for lane changing or nullptr
MSLane * getParallelOpposite() const
return the opposite direction lane of this lanes edge or nullptr
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.
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
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.
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...
int getBestLaneOffset() const
double lateralDistanceToLane(const int offset) const
Get the minimal lateral distance required to move fully onto the lane at given offset.
double getLastStepDist() const
Get the distance the vehicle covered in the previous timestep.
const std::pair< double, LinkDirection > & getNextTurn()
Get the distance and direction of the next upcoming turn for the vehicle (within its look-ahead range...
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.
double getPositionOnLane() const
Get the vehicle's position along the lane.
const MSLane * getLane() const
Returns the lane the vehicle is on.
const MSCFModel & getCarFollowModel() const
Returns the vehicle's car following model definition.
double getLateralOverlap() const
return the amount by which the vehicle extends laterally outside it's primary lane
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.
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)
const LatAlignmentDefinition & getPreferredLateralAlignment() const
Get vehicle's preferred lateral alignment procedure.
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
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.