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 92 #define DEBUG_COND (myVehicle.getID() == "disabled") 104 mySpeedGainProbabilityRight(0),
105 mySpeedGainProbabilityLeft(0),
106 myKeepRightProbability(0),
107 myLeadingBlockerLength(0),
111 myCanChangeFully(true),
120 myChangeProbThresholdRight(2.0 * myKeepRightParam /
MAX2(
NUMERICAL_EPS, mySpeedGainParam)),
122 mySpeedLossProbThreshold(-0.01 + (1 - mySublaneParam)) {
124 throw ProcessError(
"laneChangeModel 'MSLCM_SL2015' is only meant to be used when simulating with '--lateral-resoluion' > 0");
149 const std::vector<MSVehicle::LaneQ>& preb,
155 const std::string changeType = laneOffset == -1 ?
"right" : (laneOffset == 1 ?
"left" :
"current");
166 <<
" considerChangeTo=" << changeType
171 leaders, followers, blockers,
172 neighLeaders, neighFollowers, neighBlockers,
174 lastBlocked, firstBlocked, latDist, blocked);
178 assert(latDist == 0);
183 result =
keepLatGap(result, leaders, followers, blockers,
184 neighLeaders, neighFollowers, neighBlockers,
185 neighLane, laneOffset, latDist, blocked);
187 result |=
getLCA(result, latDist);
193 <<
" latDist=" << latDist
201 <<
" wantsNoChangeTo=" << changeType
217 const std::string patched = (wanted != newSpeed ?
" patched=" +
toString(newSpeed) :
"");
223 <<
" wanted=" << wanted
253 std::cout << time <<
" veh=" <<
myVehicle.
getID() <<
" slowing down for leading blocker, safe=" << safe << (safe +
NUMERICAL_EPS < min ?
" (not enough)" :
"") <<
"\n";
255 return MAX2(min, safe);
262 for (std::vector<SUMOReal>::const_iterator i =
myVSafes.begin(); i !=
myVSafes.end(); ++i) {
264 if (v >= min && v <= max) {
265 nVSafe =
MIN2(v, nVSafe);
268 std::cout << time <<
" veh=" <<
myVehicle.
getID() <<
" got nVSafe=" << nVSafe <<
"\n";
273 std::cout << time <<
" veh=" <<
myVehicle.
getID() <<
" ignoring low nVSafe=" << v <<
" min=" << min <<
"\n";
277 std::cout << time <<
" veh=" <<
myVehicle.
getID() <<
" ignoring high nVSafe=" << v <<
" max=" << max <<
"\n";
285 std::cout << time <<
" veh=" <<
myVehicle.
getID() <<
" got vSafe\n";
296 std::cout << time <<
" veh=" <<
myVehicle.
getID() <<
" LCA_WANTS_LANECHANGE (strat, no vSafe)\n";
298 return (max + wanted) / (
SUMOReal) 2.0;
303 std::cout << time <<
" veh=" <<
myVehicle.
getID() <<
" LCA_BLOCKED_BY_LEADER (coop)\n";
305 return (min + wanted) / (
SUMOReal) 2.0;
309 std::cout << time <<
" veh=" <<
myVehicle.
getID() <<
" LCA_BLOCKED_BY_FOLLOWER (coop)\n";
311 return (max + wanted) / (
SUMOReal) 2.0;
353 std::cout << time <<
" veh=" <<
myVehicle.
getID() <<
" LCA_AMBLOCKINGLEADER\n";
355 return (max + wanted) / (
SUMOReal) 2.0;
360 std::cout << time <<
" veh=" <<
myVehicle.
getID() <<
" LCA_AMBLOCKINGFOLLOWER_DONTBRAKE\n";
381 if (pinfo->first >= 0) {
389 <<
" informedBy=" << sender->
getID()
390 <<
" info=" << pinfo->second
391 <<
" vSafe=" << pinfo->first
401 assert(cld.first != 0);
413 for (std::vector<SUMOReal>::const_iterator i =
myVSafes.begin(); i !=
myVSafes.end(); ++i) {
416 plannedSpeed =
MIN2(plannedSpeed, v);
420 std::cout <<
" informLeader speed=" <<
myVehicle.
getSpeed() <<
" planned=" << plannedSpeed <<
"\n";
424 assert(neighLead.first != 0);
426 if (
gDebugFlag2) std::cout <<
" blocked by leader nv=" << nv->
getID() <<
" nvSpeed=" << nv->
getSpeed() <<
" needGap=" 430 const SUMOReal overtakeDist = (neighLead.second
442 || dv * remainingSeconds < overtakeDist) {
456 <<
" cannot overtake leader nv=" << nv->
getID()
458 <<
" remainingSeconds=" << remainingSeconds
459 <<
" targetSpeed=" << targetSpeed
460 <<
" nextSpeed=" << nextSpeed
469 <<
" cannot overtake fast leader nv=" << nv->
getID()
471 <<
" remainingSeconds=" << remainingSeconds
472 <<
" targetSpeed=" << targetSpeed
481 <<
" wants to overtake leader nv=" << nv->
getID()
483 <<
" remainingSeconds=" << remainingSeconds
484 <<
" currentGap=" << neighLead.second
486 <<
" overtakeDist=" << overtakeDist
493 }
else if (neighLead.first != 0) {
502 std::cout <<
" not blocked by leader nv=" << nv->
getID()
504 <<
" gap=" << neighLead.second
505 <<
" nextGap=" << neighLead.second - dv
507 <<
" targetSpeed=" << targetSpeed
510 return MIN2(targetSpeed, plannedSpeed);
525 assert(neighFollow.first != 0);
527 if (
gDebugFlag2) std::cout <<
" blocked by follower nv=" << nv->
getID() <<
" nvSpeed=" << nv->
getSpeed() <<
" needGap=" 533 if ((neededGap - neighFollow.second) / remainingSeconds < (plannedSpeed - nv->
getSpeed())) {
535 std::cout <<
" wants to cut in before nv=" << nv->
getID() <<
" without any help neededGap=" << neededGap <<
"\n";
554 const SUMOReal dv = plannedSpeed - neighNewSpeed1s;
556 const SUMOReal decelGap = neighFollow.second + dv;
561 <<
" egoNV=" << plannedSpeed
562 <<
" nvNewSpeed=" << neighNewSpeed
563 <<
" nvNewSpeed1s=" << neighNewSpeed1s
564 <<
" deltaGap=" << dv
565 <<
" decelGap=" << decelGap
566 <<
" secGap=" << secureGap
569 if (decelGap > 0 && decelGap >= secureGap) {
583 std::cout <<
" wants to cut in before nv=" << nv->
getID()
584 <<
" vsafe1=" << vsafe1
585 <<
" vsafe=" << vsafe
589 }
else if (dv > 0 && dv * remainingSeconds > (secureGap - decelGap +
POSITION_EPS)) {
593 std::cout <<
" wants to cut in before nv=" << nv->
getID() <<
" (eventually)\n";
599 std::cout <<
" wants to cut in before nv=" << nv->
getID() <<
" (nv cannot overtake right)\n";
613 std::cout <<
" wants right follower to slow down a bit\n";
617 std::cout <<
" wants to cut in before right follower nv=" << nv->
getID() <<
" (eventually)\n";
625 const SUMOReal overtakeDist = (neighFollow.second
631 const SUMOReal needDV = overtakeDist / remainingSeconds;
638 <<
" wants to be overtaken by=" << nv->
getID()
639 <<
" overtakeDist=" << overtakeDist
641 <<
" vhelp=" << vhelp
642 <<
" needDV=" << needDV
647 }
else if (neighFollow.first != 0) {
656 std::cout <<
" wants to cut in before non-blocking follower nv=" << nv->
getID() <<
"\n";
663 const std::vector<CLeaderDist>& blockers,
666 for (std::vector<CLeaderDist>::const_iterator it = blockers.begin(); it != blockers.end(); ++it) {
667 plannedSpeed =
MIN2(plannedSpeed,
informLeader(blocked, dir, *it, remainingSeconds));
675 const std::vector<CLeaderDist>& blockers,
678 for (std::vector<CLeaderDist>::const_iterator it = blockers.begin(); it != blockers.end(); ++it) {
679 informFollower(blocked, dir, *it, remainingSeconds, plannedSpeed);
701 std::vector<SUMOReal> newExpectedSpeeds;
706 const std::vector<MSLane*>& lanes = currEdge->
getLanes();
707 for (std::vector<MSLane*>::const_iterator it_lane = lanes.begin(); it_lane != lanes.end(); ++it_lane) {
709 for (
int i = 0; i < subLanes; ++i) {
710 newExpectedSpeeds.push_back((*it_lane)->getVehicleMaxSpeed(&
myVehicle));
720 const int newI = i + subLaneShift;
721 if (newI > 0 && newI < (
int)newExpectedSpeeds.size()) {
739 const std::vector<MSLane*>& lanes = prevEdge->
getLanes();
740 for (std::vector<MSLane*>::const_iterator it_lane = lanes.begin(); it_lane != lanes.end(); ++it_lane) {
741 const MSLane* lane = *it_lane;
742 for (MSLinkCont::const_iterator it_link = lane->
getLinkCont().begin(); it_link != lane->
getLinkCont().end(); ++it_link) {
743 if (&((*it_link)->getLane()->getEdge()) == curEdge) {
745 const MSLane* target = (*it_link)->getLane();
746 const std::vector<MSLane*>& lanes2 = curEdge->
getLanes();
747 for (std::vector<MSLane*>::const_iterator it_lane2 = lanes2.begin(); it_lane2 != lanes2.end(); ++it_lane2) {
748 const MSLane* lane2 = *it_lane2;
749 if (lane2 == target) {
750 return prevShift + curShift;
805 const std::vector<MSVehicle::LaneQ>& preb,
813 int bestLaneOffset = 0;
822 for (
int p = 0; p < (int) preb.size(); ++p) {
823 if (preb[p].lane == prebLane && p + laneOffset >= 0) {
824 assert(p + laneOffset < (
int)preb.size());
826 neigh = preb[p + laneOffset];
827 currentDist = curr.
length;
829 bestLaneOffset = curr.bestLaneOffset;
831 if (bestLaneOffset == 0 && preb[p + laneOffset].bestLaneOffset == 0) {
835 <<
" bestLaneOffsetOld=" << bestLaneOffset
836 <<
" bestLaneOffsetNew=" << laneOffset
839 bestLaneOffset = laneOffset;
846 const bool right = (laneOffset == -1);
847 const bool left = (laneOffset == 1);
851 const bool changeToBest = (right && bestLaneOffset < 0) || (left && bestLaneOffset > 0) || (laneOffset == 0 && bestLaneOffset == 0);
861 SUMOReal leftLimit = halfCurrentLaneWidth - halfVehWidth - latPos;
862 SUMOReal rightLimit = -halfCurrentLaneWidth + halfVehWidth - latPos;
864 if (laneOffset == -1) {
867 }
else if (laneOffset == 1) {
893 <<
" leaders=" << leaders.
toString()
894 <<
" followers=" << followers.
toString()
895 <<
" blockers=" << blockers.
toString()
896 <<
" neighLeaders=" << neighLeaders.
toString()
897 <<
" neighFollowers=" << neighFollowers.
toString()
898 <<
" neighBlockers=" << neighBlockers.
toString()
899 <<
" changeToBest=" << changeToBest
900 <<
" latLaneDist=" << latLaneDist
901 <<
" leftLimit=" << leftLimit
902 <<
" rightLimit=" << rightLimit
909 if (lastBlocked != firstBlocked) {
957 int roundaboutEdgesAhead = 0;
959 if ((*it) != 0 && (*it)->getEdge().isRoundabout()) {
960 roundaboutEdgesAhead += 1;
961 }
else if (roundaboutEdgesAhead > 0) {
966 int roundaboutEdgesAheadNeigh = 0;
968 if ((*it) != 0 && (*it)->getEdge().isRoundabout()) {
969 roundaboutEdgesAheadNeigh += 1;
970 }
else if (roundaboutEdgesAheadNeigh > 0) {
975 if (roundaboutEdgesAhead > 1) {
979 if (roundaboutEdgesAhead > 0) {
981 std::cout <<
" roundaboutEdgesAhead=" << roundaboutEdgesAhead <<
" roundaboutEdgesAheadNeigh=" << roundaboutEdgesAheadNeigh <<
"\n";
985 if (laneOffset != 0) {
998 roundaboutEdgesAhead);
1008 if (changeToBest &&
abs(bestLaneOffset) > 1) {
1011 std::cout <<
" reserving space for unseen blockers\n";
1020 if (*firstBlocked != neighLeadLongest) {
1023 latDist = latLaneDist;
1024 std::vector<CLeaderDist> collectLeadBlockers;
1025 std::vector<CLeaderDist> collectFollowBlockers;
1028 leaders, followers, blockers,
1029 neighLeaders, neighFollowers, neighBlockers, &collectLeadBlockers, &collectFollowBlockers);
1036 if (plannedSpeed >= 0) {
1038 informFollowers(blocked, myLca, collectFollowBlockers, remainingSeconds, plannedSpeed);
1045 <<
" remainingSeconds=" << remainingSeconds
1046 <<
" plannedSpeed=" << plannedSpeed
1053 if (roundaboutEdgesAhead > 1) {
1062 if ((ret & LCA_STAY) == 0) {
1063 latDist = latLaneDist;
1065 leaders, followers, blockers,
1066 neighLeaders, neighFollowers, neighBlockers);
1081 const SUMOReal inconvenience = (latLaneDist < 0
1094 <<
" wantsChangeToHelp=" << (right ?
"right" :
"left")
1096 << (((
myOwnState & myLcaCounter) != 0) ?
" (counter)" :
"")
1102 latDist = latLaneDist;
1104 leaders, followers, blockers,
1105 neighLeaders, neighFollowers, neighBlockers);
1131 const SUMOReal leftVehSide = rightVehSide + vehWidth;
1135 int leftmostOnEdge = (int)sublaneSides.size() - 1;
1136 while (leftmostOnEdge > 0 && sublaneSides[leftmostOnEdge] > leftVehSide) {
1139 int rightmostOnEdge = leftmostOnEdge;
1140 while (rightmostOnEdge > 0 && sublaneSides[rightmostOnEdge] > rightVehSide +
NUMERICAL_EPS) {
1143 std::cout <<
" adapted to current sublane=" << rightmostOnEdge <<
" defaultNextSpeed=" << defaultNextSpeed <<
"\n";
1146 std::cout <<
" sublaneSides[rightmostOnEdge]=" << sublaneSides[rightmostOnEdge] <<
" rightVehSide=" << rightVehSide <<
"\n";
1152 std::cout <<
" adapted to current sublane=" << rightmostOnEdge <<
" defaultNextSpeed=" << defaultNextSpeed <<
"\n";
1155 std::cout <<
" sublaneSides[rightmostOnEdge]=" << sublaneSides[rightmostOnEdge] <<
" rightVehSide=" << rightVehSide <<
"\n";
1166 assert(leftMax <= edge.
getWidth());
1167 int sublaneCompact =
MAX2(iMin, rightmostOnEdge - 1);
1170 <<
" checking sublanes rightmostOnEdge=" << rightmostOnEdge
1171 <<
" leftmostOnEdge=" << leftmostOnEdge
1173 <<
" leftMax=" << leftMax
1174 <<
" sublaneCompact=" << sublaneCompact
1176 for (
int i = iMin; i < (int)sublaneSides.size(); ++i) {
1177 if (sublaneSides[i] + vehWidth < leftMax) {
1183 while (vMin > 0 && j < (
int)sublaneSides.size() && sublaneSides[j] < sublaneSides[i] + vehWidth) {
1191 maxGain = relativeGain;
1194 latDist = sublaneSides[i] - rightVehSide;
1196 std::cout <<
" i=" << i <<
" newLatDist=" << latDist <<
" relGain=" << relativeGain <<
"\n";
1201 std::cout <<
" i=" << i <<
" rightmostOnEdge=" << rightmostOnEdge <<
" vMin=" << vMin <<
" relGain=" << relativeGain <<
" sublaneCompact=" << sublaneCompact <<
"\n";
1203 if (i < rightmostOnEdge) {
1204 maxGainRight =
MAX2(maxGainRight, relativeGain);
1205 }
else if (i > rightmostOnEdge) {
1206 maxGainLeft =
MAX2(maxGainLeft, relativeGain);
1208 const SUMOReal subAlignDist = sublaneSides[i] - rightVehSide;
1209 if (fabs(subAlignDist) < fabs(latDistNice)) {
1210 latDistNice = subAlignDist;
1212 <<
" nicest sublane=" << i
1213 <<
" side=" << sublaneSides[i]
1214 <<
" rightSide=" << rightVehSide
1215 <<
" latDistNice=" << latDistNice
1238 <<
" defaultNextSpeed=" << defaultNextSpeed
1239 <<
" maxGain=" << maxGain
1240 <<
" maxGainRight=" << maxGainRight
1241 <<
" maxGainLeft=" << maxGainLeft
1242 <<
" latDist=" << latDist
1243 <<
" latDistNice=" << latDistNice
1244 <<
" sublaneCompact=" << sublaneCompact
1249 if (right && maxGain >= 0 && latDist <= 0) {
1256 SUMOReal fullSpeedDrivingSeconds =
MIN2(acceptanceTime, fullSpeedGap / vMax);
1258 if (neighLead.first != 0 && neighLead.first->getSpeed() < vMax) {
1261 vMax, neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel())));
1262 fullSpeedDrivingSeconds =
MIN2(fullSpeedDrivingSeconds, fullSpeedGap / (vMax - neighLead.first->getSpeed()));
1271 <<
" considering keepRight:" 1273 <<
" neighDist=" << neighDist
1275 <<
" leaderSpeed=" << (neighLead.first == 0 ? -1 : neighLead.first->getSpeed())
1277 myVehicle.
getSpeed(), neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()))
1278 <<
" acceptanceTime=" << acceptanceTime
1279 <<
" fullSpeedGap=" << fullSpeedGap
1280 <<
" fullSpeedDrivingSeconds=" << fullSpeedDrivingSeconds
1281 <<
" dProb=" << deltaProb
1290 latDist = latLaneDist;
1292 leaders, followers, blockers,
1293 neighLeaders, neighFollowers, neighBlockers);
1302 <<
" latDist=" << latDist
1311 leaders, followers, blockers,
1312 neighLeaders, neighFollowers, neighBlockers);
1323 <<
" latDist=" << latDist
1324 <<
" stayInLane=" << stayInLane
1335 leaders, followers, blockers,
1336 neighLeaders, neighFollowers, neighBlockers);
1357 latDist = latDistNice;
1360 latDist = sublaneSides[sublaneCompact] - rightVehSide;
1369 <<
" latDist=" << latDist
1384 <<
" latDist=" << latDist
1389 leaders, followers, blockers,
1390 neighLeaders, neighFollowers, neighBlockers);
1432 if ((*blocked) != 0) {
1456 (*blocked)->getCarFollowModel().getMaxDecel()));
1496 <<
" potential=" << potential
1509 const MSLane* lane = lanes[laneIndex];
1511 assert(preb.size() == lanes.size());
1513 for (
int sublane = 0; sublane < (int)ahead.
numSublanes(); ++sublane) {
1514 const int edgeSublane = sublane + sublaneOffset;
1518 const MSVehicle* leader = ahead[sublane];
1544 if (ldi[i].first != 0) {
1545 const SUMOReal length = ldi[i].first->getVehicleType().getLength();
1546 if (length > maxLength) {
1561 if (ldi[i].first != 0) {
1562 const SUMOReal speed = ldi[i].first->getSpeed();
1563 if (speed < minSpeed) {
1581 std::vector<CLeaderDist>* collectLeadBlockers,
1582 std::vector<CLeaderDist>* collectFollowBlockers,
1583 bool saveOriginalLatDist) {
1585 if (saveOriginalLatDist) {
1589 latDist =
MAX2(
MIN2(latDist, maxDist), -maxDist);
1594 SUMOReal surplusGapRight =
MIN2(maxDist, center - halfWidth);
1598 if (laneOffset != 0) {
1603 std::cout <<
" checkBlocking latDist=" << latDist <<
" surplusGapRight=" << surplusGapRight <<
" surplusGapLeft=" << surplusGapLeft <<
"\n";
1606 if (surplusGapRight <= 0) {
1609 latDist =
MAX2(latDist, -surplusGapRight);
1612 if (surplusGapLeft <= 0) {
1615 latDist =
MIN2(latDist, surplusGapLeft);
1632 if (laneOffset != 0) {
1646 if (laneOffset != 0) {
1653 if (collectFollowBlockers != 0 && collectLeadBlockers != 0) {
1655 for (std::vector<CLeaderDist>::const_iterator it2 = collectLeadBlockers->begin(); it2 != collectLeadBlockers->end(); ++it2) {
1656 for (std::vector<CLeaderDist>::iterator it = collectFollowBlockers->begin(); it != collectFollowBlockers->end();) {
1657 if ((*it2).first == (*it).first) {
1659 std::cout <<
" removed follower " << (*it).first->getID() <<
" because it is already a leader\n";
1661 it = collectFollowBlockers->erase(it);
1676 std::vector<CLeaderDist>* collectBlockers) {
1680 const SUMOReal leftVehSide = rightVehSide + vehWidth;
1681 const SUMOReal rightVehSideDest = rightVehSide + latDist;
1682 const SUMOReal leftVehSideDest = leftVehSide + latDist;
1683 const SUMOReal rightNoOverlap =
MIN2(rightVehSideDest, rightVehSide);
1684 const SUMOReal leftNoOverlap =
MAX2(leftVehSideDest, leftVehSide);
1687 for (
int i = 0; i < vehicles.
numSublanes(); ++i) {
1689 if (vehDist.first != 0) {
1694 const MSVehicle* leader = vehDist.first;
1697 std::swap(leader, follower);
1699 std::cout <<
" checkBlocking" 1700 <<
" leaders=" << leaders
1701 <<
" foe=" << vehDist.first->getID()
1702 <<
" gap=" << vehDist.second
1704 <<
" foeRight=" << foeRight
1705 <<
" foeLeft=" << foeLeft
1706 <<
" rightNoOverlap=" << rightNoOverlap
1707 <<
" leftNoOverlap=" << leftNoOverlap
1708 <<
" rightVehSideDest=" << rightVehSideDest
1709 <<
" leftVehSideDest=" << leftVehSideDest
1710 <<
" overlap=" <<
overlap(rightNoOverlap, leftNoOverlap, foeRight, foeLeft)
1711 <<
" overlapDest=" <<
overlap(rightVehSideDest, leftVehSideDest, foeRight, foeLeft)
1714 if (
overlap(rightNoOverlap, leftNoOverlap, foeRight, foeLeft)) {
1715 if (vehDist.second < 0) {
1717 std::cout <<
" overlap\n";
1720 if (collectBlockers == 0) {
1723 collectBlockers->push_back(vehDist);
1725 }
else if (
overlap(rightVehSideDest, leftVehSideDest, foeRight, foeLeft)) {
1726 const MSVehicle* leader = vehDist.first;
1729 std::swap(leader, follower);
1733 std::cout <<
" blocked\n";
1735 result |= blockType;
1736 if (collectBlockers == 0) {
1739 collectBlockers->push_back(vehDist);
1753 assert(right <= left);
1754 assert(right2 <= left2);
1762 if (sd1.
state == 0) {
1764 }
else if (sd2.
state == 0) {
1776 <<
" dir1=" << sd1.
dir 1780 <<
" dir2=" << sd2.
dir 1797 }
else if (sd2.
dir == 0) {
1805 return can1 ? sd1 : sd2;
1828 const std::vector<MSVehicle::LaneQ>& preb,
1838 int roundaboutEdgesAhead
1840 const bool right = (laneOffset == -1);
1841 const bool left = (laneOffset == 1);
1848 const SUMOReal maxJam =
MAX2(preb[currIdx + laneOffset].occupation, preb[currIdx].occupation);
1855 <<
" laDist=" << laDist
1856 <<
" currentDist=" << currentDist
1857 <<
" usableDist=" << usableDist
1858 <<
" bestLaneOffset=" << bestLaneOffset
1859 <<
" best.length=" << best.
length 1860 <<
" maxJam=" << maxJam
1861 <<
" neighLeftPlace=" << neighLeftPlace
1865 if (laneOffset != 0 && changeToBest && bestLaneOffset == curr.
bestLaneOffset 1886 <<
" avoid overtaking on the right nv=" << nv->
getID()
1889 <<
" plannedSpeed=" <<
myVSafes.back()
1903 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" could not change back and forth in time (1) neighLeftPlace=" << neighLeftPlace <<
"\n";
1906 }
else if (laneOffset != 0 && bestLaneOffset == 0 && (neighLeftPlace * 2. < laDist)) {
1912 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" could not change back and forth in time (2) neighLeftPlace=" << neighLeftPlace <<
"\n";
1917 && bestLaneOffset == 0
1920 && roundaboutEdgesAhead == 0
1926 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" does not want to leave the bestLane (neighDist=" << neighDist <<
")\n";
1930 && bestLaneOffset == 0
1937 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" does not want to get stranded on the on-ramp of a highway\n";
1947 if ((ret & lcaCounter) != 0) {
1952 std::cout <<
" reqAfterInfluence=" << ret <<
" ret=" << ret <<
"\n";
2004 const bool stayInLane = laneOffset == 0 || ((state &
LCA_STRATEGIC) != 0 && (state &
LCA_STAY) != 0);
2018 SUMOReal surplusGapRight = newCenter - halfWidth;
2023 if (laneOffset != 0) {
2034 std::cout <<
" keepLatGap laneOffset=" << laneOffset
2035 <<
" latDist=" << latDist
2036 <<
" gapFactor=" << gapFactor
2037 <<
" stayInLane=" << stayInLane
2038 <<
" surplusGapRight=" << surplusGapRight
2039 <<
" surplusGapLeft=" << surplusGapLeft
2046 if (surplusGapLeft > 0) {
2048 latDist =
MIN3(latDist - surplusGapRight, latDist + surplusGapLeft, maxDist);
2053 if (surplusGapRight > 0) {
2055 latDist =
MAX3(latDist + surplusGapLeft, latDist - surplusGapRight, -maxDist);
2060 if (blocked == 0 ) {
2061 blocked =
checkBlocking(neighLane, latDist, laneOffset, leaders, followers, blockers, neighLeaders, neighFollowers, neighBlockers, 0, 0,
false);
2064 state = (state & ~LCA_STAY);
2076 if (others[i].first != 0 && others[i].second <= 0) {
2082 const SUMOReal gap =
MIN2(fabs(foeRight - newCenter), fabs(foeLeft - newCenter)) - halfWidth;
2084 if (
gDebugFlag2 &&
false) std::cout <<
" updateGaps" 2085 <<
" foe=" << foe->
getID()
2086 <<
" foeRight=" << foeRight
2087 <<
" foeLeft=" << foeLeft
2088 <<
" gap=" << others[i].second
2089 <<
" latgap=" << gap
2090 <<
" currentMinGap=" << currentMinGap
2091 <<
" surplusGapRight=" << surplusGapRight
2092 <<
" surplusGapLeft=" << surplusGapLeft
2094 if (foeCenter < newCenter) {
2095 surplusGapRight =
MIN2(surplusGapRight, gap - currentMinGap);
2097 surplusGapLeft =
MIN2(surplusGapLeft, gap - currentMinGap);
void * inform(void *info, MSVehicle *sender)
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 saveOriginalLatDist=true)
restrict latDist to permissible speed and determine blocking state depending on that distance ...
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 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...
The vehicle is blocked by left follower.
virtual std::string toString() const
print a debugging representation
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)
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].
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).
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.
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.
static int checkBlockingVehicles(const MSVehicle *ego, const MSLeaderDistanceInfo &vehicles, SUMOReal latDist, SUMOReal foeOffset, bool leaders, LaneChangeAction blockType, std::vector< CLeaderDist > *collectBlockers=0)
check whether any of the vehicles overlaps with ego
#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.
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.
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)
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
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)
virtual SUMOReal stopSpeed(const MSVehicle *const veh, const SUMOReal speed, SUMOReal gap2pred) const =0
Computes the vehicle's safe speed for approaching a non-moving obstacle (no dawdling) ...
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
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
const SUMOReal SUMO_const_haltingSpeed
the speed threshold at which vehicles are considered as halting
SUMOReal getSpeed() const
Returns the vehicle's current speed.
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
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.
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()