42 #define OPPOSITE_OVERTAKING_SAFE_TIMEGAP 0.0
43 #define OPPOSITE_OVERTAKING_SAFETYGAP_HEADWAY_FACTOR 0.0
44 #define OPPOSITE_OVERTAKING_SAFETY_FACTOR 1.2
46 #define OPPOSITE_OVERTAKING_MAX_LOOKAHEAD 150.0
47 #define OPPOSITE_OVERTAKING_MAX_LOOKAHEAD_EMERGENCY 1000.0
49 #define OPPOSITE_OVERTAKING_ONCOMING_LOOKAHEAD 200.0
64 #define DEBUG_COND (vehicle->isSelected())
76 firstBlocked(nullptr),
78 aheadNext(lane, nullptr, 0) {
84 lane->myTmpVehicles.insert(lane->myTmpVehicles.begin(), vehicle);
99 for (std::vector<MSLane*>::const_iterator lane = lanes->begin(); lane != lanes->end(); ++lane) {
101 myChanger.back().mayChangeRight = lane != lanes->begin();
102 myChanger.back().mayChangeLeft = (lane + 1) != lanes->end();
104 if ((*lane)->isInternal()) {
105 if (
myChanger.back().mayChangeRight && (*lane)->getLogicalPredecessorLane() == (*(lane - 1))->getLogicalPredecessorLane()) {
108 if (
myChanger.back().mayChangeLeft && (*lane)->getLogicalPredecessorLane() == (*(lane + 1))->getLogicalPredecessorLane()) {
129 const bool haveChanged =
change();
136 ce->lane->releaseVehicles();
148 ce->hoppedVeh =
nullptr;
149 ce->lastBlocked =
nullptr;
150 ce->firstBlocked =
nullptr;
152 ce->lane->getVehiclesSecure();
166 if (!vehHasChanged) {
189 ce->lane->swapAfterLaneChange(t);
190 ce->lane->releaseVehicles();
200 #ifdef DEBUG_CANDIDATE
201 std::cout <<
SIMTIME <<
" findCandidate() on edge " <<
myChanger.begin()->lane->getEdge().getID() << std::endl;
205 if (
veh(ce) ==
nullptr) {
208 #ifdef DEBUG_CANDIDATE
209 std::cout <<
" lane = " << ce->lane->getID() <<
"\n";
213 #ifdef DEBUG_CANDIDATE
219 assert(
veh(ce) != 0);
220 assert(
veh(max) != 0);
221 if (
veh(max)->getPositionOnLane() <
veh(ce)->getPositionOnLane()) {
222 #ifdef DEBUG_CANDIDATE
229 assert(
veh(max) != 0);
236 if (direction == 0) {
242 if (direction == -1) {
244 }
else if (direction == 1) {
262 #ifdef DEBUG_ACTIONSTEPS
281 #ifdef DEBUG_ACTIONSTEPS
283 std::cout <<
SIMTIME <<
" veh '" << vehicle->
getID() <<
"' skips regular change checks." << std::endl;
286 bool changed =
false;
310 for (
int i = 0; i < (int)
myChanger.size(); ++i) {
314 const std::vector<MSVehicle::LaneQ>& preb = vehicle->
getBestLanes();
325 (
myCandi - 1)->lastBlocked = vehicle;
326 if ((
myCandi - 1)->firstBlocked ==
nullptr) {
327 (
myCandi - 1)->firstBlocked = vehicle;
342 (
myCandi + 1)->lastBlocked = vehicle;
343 if ((
myCandi + 1)->firstBlocked ==
nullptr) {
344 (
myCandi + 1)->firstBlocked = vehicle;
388 <<
" veh=" << vehicle->
getID()
391 << ((newstate &
LCA_BLOCKED) != 0 ?
" (blocked)" :
"")
402 bool changed =
false;
404 const int dir = (state &
LCA_RIGHT) != 0 ? -1 : ((state &
LCA_LEFT) != 0 ? 1 : 0);
405 const bool execute = dir != 0 && ((state &
LCA_BLOCKED) == 0);
413 to->registerHop(vehicle);
435 to->registerHop(vehicle);
436 to->lane->requireCollisionCheck();
457 to->registerHop(vehicle);
461 from->registerHop(vehicle);
462 from->lane->requireCollisionCheck();
472 shadow->hoppedVeh = vehicle;
480 #ifdef DEBUG_CONTINUE_CHANGE
483 <<
" continueChange veh=" << vehicle->
getID()
485 <<
" dir=" << direction
486 <<
" speedLat=" << speedLat
487 <<
" pastMidpoint=" << pastMidpoint
499 std::pair<MSVehicle* const, double>
503 #ifdef DEBUG_SURROUNDING_VEHICLES
505 std::cout <<
SIMTIME <<
" veh '" << vehicle->
getID() <<
"' looks for leader on lc-target lane '" << target->lane->getID() <<
"'." << std::endl;
511 #ifdef DEBUG_SURROUNDING_VEHICLES
513 if (neighLead != 0) {
514 std::cout <<
"Considering '" << neighLead->
getID() <<
"' at position " << neighLead->
getPositionOnLane() << std::endl;
520 if (target->hoppedVeh !=
nullptr) {
521 double hoppedPos = target->hoppedVeh->getPositionOnLane();
522 #ifdef DEBUG_SURROUNDING_VEHICLES
524 std::cout <<
"Considering hopped vehicle '" << target->hoppedVeh->getID() <<
"' at position " << hoppedPos << std::endl;
528 neighLead = target->hoppedVeh;
532 if (neighLead ==
nullptr) {
533 #ifdef DEBUG_SURROUNDING_VEHICLES
535 std::cout <<
"Looking for leader on consecutive lanes." << std::endl;
540 MSLane* targetLane = target->lane;
542 double leaderBack = targetLane->
getLength();
545 if (plBack < leaderBack &&
551 if (neighLead !=
nullptr) {
552 #ifdef DEBUG_SURROUNDING_VEHICLES
554 std::cout <<
" found leader=" << neighLead->
getID() <<
" (partial)\n";
563 if (seen > dist && !
myCandi->lane->isInternal()) {
564 #ifdef DEBUG_SURROUNDING_VEHICLES
566 std::cout <<
" found no leader within dist=" << dist <<
"\n";
569 return std::pair<MSVehicle* const, double>(
static_cast<MSVehicle*
>(
nullptr), -1);
573 std::pair<MSVehicle* const, double> result = target->lane->getLeaderOnConsecutive(dist, seen, speed, *vehicle, bestLaneConts);
574 #ifdef DEBUG_SURROUNDING_VEHICLES
581 #ifdef DEBUG_SURROUNDING_VEHICLES
583 std::cout <<
" found leader=" << neighLead->
getID() <<
"\n";
591 std::pair<MSVehicle* const, double>
595 #ifdef DEBUG_SURROUNDING_VEHICLES
597 std::cout <<
SIMTIME <<
" veh '" << vehicle->
getID() <<
"' looks for follower on lc-target lane '" << target->lane->getID() <<
"'." << std::endl;
603 #ifdef DEBUG_SURROUNDING_VEHICLES
605 if (neighFollow != 0) {
606 std::cout <<
"veh(target) returns '" << neighFollow->
getID() <<
"' at position " << neighFollow->
getPositionOnLane() << std::endl;
608 std::cout <<
"veh(target) returns none." << std::endl;
614 #ifdef DEBUG_SURROUNDING_VEHICLES
616 if (
getCloserFollower(candiPos, neighFollow, target->hoppedVeh) != neighFollow) {
617 std::cout <<
"Hopped vehicle '" << target->hoppedVeh->getID() <<
"' at position " << target->hoppedVeh->getPositionOnLane() <<
" is closer." << std::endl;
626 #ifdef DEBUG_SURROUNDING_VEHICLES
629 if (partialBehind != 0 && partialBehind != neighFollow) {
630 std::cout <<
"'Partial behind'-vehicle '" << target->lane->getPartialBehind(vehicle)->getID() <<
"' at position " << partialBehind->
getPositionOnLane() <<
" is closer." << std::endl;
635 neighFollow =
getCloserFollower(candiPos, neighFollow, target->lane->getPartialBehind(vehicle));
637 if (neighFollow ==
nullptr) {
639 #ifdef DEBUG_SURROUNDING_VEHICLES
641 if (consecutiveFollower.first == 0) {
642 std::cout <<
"no follower found." << std::endl;
644 std::cout <<
"found follower '" << consecutiveFollower.first->getID() <<
"' on consecutive lanes." << std::endl;
648 return std::make_pair(
const_cast<MSVehicle*
>(consecutiveFollower.first), consecutiveFollower.second);
650 #ifdef DEBUG_SURROUNDING_VEHICLES
652 std::cout <<
"found follower '" << neighFollow->
getID() <<
"'." << std::endl;
655 return std::pair<MSVehicle* const, double>(neighFollow,
679 const std::pair<MSVehicle* const, double>& leader,
680 const std::vector<MSVehicle::LaneQ>& preb)
const {
684 if (neighLead.first !=
nullptr && neighLead.first == neighFollow.first) {
687 neighFollow.first = 0;
690 return checkChange(laneOffset, target->lane, leader, neighLead, neighFollow, preb);
697 const std::pair<MSVehicle* const, double>& leader,
698 const std::pair<MSVehicle* const, double>& neighLead,
699 const std::pair<MSVehicle* const, double>& neighFollow,
700 const std::vector<MSVehicle::LaneQ>& preb)
const {
704 #ifdef DEBUG_CHECK_CHANGE
707 <<
"\n" <<
SIMTIME <<
" checkChange() for vehicle '" << vehicle->
getID() <<
"'"
716 if (neighFollow.first !=
nullptr && neighFollow.second < 0) {
720 #ifdef DEBUG_CHECK_CHANGE
723 <<
" overlapping with follower..."
729 if (neighLead.first !=
nullptr && neighLead.second < 0) {
732 #ifdef DEBUG_CHECK_CHANGE
735 <<
" overlapping with leader..."
747 if ((blocked & blockedByFollower) == 0 && neighFollow.first !=
nullptr) {
755 const double vNextFollower = neighFollow.first->getSpeed() +
MAX2(0., tauRemainder * neighFollow.first->getAcceleration());
758 secureBackGap = neighFollow.first->getCarFollowModel().getSecureGap(neighFollow.first, vehicle, vNextFollower,
760 #ifdef DEBUG_CHECK_CHANGE
763 <<
" backGap=" << neighFollow.second
764 <<
" vNextFollower=" << vNextFollower
765 <<
" vNextEgo=" << vNextLeader
766 <<
" secureGap=" << secureBackGap
773 blocked |= blockedByFollower;
778 if ((blocked & blockedByLeader) == 0 && neighLead.first !=
nullptr) {
787 const double vNextLeader = neighLead.first->getSpeed() +
MIN2(0., tauRemainder * neighLead.first->getAcceleration());
791 #ifdef DEBUG_CHECK_CHANGE
794 <<
" frontGap=" << neighFollow.second
795 <<
" vNextEgo=" << vNextFollower
796 <<
" vNextLeader=" << vNextLeader
797 <<
" secureGap=" << secureFrontGap
804 blocked |= blockedByLeader;
811 if (nextLeader.first != 0) {
815 #ifdef DEBUG_CHECK_CHANGE
817 std::cout <<
SIMTIME <<
" pedestrian on road " + leader.first->getID() <<
" gap=" << gap <<
" brakeGap=" << brakeGap <<
"\n";
820 if (brakeGap > gap) {
821 blocked |= blockedByLeader;
822 #ifdef DEBUG_CHECK_CHANGE
824 std::cout <<
SIMTIME <<
" blocked by pedestrian " + leader.first->getID() <<
"\n";
831 if (leader.first !=
nullptr) {
832 secureOrigFrontGap = vehicle->
getCarFollowModel().
getSecureGap(vehicle, leader.first, vehicle->
getSpeed(), leader.first->getSpeed(), leader.first->getCarFollowModel().getMaxDecel());
837 laneOffset, msg, blocked, leader, neighLead, neighFollow, *targetLane, preb, &(
myCandi->lastBlocked), &(
myCandi->firstBlocked));
846 const double speed = vehicle->
getSpeed();
848 if (seen < dist || myCandi->lane->isInternal()) {
849 std::pair<MSVehicle* const, double> neighLead2 = targetLane->
getCriticalLeader(dist, seen, speed, *vehicle);
850 if (neighLead2.first !=
nullptr && neighLead2.first != neighLead.first) {
852 neighLead2.first->getSpeed(), neighLead2.first->getCarFollowModel().getMaxDecel());
854 #ifdef DEBUG_SURROUNDING_VEHICLES
856 std::cout <<
SIMTIME <<
" found critical leader=" << neighLead2.first->getID()
857 <<
" gap=" << neighLead2.second <<
" secGap=" << secureGap <<
" secGap2=" << secureGap2 <<
"\n";
860 if (neighLead2.second < secureGap2) {
861 state |= blockedByLeader;
869 state |= blockedByLeader;
881 if (estimatedLCDuration == -1) {
883 #ifdef DEBUG_CHECK_CHANGE
885 std::cout <<
SIMTIME <<
" checkChange() too slow to guarantee completion of continuous lane change."
886 <<
"\nestimatedLCDuration=" << estimatedLCDuration
887 <<
"\ndistToNeighLane=" << distToNeighLane
895 const double avgSpeed = 0.5 * (
899 const double space2change = avgSpeed * estimatedLCDuration;
902 #ifdef DEBUG_CHECK_CHANGE
904 std::cout <<
SIMTIME <<
" checkChange() checking continuous lane change..."
905 <<
"\ndistToNeighLane=" << distToNeighLane
906 <<
" estimatedLCDuration=" << estimatedLCDuration
907 <<
" space2change=" << space2change
908 <<
" avgSpeed=" << avgSpeed
917 std::vector<MSLink*>::const_iterator link =
MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
918 while (!nextLane->
isLinkEnd(link) && seen <= space2change) {
922 || (nextLane->
getEdge().
isInternal() && (*link)->getViaLaneOrLane()->getEdge().isInternal())
927 if ((*link)->getViaLane() ==
nullptr) {
930 nextLane = (*link)->getViaLaneOrLane();
935 #ifdef DEBUG_CHECK_CHANGE
937 std::cout <<
" available distance=" << seen << std::endl;
940 if (nextLane->
isLinkEnd(link) && seen < space2change) {
941 #ifdef DEBUG_CHECK_CHANGE
943 std::cout <<
SIMTIME <<
" checkChange insufficientSpace: seen=" << seen <<
" space2change=" << space2change <<
"\n";
952 const double speed = vehicle->
getSpeed();
957 std::vector<MSLink*>::const_iterator nextLink =
MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
958 while (!nextLane->
isLinkEnd(nextLink) && seen <= space2change && seen <= dist) {
959 nextLane = (*nextLink)->getViaLaneOrLane();
961 if (parallelLane ==
nullptr) {
965 std::pair<MSVehicle* const, double> neighLead2 = parallelLane->
getLeader(vehicle, -seen, std::vector<MSLane*>());
966 if (neighLead2.first !=
nullptr && neighLead2.first != neighLead.first
968 vehicle->
getSpeed(), neighLead2.first->getSpeed(), neighLead2.first->getCarFollowModel().getMaxDecel()))) {
969 state |= blockedByLeader;
973 if ((*nextLink)->getViaLane() ==
nullptr) {
983 const int oldstate = state;
986 #ifdef DEBUG_CHECK_CHANGE
989 <<
" veh=" << vehicle->
getID()
1004 if (laneOffset != 0) {
1031 bool oppositeChangeByTraci =
false;
1034 if (isOpposite && (ret &
LCA_LEFT) != 0) {
1038 oppositeChangeByTraci =
true;
1040 if (!isOpposite && leader.first == 0 && !oppositeChangeByTraci) {
1046 if (!isOpposite && !oppositeChangeByTraci
1048 && leader.first != 0) {
1053 #ifdef DEBUG_CHANGE_OPPOSITE
1055 std::cout <<
" not overtaking leader " << leader.first->getID() <<
" that has blinker set\n";
1059 }
else if (leader.second < 0) {
1061 #ifdef DEBUG_CHANGE_OPPOSITE
1063 std::cout <<
" not overtaking leader " << leader.first->getID() <<
" with gap " << leader.second <<
"\n";
1077 int direction = isOpposite ? -1 : 1;
1078 std::pair<MSVehicle*, double> neighLead((
MSVehicle*)
nullptr, -1);
1081 double timeToOvertake;
1082 double spaceToOvertake;
1091 std::pair<MSVehicle*, double> overtaken;
1093 if (!isOpposite && !oppositeChangeByTraci) {
1095 if (overtaken.first == 0) {
1098 #ifdef DEBUG_CHANGE_OPPOSITE
1100 std::cout <<
" compute time/space to overtake for columnLeader=" << overtaken.first->getID() <<
" egoGap=" << overtaken.second <<
"\n";
1106 #ifdef DEBUG_CHANGE_OPPOSITE
1108 std::cout <<
" cannot changeOpposite due to upcoming stop (dist=" << vehicle->
nextStopDist() <<
" spaceToOvertake=" << spaceToOvertake <<
")\n";
1115 #ifdef DEBUG_CHANGE_OPPOSITE
1118 <<
" veh=" << vehicle->
getID()
1119 <<
" changeOpposite opposite=" << opposite->
getID()
1121 <<
" timeToOvertake=" << timeToOvertake
1122 <<
" spaceToOvertake=" << spaceToOvertake
1127 if (neighLead.first != 0) {
1128 const MSVehicle* oncoming = neighLead.first;
1134 const double surplusGap = neighLead.second - spaceToOvertake - timeToOvertake * oncomingSpeed - safetyGap;
1135 #ifdef DEBUG_CHANGE_OPPOSITE
1138 <<
" oncoming=" << oncoming->
getID()
1139 <<
" oncomingGap=" << neighLead.second
1140 <<
" leaderGap=" << leader.second
1141 <<
" safetyGap=" << safetyGap
1142 <<
" surplusGap=" << surplusGap
1146 if (surplusGap < 0) {
1148 #ifdef DEBUG_CHANGE_OPPOSITE
1150 std::cout <<
" cannot changeOpposite due to dangerous oncoming (surplusGap=" << surplusGap <<
")\n";
1154 #ifdef DEBUG_CHANGE_OPPOSITE
1157 std::cout <<
SIMTIME <<
" ego=" << vehicle->
getID() <<
" does not changeOpposite due to dangerous oncoming " << oncoming->
getID() <<
" (but the leader is also opposite)\n";
1165 }
else if (!oppositeChangeByTraci) {
1166 timeToOvertake = -1;
1168 spaceToOvertake = std::numeric_limits<double>::max();
1171 double gap = leader.second;
1172 while (leader.first !=
nullptr && leader.first->getLaneChangeModel().isOpposite() && dist > 0) {
1174 #ifdef DEBUG_CHANGE_OPPOSITE
1176 std::cout <<
SIMTIME <<
" ego=" << vehicle->
getID() <<
" opposite leader=" << leader.first->getID() <<
" gap=" << gap <<
" is driving against the flow\n";
1179 const double gapToLeaderFront = leader.second + leader.first->getVehicleType().getLengthWithGap();
1180 if (gapToLeaderFront < 0) {
1183 dist -= gapToLeaderFront;
1185 if (leader.first != 0) {
1186 gap += gapToLeaderFront;
1189 leader.second = gap;
1200 if (usableDist < spaceToOvertake) {
1203 assert(bestLaneConts.size() >= 1);
1204 std::vector<MSLane*>::const_iterator it = bestLaneConts.begin() + 1;
1205 while (usableDist < spaceToOvertake && it != bestLaneConts.end()) {
1206 #ifdef DEBUG_CHANGE_OPPOSITE
1208 std::cout <<
" usableDist=" << usableDist <<
" opposite=" <<
Named::getIDSecure((*it)->getOpposite()) <<
"\n";
1211 if ((*it)->getOpposite() ==
nullptr || !(*it)->getOpposite()->allowsVehicleClass(vehicle->
getVClass())) {
1216 const MSLane*
const prev = *(it - 1);
1217 if (prev !=
nullptr) {
1226 #ifdef DEBUG_CHANGE_OPPOSITE
1228 std::cout <<
" stop lookahead at link=" << (link == 0 ?
"NULL" : link->
getViaLaneOrLane()->
getID()) <<
" state=" << (link == 0 ?
"?" :
toString(link->getState())) <<
" ignoreRed=" << vehicle->
ignoreRed(link,
true) <<
"\n";
1234 usableDist += (*it)->getLength();
1238 if (!isOpposite && usableDist < spaceToOvertake) {
1239 #ifdef DEBUG_CHANGE_OPPOSITE
1241 std::cout <<
" cannot changeOpposite due to insufficient space (seen=" << usableDist <<
" spaceToOvertake=" << spaceToOvertake <<
")\n";
1246 #ifdef DEBUG_CHANGE_OPPOSITE
1248 std::cout <<
" usableDist=" << usableDist <<
" spaceToOvertake=" << spaceToOvertake <<
" timeToOvertake=" << timeToOvertake <<
"\n";
1253 std::vector<MSVehicle::LaneQ> preb = vehicle->
getBestLanes();
1268 if (leader.first != 0) {
1269 if (!leader.first->getLaneChangeModel().isOpposite()) {
1275 laneQ.
length =
MIN2(laneQ.
length, leader.second / 2 + forwardPos - safetyGap);
1276 #ifdef DEBUG_CHANGE_OPPOSITE
1278 std::cout <<
SIMTIME <<
" found oncoming leader=" << oncoming->
getID() <<
" gap=" << leader.second <<
"\n";
1282 #ifdef DEBUG_CHANGE_OPPOSITE
1284 std::cout <<
SIMTIME <<
" opposite leader=" << leader.first->getID() <<
" gap=" << leader.second <<
" is driving against the flow\n";
1288 if (neighLead.first != 0) {
1290 if (overtaken.first == 0) {
1291 #ifdef DEBUG_CHANGE_OPPOSITE
1293 std::cout <<
SIMTIME <<
" ego=" << vehicle->
getID() <<
" did not find columnleader to overtake\n";
1297 const double remainingDist = laneQ.
length - forwardPos;
1299 #ifdef DEBUG_CHANGE_OPPOSITE
1301 std::cout <<
SIMTIME <<
" ego=" << vehicle->
getID() <<
" is overtaking " << overtaken.first->getID()
1302 <<
" remainingDist=" << remainingDist <<
" spaceToOvertake=" << spaceToOvertake <<
" timeToOvertake=" << timeToOvertake <<
"\n";
1305 if (remainingDist > spaceToOvertake) {
1314 #ifdef DEBUG_CHANGE_OPPOSITE
1316 std::cout <<
SIMTIME <<
" veh=" << vehicle->
getID() <<
" remaining dist=" << laneQ.
length - forwardPos <<
" forwardPos=" << forwardPos <<
" laneQ.length=" << laneQ.
length <<
"\n";
1321 int state =
checkChange(direction, opposite, leader, neighLead, neighFollow, preb);
1324 bool changingAllowed = (state &
LCA_BLOCKED) == 0;
1330 #ifdef DEBUG_CHANGE_OPPOSITE
1332 std::cout <<
SIMTIME <<
" changing to opposite veh=" << vehicle->
getID() <<
" dir=" << direction <<
" opposite=" <<
Named::getIDSecure(opposite) <<
" state=" << state <<
"\n";
1340 #ifdef DEBUG_CHANGE_OPPOSITE
1342 std::cout <<
SIMTIME <<
" not changing to opposite veh=" << vehicle->
getID() <<
" dir=" << direction
1360 const double v = vehicle->
getSpeed();
1364 const double g =
MAX2(0.0, (
1373 const double sign = -1;
1377 double t = (u - v - sqrt(4 * (u - v) * (u - v) + 8 * a * g) * sign * 0.5) / a;
1378 #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
1380 std::cout <<
" computeOvertakingTime v=" << v <<
" vMax=" << vMax <<
" u=" << u <<
" a=" << a <<
" d=" << d <<
" gap=" << gap <<
" g=" << g <<
" t=" << t
1381 <<
" distEgo=" << v* t + t* t* a * 0.5 <<
" distLead=" << g + u* t
1390 t = ceil(t /
TS) *
TS;
1393 const double timeToMaxSpeed = (vMax - v) / a;
1395 #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
1397 std::cout <<
" t=" << t <<
" tvMax=" << timeToMaxSpeed <<
"\n";
1400 if (t <= timeToMaxSpeed) {
1402 spaceToOvertake = v * t + t * t * a * 0.5;
1403 #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
1405 std::cout <<
" sto=" << spaceToOvertake <<
"\n";
1410 const double s = v * timeToMaxSpeed + timeToMaxSpeed * timeToMaxSpeed * a * 0.5;
1411 const double m = timeToMaxSpeed;
1414 t = (g - s + m * vMax) / (vMax - u);
1417 #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
1419 std::cout <<
" t2=" << t <<
"\n";
1422 timeToOvertake = std::numeric_limits<double>::max();
1423 spaceToOvertake = std::numeric_limits<double>::max();
1428 t = ceil(t /
TS) *
TS;
1431 spaceToOvertake = s + (t - m) * vMax;
1432 #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
1434 std::cout <<
" t2=" << t <<
" s=" << s <<
" sto=" << spaceToOvertake <<
" m=" << m <<
"\n";
1440 timeToOvertake *= safetyFactor;
1441 spaceToOvertake *= safetyFactor;
1442 #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
1444 if (safetyFactor != 1) {
1445 std::cout <<
" applying safetyFactor=" << safetyFactor
1446 <<
" tto=" << timeToOvertake <<
" sto=" << spaceToOvertake <<
"\n";
1455 std::pair<MSVehicle*, double>
1457 assert(leader.first != 0);
1462 std::pair<MSVehicle*, double> columnLeader = leader;
1463 double egoGap = leader.second;
1464 bool foundSpaceAhead =
false;
1465 double seen = leader.second + leader.first->getVehicleType().getLengthWithGap();
1467 if (maxLookAhead == std::numeric_limits<double>::max()) {
1472 #ifdef DEBUG_CHANGE_OPPOSITE
1474 std::cout <<
" getColumnleader vehicle=" << vehicle->
getID() <<
" leader=" << leader.first->getID() <<
" gap=" << leader.second <<
" maxLookAhead=" << maxLookAhead <<
"\n";
1478 while (!foundSpaceAhead) {
1479 const double requiredSpaceAfterLeader = (columnLeader.first->getCarFollowModel().getSecureGap(
1480 columnLeader.first, vehicle,
1482 + columnLeader.first->getVehicleType().getMinGap()
1487 const bool checkTmpVehicles = (&columnLeader.first->getLane()->getEdge() == &source->
getEdge());
1488 std::pair<MSVehicle* const, double> leadLead = columnLeader.first->getLane()->getLeader(
1489 columnLeader.first, columnLeader.first->getPositionOnLane(), conts, requiredSpaceAfterLeader + mergeBrakeGap,
1492 #ifdef DEBUG_CHANGE_OPPOSITE
1494 std::cout <<
" leadLead=" <<
Named::getIDSecure(leadLead.first) <<
" gap=" << leadLead.second <<
"\n";
1497 if (leadLead.first ==
nullptr) {
1498 double availableSpace = columnLeader.first->getLane()->getLength() - columnLeader.first->getPositionOnLane();
1499 const double requiredSpace = safetyFactor * (requiredSpaceAfterLeader
1501 #ifdef DEBUG_CHANGE_OPPOSITE
1503 std::cout <<
" no direct leader found after columnLeader " << columnLeader.first->getID()
1504 <<
" availableSpace=" << availableSpace
1505 <<
" req1=" << requiredSpaceAfterLeader
1506 <<
" req2=" << requiredSpace / safetyFactor
1507 <<
" req3=" << requiredSpace
1511 if (availableSpace > requiredSpace) {
1512 foundSpaceAhead =
true;
1517 #ifdef DEBUG_CHANGE_OPPOSITE
1519 std::cout <<
" look for another leader on lane " <<
Named::getIDSecure(next) <<
"\n";
1522 while (next !=
nullptr && seen < maxLookAhead) {
1525 if (cand ==
nullptr) {
1527 if (availableSpace > requiredSpace) {
1528 foundSpaceAhead =
true;
1533 if (availableSpace > requiredSpace) {
1534 foundSpaceAhead =
true;
1541 if (!foundSpaceAhead) {
1542 return std::make_pair(
nullptr, -1);
1546 const double requiredSpace = safetyFactor * (requiredSpaceAfterLeader
1548 overtakingSpeed, leadLead.first->
getSpeed(), leadLead.first->getCarFollowModel().getMaxDecel()));
1549 #ifdef DEBUG_CHANGE_OPPOSITE
1551 std::cout <<
" leader's leader " << leadLead.first->getID() <<
" space=" << leadLead.second
1552 <<
" req1=" << requiredSpaceAfterLeader
1553 <<
" req2=" << requiredSpace / safetyFactor
1554 <<
" req3=" << requiredSpace
1558 if (leadLead.second > requiredSpace) {
1559 foundSpaceAhead =
true;
1561 #ifdef DEBUG_CHANGE_OPPOSITE
1563 std::cout <<
" not enough space after columnLeader=" << columnLeader.first->getID() <<
" required=" << requiredSpace <<
"\n";
1566 seen +=
MAX2(0., leadLead.second) + leadLead.first->getVehicleType().getLengthWithGap();
1567 if (seen > maxLookAhead) {
1568 #ifdef DEBUG_CHANGE_OPPOSITE
1570 std::cout <<
" cannot changeOpposite due to insufficient free space after columnLeader (seen=" << seen <<
" columnLeader=" << columnLeader.first->getID() <<
")\n";
1573 return std::make_pair(
nullptr, -1);
1576 egoGap += columnLeader.first->getVehicleType().getLengthWithGap() + leadLead.second;
1577 columnLeader = leadLead;
1578 #ifdef DEBUG_CHANGE_OPPOSITE
1580 std::cout <<
" new columnLeader=" << columnLeader.first->getID() <<
"\n";
1586 columnLeader.second = egoGap;
1587 return columnLeader;
1593 for (
auto it = conts.begin(); it != conts.end(); ++it) {
1595 if (it + 1 != conts.end()) {
#define OPPOSITE_OVERTAKING_MAX_LOOKAHEAD
#define OPPOSITE_OVERTAKING_SAFETYGAP_HEADWAY_FACTOR
#define OPPOSITE_OVERTAKING_MAX_LOOKAHEAD_EMERGENCY
#define OPPOSITE_OVERTAKING_ONCOMING_LOOKAHEAD
#define OPPOSITE_OVERTAKING_SAFETY_FACTOR
#define OPPOSITE_OVERTAKING_SAFE_TIMEGAP
std::pair< const MSVehicle *, double > CLeaderDist
std::pair< const MSPerson *, double > PersonDist
@ SVC_EMERGENCY
public emergency vehicles
@ RIGHT
The link is a (hard) right direction.
@ LEFT
The link is a (hard) left direction.
@ STRAIGHT
The link is a straight direction.
@ LINKSTATE_ZIPPER
This is an uncontrolled, zipper-merge link.
LaneChangeAction
The state of a vehicle's lane-change behavior.
@ 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_INSUFFICIENT_SPACE
The vehicle does not have enough space to complete a continuous change before the next turn.
@ LCA_BLOCKED_BY_LEFT_FOLLOWER
The vehicle is blocked by left follower.
@ LCA_COOPERATIVE
The action is done to help someone else.
@ LCA_INSUFFICIENT_SPEED
Vehicle is too slow to complete a continuous lane change (in case that maxSpeedLatStanding==0)
@ LCA_OVERLAPPING
The vehicle is blocked being overlapping.
@ LCA_LEFT
Wants go to the left.
@ LCA_BLOCKED_BY_RIGHT_FOLLOWER
The vehicle is blocked by right follower.
@ LCA_TRACI
The action is due to a TraCI request.
@ LCA_WANTS_LANECHANGE
lane can change
@ LCA_RIGHT
Wants go to the right.
@ LCA_BLOCKED_BY_LEFT_LEADER
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.
void saveLCState(const int dir, const int stateWithoutTraCI, const int state)
void setFollowerGaps(CLeaderDist follower, double secGap)
virtual void setOwnState(const int state)
virtual double getAssumedDecelForLaneChangeDuration() const
Returns a deceleration value which is used for the estimation of the duration of a lane change.
virtual double estimateLCDuration(const double speed, const double remainingManeuverDist, const double decel) const
Calculates the maximal time needed to complete a lane change maneuver if lcMaxSpeedLatFactor and lcMa...
int getLaneChangeDirection() const
return the direction of the current lane change maneuver
MSLane * getShadowLane() const
Returns the lane the vehicle's shadow is on during continuous/sublane lane change.
bool startLaneChangeManeuver(MSLane *source, MSLane *target, int direction)
start the lane change maneuver and return whether it continues
virtual int wantsChange(int laneOffset, MSAbstractLaneChangeModel::MSLCMessager &msgPass, int blocked, const std::pair< MSVehicle *, double > &leader, 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)
Called to examine whether the vehicle wants to change using the given laneOffset. This method gets th...
void endLaneChangeManeuver(const MSMoveReminder::Notification reason=MSMoveReminder::NOTIFICATION_LANE_CHANGE)
virtual double getSafetyFactor() const
return factor for modifying the safety constraints of the car-following model
static const double NO_NEIGHBOR
virtual double getOppositeSafetyFactor() const
return factor for modifying the safety constraints for opposite-diretction overtaking of the car-foll...
void setLeaderGaps(CLeaderDist, double secGap)
virtual LaneChangeModel getModelID() const =0
Returns the model's ID;.
void setOrigLeaderGaps(CLeaderDist, double secGap)
void setSpeedLat(double speedLat)
set the lateral speed and update lateral acceleraton
void checkTraCICommands()
Check for commands issued for the vehicle via TraCI and apply the appropriate state changes For the s...
void primaryLaneChanged(MSLane *source, MSLane *target, int direction)
called once when the vehicles primary lane changes
int getShadowDirection() const
return the direction in which the current shadow lane lies
double getSpeedLat() const
return the lateral speed of the current lane change maneuver
bool alreadyChanged() const
reset the flag whether a vehicle already moved to false
bool isChangingLanes() const
return true if the vehicle currently performs a lane change maneuver
void clearNeighbors()
Clear info on neighboring vehicle from previous step.
void saveNeighbors(const int dir, const MSLeaderDistanceInfo &followers, const MSLeaderDistanceInfo &leaders)
Saves the lane change relevant vehicles, which are currently on neighboring lanes in the given direct...
const MSVehicleType & getVehicleType() const
Returns the vehicle's type definition.
SUMOVehicleClass getVClass() const
Returns the vehicle's access class.
bool isStopped() const
Returns whether the vehicle is at a stop.
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 getMaxAccel() const
Get the vehicle type's maximum acceleration [m/s^2].
double brakeGap(const double speed) const
Returns the distance the vehicle needs to halt including driver's reaction time tau (i....
double getMaxDecel() const
Get the vehicle type's maximal comfortable deceleration [m/s^2].
virtual double getHeadwayTime() const
Get the driver's desired headway [s].
bool isInternal() const
return whether this edge is an internal edge
static bool gLefthand
Whether lefthand-drive is being simulated.
static SUMOTime gLaneChangeDuration
virtual ~MSLaneChanger()
Destructor.
bool applyTraCICommands(MSVehicle *vehicle)
Execute TraCI LC-commands.
bool continueChange(MSVehicle *vehicle, ChangerIt &from)
continue a lane change maneuver and return whether the vehicle has completely moved onto the new lane...
const bool myChangeToOpposite
whether this edge allows changing to the opposite direction edge
static std::pair< MSVehicle *, double > getColumnleader(MSVehicle *vehicle, std::pair< MSVehicle *, double > leader, double maxLookAhead=std::numeric_limits< double >::max())
MSLaneChanger()
Default constructor.
void checkTraCICommands(MSVehicle *vehicle)
Take into account traci LC-commands.
void laneChange(SUMOTime t)
Start lane-change-process for all vehicles on the edge'e lanes.
virtual void initChanger()
Initialize the changer before looping over all vehicles.
bool vehInChanger() const
Check if there is a single change-candidate in the changer. Returns true if there is one.
std::pair< MSVehicle *const, double > getRealFollower(const ChangerIt &target) const
int checkChange(int laneOffset, const MSLane *targetLane, const std::pair< MSVehicle *const, double > &leader, const std::pair< MSVehicle *const, double > &neighLead, const std::pair< MSVehicle *const, double > &neighFollow, const std::vector< MSVehicle::LaneQ > &preb) const
bool startChange(MSVehicle *vehicle, ChangerIt &from, int direction)
const bool myAllowsChanging
static void computeOvertakingTime(const MSVehicle *vehicle, const MSVehicle *leader, double gap, double &timeToOvertake, double &spaceToOvertake)
Compute the time and space required for overtaking the given leader.
int checkChangeWithinEdge(int laneOffset, const std::pair< MSVehicle *const, double > &leader, const std::vector< MSVehicle::LaneQ > &preb) const
MSVehicle * veh(ConstChangerIt ce) const
Changer myChanger
Container for ChangeElemements, one for every lane in the edge.
virtual bool changeOpposite(std::pair< MSVehicle *, double > leader)
ChangerIt findCandidate()
Find current candidate. If there is none, myChanger.end() is returned.
bool mayChange(int direction) const
whether changing to the lane in the given direction should be considered
static MSVehicle * getCloserFollower(const double maxPos, MSVehicle *follow1, MSVehicle *follow2)
return the closer follower of ego
void registerUnchanged(MSVehicle *vehicle)
void updateLanes(SUMOTime t)
std::pair< MSVehicle *const, double > getRealLeader(const ChangerIt &target) const
Changer::iterator ChangerIt
the iterator moving over the ChangeElems
static MSLane * getLaneAfter(const MSLane *lane, const std::vector< MSLane * > &conts)
return the next lane in conts beyond lane or nullptr
virtual void updateChanger(bool vehHasChanged)
Representation of a lane in the micro simulation.
MSLane * getParallelLane(int offset, bool includeOpposite=true) const
Returns the lane with the given offset parallel to this one or 0 if it does not exist.
static std::vector< MSLink * >::const_iterator succLinkSec(const SUMOVehicle &veh, int nRouteSuccs, const MSLane &succLinkSource, const std::vector< MSLane * > &conts)
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...
const MSLink * getLinkTo(const MSLane *const) const
returns the link to the given lane or nullptr, if it is not connected
double getSpeedLimit() const
Returns the lane's maximum allowed speed.
std::vector< MSVehicle * > VehCont
Container for vehicles.
double getLength() const
Returns the lane's length.
std::pair< MSVehicle *const, double > getCriticalLeader(double dist, double seen, double speed, const MSVehicle &veh) const
Returns the most dangerous leader and the distance to him.
bool isLinkEnd(std::vector< MSLink * >::const_iterator &i) const
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.
std::pair< MSVehicle *const, double > getOppositeFollower(const MSVehicle *ego) const
bool hasPedestrians() const
whether the lane has pedestrians on it
void requireCollisionCheck()
require another collision check due to relevant changes in the simulation
double getOppositePos(double pos) const
return the corresponding position on the opposite lane
MSVehicle * getLastAnyVehicle() const
returns the last vehicle that is fully or partially on this lane
VehCont myPartialVehicles
The lane's partial vehicles. This container holds all vehicles that are partially on this lane but wh...
MSEdge & getEdge() const
Returns the lane's edge.
std::pair< MSVehicle *const, double > getLeader(const MSVehicle *veh, const double vehPos, const std::vector< MSLane * > &bestLaneConts, double dist=-1, bool checkTmpVehicles=false) const
Returns the immediate leader of veh and the distance to veh starting on this lane.
MSLane * getOpposite() const
return the opposite direction lane for lane changing or 0
std::pair< MSVehicle *const, double > getOppositeLeader(const MSVehicle *ego, double dist, bool oppositeDir) const
double getWidth() const
Returns the lane's width.
LinkState getState() const
Returns the current state of the link.
MSLane * getViaLaneOrLane() const
return the via lane if it exists and the lane otherwise
bool havePriority() const
Returns whether this link is a major link.
LinkDirection getDirection() const
Returns the direction the vehicle passing this link take.
bool haveRed() const
Returns whether this link is blocked by a red (or redyellow) traffic light.
bool getRespectJunctionPriority() const
Returns whether junction priority rules shall be respected.
SUMOTime getLaneTimeLineDuration()
double myPosLat
the stored lateral position
Representation of a vehicle in the micro simulation.
void updateBestLanes(bool forceRebuild=false, const MSLane *startLane=0)
computes the best lanes to use in order to continue the route
double computeAngle() const
compute the current vehicle angle
bool isStoppedOnLane() const
bool isActive() const
Returns whether the current simulation step is an action point for the vehicle.
bool isFrontOnLane(const MSLane *lane) const
Returns the information whether the front of the vehicle is on the given lane.
MSAbstractLaneChangeModel & getLaneChangeModel()
MSLane * getMutableLane() const
Returns the lane the vehicle is on Non const version indicates that something volatile is going on.
int influenceChangeDecision(int state)
allow TraCI to influence a lane change decision
bool isRemoteControlled() const
Returns the information whether the vehicle is fully controlled via TraCI.
double nextStopDist() const
return the distance to the next stop or doubleMax if there is none.
void adaptBestLanesOccupation(int laneIndex, double density)
update occupation from MSLaneChanger
double getAcceleration() const
Returns the vehicle's acceleration in m/s (this is computed as the last step's mean acceleration in c...
const std::vector< MSLane * > & getBestLanesContinuation() const
Returns the best sequence of lanes to continue the route starting at myLane.
double getBackPositionOnLane(const MSLane *lane) const
Get the vehicle's position relative to the given lane.
@ VEH_SIGNAL_BLINKER_RIGHT
Right blinker lights are switched on.
@ VEH_SIGNAL_BLINKER_LEFT
Left blinker lights are switched on.
SUMOTime getActionStepLength() const
Returns the vehicle's action step length in millisecs, i.e. the interval between two action points.
Influencer & getInfluencer()
double getRightSideOnLane() const
Get the vehicle's lateral position on the lane:
bool unsafeLinkAhead(const MSLane *lane) const
whether the vehicle may safely move to the given lane with regard to upcoming links
double getLateralPositionOnLane() const
Get the vehicle's lateral position on the lane.
double getSpeed() const
Returns the vehicle's current speed.
Position myCachedPosition
const std::vector< LaneQ > & getBestLanes() const
Returns the description of best lanes to use in order to continue the route.
double myAngle
the angle in radians (
bool ignoreRed(const MSLink *link, bool canBrake) const
decide whether a red (or yellow light) may be ignore
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.
bool hasInfluencer() const
State myState
This Vehicles driving state (pos and speed)
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 getLength() const
Get vehicle's length [m].
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.
static const Position INVALID
used to indicate that a position is valid
ChangeElem(MSLane *_lane)
void registerHop(MSVehicle *vehicle)
Register that vehicle belongs to Changer Item to after LC decisions.
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.