39 #ifdef CHECK_MEMORY_LEAKS
41 #endif // CHECK_MEMORY_LEAKS
49 #define LOOK_FORWARD_SPEED_DIVIDER (SUMOReal)14.
55 #define LOOK_FORWARD_RIGHT (SUMOReal)10.
56 #define LOOK_FORWARD_LEFT (SUMOReal)20.
58 #define JAM_FACTOR (SUMOReal)1.
61 #define LCA_RIGHT_IMPATIENCE (SUMOReal)-1.
62 #define CUT_IN_LEFT_SPEED_THRESHOLD (SUMOReal)27.
63 #define MAX_ONRAMP_LENGTH (SUMOReal)200.
65 #define LOOK_AHEAD_MIN_SPEED (SUMOReal)0.0
66 #define LOOK_AHEAD_SPEED_MEMORY (SUMOReal)0.9
67 #define LOOK_AHEAD_SPEED_DECREMENT 6.
69 #define HELP_DECEL_FACTOR (SUMOReal)1.0
71 #define HELP_OVERTAKE (SUMOReal)(10.0 / 3.6)
72 #define MIN_FALLBEHIND (SUMOReal)(7.0 / 3.6)
74 #define KEEP_RIGHT_HEADWAY (SUMOReal)2.0
76 #define URGENCY (SUMOReal)2.0
78 #define ROUNDABOUT_DIST_BONUS (SUMOReal)100.0
80 #define KEEP_RIGHT_TIME (SUMOReal)5.0 // the number of seconds after which a vehicle should move to the right lane
81 #define KEEP_RIGHT_ACCEPTANCE (SUMOReal)7.0 // calibration factor for determining the desire to keep right
83 #define RELGAIN_NORMALIZATION_MIN_SPEED (SUMOReal)10.0
85 #define TURN_LANE_DIST (SUMOReal)200.0 // the distance at which a lane leading elsewhere is considered to be a turn-lane that must be avoided
86 #define GAIN_PERCEPTION_THRESHOLD (SUMOReal)0.05 // the minimum relative speed gain which affects the behavior
88 #define SPEED_GAIN_MIN_SECONDS 20.0
90 #define ARRIVALPOS_LAT_THRESHOLD 100.0
94 #define DEBUG_COND (myVehicle.getID() == "disabled")
106 mySpeedGainProbabilityRight(0),
107 mySpeedGainProbabilityLeft(0),
108 myKeepRightProbability(0),
109 myLeadingBlockerLength(0),
113 myCanChangeFully(true),
123 myChangeProbThresholdRight(2.0 * myKeepRightParam /
MAX2(
NUMERICAL_EPS, mySpeedGainParam)),
125 mySpeedLossProbThreshold(-0.01 + (1 - mySublaneParam)) {
127 throw ProcessError(
"laneChangeModel 'MSLCM_SL2015' is only meant to be used when simulating with '--lateral-resoluion' > 0");
152 const std::vector<MSVehicle::LaneQ>& preb,
158 const std::string changeType = laneOffset == -1 ?
"right" : (laneOffset == 1 ?
"left" :
"current");
169 <<
" considerChangeTo=" << changeType
174 leaders, followers, blockers,
175 neighLeaders, neighFollowers, neighBlockers,
177 lastBlocked, firstBlocked, latDist, blocked);
181 assert(latDist == 0);
186 result =
keepLatGap(result, leaders, followers, blockers,
187 neighLeaders, neighFollowers, neighBlockers,
188 neighLane, laneOffset, latDist, blocked);
190 result |=
getLCA(result, latDist);
196 <<
" latDist=" << latDist
204 <<
" wantsNoChangeTo=" << changeType
220 const std::string patched = (wanted != newSpeed ?
" patched=" +
toString(newSpeed) :
"");
226 <<
" wanted=" << wanted
256 std::cout << time <<
" veh=" <<
myVehicle.
getID() <<
" slowing down for leading blocker, safe=" << safe << (safe +
NUMERICAL_EPS < min ?
" (not enough)" :
"") <<
"\n";
258 return MAX2(min, safe);
265 for (std::vector<SUMOReal>::const_iterator i =
myVSafes.begin(); i !=
myVSafes.end(); ++i) {
267 if (v >= min && v <= max) {
268 nVSafe =
MIN2(v, nVSafe);
271 std::cout << time <<
" veh=" <<
myVehicle.
getID() <<
" got nVSafe=" << nVSafe <<
"\n";
276 std::cout << time <<
" veh=" <<
myVehicle.
getID() <<
" ignoring low nVSafe=" << v <<
" min=" << min <<
"\n";
280 std::cout << time <<
" veh=" <<
myVehicle.
getID() <<
" ignoring high nVSafe=" << v <<
" max=" << max <<
"\n";
288 std::cout << time <<
" veh=" <<
myVehicle.
getID() <<
" got vSafe\n";
299 std::cout << time <<
" veh=" <<
myVehicle.
getID() <<
" LCA_WANTS_LANECHANGE (strat, no vSafe)\n";
301 return (max + wanted) / (
SUMOReal) 2.0;
306 std::cout << time <<
" veh=" <<
myVehicle.
getID() <<
" LCA_BLOCKED_BY_LEADER (coop)\n";
308 return (min + wanted) / (
SUMOReal) 2.0;
312 std::cout << time <<
" veh=" <<
myVehicle.
getID() <<
" LCA_BLOCKED_BY_FOLLOWER (coop)\n";
314 return (max + wanted) / (
SUMOReal) 2.0;
356 std::cout << time <<
" veh=" <<
myVehicle.
getID() <<
" LCA_AMBLOCKINGLEADER\n";
358 return (max + wanted) / (
SUMOReal) 2.0;
363 std::cout << time <<
" veh=" <<
myVehicle.
getID() <<
" LCA_AMBLOCKINGFOLLOWER_DONTBRAKE\n";
380 if (pinfo->first >= 0) {
388 <<
" informedBy=" << sender->
getID()
389 <<
" info=" << pinfo->second
390 <<
" vSafe=" << pinfo->first
400 assert(cld.first != 0);
412 for (std::vector<SUMOReal>::const_iterator i =
myVSafes.begin(); i !=
myVSafes.end(); ++i) {
415 plannedSpeed =
MIN2(plannedSpeed, v);
419 std::cout <<
" informLeader speed=" <<
myVehicle.
getSpeed() <<
" planned=" << plannedSpeed <<
"\n";
423 assert(neighLead.first != 0);
425 if (
gDebugFlag2) std::cout <<
" blocked by leader nv=" << nv->
getID() <<
" nvSpeed=" << nv->
getSpeed() <<
" needGap="
429 const SUMOReal overtakeDist = (neighLead.second
441 || dv * remainingSeconds < overtakeDist) {
455 <<
" cannot overtake leader nv=" << nv->
getID()
457 <<
" remainingSeconds=" << remainingSeconds
458 <<
" targetSpeed=" << targetSpeed
459 <<
" nextSpeed=" << nextSpeed
468 <<
" cannot overtake fast leader nv=" << nv->
getID()
470 <<
" remainingSeconds=" << remainingSeconds
471 <<
" targetSpeed=" << targetSpeed
480 <<
" wants to overtake leader nv=" << nv->
getID()
482 <<
" remainingSeconds=" << remainingSeconds
483 <<
" currentGap=" << neighLead.second
485 <<
" overtakeDist=" << overtakeDist
492 }
else if (neighLead.first != 0) {
501 std::cout <<
" not blocked by leader nv=" << nv->
getID()
503 <<
" gap=" << neighLead.second
504 <<
" nextGap=" << neighLead.second - dv
506 <<
" targetSpeed=" << targetSpeed
509 return MIN2(targetSpeed, plannedSpeed);
524 assert(neighFollow.first != 0);
526 if (
gDebugFlag2) std::cout <<
" blocked by follower nv=" << nv->
getID() <<
" nvSpeed=" << nv->
getSpeed() <<
" needGap="
532 if ((neededGap - neighFollow.second) / remainingSeconds < (plannedSpeed - nv->
getSpeed())) {
534 std::cout <<
" wants to cut in before nv=" << nv->
getID() <<
" without any help neededGap=" << neededGap <<
"\n";
553 const SUMOReal dv = plannedSpeed - neighNewSpeed1s;
555 const SUMOReal decelGap = neighFollow.second + dv;
560 <<
" egoNV=" << plannedSpeed
561 <<
" nvNewSpeed=" << neighNewSpeed
562 <<
" nvNewSpeed1s=" << neighNewSpeed1s
563 <<
" deltaGap=" << dv
564 <<
" decelGap=" << decelGap
565 <<
" secGap=" << secureGap
568 if (decelGap > 0 && decelGap >= secureGap) {
582 std::cout <<
" wants to cut in before nv=" << nv->
getID()
583 <<
" vsafe1=" << vsafe1
584 <<
" vsafe=" << vsafe
588 }
else if (dv > 0 && dv * remainingSeconds > (secureGap - decelGap +
POSITION_EPS)) {
592 std::cout <<
" wants to cut in before nv=" << nv->
getID() <<
" (eventually)\n";
598 std::cout <<
" wants to cut in before nv=" << nv->
getID() <<
" (nv cannot overtake right)\n";
612 std::cout <<
" wants right follower to slow down a bit\n";
616 std::cout <<
" wants to cut in before right follower nv=" << nv->
getID() <<
" (eventually)\n";
624 const SUMOReal overtakeDist = (neighFollow.second
630 const SUMOReal needDV = overtakeDist / remainingSeconds;
637 <<
" wants to be overtaken by=" << nv->
getID()
638 <<
" overtakeDist=" << overtakeDist
640 <<
" vhelp=" << vhelp
641 <<
" needDV=" << needDV
646 }
else if (neighFollow.first != 0) {
655 std::cout <<
" wants to cut in before non-blocking follower nv=" << nv->
getID() <<
"\n";
662 const std::vector<CLeaderDist>& blockers,
665 for (std::vector<CLeaderDist>::const_iterator it = blockers.begin(); it != blockers.end(); ++it) {
666 plannedSpeed =
MIN2(plannedSpeed,
informLeader(blocked, dir, *it, remainingSeconds));
674 const std::vector<CLeaderDist>& blockers,
677 for (std::vector<CLeaderDist>::const_iterator it = blockers.begin(); it != blockers.end(); ++it) {
678 informFollower(blocked, dir, *it, remainingSeconds, plannedSpeed);
700 std::vector<SUMOReal> newExpectedSpeeds;
705 const std::vector<MSLane*>& lanes = currEdge->
getLanes();
706 for (std::vector<MSLane*>::const_iterator it_lane = lanes.begin(); it_lane != lanes.end(); ++it_lane) {
708 for (
int i = 0; i < subLanes; ++i) {
709 newExpectedSpeeds.push_back((*it_lane)->getVehicleMaxSpeed(&
myVehicle));
719 const int newI = i + subLaneShift;
720 if (newI > 0 && newI < (
int)newExpectedSpeeds.size()) {
738 const std::vector<MSLane*>& lanes = prevEdge->
getLanes();
739 for (std::vector<MSLane*>::const_iterator it_lane = lanes.begin(); it_lane != lanes.end(); ++it_lane) {
740 const MSLane* lane = *it_lane;
741 for (MSLinkCont::const_iterator it_link = lane->
getLinkCont().begin(); it_link != lane->
getLinkCont().end(); ++it_link) {
742 if (&((*it_link)->getLane()->getEdge()) == curEdge) {
744 const MSLane* target = (*it_link)->getLane();
745 const std::vector<MSLane*>& lanes2 = curEdge->
getLanes();
746 for (std::vector<MSLane*>::const_iterator it_lane2 = lanes2.begin(); it_lane2 != lanes2.end(); ++it_lane2) {
747 const MSLane* lane2 = *it_lane2;
748 if (lane2 == target) {
749 return prevShift + curShift;
804 const std::vector<MSVehicle::LaneQ>& preb,
812 int bestLaneOffset = 0;
821 for (
int p = 0; p < (int) preb.size(); ++p) {
822 if (preb[p].lane == prebLane && p + laneOffset >= 0) {
823 assert(p + laneOffset < (
int)preb.size());
825 neigh = preb[p + laneOffset];
826 currentDist = curr.
length;
828 bestLaneOffset = curr.bestLaneOffset;
830 if (bestLaneOffset == 0 && preb[p + laneOffset].bestLaneOffset == 0) {
834 <<
" bestLaneOffsetOld=" << bestLaneOffset
835 <<
" bestLaneOffsetNew=" << laneOffset
838 bestLaneOffset = laneOffset;
845 const bool right = (laneOffset == -1);
846 const bool left = (laneOffset == 1);
850 const bool changeToBest = (right && bestLaneOffset < 0) || (left && bestLaneOffset > 0) || (laneOffset == 0 && bestLaneOffset == 0);
860 SUMOReal leftLimit = halfCurrentLaneWidth - halfVehWidth - latPos;
861 SUMOReal rightLimit = -halfCurrentLaneWidth + halfVehWidth - latPos;
863 if (laneOffset == -1) {
866 }
else if (laneOffset == 1) {
892 <<
" leaders=" << leaders.
toString()
893 <<
" followers=" << followers.
toString()
894 <<
" blockers=" << blockers.
toString()
895 <<
" neighLeaders=" << neighLeaders.
toString()
896 <<
" neighFollowers=" << neighFollowers.
toString()
897 <<
" neighBlockers=" << neighBlockers.
toString()
898 <<
" changeToBest=" << changeToBest
899 <<
" latLaneDist=" << latLaneDist
900 <<
" leftLimit=" << leftLimit
901 <<
" rightLimit=" << rightLimit
908 if (lastBlocked != firstBlocked) {
956 int roundaboutEdgesAhead = 0;
958 if ((*it) != 0 && (*it)->getEdge().isRoundabout()) {
959 roundaboutEdgesAhead += 1;
960 }
else if (roundaboutEdgesAhead > 0) {
965 int roundaboutEdgesAheadNeigh = 0;
967 if ((*it) != 0 && (*it)->getEdge().isRoundabout()) {
968 roundaboutEdgesAheadNeigh += 1;
969 }
else if (roundaboutEdgesAheadNeigh > 0) {
974 if (roundaboutEdgesAhead > 1) {
978 if (roundaboutEdgesAhead > 0) {
980 std::cout <<
" roundaboutEdgesAhead=" << roundaboutEdgesAhead <<
" roundaboutEdgesAheadNeigh=" << roundaboutEdgesAheadNeigh <<
"\n";
984 if (laneOffset != 0) {
997 roundaboutEdgesAhead);
1007 if (changeToBest &&
abs(bestLaneOffset) > 1) {
1010 std::cout <<
" reserving space for unseen blockers\n";
1019 if (*firstBlocked != neighLeadLongest) {
1022 latDist = latLaneDist;
1023 std::vector<CLeaderDist> collectLeadBlockers;
1024 std::vector<CLeaderDist> collectFollowBlockers;
1027 leaders, followers, blockers,
1028 neighLeaders, neighFollowers, neighBlockers, &collectLeadBlockers, &collectFollowBlockers);
1035 if (plannedSpeed >= 0) {
1037 informFollowers(blocked, myLca, collectFollowBlockers, remainingSeconds, plannedSpeed);
1044 <<
" remainingSeconds=" << remainingSeconds
1045 <<
" plannedSpeed=" << plannedSpeed
1052 if (roundaboutEdgesAhead > 1) {
1061 if ((ret & LCA_STAY) == 0) {
1062 latDist = latLaneDist;
1064 leaders, followers, blockers,
1065 neighLeaders, neighFollowers, neighBlockers);
1080 const SUMOReal inconvenience = (latLaneDist < 0
1093 <<
" wantsChangeToHelp=" << (right ?
"right" :
"left")
1095 << (((
myOwnState & myLcaCounter) != 0) ?
" (counter)" :
"")
1101 latDist = latLaneDist;
1103 leaders, followers, blockers,
1104 neighLeaders, neighFollowers, neighBlockers);
1130 const SUMOReal leftVehSide = rightVehSide + vehWidth;
1134 int leftmostOnEdge = (int)sublaneSides.size() - 1;
1135 while (leftmostOnEdge > 0 && sublaneSides[leftmostOnEdge] > leftVehSide) {
1138 int rightmostOnEdge = leftmostOnEdge;
1139 while (rightmostOnEdge > 0 && sublaneSides[rightmostOnEdge] > rightVehSide +
NUMERICAL_EPS) {
1142 std::cout <<
" adapted to current sublane=" << rightmostOnEdge <<
" defaultNextSpeed=" << defaultNextSpeed <<
"\n";
1145 std::cout <<
" sublaneSides[rightmostOnEdge]=" << sublaneSides[rightmostOnEdge] <<
" rightVehSide=" << rightVehSide <<
"\n";
1151 std::cout <<
" adapted to current sublane=" << rightmostOnEdge <<
" defaultNextSpeed=" << defaultNextSpeed <<
"\n";
1154 std::cout <<
" sublaneSides[rightmostOnEdge]=" << sublaneSides[rightmostOnEdge] <<
" rightVehSide=" << rightVehSide <<
"\n";
1165 assert(leftMax <= edge.
getWidth());
1166 int sublaneCompact =
MAX2(iMin, rightmostOnEdge - 1);
1169 <<
" checking sublanes rightmostOnEdge=" << rightmostOnEdge
1170 <<
" leftmostOnEdge=" << leftmostOnEdge
1172 <<
" leftMax=" << leftMax
1173 <<
" sublaneCompact=" << sublaneCompact
1175 for (
int i = iMin; i < (int)sublaneSides.size(); ++i) {
1176 if (sublaneSides[i] + vehWidth < leftMax) {
1182 while (vMin > 0 && j < (
int)sublaneSides.size() && sublaneSides[j] < sublaneSides[i] + vehWidth) {
1190 maxGain = relativeGain;
1193 latDist = sublaneSides[i] - rightVehSide;
1195 std::cout <<
" i=" << i <<
" newLatDist=" << latDist <<
" relGain=" << relativeGain <<
"\n";
1200 std::cout <<
" i=" << i <<
" rightmostOnEdge=" << rightmostOnEdge <<
" vMin=" << vMin <<
" relGain=" << relativeGain <<
" sublaneCompact=" << sublaneCompact <<
"\n";
1202 if (i < rightmostOnEdge) {
1203 maxGainRight =
MAX2(maxGainRight, relativeGain);
1204 }
else if (i > rightmostOnEdge) {
1205 maxGainLeft =
MAX2(maxGainLeft, relativeGain);
1207 const SUMOReal subAlignDist = sublaneSides[i] - rightVehSide;
1208 if (fabs(subAlignDist) < fabs(latDistNice)) {
1209 latDistNice = subAlignDist;
1211 <<
" nicest sublane=" << i
1212 <<
" side=" << sublaneSides[i]
1213 <<
" rightSide=" << rightVehSide
1214 <<
" latDistNice=" << latDistNice
1237 <<
" defaultNextSpeed=" << defaultNextSpeed
1238 <<
" maxGain=" << maxGain
1239 <<
" maxGainRight=" << maxGainRight
1240 <<
" maxGainLeft=" << maxGainLeft
1241 <<
" latDist=" << latDist
1242 <<
" latDistNice=" << latDistNice
1243 <<
" sublaneCompact=" << sublaneCompact
1248 if (right && maxGain >= 0 && latDist <= 0) {
1255 SUMOReal fullSpeedDrivingSeconds =
MIN2(acceptanceTime, fullSpeedGap / vMax);
1257 if (neighLead.first != 0 && neighLead.first->getSpeed() < vMax) {
1260 vMax, neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel())));
1261 fullSpeedDrivingSeconds =
MIN2(fullSpeedDrivingSeconds, fullSpeedGap / (vMax - neighLead.first->getSpeed()));
1270 <<
" considering keepRight:"
1272 <<
" neighDist=" << neighDist
1274 <<
" leaderSpeed=" << (neighLead.first == 0 ? -1 : neighLead.first->getSpeed())
1276 myVehicle.
getSpeed(), neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()))
1277 <<
" acceptanceTime=" << acceptanceTime
1278 <<
" fullSpeedGap=" << fullSpeedGap
1279 <<
" fullSpeedDrivingSeconds=" << fullSpeedDrivingSeconds
1280 <<
" dProb=" << deltaProb
1289 latDist = latLaneDist;
1291 leaders, followers, blockers,
1292 neighLeaders, neighFollowers, neighBlockers);
1301 <<
" latDist=" << latDist
1310 leaders, followers, blockers,
1311 neighLeaders, neighFollowers, neighBlockers);
1322 <<
" latDist=" << latDist
1323 <<
" stayInLane=" << stayInLane
1334 leaders, followers, blockers,
1335 neighLeaders, neighFollowers, neighBlockers);
1347 && bestLaneOffset == 0
1383 latDist = latDistNice;
1386 latDist = sublaneSides[sublaneCompact] - rightVehSide;
1396 <<
" latDist=" << latDist
1411 <<
" latDist=" << latDist
1416 leaders, followers, blockers,
1417 neighLeaders, neighFollowers, neighBlockers);
1459 if ((*blocked) != 0) {
1483 (*blocked)->getCarFollowModel().getMaxDecel()));
1523 <<
" potential=" << potential
1536 const MSLane* lane = lanes[laneIndex];
1538 assert(preb.size() == lanes.size());
1540 for (
int sublane = 0; sublane < (int)ahead.
numSublanes(); ++sublane) {
1541 const int edgeSublane = sublane + sublaneOffset;
1545 const MSVehicle* leader = ahead[sublane];
1571 if (ldi[i].first != 0) {
1572 const SUMOReal length = ldi[i].first->getVehicleType().getLength();
1573 if (length > maxLength) {
1588 if (ldi[i].first != 0) {
1589 const SUMOReal speed = ldi[i].first->getSpeed();
1590 if (speed < minSpeed) {
1608 std::vector<CLeaderDist>* collectLeadBlockers,
1609 std::vector<CLeaderDist>* collectFollowBlockers,
1610 bool keepLatGapManeuver) {
1612 if (!keepLatGapManeuver) {
1616 latDist =
MAX2(
MIN2(latDist, maxDist), -maxDist);
1621 SUMOReal surplusGapRight =
MIN2(maxDist, center - halfWidth);
1625 if (laneOffset != 0) {
1630 std::cout <<
" checkBlocking latDist=" << latDist <<
" surplusGapRight=" << surplusGapRight <<
" surplusGapLeft=" << surplusGapLeft <<
"\n";
1633 if (surplusGapRight <= 0) {
1636 latDist =
MAX2(latDist, -surplusGapRight);
1639 if (surplusGapLeft <= 0) {
1642 latDist =
MIN2(latDist, surplusGapLeft);
1659 if (laneOffset != 0) {
1673 if (laneOffset != 0) {
1680 if (collectFollowBlockers != 0 && collectLeadBlockers != 0) {
1682 for (std::vector<CLeaderDist>::const_iterator it2 = collectLeadBlockers->begin(); it2 != collectLeadBlockers->end(); ++it2) {
1683 for (std::vector<CLeaderDist>::iterator it = collectFollowBlockers->begin(); it != collectFollowBlockers->end();) {
1684 if ((*it2).first == (*it).first) {
1686 std::cout <<
" removed follower " << (*it).first->getID() <<
" because it is already a leader\n";
1688 it = collectFollowBlockers->erase(it);
1703 std::vector<CLeaderDist>* collectBlockers)
const {
1707 const SUMOReal leftVehSide = rightVehSide + vehWidth;
1708 const SUMOReal rightVehSideDest = rightVehSide + latDist;
1709 const SUMOReal leftVehSideDest = leftVehSide + latDist;
1710 const SUMOReal rightNoOverlap =
MIN2(rightVehSideDest, rightVehSide);
1711 const SUMOReal leftNoOverlap =
MAX2(leftVehSideDest, leftVehSide);
1714 for (
int i = 0; i < vehicles.
numSublanes(); ++i) {
1716 if (vehDist.first != 0) {
1721 const MSVehicle* leader = vehDist.first;
1724 std::swap(leader, follower);
1726 std::cout <<
" checkBlocking"
1727 <<
" leaders=" << leaders
1728 <<
" foe=" << vehDist.first->getID()
1729 <<
" gap=" << vehDist.second
1731 <<
" foeRight=" << foeRight
1732 <<
" foeLeft=" << foeLeft
1733 <<
" rightNoOverlap=" << rightNoOverlap
1734 <<
" leftNoOverlap=" << leftNoOverlap
1735 <<
" rightVehSideDest=" << rightVehSideDest
1736 <<
" leftVehSideDest=" << leftVehSideDest
1737 <<
" overlap=" <<
overlap(rightNoOverlap, leftNoOverlap, foeRight, foeLeft)
1738 <<
" overlapDest=" <<
overlap(rightVehSideDest, leftVehSideDest, foeRight, foeLeft)
1741 if (
overlap(rightNoOverlap, leftNoOverlap, foeRight, foeLeft)) {
1742 if (vehDist.second < 0) {
1744 std::cout <<
" overlap\n";
1747 if (collectBlockers == 0) {
1750 collectBlockers->push_back(vehDist);
1752 }
else if (
overlap(rightVehSideDest, leftVehSideDest, foeRight, foeLeft)) {
1753 const MSVehicle* leader = vehDist.first;
1756 std::swap(leader, follower);
1765 if (vehDist.second < secureGap) {
1767 std::cout <<
" blocked decelFactor=" << decelFactor <<
"\n";
1769 result |= blockType;
1770 if (collectBlockers == 0) {
1773 collectBlockers->push_back(vehDist);
1787 assert(right <= left);
1788 assert(right2 <= left2);
1796 if (sd1.
state == 0) {
1798 }
else if (sd2.
state == 0) {
1810 <<
" dir1=" << sd1.
dir
1814 <<
" dir2=" << sd2.
dir
1831 }
else if (sd2.
dir == 0) {
1839 return can1 ? sd1 : sd2;
1862 const std::vector<MSVehicle::LaneQ>& preb,
1872 int roundaboutEdgesAhead
1874 const bool right = (laneOffset == -1);
1875 const bool left = (laneOffset == 1);
1882 const SUMOReal maxJam =
MAX2(preb[currIdx + laneOffset].occupation, preb[currIdx].occupation);
1889 <<
" laDist=" << laDist
1890 <<
" currentDist=" << currentDist
1891 <<
" usableDist=" << usableDist
1892 <<
" bestLaneOffset=" << bestLaneOffset
1893 <<
" best.length=" << best.
length
1894 <<
" maxJam=" << maxJam
1895 <<
" neighLeftPlace=" << neighLeftPlace
1899 if (laneOffset != 0 && changeToBest && bestLaneOffset == curr.
bestLaneOffset
1920 <<
" avoid overtaking on the right nv=" << nv->
getID()
1923 <<
" plannedSpeed=" <<
myVSafes.back()
1937 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" could not change back and forth in time (1) neighLeftPlace=" << neighLeftPlace <<
"\n";
1940 }
else if (laneOffset != 0 && bestLaneOffset == 0 && (neighLeftPlace * 2. < laDist)) {
1946 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" could not change back and forth in time (2) neighLeftPlace=" << neighLeftPlace <<
"\n";
1951 && bestLaneOffset == 0
1954 && roundaboutEdgesAhead == 0
1960 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" does not want to leave the bestLane (neighDist=" << neighDist <<
")\n";
1964 && bestLaneOffset == 0
1971 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" does not want to get stranded on the on-ramp of a highway\n";
1981 if ((ret & lcaCounter) != 0) {
1986 std::cout <<
" reqAfterInfluence=" << ret <<
" ret=" << ret <<
"\n";
2038 const bool stayInLane = laneOffset == 0 || ((state &
LCA_STRATEGIC) != 0 && (state &
LCA_STAY) != 0);
2039 const SUMOReal oldLatDist = latDist;
2053 SUMOReal surplusGapRight = newCenter - halfWidth;
2058 if (laneOffset != 0) {
2069 std::cout <<
" keepLatGap laneOffset=" << laneOffset
2070 <<
" latDist=" << latDist
2071 <<
" gapFactor=" << gapFactor
2072 <<
" stayInLane=" << stayInLane
2073 <<
" surplusGapRight=" << surplusGapRight
2074 <<
" surplusGapLeft=" << surplusGapLeft
2080 if (surplusGapLeft > 0) {
2082 latDist =
MIN3(latDist - surplusGapRight, latDist + surplusGapLeft, maxDist);
2087 if (surplusGapRight > 0) {
2089 latDist =
MAX3(latDist + surplusGapLeft, latDist - surplusGapRight, -maxDist);
2094 if (latDist != oldLatDist) {
2095 blocked =
checkBlocking(neighLane, latDist, laneOffset, leaders, followers, blockers, neighLeaders, neighFollowers, neighBlockers, 0, 0,
true);
2098 state = (state & ~LCA_STAY);
2101 std::cout <<
" keepLatGap (checked)"
2102 <<
" latDist2=" << latDist
2116 if (others[i].first != 0 && others[i].second <= 0) {
2122 const SUMOReal gap =
MIN2(fabs(foeRight - newCenter), fabs(foeLeft - newCenter)) - halfWidth;
2124 if (
gDebugFlag2 &&
false) std::cout <<
" updateGaps"
2125 <<
" foe=" << foe->
getID()
2126 <<
" foeRight=" << foeRight
2127 <<
" foeLeft=" << foeLeft
2128 <<
" gap=" << others[i].second
2129 <<
" latgap=" << gap
2130 <<
" currentMinGap=" << currentMinGap
2131 <<
" surplusGapRight=" << surplusGapRight
2132 <<
" surplusGapLeft=" << surplusGapLeft
2134 if (foeCenter < newCenter) {
2135 surplusGapRight =
MIN2(surplusGapRight, gap - currentMinGap);
2137 surplusGapLeft =
MIN2(surplusGapLeft, gap - currentMinGap);
void * inform(void *info, MSVehicle *sender)
int getRightmostSublane() const
void informFollower(int blocked, int dir, const CLeaderDist &neighFollow, SUMOReal remainingSeconds, SUMOReal plannedSpeed)
decide whether we will try cut in before the follower or allow to be overtaken
void informFollowers(int blocked, int dir, const std::vector< CLeaderDist > &blockers, SUMOReal remainingSeconds, SUMOReal plannedSpeed)
call informFollower for multiple followers
SUMOReal myKeepRightProbability
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, SUMOReal &latDist, int &blocked)
check whether lateral gap requirements are met override the current maneuver if necessary ...
saves leader/follower vehicles and their distances relative to an ego vehicle
MSEdge & getEdge() const
Returns the lane's edge.
Representation of a vehicle in the micro simulation.
SUMOVehicleClass getVehicleClass() const
Get this vehicle type's vehicle class.
SUMOReal informLeader(int blocked, int dir, const CLeaderDist &neighLead, SUMOReal remainingSeconds)
The action is due to the default of keeping right "Rechtsfahrgebot".
The action is done to help someone else.
#define RELGAIN_NORMALIZATION_MIN_SPEED
SUMOReal myOrigLatDist
the complete lateral distance the vehicle wants to travel to finish its maneuver
std::vector< SUMOReal > myVSafes
speed adaptation requests by ego and surrounding vehicles
const SUMOReal myAssertive
const MSCFModel & getCarFollowModel() const
Returns the vehicle's car following model definition.
SUMOReal 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) ...
#define ROUNDABOUT_DIST_BONUS
void msg(const CLeaderDist &cld, SUMOReal speed, int state)
send a speed recommendation to the given vehicle
bool myCanChangeFully
whether the current lane changing meneuver can be finished in a single step
const std::vector< MSLane * > & getLanes() const
Returns this edge's lanes.
bool debugVehicle() const
whether the current vehicles shall be debugged
SUMOReal getLengthWithGap() const
Get vehicle's length including the minimum gap [m].
int bestLaneOffset
The (signed) number of lanes to be crossed to get to the lane which allows to continue the drive...
SUMOReal getImpatience() const
Returns this vehicles impatience.
The vehicle is blocked by left follower.
virtual std::string toString() const
print a debugging representation
At the leftmost side of the lane.
virtual SUMOReal followSpeed(const MSVehicle *const veh, SUMOReal speed, SUMOReal gap2pred, SUMOReal predSpeed, SUMOReal predMaxDecel) const =0
Computes the vehicle's follow speed (no dawdling)
const MSEdge * getLastEdge() const
returns the destination edge
The car-following model abstraction.
bool currentDistAllows(SUMOReal dist, int laneOffset, SUMOReal lookForwardDist)
static CLeaderDist getSlowest(const MSLeaderDistanceInfo &ldi)
get the slowest vehicle in the given info
SUMOReal getLength() const
Get vehicle's length [m].
ArrivalPosLatDefinition arrivalPosLatProcedure
Information how the vehicle shall choose the lateral arrival position.
std::pair< const MSVehicle *, SUMOReal > CLeaderDist
void updateExpectedSublaneSpeeds(const MSLeaderInfo &ahead, int sublaneOffset, int laneIndex)
update expected speeds for each sublane of the current edge
SUMOReal getWidth() const
Returns the lane's width.
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
const MSRoute & getRoute() const
Returns the current route.
static std::string getIDSecure(const T *obj, const std::string &fallBack="NULL")
get an identifier for Named-like object which may be Null
SUMOReal getMaxSpeedLat() const
Get vehicle's maximum lateral speed [m/s].
SUMOReal getSecureGap(const SUMOReal speed, const SUMOReal leaderSpeed, const SUMOReal leaderMaxDecel) const
Returns the minimum gap to reserve if the leader is braking at maximum (>=0)
SUMOReal getPositionOnLane() const
Get the vehicle's position along the lane.
LateralAlignment getPreferredLateralAlignment() const
Get vehicle's preferred lateral alignment.
StateAndDist decideDirection(StateAndDist sd1, StateAndDist sd2) const
decide in which direction to move in case both directions are desirable
void updateGaps(const MSLeaderDistanceInfo &others, SUMOReal foeOffset, SUMOReal newCenter, SUMOReal gapFactor, SUMOReal &surplusGapRight, SUMOReal &surplusGapLeft) const
check remaining lateral gaps for the given foe vehicles
SUMOTime getCurrentTimeStep() const
Returns the current simulation step.
The action is due to the wish to be faster (tactical lc)
SUMOReal length
The overall length which may be driven when using this lane without a lane change.
MSLCM_SL2015(MSVehicle &v)
const std::vector< SUMOReal > getSubLaneSides() const
Returns the right side offsets of this edge's sublanes.
#define LCA_RIGHT_IMPATIENCE
bool hasStoppedVehicle() const
whether a stopped vehicle is leader
MSAbstractLaneChangeModel & getLaneChangeModel()
#define LOOK_FORWARD_LEFT
#define MAX_ONRAMP_LENGTH
#define GAIN_PERCEPTION_THRESHOLD
SUMOReal getLateralPositionOnLane() const
Get the vehicle's lateral position on the lane.
#define ARRIVALPOS_LAT_THRESHOLD
int checkBlockingVehicles(const MSVehicle *ego, const MSLeaderDistanceInfo &vehicles, SUMOReal latDist, SUMOReal foeOffset, bool leaders, LaneChangeAction blockType, std::vector< CLeaderDist > *collectBlockers=0) const
check whether any of the vehicles overlaps with ego
Needs to stay on the current lane.
void saveBlockerLength(const MSVehicle *blocker, int lcaCounter)
save space for vehicles which need to counter-lane-change
static bool myAllowOvertakingRight
whether overtaking on the right is permitted
const std::string & getID() const
Returns the id.
A road/street connecting two junctions.
SUMOReal myLeadingBlockerLength
bool cancelRequest(int state)
whether the influencer cancels the given request
SUMOReal brakeGap(const SUMOReal speed) const
Returns the distance the vehicle needs to halt including driver's reaction time, assuming that during...
bool allowsVehicleClass(SUMOVehicleClass vclass) const
static bool overlap(SUMOReal right, SUMOReal left, SUMOReal right2, SUMOReal left2)
return whether the given intervals overlap
The action is urgent (to be defined by lc-model)
SUMOReal informLeaders(int blocked, int dir, const std::vector< CLeaderDist > &blockers, SUMOReal remainingSeconds)
At the center of the lane.
SUMOReal getMinGap() const
Get the free space in front of vehicles of this class.
std::vector< SUMOReal > myExpectedSublaneSpeeds
expected travel speeds on all sublanes on the current edge(!)
const MSEdge * myLastEdge
expected travel speeds on all sublanes on the current edge(!)
int wantsChangeSublane(int laneOffset, 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, SUMOReal &latDist, int &blocked)
Called to examine whether the vehicle wants to change using the given laneOffset. This method gets th...
const SUMOReal myChangeProbThresholdLeft
int checkBlocking(const MSLane &neighLane, SUMOReal &latDist, 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)
restrict latDist to permissible speed and determine blocking state depending on that distance ...
At the rightmost side of the lane.
SUMOReal patchSpeed(const SUMOReal min, const SUMOReal wanted, const SUMOReal max, const MSCFModel &cfModel)
Called to adapt the speed in order to allow a lane change.
std::pair< SUMOReal, int > Info
information regarding save velocity (unused) and state flags of the ego vehicle
SUMOReal myLookAheadSpeed
#define KEEP_RIGHT_ACCEPTANCE
The action is needed to follow the route (navigational lc)
SUMOReal getSpeedLimit() const
Returns the lane's maximum allowed speed.
A structure representing the best lanes for continuing the route.
int _wantsChangeSublane(int laneOffset, 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, SUMOReal &latDist, int &blocked)
helper function for doing the actual work
SUMOReal getMaxDecel() const
Get the vehicle type's maximum deceleration [m/s^2].
SUMOReal changeRequestRemainingSeconds(const SUMOTime currentTime) const
Return the remaining number of seconds of the current laneTimeLine assuming one exists.
SUMOReal _patchSpeed(const SUMOReal min, const SUMOReal wanted, const SUMOReal max, const MSCFModel &cfModel)
int myOwnState
The current state of the vehicle.
std::string toString(const T &t, std::streamsize accuracy=OUTPUT_ACCURACY)
int getIndex() const
Returns the lane's index.
#define LOOK_AHEAD_SPEED_MEMORY
#define LOOK_AHEAD_MIN_SPEED
static CLeaderDist getLongest(const MSLeaderDistanceInfo &ldi)
get the longest vehicle in the given info
virtual SUMOReal getHeadwayTime() const
Get the driver's reaction time [s].
const std::vector< LaneQ > & getBestLanes() const
Returns the description of best lanes to use in order to continue the route.
#define LOOK_FORWARD_RIGHT
SUMOReal getWidth() const
Get the width which vehicles of this class shall have when being drawn.
MSVehicle & myVehicle
The vehicle this lane-changer belongs to.
int checkStrategicChange(int ret, int laneOffset, const std::vector< MSVehicle::LaneQ > &preb, const MSLeaderDistanceInfo &leaders, const MSLeaderDistanceInfo &neighLeaders, int currIdx, int bestLaneOffset, bool changeToBest, int lcaCounter, SUMOReal currentDist, SUMOReal neighDist, SUMOReal laDist, int roundaboutEdgesAhead)
compute strategic lane change actions
EdgeBasicFunction getPurpose() const
Returns the edge type (EdgeBasicFunction)
Influencer & getInfluencer()
Returns the velocity/lane influencer.
LaneChangeAction
The state of a vehicle's lane-change behavior.
SUMOReal occupation
The overall vehicle sum on consecutive lanes which can be passed without a lane change.
const SUMOReal myCooperativeParam
SUMOReal 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) ...
int slowDownForBlocked(MSVehicle **blocked, int state)
compute useful slowdowns for blocked vehicles
std::vector< MSLane * > bestContinuations
Consecutive lane that can be followed without a lane change (contribute to length and occupation) ...
SUMOReal getMinGapLat() const
Get the minimum lateral gap that vehicles of this type maintain.
SUMOReal mySpeedGainProbabilityRight
a value for tracking the probability that a change to the right is beneficial
const MSVehicleType & getVehicleType() const
Returns the vehicle's type definition.
static SUMOReal gLateralResolution
virtual SUMOReal stopSpeed(const MSVehicle *const veh, const SUMOReal speed, SUMOReal gap) const =0
Computes the vehicle's safe speed for approaching a non-moving obstacle (no dawdling) ...
const SUMOReal SUMO_const_haltingSpeed
the speed threshold at which vehicles are considered as halting
SUMOReal getSpeed() const
Returns the vehicle's current speed.
No information given; use default.
const SUMOVehicleParameter & getParameter() const
Returns the vehicle's parameter (including departure definition)
const SUMOReal myStrategicParam
SUMOReal getWaitingSeconds() const
Returns the number of seconds waited (speed was lesser than 0.1m/s)
#define CUT_IN_LEFT_SPEED_THRESHOLD
SUMOReal getBackPositionOnLane(const MSLane *lane) const
Get the vehicle's position relative to the given lane.
SUMOReal getWidth() const
Returns the edges's width (sum over all lanes)
The action is due to a TraCI request.
bool currentDistDisallows(SUMOReal dist, int laneOffset, SUMOReal lookForwardDist)
#define HELP_DECEL_FACTOR
SUMOReal arrivalPosLat
(optional) The lateral position the vehicle shall arrive on
const MSLinkCont & getLinkCont() const
returns the container with all links !!!
bool myDontBrake
flag to prevent speed adaptation by slowing down
const SUMOReal myKeepRightParam
const SUMOReal myChangeProbThresholdRight
SUMOReal getVehicleMaxSpeed(const SUMOVehicle *const veh) const
Returns the lane's maximum speed, given a vehicle's speed limit adaptation.
The vehicle is blocked by right leader.
MSLane * getLane() const
Returns the lane the vehicle is on.
virtual SUMOReal getArrivalPos() const
Returns this vehicle's desired arrivalPos for its current route (may change on reroute) ...
int influenceChangeDecision(int state)
allow TraCI to influence a lane change decision
const SUMOReal mySpeedLossProbThreshold
The edge is an internal edge.
static LaneChangeAction getLCA(int state, SUMOReal latDist)
compute lane change action from desired lateral distance
Representation of a lane in the micro simulation.
const MSCFModel & myCarFollowModel
The vehicle's car following model.
int myPreviousState
lane changing state from the previous simulation step
The vehicle is blocked by right follower.
int computeSublaneShift(const MSEdge *prevEdge, const MSEdge *curEdge)
compute shift so that prevSublane + shift = newSublane
SUMOReal mySpeedGainProbabilityLeft
a value for tracking the probability that a change to the left is beneficial
Interface for lane-change models.
int getBestLaneOffset() const
returns the current offset from the best lane
The vehicle is blocked by left leader.
SUMOReal getRightSideOnEdge() const
const std::string & getID() const
Returns the name of the vehicle.
#define SPEED_GAIN_MIN_SECONDS
bool amBlockingFollowerPlusNB()