41 #ifdef CHECK_MEMORY_LEAKS
43 #endif // CHECK_MEMORY_LEAKS
46 #ifdef DEBUG_VEHICLE_GUI_SELECTION
58 #define LOOK_FORWARD_SPEED_DIVIDER (SUMOReal)14.
64 #define LOOK_FORWARD_RIGHT (SUMOReal)10.
65 #define LOOK_FORWARD_LEFT (SUMOReal)20.
67 #define JAM_FACTOR (SUMOReal)1.
70 #define LCA_RIGHT_IMPATIENCE (SUMOReal)-1.
71 #define CUT_IN_LEFT_SPEED_THRESHOLD (SUMOReal)27.
73 #define LOOK_AHEAD_MIN_SPEED (SUMOReal)0.0
74 #define LOOK_AHEAD_SPEED_MEMORY (SUMOReal)0.9
75 #define LOOK_AHEAD_SPEED_DECREMENT 6.
77 #define HELP_DECEL_FACTOR (SUMOReal)1.0
79 #define HELP_OVERTAKE (SUMOReal)(10.0 / 3.6)
80 #define MIN_FALLBEHIND (SUMOReal)(14.0 / 3.6)
82 #define KEEP_RIGHT_HEADWAY (SUMOReal)2.0
84 #define URGENCY (SUMOReal)2.0
86 #define ROUNDABOUT_DIST_BONUS (SUMOReal)100.0
88 #define CHANGE_PROB_THRESHOLD_RIGHT (SUMOReal)2.0
89 #define CHANGE_PROB_THRESHOLD_LEFT (SUMOReal)0.2
90 #define KEEP_RIGHT_TIME (SUMOReal)5.0 // the number of seconds after which a vehicle should move to the right lane
91 #define KEEP_RIGHT_ACCEPTANCE (SUMOReal)2.0 // calibration factor for determining the desire to keep right
93 #define RELGAIN_NORMALIZATION_MIN_SPEED (SUMOReal)10.0
95 #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
101 #define DEBUG_COND false
106 return v == 0 ?
"NULL" : v->
getID();
115 mySpeedGainProbability(0),
116 myKeepRightProbability(0),
117 myLeadingBlockerLength(0),
132 const std::pair<MSVehicle*, SUMOReal>& leader,
133 const std::pair<MSVehicle*, SUMOReal>& neighLead,
134 const std::pair<MSVehicle*, SUMOReal>& neighFollow,
136 const std::vector<MSVehicle::LaneQ>& preb,
147 <<
" considerChangeTo=" << (laneOffset == -1 ?
"right" :
"left")
151 const int result =
_wantsChange(laneOffset, msgPass, blocked, leader, neighLead, neighFollow, neighLane, preb, lastBlocked, firstBlocked);
156 <<
" wantsChangeTo=" << (laneOffset == -1 ?
"right" :
"left")
163 << ((result &
LCA_TRACI) ?
" (traci)" :
"")
180 const std::string patched = (wanted != newSpeed ?
" patched=" +
toString(newSpeed) :
"");
186 <<
" wanted=" << wanted
217 std::cout << time <<
" veh=" <<
myVehicle.
getID() <<
" slowing down for leading blocker" << (safe +
NUMERICAL_EPS < min ?
" (not enough)" :
"") <<
"\n";
219 return MAX2(min, safe);
226 for (std::vector<SUMOReal>::const_iterator i =
myVSafes.begin(); i !=
myVSafes.end(); ++i) {
228 if (v >= min && v <= max) {
229 nVSafe =
MIN2(v, nVSafe);
232 std::cout << time <<
" veh=" <<
myVehicle.
getID() <<
" got nVSafe=" << nVSafe <<
"\n";
237 std::cout << time <<
" veh=" <<
myVehicle.
getID() <<
" ignoring low nVSafe=" << v <<
" min=" << min <<
"\n";
241 std::cout << time <<
" veh=" <<
myVehicle.
getID() <<
" ignoring high nVSafe=" << v <<
" max=" << max <<
"\n";
249 std::cout << time <<
" veh=" <<
myVehicle.
getID() <<
" got vSafe\n";
260 std::cout << time <<
" veh=" <<
myVehicle.
getID() <<
" LCA_WANTS_LANECHANGE (strat, no vSafe)\n";
262 return (max + wanted) / (
SUMOReal) 2.0;
267 std::cout << time <<
" veh=" <<
myVehicle.
getID() <<
" LCA_BLOCKED_BY_LEADER (coop)\n";
269 return (min + wanted) / (
SUMOReal) 2.0;
273 std::cout << time <<
" veh=" <<
myVehicle.
getID() <<
" LCA_BLOCKED_BY_FOLLOWER (coop)\n";
275 return (max + wanted) / (
SUMOReal) 2.0;
317 std::cout << time <<
" veh=" <<
myVehicle.
getID() <<
" LCA_AMBLOCKINGLEADER\n";
319 return (max + wanted) / (
SUMOReal) 2.0;
324 std::cout << time <<
" veh=" <<
myVehicle.
getID() <<
" LCA_AMBLOCKINGFOLLOWER_DONTBRAKE\n";
345 if (pinfo->first >= 0) {
353 <<
" informedBy=" << sender->
getID()
354 <<
" info=" << pinfo->second
355 <<
" vSafe=" << pinfo->first
367 const std::pair<MSVehicle*, SUMOReal>& neighLead,
371 for (std::vector<SUMOReal>::const_iterator i =
myVSafes.begin(); i !=
myVSafes.end(); ++i) {
374 plannedSpeed =
MIN2(plannedSpeed, v);
378 std::cout <<
" informLeader speed=" <<
myVehicle.
getSpeed() <<
" planned=" << plannedSpeed <<
"\n";
382 assert(neighLead.first != 0);
384 if (
gDebugFlag2) std::cout <<
" blocked by leader nv=" << nv->
getID() <<
" nvSpeed=" << nv->
getSpeed() <<
" needGap="
388 const SUMOReal overtakeDist = (neighLead.second
400 || dv * remainingSeconds < overtakeDist) {
413 <<
" cannot overtake leader nv=" << nv->
getID()
415 <<
" remainingSeconds=" << remainingSeconds
416 <<
" targetSpeed=" << targetSpeed
417 <<
" nextSpeed=" << nextSpeed
426 <<
" cannot overtake fast leader nv=" << nv->
getID()
428 <<
" remainingSeconds=" << remainingSeconds
429 <<
" targetSpeed=" << targetSpeed
438 <<
" wants to overtake leader nv=" << nv->
getID()
440 <<
" remainingSeconds=" << remainingSeconds
441 <<
" currentGap=" << neighLead.second
449 }
else if (neighLead.first != 0) {
458 std::cout <<
" not blocked by leader nv=" << nv->
getID()
460 <<
" gap=" << neighLead.second
461 <<
" nextGap=" << neighLead.second - dv
463 <<
" targetSpeed=" << targetSpeed
466 return MIN2(targetSpeed, plannedSpeed);
478 const std::pair<MSVehicle*, SUMOReal>& neighFollow,
482 assert(neighFollow.first != 0);
484 if (
gDebugFlag2) std::cout <<
" blocked by follower nv=" << nv->
getID() <<
" nvSpeed=" << nv->
getSpeed() <<
" needGap="
490 if ((neededGap - neighFollow.second) / remainingSeconds < (plannedSpeed - nv->
getSpeed())) {
492 std::cout <<
" wants to cut in before nv=" << nv->
getID() <<
" without any help neededGap=" << neededGap <<
"\n";
511 const SUMOReal dv = plannedSpeed - neighNewSpeed1s;
513 const SUMOReal decelGap = neighFollow.second + dv;
518 <<
" egoNV=" << plannedSpeed
519 <<
" nvNewSpeed=" << neighNewSpeed
520 <<
" nvNewSpeed1s=" << neighNewSpeed1s
521 <<
" deltaGap=" << dv
522 <<
" decelGap=" << decelGap
523 <<
" secGap=" << secureGap
526 if (decelGap > 0 && decelGap >= secureGap) {
533 std::cout <<
" wants to cut in before nv=" << nv->
getID() <<
"\n";
535 }
else if (dv > 0 && dv * remainingSeconds > (secureGap - decelGap +
POSITION_EPS)) {
539 std::cout <<
" wants to cut in before nv=" << nv->
getID() <<
" (eventually)\n";
551 std::cout <<
" wants right follower to slow down a bit\n";
555 std::cout <<
" wants to cut in before right follower nv=" << nv->
getID() <<
" (eventually)\n";
563 const SUMOReal overtakeDist = (neighFollow.second
569 const SUMOReal needDV = overtakeDist / remainingSeconds;
576 <<
" wants to be overtaken by=" << nv->
getID()
577 <<
" overtakeDist=" << overtakeDist
579 <<
" vhelp=" << vhelp
580 <<
" needDV=" << needDV
625 const std::pair<MSVehicle*, SUMOReal>& leader,
626 const std::pair<MSVehicle*, SUMOReal>& neighLead,
627 const std::pair<MSVehicle*, SUMOReal>& neighFollow,
629 const std::vector<MSVehicle::LaneQ>& preb,
632 assert(laneOffset == 1 || laneOffset == -1);
636 int bestLaneOffset = 0;
645 for (
int p = 0; p < (
int) preb.size(); ++p) {
646 if (preb[p].lane == prebLane && p + laneOffset >= 0) {
647 assert(p + laneOffset < (
int)preb.size());
649 neigh = preb[p + laneOffset];
650 currentDist = curr.
length;
652 bestLaneOffset = curr.bestLaneOffset;
654 if (bestLaneOffset == 0 && preb[p + laneOffset].bestLaneOffset == 0) {
658 <<
" bestLaneOffsetOld=" << bestLaneOffset
659 <<
" bestLaneOffsetNew=" << laneOffset
662 bestLaneOffset = laneOffset;
664 best = preb[p + bestLaneOffset];
670 const bool right = (laneOffset == -1);
675 const bool changeToBest = (right && bestLaneOffset < 0) || (!right && bestLaneOffset > 0);
699 <<
" firstBlocked=" <<
tryID(*firstBlocked)
700 <<
" lastBlocked=" <<
tryID(*lastBlocked)
701 <<
" neighLead=" <<
tryID(neighLead.first)
702 <<
" neighLeadGap=" << neighLead.second
703 <<
" neighFollow=" <<
tryID(neighFollow.first)
704 <<
" neighFollowGap=" << neighFollow.second
710 if (lastBlocked != firstBlocked) {
749 int roundaboutEdgesAhead = 0;
751 if ((*it) != 0 && (*it)->getEdge().isRoundabout()) {
752 roundaboutEdgesAhead += 1;
753 }
else if (roundaboutEdgesAhead > 0) {
758 int roundaboutEdgesAheadNeigh = 0;
760 if ((*it) != 0 && (*it)->getEdge().isRoundabout()) {
761 roundaboutEdgesAheadNeigh += 1;
762 }
else if (roundaboutEdgesAheadNeigh > 0) {
767 if (roundaboutEdgesAhead > 1) {
771 if (roundaboutEdgesAhead > 0) {
773 std::cout <<
" roundaboutEdgesAhead=" << roundaboutEdgesAhead <<
" roundaboutEdgesAheadNeigh=" << roundaboutEdgesAheadNeigh <<
"\n";
779 const SUMOReal maxJam =
MAX2(preb[currIdx + laneOffset].occupation, preb[currIdx].occupation);
786 <<
" laDist=" << laDist
787 <<
" currentDist=" << currentDist
788 <<
" usableDist=" << usableDist
789 <<
" bestLaneOffset=" << bestLaneOffset
790 <<
" best.length=" << best.
length
791 <<
" maxJam=" << maxJam
792 <<
" neighLeftPlace=" << neighLeftPlace
814 <<
" avoid overtaking on the right nv=" << nv->
getID()
817 <<
" plannedSpeed=" <<
myVSafes.back()
831 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" could not change back and forth in time (1) neighLeftPlace=" << neighLeftPlace <<
"\n";
834 }
else if (bestLaneOffset == 0 && (neighLeftPlace * 2. < laDist)) {
840 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" could not change back and forth in time (2) neighLeftPlace=" << neighLeftPlace <<
"\n";
843 }
else if (bestLaneOffset == 0
844 && (leader.first == 0 || !leader.first->isStopped())
846 && roundaboutEdgesAhead == 0
852 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" does not want to leave the bestLane (neighDist=" << neighDist <<
")\n";
862 if ((ret & lcaCounter) != 0) {
867 std::cout <<
" retAfterInfluence=" << ret <<
"\n";
877 if (changeToBest &&
abs(bestLaneOffset) > 1) {
880 std::cout <<
" reserving space for unseen blockers\n";
888 if (*firstBlocked != neighLead.first) {
895 const SUMOReal plannedSpeed =
informLeader(msgPass, blocked, myLca, neighLead, remainingSeconds);
896 if (plannedSpeed >= 0) {
898 informFollower(msgPass, blocked, myLca, neighFollow, remainingSeconds, plannedSpeed);
905 <<
" remainingSeconds=" << remainingSeconds
906 <<
" plannedSpeed=" << plannedSpeed
913 if (roundaboutEdgesAhead > 1) {
931 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" does not want to get stranded on the on-ramp of a highway\n";
956 <<
" wantsChangeToHelp=" << (right ?
"right" :
"left")
958 << (((
myOwnState & myLcaCounter) != 0) ?
" (counter)" :
"")
983 if (neighLead.first == 0) {
988 &
myVehicle,
myVehicle.
getSpeed(), neighLead.second, neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()));
990 if (leader.first == 0) {
998 thisLaneVSafe =
MIN2(thisLaneVSafe, vMax);
999 neighLaneVSafe =
MIN2(neighLaneVSafe, vMax);
1000 const SUMOReal relativeGain = (neighLaneVSafe - thisLaneVSafe) /
MAX2(neighLaneVSafe,
1005 if (thisLaneVSafe - 5 / 3.6 > neighLaneVSafe) {
1019 SUMOReal fullSpeedDrivingSeconds =
MIN2(acceptanceTime, fullSpeedGap / vMax);
1020 if (neighLead.first != 0 && neighLead.first->getSpeed() < vMax) {
1023 vMax, neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel())));
1024 fullSpeedDrivingSeconds =
MIN2(fullSpeedDrivingSeconds, fullSpeedGap / (vMax - neighLead.first->getSpeed()));
1035 <<
" neighDist=" << neighDist
1037 <<
" leaderSpeed=" << (neighLead.first == 0 ? -1 : neighLead.first->getSpeed())
1039 myVehicle.
getSpeed(), neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()))
1040 <<
" acceptanceTime=" << acceptanceTime
1041 <<
" fullSpeedGap=" << fullSpeedGap
1042 <<
" fullSpeedDrivingSeconds=" << fullSpeedDrivingSeconds
1043 <<
" dProb=" << deltaProb
1059 <<
" thisLaneVSafe=" << thisLaneVSafe
1060 <<
" neighLaneVSafe=" << neighLaneVSafe
1061 <<
" relativeGain=" << relativeGain
1062 <<
" blocked=" << blocked
1075 if (thisLaneVSafe > neighLaneVSafe) {
1102 && (right ? mySpeedGainProbability < 0 : mySpeedGainProbability > 0)) {
1114 <<
" thisLaneVSafe=" << thisLaneVSafe
1115 <<
" neighLaneVSafe=" << neighLaneVSafe
1125 if ((*blocked) != 0) {
1130 <<
" blocked=" <<
tryID(*blocked)
1149 (*blocked)->getCarFollowModel().getMaxDecel()));
1170 <<
" blocker=" <<
tryID(blocker)
1180 <<
" blocker=" <<
tryID(blocker)