43 #define MAGIC_OFFSET 1. 44 #define LOOK_FORWARD (double)10. 46 #define JAM_FACTOR (double)1. 49 #define LCA_RIGHT_IMPATIENCE (double)-1. 50 #define CUT_IN_LEFT_SPEED_THRESHOLD (double)27. 51 #define MAX_ONRAMP_LENGTH (double)200. 53 #define LOOK_AHEAD_MIN_SPEED 0.0 54 #define LOOK_AHEAD_SPEED_MEMORY 0.9 56 #define HELP_DECEL_FACTOR (double)1.0 58 #define HELP_OVERTAKE (double)(10.0 / 3.6) 59 #define MIN_FALLBEHIND (double)(7.0 / 3.6) 61 #define KEEP_RIGHT_HEADWAY (double)2.0 63 #define URGENCY (double)2.0 65 #define ROUNDABOUT_DIST_BONUS (double)100.0 67 #define KEEP_RIGHT_TIME (double)5.0 // the number of seconds after which a vehicle should move to the right lane 68 #define KEEP_RIGHT_ACCEPTANCE (double)7.0 // calibration factor for determining the desire to keep right 70 #define RELGAIN_NORMALIZATION_MIN_SPEED (double)10.0 72 #define TURN_LANE_DIST (double)200.0 // the distance at which a lane leading elsewhere is considered to be a turn-lane that must be avoided 73 #define GAIN_PERCEPTION_THRESHOLD 0.05 // the minimum relative speed gain which affects the behavior 75 #define SPEED_GAIN_MIN_SECONDS 20.0 77 #define ARRIVALPOS_LAT_THRESHOLD 100.0 80 #define LATGAP_SPEED_THRESHOLD (50 / 3.6) 83 #define LATGAP_SPEED_THRESHOLD2 (50 / 3.6) 86 #define SPEEDGAIN_DECAY_FACTOR 0.5 88 #define SPEEDGAIN_MEMORY_FACTOR 0.5 111 #define DEBUG_COND (myVehicle.isSelected()) 123 mySpeedGainProbabilityRight(0),
124 mySpeedGainProbabilityLeft(0),
125 myKeepRightProbability(0),
126 myLeadingBlockerLength(0),
130 myCanChangeFully(true),
131 mySafeLatDistRight(0),
132 mySafeLatDistLeft(0),
141 v.getVehicleType().getMinGapLat()) /
142 v.getVehicleType().getMinGapLat()))),
145 myMinImpatience(myImpatience),
183 const std::vector<MSVehicle::LaneQ>& preb,
186 double& latDist,
double& maneuverDist,
int& blocked) {
189 const std::string changeType = laneOffset == -1 ?
"right" : (laneOffset == 1 ?
"left" :
"current");
191 #ifdef DEBUG_MANEUVER 200 <<
" considerChangeTo=" << changeType
207 leaders, followers, blockers,
208 neighLeaders, neighFollowers, neighBlockers,
210 lastBlocked, firstBlocked, latDist, maneuverDist, blocked);
212 result =
keepLatGap(result, leaders, followers, blockers,
213 neighLeaders, neighFollowers, neighBlockers,
214 neighLane, laneOffset, latDist, maneuverDist, blocked);
216 result |=
getLCA(result, latDist);
218 double latDistTmp = latDist;
221 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" maneuverDist=" << maneuverDist <<
" latDist=" << latDistTmp <<
" mySpeedPrev=" <<
mySpeedLat <<
" speedLat=" <<
DIST2SPEED(latDist) <<
" latDist2=" << latDist <<
"\n";
224 #if defined(DEBUG_MANEUVER) || defined(DEBUG_STATE) 229 <<
" wantsChangeTo=" << changeType
230 <<
" latDist=" << latDist
231 <<
" maneuverDist=" << maneuverDist
239 <<
" wantsNoChangeTo=" << changeType
272 std::cout <<
" myCanChangeFully=true\n";
297 const double newSpeed =
_patchSpeed(
MAX2(min, 0.0), wanted, max, cfModel);
298 #ifdef DEBUG_PATCHSPEED 300 const std::string patched = (wanted != newSpeed ?
" patched=" +
toString(newSpeed) :
"");
307 <<
" wanted=" << wanted
330 #ifdef DEBUG_PATCHSPEED 340 #ifdef DEBUG_PATCHSPEED 342 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" slowing down for leading blocker, safe=" << safe << (safe +
NUMERICAL_EPS < min ?
" (not enough)" :
"") <<
"\n";
345 return MAX2(min, safe);
349 double nVSafe = wanted;
353 if (v >= min && v <= max) {
354 nVSafe =
MIN2(v, nVSafe);
356 #ifdef DEBUG_PATCHSPEED 362 #ifdef DEBUG_PATCHSPEED 365 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" ignoring low nVSafe=" << v <<
" min=" << min <<
"\n";
369 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" ignoring high nVSafe=" << v <<
" max=" << max <<
"\n";
377 #ifdef DEBUG_PATCHSPEED 390 #if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE) 395 return (max + wanted) / (double) 2.0;
399 #if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE) 404 return (min + wanted) / (double) 2.0;
407 #if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE) 412 return (max + wanted) / (double) 2.0;
453 #if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE) 458 return (max + wanted) / (double) 2.0;
462 #if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE) 482 if (pinfo->first >= 0) {
491 <<
" informedBy=" << sender->
getID()
492 <<
" info=" << pinfo->second
493 <<
" vSafe=" << pinfo->first
506 assert(cld.first != 0);
515 double remainingSeconds) {
521 plannedSpeed =
MIN2(plannedSpeed, v);
526 std::cout <<
" informLeader speed=" <<
myVehicle.
getSpeed() <<
" planned=" << plannedSpeed <<
"\n";
531 assert(neighLead.first != 0);
534 if (
gDebugFlag2) std::cout <<
" blocked by leader nv=" << nv->
getID() <<
" nvSpeed=" << nv->
getSpeed() <<
" needGap=" 538 const double dv = plannedSpeed - nv->
getSpeed();
539 const double overtakeDist = (neighLead.second
551 || dv * remainingSeconds < overtakeDist) {
566 <<
" cannot overtake leader nv=" << nv->
getID()
568 <<
" remainingSeconds=" << remainingSeconds
569 <<
" targetSpeed=" << targetSpeed
570 <<
" nextSpeed=" << nextSpeed
581 <<
" cannot overtake fast leader nv=" << nv->
getID()
583 <<
" remainingSeconds=" << remainingSeconds
584 <<
" targetSpeed=" << targetSpeed
595 <<
" wants to overtake leader nv=" << nv->
getID()
597 <<
" remainingSeconds=" << remainingSeconds
598 <<
" currentGap=" << neighLead.second
600 <<
" overtakeDist=" << overtakeDist
610 }
else if (neighLead.first != 0) {
613 double dv, nextNVSpeed;
633 std::cout <<
" not blocked by leader nv=" << nv->
getID()
635 <<
" gap=" << neighLead.second
636 <<
" nextGap=" << neighLead.second - dv
638 <<
" targetSpeed=" << targetSpeed
642 return MIN2(targetSpeed, plannedSpeed);
654 double remainingSeconds,
655 double plannedSpeed) {
657 assert(neighFollow.first != 0);
660 if (
gDebugFlag2) std::cout <<
" blocked by follower nv=" << nv->
getID() <<
" nvSpeed=" << nv->
getSpeed() <<
" needGap=" 667 if ((neededGap - neighFollow.second) / remainingSeconds < (plannedSpeed - nv->
getSpeed())) {
670 std::cout <<
" wants to cut in before nv=" << nv->
getID() <<
" without any help neededGap=" << neededGap <<
"\n";
689 const double neighNewSpeed1s =
MAX2(0., nv->
getSpeed() - helpDecel);
690 const double dv = plannedSpeed - neighNewSpeed1s;
692 const double decelGap = neighFollow.second + dv;
698 <<
" egoNV=" << plannedSpeed
699 <<
" nvNewSpeed=" << neighNewSpeed
700 <<
" nvNewSpeed1s=" << neighNewSpeed1s
701 <<
" deltaGap=" << dv
702 <<
" decelGap=" << decelGap
703 <<
" secGap=" << secureGap
707 if (decelGap > 0 && decelGap >= secureGap) {
722 std::cout <<
" wants to cut in before nv=" << nv->
getID()
723 <<
" vsafe1=" << vsafe1
724 <<
" vsafe=" << vsafe
729 }
else if (dv > 0 && dv * remainingSeconds > (secureGap - decelGap +
POSITION_EPS)) {
734 std::cout <<
" wants to cut in before nv=" << nv->
getID() <<
" (eventually)\n";
742 std::cout <<
" wants to cut in before nv=" << nv->
getID() <<
" (nv cannot overtake right)\n";
758 std::cout <<
" wants right follower to slow down a bit\n";
764 std::cout <<
" wants to cut in before right follower nv=" << nv->
getID() <<
" (eventually)\n";
773 const double overtakeDist = (neighFollow.second
779 const double needDV = overtakeDist / remainingSeconds;
787 <<
" wants to be overtaken by=" << nv->
getID()
788 <<
" overtakeDist=" << overtakeDist
790 <<
" vhelp=" << vhelp
791 <<
" needDV=" << needDV
797 }
else if (neighFollow.first != 0) {
807 std::cout <<
" wants to cut in before non-blocking follower nv=" << nv->
getID() <<
"\n";
815 const std::vector<CLeaderDist>& blockers,
816 double remainingSeconds) {
823 plannedSpeed =
MIN2(plannedSpeed, safe);
826 for (std::vector<CLeaderDist>::const_iterator it = blockers.begin(); it != blockers.end(); ++it) {
827 plannedSpeed =
MIN2(plannedSpeed,
informLeader(blocked, dir, *it, remainingSeconds));
835 const std::vector<CLeaderDist>& blockers,
836 double remainingSeconds,
837 double plannedSpeed) {
838 for (std::vector<CLeaderDist>::const_iterator it = blockers.begin(); it != blockers.end(); ++it) {
839 informFollower(blocked, dir, *it, remainingSeconds, plannedSpeed);
867 const double halfWidth =
getWidth() * 0.5;
877 std::vector<double> newExpectedSpeeds;
886 const std::vector<MSLane*>& lanes = currEdge->
getLanes();
887 for (std::vector<MSLane*>::const_iterator it_lane = lanes.begin(); it_lane != lanes.end(); ++it_lane) {
889 for (
int i = 0; i < subLanes; ++i) {
890 newExpectedSpeeds.push_back((*it_lane)->getVehicleMaxSpeed(&
myVehicle));
898 if (subLaneShift < std::numeric_limits<int>::max()) {
900 const int newI = i + subLaneShift;
901 if (newI > 0 && newI < (
int)newExpectedSpeeds.size()) {
919 const std::vector<MSLane*>& lanes = prevEdge->
getLanes();
920 for (std::vector<MSLane*>::const_iterator it_lane = lanes.begin(); it_lane != lanes.end(); ++it_lane) {
921 const MSLane* lane = *it_lane;
922 for (MSLinkCont::const_iterator it_link = lane->
getLinkCont().begin(); it_link != lane->
getLinkCont().end(); ++it_link) {
923 if (&((*it_link)->getLane()->getEdge()) == curEdge) {
925 const MSLane* target = (*it_link)->getLane();
926 const std::vector<MSLane*>& lanes2 = curEdge->
getLanes();
927 for (std::vector<MSLane*>::const_iterator it_lane2 = lanes2.begin(); it_lane2 != lanes2.end(); ++it_lane2) {
928 const MSLane* lane2 = *it_lane2;
929 if (lane2 == target) {
930 return prevShift + curShift;
941 return std::numeric_limits<int>::max();
971 #if defined(DEBUG_MANEUVER) || defined(DEBUG_STATE) 990 const std::vector<MSVehicle::LaneQ>& preb,
993 double& latDist,
double& maneuverDist,
int& blocked) {
998 int bestLaneOffset = 0;
999 double currentDist = 0;
1000 double neighDist = 0;
1007 for (
int p = 0; p < (int) preb.size(); ++p) {
1008 if (preb[p].lane == prebLane && p + laneOffset >= 0) {
1009 assert(p + laneOffset < (
int)preb.size());
1011 neigh = preb[p + laneOffset];
1012 currentDist = curr.
length;
1013 neighDist = neigh.
length;
1014 bestLaneOffset = curr.bestLaneOffset;
1016 if (bestLaneOffset == 0 && preb[p + laneOffset].bestLaneOffset == 0) {
1017 #ifdef DEBUG_WANTSCHANGE 1021 <<
" bestLaneOffsetOld=" << bestLaneOffset
1022 <<
" bestLaneOffsetNew=" << laneOffset
1026 bestLaneOffset = laneOffset;
1033 const bool right = (laneOffset == -1);
1034 const bool left = (laneOffset == 1);
1037 const bool changeToBest = (right && bestLaneOffset < 0) || (left && bestLaneOffset > 0) || (laneOffset == 0 && bestLaneOffset == 0);
1063 #ifdef DEBUG_WANTSCHANGE 1070 <<
"\n leaders=" << leaders.
toString()
1071 <<
"\n followers=" << followers.
toString()
1072 <<
"\n blockers=" << blockers.
toString()
1073 <<
"\n neighLeaders=" << neighLeaders.
toString()
1074 <<
"\n neighFollowers=" << neighFollowers.
toString()
1075 <<
"\n neighBlockers=" << neighBlockers.
toString()
1076 <<
"\n changeToBest=" << changeToBest
1077 <<
" latLaneDist=" << latLaneDist
1085 if (lastBlocked != firstBlocked) {
1145 int roundaboutEdgesAhead = 0;
1147 if ((*it) != 0 && (*it)->getEdge().isRoundabout()) {
1148 roundaboutEdgesAhead += 1;
1149 }
else if (roundaboutEdgesAhead > 0) {
1154 int roundaboutEdgesAheadNeigh = 0;
1156 if ((*it) != 0 && (*it)->getEdge().isRoundabout()) {
1157 roundaboutEdgesAheadNeigh += 1;
1158 }
else if (roundaboutEdgesAheadNeigh > 0) {
1163 if (roundaboutEdgesAhead > 1) {
1167 if (roundaboutEdgesAhead > 0) {
1168 #ifdef DEBUG_ROUNDABOUTS 1170 std::cout <<
" roundaboutEdgesAhead=" << roundaboutEdgesAhead <<
" roundaboutEdgesAheadNeigh=" << roundaboutEdgesAheadNeigh <<
"\n";
1175 if (laneOffset != 0) {
1187 roundaboutEdgesAhead,
1192 if ((ret &
LCA_STAY) != 0 && latDist == 0) {
1202 if (changeToBest && abs(bestLaneOffset) > 1
1207 #ifdef DEBUG_WANTSCHANGE 1209 std::cout <<
" reserving space for unseen blockers myLeadingBlockerLength=" <<
myLeadingBlockerLength <<
"\n";
1218 if (*firstBlocked != neighLeadLongest) {
1221 std::vector<CLeaderDist> collectLeadBlockers;
1222 std::vector<CLeaderDist> collectFollowBlockers;
1223 int blockedFully = 0;
1226 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1227 leaders, followers, blockers,
1228 neighLeaders, neighFollowers, neighBlockers, &collectLeadBlockers, &collectFollowBlockers,
1229 false, gapFactor, &blockedFully);
1231 const double absLaneOffset = fabs(bestLaneOffset != 0 ? bestLaneOffset : latDist /
SUMO_const_laneWidth);
1232 const double remainingSeconds = ((ret &
LCA_TRACI) == 0 ?
1235 const double plannedSpeed =
informLeaders(blocked, myLca, collectLeadBlockers, remainingSeconds);
1237 if (plannedSpeed >= 0) {
1239 informFollowers(blocked, myLca, collectFollowBlockers, remainingSeconds, plannedSpeed);
1241 if (plannedSpeed > 0) {
1242 commitManoeuvre(blocked, blockedFully, leaders, neighLeaders, neighLane, maneuverDist);
1244 #if defined(DEBUG_WANTSCHANGE) || defined(DEBUG_STATE) 1251 <<
" remainingSeconds=" << remainingSeconds
1252 <<
" plannedSpeed=" << plannedSpeed
1262 if (roundaboutEdgesAhead > 1) {
1271 if ((ret & LCA_STAY) == 0) {
1272 latDist = latLaneDist;
1273 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1274 leaders, followers, blockers,
1275 neighLeaders, neighFollowers, neighBlockers);
1290 const double inconvenience = (latLaneDist < 0
1302 && (changeToBest ||
currentDistAllows(neighDist, abs(bestLaneOffset) + 1, laDist))) {
1305 #ifdef DEBUG_WANTSCHANGE 1312 <<
" wantsChangeToHelp=" << (right ?
"right" :
"left")
1322 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1323 leaders, followers, blockers,
1324 neighLeaders, neighFollowers, neighBlockers);
1348 const double vehWidth =
getWidth();
1350 const double leftVehSide = rightVehSide + vehWidth;
1352 double defaultNextSpeed = std::numeric_limits<double>::max();
1354 int leftmostOnEdge = (int)sublaneSides.size() - 1;
1355 while (leftmostOnEdge > 0 && sublaneSides[leftmostOnEdge] > leftVehSide) {
1358 int rightmostOnEdge = leftmostOnEdge;
1359 while (rightmostOnEdge > 0 && sublaneSides[rightmostOnEdge] > rightVehSide +
NUMERICAL_EPS) {
1361 #ifdef DEBUG_WANTSCHANGE 1363 std::cout <<
" adapted to current sublane=" << rightmostOnEdge <<
" defaultNextSpeed=" << defaultNextSpeed <<
"\n";
1364 std::cout <<
" sublaneSides[rightmostOnEdge]=" << sublaneSides[rightmostOnEdge] <<
" rightVehSide=" << rightVehSide <<
"\n";
1370 #ifdef DEBUG_WANTSCHANGE 1372 std::cout <<
" adapted to current sublane=" << rightmostOnEdge <<
" defaultNextSpeed=" << defaultNextSpeed <<
"\n";
1373 std::cout <<
" sublaneSides[rightmostOnEdge]=" << sublaneSides[rightmostOnEdge] <<
" rightVehSide=" << rightVehSide <<
"\n";
1376 double maxGain = -std::numeric_limits<double>::max();
1377 double maxGainRight = -std::numeric_limits<double>::max();
1378 double maxGainLeft = -std::numeric_limits<double>::max();
1379 double latDistNice = std::numeric_limits<double>::max();
1382 const double leftMax =
MAX2(
1385 assert(leftMax <= edge.
getWidth());
1386 int sublaneCompact =
MAX2(iMin, rightmostOnEdge - 1);
1388 #ifdef DEBUG_WANTSCHANGE 1390 <<
" checking sublanes rightmostOnEdge=" << rightmostOnEdge
1391 <<
" leftmostOnEdge=" << leftmostOnEdge
1393 <<
" leftMax=" << leftMax
1394 <<
" sublaneCompact=" << sublaneCompact
1397 for (
int i = iMin; i < (int)sublaneSides.size(); ++i) {
1398 if (sublaneSides[i] + vehWidth < leftMax) {
1404 while (vMin > 0 && j < (
int)sublaneSides.size() && sublaneSides[j] < sublaneSides[i] + vehWidth) {
1410 const double currentLatDist = sublaneSides[i] - rightVehSide;
1412 if (relativeGain > maxGain) {
1413 maxGain = relativeGain;
1416 latDist = currentLatDist;
1417 #ifdef DEBUG_WANTSCHANGE 1419 std::cout <<
" i=" << i <<
" newLatDist=" << latDist <<
" relGain=" << relativeGain <<
"\n";
1425 if (currentLatDist > 0
1430 latDist = currentLatDist;
1434 std::cout <<
" i=" << i <<
" rightmostOnEdge=" << rightmostOnEdge <<
" vMin=" << vMin <<
" relGain=" << relativeGain <<
" sublaneCompact=" << sublaneCompact <<
" curLatDist=" << currentLatDist <<
"\n";
1437 maxGainRight =
MAX2(maxGainRight, relativeGain);
1439 maxGainLeft =
MAX2(maxGainLeft, relativeGain);
1441 const double subAlignDist = sublaneSides[i] - rightVehSide;
1442 if (fabs(subAlignDist) < fabs(latDistNice)) {
1443 latDistNice = subAlignDist;
1444 #ifdef DEBUG_WANTSCHANGE 1446 <<
" nicest sublane=" << i
1447 <<
" side=" << sublaneSides[i]
1448 <<
" rightSide=" << rightVehSide
1449 <<
" latDistNice=" << latDistNice
1450 <<
" maxGainR=" << maxGainRight
1451 <<
" maxGainL=" << maxGainLeft
1458 if (maxGainRight != -std::numeric_limits<double>::max()) {
1459 #ifdef DEBUG_WANTSCHANGE 1465 #ifdef DEBUG_WANTSCHANGE 1471 if (maxGainLeft != -std::numeric_limits<double>::max()) {
1472 #ifdef DEBUG_WANTSCHANGE 1478 #ifdef DEBUG_WANTSCHANGE 1485 if ((fabs(maxGainRight) <
NUMERICAL_EPS || maxGainRight == -std::numeric_limits<double>::max())
1486 && (right || (alternatives &
LCA_RIGHT) == 0)) {
1489 if ((fabs(maxGainLeft) <
NUMERICAL_EPS || maxGainLeft == -std::numeric_limits<double>::max())
1490 && (left || (alternatives &
LCA_LEFT) == 0)) {
1495 #ifdef DEBUG_WANTSCHANGE 1498 <<
" defaultNextSpeed=" << defaultNextSpeed
1499 <<
" maxGain=" << maxGain
1500 <<
" maxGainRight=" << maxGainRight
1501 <<
" maxGainLeft=" << maxGainLeft
1502 <<
" latDist=" << latDist
1503 <<
" latDistNice=" << latDistNice
1504 <<
" sublaneCompact=" << sublaneCompact
1510 if (right && maxGain >= 0 && latDist <= 0) {
1517 double fullSpeedDrivingSeconds =
MIN2(acceptanceTime, fullSpeedGap / vMax);
1519 if (neighLead.first != 0 && neighLead.first->getSpeed() < vMax) {
1520 fullSpeedGap =
MAX2(0.,
MIN2(fullSpeedGap,
1522 vMax, neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel())));
1523 fullSpeedDrivingSeconds =
MIN2(fullSpeedDrivingSeconds, fullSpeedGap / (vMax - neighLead.first->getSpeed()));
1528 #ifdef DEBUG_WANTSCHANGE 1531 <<
" considering keepRight:" 1533 <<
" neighDist=" << neighDist
1535 <<
" leaderSpeed=" << (neighLead.first == 0 ? -1 : neighLead.first->getSpeed())
1537 myVehicle.
getSpeed(), neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()))
1538 <<
" acceptanceTime=" << acceptanceTime
1539 <<
" fullSpeedGap=" << fullSpeedGap
1540 <<
" fullSpeedDrivingSeconds=" << fullSpeedDrivingSeconds
1541 <<
" dProb=" << deltaProb
1552 latDist = latLaneDist;
1553 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1554 leaders, followers, blockers,
1555 neighLeaders, neighFollowers, neighBlockers);
1561 #ifdef DEBUG_WANTSCHANGE 1566 <<
" neighDist=" << neighDist
1569 <<
" latDist=" << latDist
1578 int blockedFully = 0;
1579 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1580 leaders, followers, blockers,
1581 neighLeaders, neighFollowers, neighBlockers,
1582 0, 0,
false, 0, &blockedFully);
1591 #ifdef DEBUG_WANTSCHANGE 1596 <<
" latDist=" << latDist
1597 <<
" neighDist=" << neighDist
1600 <<
" stayInLane=" << stayInLane
1611 int blockedFully = 0;
1612 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1613 leaders, followers, blockers,
1614 neighLeaders, neighFollowers, neighBlockers,
1615 0, 0,
false, 0, &blockedFully);
1624 double latDistSublane = 0.;
1626 const double halfVehWidth =
getWidth() * 0.5;
1629 && bestLaneOffset == 0
1649 #ifdef DEBUG_WANTSCHANGE 1667 latDistSublane = latDistNice;
1670 latDistSublane = sublaneSides[sublaneCompact] - rightVehSide;
1678 #if defined(DEBUG_WANTSCHANGE) || defined(DEBUG_STATE) || defined(DEBUG_MANEUVER) 1683 <<
" latDist=" << latDist
1684 <<
" latDistSublane=" << latDistSublane
1685 <<
" relGainSublane=" <<
computeSpeedGain(latDistSublane, defaultNextSpeed)
1686 <<
" maneuverDist=" << maneuverDist
1701 && ((myManeuverDist < 0 && latDistSublane > 0) || (
myManeuverDist > 0 && latDistSublane < 0))) {
1704 latDist = latDistSublane;
1709 #ifdef DEBUG_WANTSCHANGE 1712 <<
" latDist=" << latDist
1717 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1718 leaders, followers, blockers,
1719 neighLeaders, neighFollowers, neighBlockers);
1746 #ifdef DEBUG_WANTSCHANGE 1763 if ((*blocked) != 0) {
1765 #ifdef DEBUG_SLOWDOWN 1789 (*blocked)->getCarFollowModel().getMaxDecel()));
1800 #ifdef DEBUG_SAVE_BLOCKER_LENGTH 1816 #ifdef DEBUG_SAVE_BLOCKER_LENGTH 1828 #ifdef DEBUG_SAVE_BLOCKER_LENGTH 1834 <<
" potential=" << potential
1850 <<
"vSafe=" << vSafe <<
" -> accel=" << accel <<
"\n";
1860 const MSLane* lane = lanes[laneIndex];
1862 assert(preb.size() == lanes.size());
1865 for (
int sublane = 0; sublane < (int)ahead.
numSublanes(); ++sublane) {
1866 const int edgeSublane = sublane + sublaneOffset;
1870 const MSVehicle* leader = ahead[sublane].first;
1871 const double gap = ahead[sublane].second;
1883 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" updateExpectedSublaneSpeeds edgeSublane=" << edgeSublane <<
" leader=" << leader->
getID() <<
" gap=" << gap <<
" vSafe=" << vSafe <<
"\n";
1885 const double deltaV = vMax - leader->
getSpeed();
1886 if (deltaV > 0 && gap / deltaV < 5) {
1895 double foeRight, foeLeft;
1899 if (leader.first != 0) {
1902 vSafe =
MIN2(vSafe, vSafePed);
1905 vSafe =
MIN2(vMax, vSafe);
1907 myExpectedSublaneSpeeds[edgeSublane] = memoryFactor * myExpectedSublaneSpeeds[edgeSublane] + (1 - memoryFactor) * vSafe;
1919 double result = std::numeric_limits<double>::max();
1922 const double vehWidth =
getWidth();
1924 const double leftVehSide = rightVehSide + vehWidth;
1925 for (
int i = 0; i < (int)sublaneSides.size(); ++i) {
1926 if (
overlap(rightVehSide, leftVehSide, sublaneSides[i], sublaneSides[i] + res)) {
1932 return result - defaultNextSpeed;
1939 double maxLength = -1;
1941 if (ldi[i].first != 0) {
1942 const double length = ldi[i].first->getVehicleType().getLength();
1943 if (length > maxLength) {
1956 double minSpeed = std::numeric_limits<double>::max();
1958 if (ldi[i].first != 0) {
1959 const double speed = ldi[i].first->getSpeed();
1960 if (speed < minSpeed) {
1978 std::vector<CLeaderDist>* collectLeadBlockers,
1979 std::vector<CLeaderDist>* collectFollowBlockers,
1980 bool keepLatGapManeuver,
1982 int* retBlockedFully) {
1984 if (!keepLatGapManeuver) {
1986 maneuverDist = latDist;
1989 latDist =
MAX2(
MIN2(latDist, maxDist), -maxDist);
1994 if (laneOffset != 0) {
2005 if (laneOffset != 0) {
2009 #ifdef DEBUG_BLOCKING 2011 std::cout <<
" checkBlocking latDist=" << latDist <<
" mySafeLatDistRight=" << mySafeLatDistRight <<
" mySafeLatDistLeft=" << mySafeLatDistLeft <<
"\n";
2019 latDist =
MAX2(latDist, -mySafeLatDistRight);
2025 latDist =
MIN2(latDist, mySafeLatDistLeft);
2030 #ifdef DEBUG_BLOCKING 2046 if (laneOffset != 0) {
2049 mySafeLatDistRight, mySafeLatDistLeft, collectLeadBlockers);
2052 mySafeLatDistRight, mySafeLatDistLeft, collectFollowBlockers);
2055 int blockedFully = 0;
2060 if (laneOffset != 0) {
2063 mySafeLatDistRight, mySafeLatDistLeft, collectLeadBlockers);
2066 mySafeLatDistRight, mySafeLatDistLeft, collectFollowBlockers);
2068 if (retBlockedFully != 0) {
2069 *retBlockedFully = blockedFully;
2076 blocked |= blockedFully;
2081 if (collectFollowBlockers != 0 && collectLeadBlockers != 0) {
2083 for (std::vector<CLeaderDist>::const_iterator it2 = collectLeadBlockers->begin(); it2 != collectLeadBlockers->end(); ++it2) {
2084 for (std::vector<CLeaderDist>::iterator it = collectFollowBlockers->begin(); it != collectFollowBlockers->end();) {
2085 if ((*it2).first == (*it).first) {
2086 #ifdef DEBUG_BLOCKING 2088 std::cout <<
" removed follower " << (*it).first->getID() <<
" because it is already a leader\n";
2091 it = collectFollowBlockers->erase(it);
2105 double latDist,
double foeOffset,
bool leaders,
LaneChangeAction blockType,
2106 double& safeLatGapRight,
double& safeLatGapLeft,
2107 std::vector<CLeaderDist>* collectBlockers)
const {
2109 const double vehWidth =
getWidth();
2111 const double leftVehSide = rightVehSide + vehWidth;
2112 const double rightVehSideDest = rightVehSide + latDist;
2113 const double leftVehSideDest = leftVehSide + latDist;
2114 const double rightNoOverlap =
MIN2(rightVehSideDest, rightVehSide);
2115 const double leftNoOverlap =
MAX2(leftVehSideDest, leftVehSide);
2116 #ifdef DEBUG_BLOCKING 2118 std::cout <<
" checkBlockingVehicles" 2119 <<
" latDist=" << latDist
2120 <<
" foeOffset=" << foeOffset
2121 <<
" vehRight=" << rightVehSide
2122 <<
" vehLeft=" << leftVehSide
2123 <<
" rightNoOverlap=" << rightNoOverlap
2124 <<
" leftNoOverlap=" << leftNoOverlap
2125 <<
" destRight=" << rightVehSideDest
2126 <<
" destLeft=" << leftVehSideDest
2127 <<
" leaders=" << leaders
2133 for (
int i = 0; i < vehicles.
numSublanes(); ++i) {
2135 if (vehDist.first != 0 &&
myCFRelated.count(vehDist.first) == 0) {
2136 const MSVehicle* leader = vehDist.first;
2139 std::swap(leader, follower);
2142 double foeRight, foeLeft;
2144 const bool overlapBefore =
overlap(rightVehSide, leftVehSide, foeRight, foeLeft);
2145 const bool overlapDest =
overlap(rightVehSideDest, leftVehSideDest, foeRight, foeLeft);
2146 const bool overlapAny =
overlap(rightNoOverlap, leftNoOverlap, foeRight, foeLeft);
2147 #ifdef DEBUG_BLOCKING 2149 std::cout <<
" foe=" << vehDist.first->getID()
2150 <<
" gap=" << vehDist.second
2152 <<
" foeRight=" << foeRight
2153 <<
" foeLeft=" << foeLeft
2154 <<
" overlapBefore=" << overlapBefore
2155 <<
" overlap=" << overlapAny
2156 <<
" overlapDest=" << overlapDest
2161 if (vehDist.second < 0) {
2162 if (overlapBefore && !overlapDest) {
2163 #ifdef DEBUG_BLOCKING 2165 std::cout <<
" ignoring current overlap to come clear\n";
2169 #ifdef DEBUG_BLOCKING 2175 if (collectBlockers == 0) {
2178 collectBlockers->push_back(vehDist);
2194 const double expectedGap =
MSCFModel::gapExtrapolation(timeTillAction, vehDist.second, leader->
getSpeed(), follower->
getSpeed(), leaderAccel, followerAccel, std::numeric_limits<double>::max(), std::numeric_limits<double>::max());
2197 const double followerExpectedSpeed = follower->
getSpeed() + timeTillAction * followerAccel;
2198 const double leaderExpectedSpeed =
MAX2(0., leader->
getSpeed() + timeTillAction * leaderAccel);
2201 #if defined(DEBUG_ACTIONSTEPS) && defined(DEBUG_BLOCKING) 2203 std::cout <<
" timeTillAction=" << timeTillAction
2204 <<
" followerAccel=" << followerAccel
2205 <<
" followerExpectedSpeed=" << followerExpectedSpeed
2206 <<
" leaderAccel=" << leaderAccel
2207 <<
" leaderExpectedSpeed=" << leaderExpectedSpeed
2208 <<
"\n gap=" << vehDist.second
2209 <<
" gapChange=" << (expectedGap - vehDist.second)
2210 <<
" expectedGap=" << expectedGap
2211 <<
" expectedSecureGap=" << expectedSecureGap
2212 <<
" safeLatGapLeft=" << safeLatGapLeft
2213 <<
" safeLatGapRight=" << safeLatGapRight
2221 const double secureGap2 = expectedSecureGap / decelFactor;
2222 if (expectedGap < secureGap2) {
2224 if (foeRight > leftVehSide) {
2225 safeLatGapLeft =
MIN2(safeLatGapLeft, foeRight - leftVehSide);
2226 }
else if (foeLeft < rightVehSide) {
2227 safeLatGapRight =
MIN2(safeLatGapRight, rightVehSide - foeLeft);
2230 #ifdef DEBUG_BLOCKING 2232 std::cout <<
" blocked by " << vehDist.first->getID() <<
" gap=" << vehDist.second <<
" expectedGap=" << expectedGap
2233 <<
" expectedSecureGap=" << expectedSecureGap <<
" secGap2=" << secureGap2 <<
" decelFactor=" << decelFactor
2234 <<
" safeLatGapLeft=" << safeLatGapLeft <<
" safeLatGapRight=" << safeLatGapRight
2238 result |= blockType;
2239 if (collectBlockers == 0) {
2242 collectBlockers->push_back(vehDist);
2244 #ifdef DEBUG_BLOCKING 2245 }
else if (
gDebugFlag2 && expectedGap < expectedSecureGap) {
2246 std::cout <<
" ignore blocker " << vehDist.first->getID() <<
" gap=" << vehDist.second <<
" expectedGap=" << expectedGap
2247 <<
" expectedSecureGap=" << expectedSecureGap <<
" secGap2=" << secureGap2 <<
" decelFactor=" << decelFactor <<
"\n";
2265 const double leftVehSide = rightVehSide + vehWidth;
2266 #ifdef DEBUG_BLOCKING 2268 std::cout <<
" updateCFRelated foeOffset=" << foeOffset <<
" vehicles=" << vehicles.
toString() <<
"\n";
2271 for (
int i = 0; i < vehicles.
numSublanes(); ++i) {
2273 if (vehDist.first != 0 &&
myCFRelated.count(vehDist.first) == 0) {
2274 double foeRight, foeLeft;
2276 if (
overlap(rightVehSide, leftVehSide, foeRight, foeLeft) && (vehDist.second >= 0
2282 && -vehDist.second < vehDist.first->getVehicleType().getMinGap()
2285 #ifdef DEBUG_BLOCKING 2287 std::cout <<
" ignoring cfrelated foe=" << vehDist.first->getID() <<
" gap=" << vehDist.second
2289 <<
" foeOffset=" << foeOffset
2290 <<
" egoR=" << rightVehSide <<
" egoL=" << leftVehSide
2291 <<
" iR=" << foeRight <<
" iL=" << foeLeft
2306 assert(right <= left);
2307 assert(right2 <= left2);
2315 if (sd1.
state == 0) {
2317 }
else if (sd2.
state == 0) {
2325 #ifdef DEBUG_WANTSCHANGE 2331 <<
" dir1=" << sd1.
dir 2335 <<
" dir2=" << sd2.
dir 2343 return (!can1 && can2 && sd1.
sameDirection(sd2)) ? sd2 : sd1;
2347 return (!can2 && can1 && sd1.
sameDirection(sd2)) ? sd1 : sd2;
2355 }
else if (sd2.
dir == 0) {
2360 assert(sd1.
dir == -1);
2361 assert(sd2.
dir == 1);
2364 }
else if (sd2.
latDist >= 0) {
2372 return can1 ? sd1 : sd2;
2395 const std::vector<MSVehicle::LaneQ>& preb,
2404 int roundaboutEdgesAhead,
2408 const bool right = (laneOffset == -1);
2409 const bool left = (laneOffset == 1);
2417 const double maxJam =
MAX2(preb[currIdx + laneOffset].occupation, preb[currIdx].occupation);
2421 #ifdef DEBUG_STRATEGIC_CHANGE 2426 <<
" laDist=" << laDist
2427 <<
" currentDist=" << currentDist
2428 <<
" usableDist=" << usableDist
2429 <<
" bestLaneOffset=" << bestLaneOffset
2430 <<
" best.length=" << best.
length 2431 <<
" maxJam=" << maxJam
2432 <<
" neighLeftPlace=" << neighLeftPlace
2437 if (laneOffset != 0 && changeToBest && bestLaneOffset == curr.
bestLaneOffset 2440 latDist = latLaneDist;
2457 #ifdef DEBUG_STRATEGIC_CHANGE 2460 <<
" avoid overtaking on the right nv=" << nv->
getID()
2470 if (!changeToBest && (
currentDistDisallows(neighLeftPlace, abs(bestLaneOffset) + 2, laDist))) {
2477 #ifdef DEBUG_STRATEGIC_CHANGE 2479 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" could not change back and forth in time (1) neighLeftPlace=" << neighLeftPlace <<
"\n";
2483 }
else if (laneOffset != 0 && bestLaneOffset == 0 && (neighLeftPlace * 2. < laDist)) {
2488 #ifdef DEBUG_STRATEGIC_CHANGE 2490 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" could not change back and forth in time (2) neighLeftPlace=" << neighLeftPlace <<
"\n";
2496 && bestLaneOffset == 0
2499 && roundaboutEdgesAhead == 0
2504 #ifdef DEBUG_STRATEGIC_CHANGE 2506 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" does not want to leave the bestLane (neighDist=" << neighDist <<
")\n";
2511 && bestLaneOffset == 0
2517 #ifdef DEBUG_STRATEGIC_CHANGE 2519 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" does not want to get stranded on the on-ramp of a highway\n";
2538 if (shadow == 0 || currentShadowDist >= requiredDist) {
2541 currentShadowDist += shadow->
getLength();
2543 #ifdef DEBUG_STRATEGIC_CHANGE 2548 if (currentShadowDist < requiredDist && currentShadowDist < usableDist) {
2551 #ifdef DEBUG_STRATEGIC_CHANGE 2553 std::cout <<
" must change for shadowLane end latDist=" << latDist <<
" myLeftSpace=" <<
myLeftSpace <<
"\n";
2561 #if defined(DEBUG_STRATEGIC_CHANGE) || defined(DEBUG_TRACI) 2571 }
else if (((retTraCI &
LCA_RIGHT) != 0 && laneOffset < 0)
2572 || ((retTraCI &
LCA_LEFT) != 0 && laneOffset > 0)) {
2574 latDist = latLaneDist;
2577 #if defined(DEBUG_STRATEGIC_CHANGE) || defined(DEBUG_TRACI) 2579 std::cout <<
" reqAfterInfluence=" << ret <<
" ret=" << ret <<
"\n";
2603 double& maneuverDist,
2639 const bool stayInLane = laneOffset == 0 || ((state &
LCA_STRATEGIC) != 0 && (state &
LCA_STAY) != 0);
2640 const double oldLatDist = latDist;
2643 const double halfWidth =
getWidth() * 0.5;
2649 double surplusGapRight = oldCenter - halfWidth;
2651 #ifdef DEBUG_KEEP_LATGAP 2653 std::cout <<
"\n " <<
SIMTIME <<
" keepLatGap() laneOffset=" << laneOffset
2654 <<
" latDist=" << latDist
2657 <<
" gapFactor=" << gapFactor
2658 <<
" stayInLane=" << stayInLane <<
"\n" 2659 <<
" stayInEdge: surplusGapRight=" << surplusGapRight <<
" surplusGapLeft=" << surplusGapLeft <<
"\n";
2663 if (surplusGapLeft < 0 || surplusGapRight < 0) {
2673 if (laneOffset != 0) {
2678 #ifdef DEBUG_KEEP_LATGAP 2680 std::cout <<
" minGapLat: surplusGapRight=" << surplusGapRight <<
" surplusGapLeft=" << surplusGapLeft <<
"\n" 2689 if (stayInLane || laneOffset == 1) {
2695 if (stayInLane || laneOffset == -1) {
2701 #ifdef DEBUG_KEEP_LATGAP 2703 std::cout <<
" stayInLane: surplusGapRight=" << surplusGapRight <<
" surplusGapLeft=" << surplusGapLeft <<
"\n";
2707 if (surplusGapRight + surplusGapLeft < 0) {
2712 const double equalDeficit = 0.5 * (surplusGapLeft + surplusGapRight);
2713 if (surplusGapRight < surplusGapLeft) {
2715 const double delta =
MIN2(equalDeficit - surplusGapRight, physicalGapLeft);
2717 maneuverDist = delta;
2718 #ifdef DEBUG_KEEP_LATGAP 2720 std::cout <<
" insufficient latSpace, move left: delta=" << delta <<
"\n";
2725 const double delta =
MIN2(equalDeficit - surplusGapLeft, physicalGapRight);
2727 maneuverDist = -delta;
2728 #ifdef DEBUG_KEEP_LATGAP 2730 std::cout <<
" insufficient latSpace, move right: delta=" << delta <<
"\n";
2736 latDist =
MAX2(
MIN2(latDist, surplusGapLeft), -surplusGapRight);
2737 maneuverDist =
MAX2(
MIN2(maneuverDist, surplusGapLeft), -surplusGapRight);
2738 #ifdef DEBUG_KEEP_LATGAP 2740 std::cout <<
" adapted latDist=" << latDist <<
" maneuverDist=" << maneuverDist <<
" (old=" << oldLatDist <<
")\n";
2750 #ifdef DEBUG_KEEP_LATGAP 2752 std::cout <<
" traci influenced latDist=" << latDist <<
"\n";
2758 if (nonSublaneChange) {
2760 #ifdef DEBUG_KEEP_LATGAP 2762 std::cout <<
" wanted changeToLeft oldLatDist=" << oldLatDist <<
", blocked latGap changeToRight\n";
2765 latDist = oldLatDist;
2768 #ifdef DEBUG_KEEP_LATGAP 2770 std::cout <<
" wanted changeToRight oldLatDist=" << oldLatDist <<
", blocked latGap changeToLeft\n";
2773 latDist = oldLatDist;
2783 #ifdef DEBUG_KEEP_LATGAP 2785 std::cout <<
" latDistUpdated=" << latDist <<
" oldLatDist=" << oldLatDist <<
"\n";
2788 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset, leaders, followers, blockers, neighLeaders, neighFollowers, neighBlockers, 0, 0, nonSublaneChange);
2791 state = (state & ~LCA_STAY);
2802 #if defined(DEBUG_KEEP_LATGAP) || defined(DEBUG_STATE) 2804 std::cout <<
" latDist2=" << latDist
2818 double& surplusGapRight,
double& surplusGapLeft,
2819 bool saveMinGap,
double netOverlap,
2821 std::vector<CLeaderDist>* collectBlockers) {
2826 if (others[i].first != 0 && others[i].second <= 0
2828 && (netOverlap == 0 || others[i].second + others[i].first->getVehicleType().getMinGap() < netOverlap)) {
2832 double foeRight, foeLeft;
2834 const double foeCenter = foeRight + 0.5 * res;
2835 const double gap =
MIN2(fabs(foeRight - oldCenter), fabs(foeLeft - oldCenter)) - halfWidth;
2838 const double currentMinGap = desiredMinGap * gapFactor;
2849 #if defined(DEBUG_BLOCKING) || defined(DEBUG_KEEP_LATGAP) 2851 std::cout <<
" updateGaps" 2853 <<
" foe=" << foe->
getID()
2854 <<
" foeRight=" << foeRight
2855 <<
" foeLeft=" << foeLeft
2856 <<
" oldCenter=" << oldCenter
2857 <<
" gap=" << others[i].second
2858 <<
" latgap=" << gap
2859 <<
" currentMinGap=" << currentMinGap
2860 <<
" surplusGapRight=" << surplusGapRight
2861 <<
" surplusGapLeft=" << surplusGapLeft
2869 if (foeCenter < oldCenter) {
2871 surplusGapRight =
MIN3(surplusGapRight, gap - currentMinGap,
MAX2(currentMinGap, gap - foeManeuverDist));
2874 surplusGapLeft =
MIN3(surplusGapLeft, gap - currentMinGap,
MAX2(currentMinGap, gap - foeManeuverDist));
2877 if (foeCenter < oldCenter) {
2878 #if defined(DEBUG_BLOCKING) || defined(DEBUG_KEEP_LATGAP) 2880 std::cout <<
" new minimum rightGap=" << gap <<
"\n";
2885 #if defined(DEBUG_BLOCKING) || defined(DEBUG_KEEP_LATGAP) 2887 std::cout <<
" new minimum leftGap=" << gap <<
"\n";
2893 if (collectBlockers != 0) {
2895 if ((foeCenter < oldCenter && latDist < 0 && gap < (desiredMinGap - latDist))
2896 || (foeCenter > oldCenter && latDist > 0 && gap < (desiredMinGap + latDist))) {
2897 collectBlockers->push_back(others[i]);
2914 int currentDirection =
mySpeedLat >= 0 ? 1 : -1;
2915 int directionWish = latDist >= 0 ? 1 : -1;
2918 #ifdef DEBUG_MANEUVER 2922 <<
" computeSpeedLat()" 2923 <<
" currentDirection=" << currentDirection
2924 <<
" directionWish=" << directionWish
2930 if (directionWish == 1) {
2941 double speedAccelSafe = latDist * speedAccel >= 0 ? speedAccel : 0;
2949 if (maneuverDist * latDist > 0) {
2950 maneuverDist = fullLatDist;
2953 #ifdef DEBUG_MANEUVER 2958 <<
" latDist=" << latDist
2959 <<
" maneuverDist=" << maneuverDist
2962 <<
" fullLatDist=" << fullLatDist
2963 <<
" speedAccel=" << speedAccel
2964 <<
" speedDecel=" << speedDecel
2965 <<
" speedBound=" << speedBound
2969 if (speedDecel * speedAccel <= 0 && (
2971 (latDist >= 0 && speedAccel >= speedBound && speedBound >= speedDecel)
2972 || (latDist <= 0 && speedAccel <= speedBound && speedBound <= speedDecel))) {
2974 #ifdef DEBUG_MANEUVER 2976 std::cout <<
" computeSpeedLat a)\n";
2983 #ifdef DEBUG_MANEUVER 2985 std::cout <<
" computeSpeedLat b)\n";
2988 return speedAccelSafe;
2992 if ((fabs(minDistAccel) < fabs(fullLatDist)) || (fabs(minDistAccel - fullLatDist) <
NUMERICAL_EPS)) {
2993 #ifdef DEBUG_MANEUVER 2995 std::cout <<
" computeSpeedLat c)\n";
3000 #ifdef DEBUG_MANEUVER 3002 std::cout <<
" minDistAccel=" << minDistAccel <<
"\n";
3007 if ((fabs(minDistCurrent) < fabs(fullLatDist)) || (fabs(minDistCurrent - fullLatDist) <
NUMERICAL_EPS)) {
3008 #ifdef DEBUG_MANEUVER 3010 std::cout <<
" computeSpeedLat d)\n";
3017 #ifdef DEBUG_MANEUVER 3019 std::cout <<
" computeSpeedLat e)\n";
3022 return speedDecelSafe;
3031 double maneuverDist) {
3034 double secondsToLeaveLane;
3055 double nextLeftSpace;
3056 if (nextActionStepSpeed > 0.) {
3064 const double avoidArrivalSpeed = nextActionStepSpeed +
TS *
MSCFModel::avoidArrivalAccel(nextLeftSpace, secondsToLeaveLane - timeTillActionStep, nextActionStepSpeed);
3070 #ifdef DEBUG_MANEUVER 3074 <<
" avoidArrivalSpeed=" << avoidArrivalSpeed
3077 <<
"\n nextLeftSpace=" << nextLeftSpace
3078 <<
" nextLeftSpace=" << nextActionStepSpeed
3079 <<
" nextActionStepRemainingSeconds=" << secondsToLeaveLane - timeTillActionStep
3087 myCommittedSpeed = 0;
3089 #ifdef DEBUG_MANEUVER 3093 <<
" secondsToLeave=" << secondsToLeaveLane
3095 <<
" committed=" << myCommittedSpeed
3116 const double vehWidth =
getWidth();
3118 const double leftVehSide = rightVehSide + vehWidth;
3119 const double rightVehSideDest = rightVehSide + latDist;
3120 const double leftVehSideDest = leftVehSide + latDist;
3121 #ifdef DEBUG_MANEUVER 3123 std::cout <<
" commitFollowSpeed" 3124 <<
" latDist=" << latDist
3125 <<
" foeOffset=" << foeOffset
3126 <<
" vehRight=" << rightVehSide
3127 <<
" vehLeft=" << leftVehSide
3128 <<
" destRight=" << rightVehSideDest
3129 <<
" destLeft=" << leftVehSideDest
3135 if (vehDist.first != 0) {
3136 const MSVehicle* leader = vehDist.first;
3138 double foeRight, foeLeft;
3140 #ifdef DEBUG_MANEUVER 3142 std::cout <<
" foe=" << vehDist.first->getID()
3143 <<
" gap=" << vehDist.second
3145 <<
" foeRight=" << foeRight
3146 <<
" foeLeft=" << foeLeft
3147 <<
" overlapBefore=" <<
overlap(rightVehSide, leftVehSide, foeRight, foeLeft)
3148 <<
" overlapDest=" <<
overlap(rightVehSideDest, leftVehSideDest, foeRight, foeLeft)
3152 if (
overlap(rightVehSideDest, leftVehSideDest, foeRight, foeLeft)) {
3154 assert(vehDist.second >= 0);
3157 speed =
MIN2(speed, vSafe);
3158 #ifdef DEBUG_MANEUVER 3160 std::cout <<
" case1 vsafe=" << vSafe <<
" speed=" << speed <<
"\n";
3163 }
else if (
overlap(rightVehSide, leftVehSide, foeRight, foeLeft)) {
3168 speed =
MIN2(speed, vSafe);
3169 #ifdef DEBUG_MANEUVER 3171 std::cout <<
" case2 vsafe=" << vSafe <<
" speed=" << speed <<
"\n";
3260 const std::pair<MSVehicle*, double>& leader,
3261 const std::pair<MSVehicle*, double>& neighLead,
3262 const std::pair<MSVehicle*, double>& neighFollow,
3264 const std::vector<MSVehicle::LaneQ>& preb,
3270 #ifdef DEBUG_WANTSCHANGE 3272 std::cout <<
"\nWANTS_CHANGE\n" <<
SIMTIME 3279 <<
" considerChangeTo=" << (laneOffset == -1 ?
"right" :
"left")
3293 double maneuverDist;
3296 leaders, followers, blockers,
3297 neighLeaders, neighFollowers, neighBlockers,
3299 lastBlocked, firstBlocked, latDist, maneuverDist, blocked);
3305 result |=
getLCA(result, latDist);
3307 #if defined(DEBUG_WANTSCHANGE) || defined(DEBUG_STATE) 3312 <<
" wantsChangeTo=" << (laneOffset == -1 ?
"right" :
"left")
3313 << ((result &
LCA_URGENT) ?
" (urgent)" :
"")
3319 << ((result &
LCA_TRACI) ?
" (traci)" :
"")
void * inform(void *info, MSVehicle *sender)
double getLengthWithGap() const
Get vehicle's length including the minimum gap [m].
static double gLateralResolution
void initDerivedParameters()
init cached parameters derived directly from model parameters
const std::vector< double > getSubLaneSides() const
Returns the right side offsets of this edge's sublanes.
double getVehicleMaxSpeed(const SUMOVehicle *const veh) const
Returns the lane's maximum speed, given a vehicle's speed limit adaptation.
double brakeGap(const double speed) const
Returns the distance the vehicle needs to halt including driver's reaction time tau (i...
saves leader/follower vehicles and their distances relative to an ego vehicle
double myChangeProbThresholdLeft
MSEdge & getEdge() const
Returns the lane's edge.
Representation of a vehicle in the micro simulation.
void msg(const CLeaderDist &cld, double speed, int state)
send a speed recommendation to the given vehicle
void setParameter(const std::string &key, const std::string &value)
try to set the given parameter for this laneChangeModel. Throw exception for unsupported key ...
double mySpeedGainProbabilityLeft
a value for tracking the probability that a change to the left is beneficial
The action is due to the default of keeping right "Rechtsfahrgebot".
double myLastLateralGapRight
The action is done to help someone else.
#define RELGAIN_NORMALIZATION_MIN_SPEED
std::set< const MSVehicle * > myCFRelated
set of vehicles that are in a car-following relationship with ego (leader of followers) ...
double getWaitingSeconds() const
Returns the number of seconds waited (speed was lesser than 0.1m/s)
virtual bool hasPedestrians(const MSLane *lane)
whether the given lane has pedestrians on it
virtual std::string toString() const
print a debugging representation
#define ROUNDABOUT_DIST_BONUS
int myPreviousState
lane changing state from the previous simulation step
double informLeaders(int blocked, int dir, const std::vector< CLeaderDist > &blockers, double remainingSeconds)
MSLane * getLane() const
Returns the lane the vehicle is on.
bool myCanChangeFully
whether the current lane changing maneuver can be finished in a single step
double computeGapFactor(int state) const
compute the gap factor for the given state
virtual double minNextSpeed(double speed, const MSVehicle *const veh=0) const
Returns the minimum speed given the current speed (depends on the numerical update scheme and its ste...
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.
At the leftmost side of the lane.
int gPrecision
the precision for floating point outputs
LateralAlignment getPreferredLateralAlignment() const
Get vehicle's preferred lateral alignment.
virtual double followSpeed(const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel) const =0
Computes the vehicle's follow speed (no dawdling)
double getManeuverDist() const
Returns the remaining unblocked distance for the current maneuver. (only used by sublane model) ...
int checkBlockingVehicles(const MSVehicle *ego, const MSLeaderDistanceInfo &vehicles, double latDist, double foeOffset, bool leaders, LaneChangeAction blockType, double &safeLatGapRight, double &safeLatGapLeft, std::vector< CLeaderDist > *collectBlockers=0) const
check whether any of the vehicles overlaps with ego
const double SUMO_const_laneWidth
The car-following model abstraction.
double computeSpeedLat(double latDist, double &maneuverDist)
decides the next lateral speed depending on the remaining lane change distance to be covered and upda...
int checkBlocking(const MSLane &neighLane, double &latDist, double &maneuverDist, int laneOffset, const MSLeaderDistanceInfo &leaders, const MSLeaderDistanceInfo &followers, const MSLeaderDistanceInfo &blockers, const MSLeaderDistanceInfo &neighLeaders, const MSLeaderDistanceInfo &neighFollowers, const MSLeaderDistanceInfo &neighBlockers, std::vector< CLeaderDist > *collectLeadBlockers=0, std::vector< CLeaderDist > *collectFollowBlockers=0, bool keepLatGapManeuver=false, double gapFactor=0, int *retBlockedFully=0)
restrict latDist to permissible speed and determine blocking state depending on that distance ...
double getPositionOnLane() const
Get the vehicle's position along the lane.
double mySpeedLossProbThreshold
const SUMOVehicleParameter & getParameter() const
Returns the vehicle's parameter (including departure definition)
void setOwnState(const int state)
static CLeaderDist getSlowest(const MSLeaderDistanceInfo &ldi)
get the slowest vehicle in the given info
int getBestLaneOffset() const
ArrivalPosLatDefinition arrivalPosLatProcedure
Information how the vehicle shall choose the lateral arrival position.
double lateralDistanceToLane(const int offset) const
Get the minimal lateral distance required to move fully onto the lane at given offset.
double arrivalPosLat
(optional) The lateral position the vehicle shall arrive on
std::vector< double > myExpectedSublaneSpeeds
expected travel speeds on all sublanes on the current edge(!)
virtual double maxNextSpeed(double speed, const MSVehicle *const veh) const
Returns the maximum speed given the current speed.
double myTimeToImpatience
const std::vector< MSLane * > & getLanes() const
Returns this edge's lanes.
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
double getLength() const
Returns the lane's length.
static std::string getIDSecure(const T *obj, const std::string &fallBack="NULL")
get an identifier for Named-like object which may be Null
const MSRoute & getRoute() const
Returns the current route.
double getMinGapLat() const
Get the minimum lateral gap that vehicles of this type maintain.
const MSEdge * getLastEdge() const
returns the destination edge
static const double NO_NEIGHBOR
const std::string & getID() const
Returns the id.
bool sameDirection(const StateAndDist &other) const
align with the closest sublane border
static double avoidArrivalAccel(double dist, double time, double speed)
Computes the acceleration needed to arrive not before the given time.
double length
The overall length which may be driven when using this lane without a lane change.
bool debugVehicle() const
whether the current vehicles shall be debugged
The action is due to the wish to be faster (tactical lc)
double getWidth() const
Returns the lane's width.
#define UNUSED_PARAMETER(x)
MSLCM_SL2015(MSVehicle &v)
used by the sublane model
#define LCA_RIGHT_IMPATIENCE
double myChangeProbThresholdRight
MSAbstractLaneChangeModel & getLaneChangeModel()
int keepLatGap(int state, const MSLeaderDistanceInfo &leaders, const MSLeaderDistanceInfo &followers, const MSLeaderDistanceInfo &blockers, const MSLeaderDistanceInfo &neighLeaders, const MSLeaderDistanceInfo &neighFollowers, const MSLeaderDistanceInfo &neighBlockers, const MSLane &neighLane, int laneOffset, double &latDist, double &maneuverDist, int &blocked)
check whether lateral gap requirements are met override the current maneuver if necessary ...
#define MAX_ONRAMP_LENGTH
#define GAIN_PERCEPTION_THRESHOLD
#define ARRIVALPOS_LAT_THRESHOLD
double mySafeLatDistRight
the lateral distance the vehicle can safely move in the currently considered direction ...
double getMaxAccel() const
Get the vehicle type's maximum acceleration [m/s^2].
Needs to stay on the current lane.
void saveBlockerLength(const MSVehicle *blocker, int lcaCounter)
save space for vehicles which need to counter-lane-change
const LaneChangeModel myModel
the type of this model
double changeRequestRemainingSeconds(const SUMOTime currentTime) const
Return the remaining number of seconds of the current laneTimeLine assuming one exists.
double getWidth() const
return the widht of this vehicle (padded for numerical stability)
static bool overlap(double right, double left, double right2, double left2)
return whether the given intervals overlap
static bool myAllowOvertakingRight
whether overtaking on the right is permitted
A class responsible for exchanging messages between cars involved in lane-change interaction.
StateAndDist decideDirection(StateAndDist sd1, StateAndDist sd2) const
decide in which direction to move in case both directions are desirable
A road/street connecting two junctions.
void commitManoeuvre(int blocked, int blockedFully, const MSLeaderDistanceInfo &leaders, const MSLeaderDistanceInfo &neighLeaders, const MSLane &neighLane, double maneuverDist)
commit to lane change maneuvre potentially overriding safe speed
double myLeadingBlockerLength
const MSCFModel & getCarFollowModel() const
Returns the vehicle's car following model definition.
int getIndex() const
Returns the lane's index.
bool cancelRequest(int state)
whether the influencer cancels the given request
double getActionStepLengthSecs() const
Returns the vehicle's action step length in secs, i.e. the interval between two action points...
double myManeuverDist
The complete lateral distance the vehicle wants to travel to finish its maneuver Only used by sublane...
double commitFollowSpeed(double speed, double latDist, double secondsToLeaveLane, const MSLeaderDistanceInfo &leaders, double foeOffset) const
compute speed when committing to an urgent change that is safe in regard to leading vehicles ...
blocked in all directions
void updateCFRelated(const MSLeaderDistanceInfo &vehicles, double foeOffset, bool leaders)
find leaders/followers that are already in a car-following relationship with ego
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
double getLateralOverlap() const
return the amount by which the vehicle extends laterally outside it's primary lane ...
The action is urgent (to be defined by lc-model)
static double brakeGapEuler(const double speed, const double decel, const double headwayTime)
At the center of the lane.
static MSPModel * getModel()
void addLCSpeedAdvice(const double vSafe)
Takes a vSafe (speed advice for speed in the next simulation step), converts it into an acceleration ...
const MSEdge * myLastEdge
expected travel speeds on all sublanes on the current edge(!)
double informLeader(int blocked, int dir, const CLeaderDist &neighLead, double remainingSeconds)
static LaneChangeAction getLCA(int state, double latDist)
compute lane change action from desired lateral distance
void informFollowers(int blocked, int dir, const std::vector< CLeaderDist > &blockers, double remainingSeconds, double plannedSpeed)
call informFollower for multiple followers
const std::set< MSTransportable * > & getPersons() const
Returns this edge's persons set.
double getCenterOnEdge(const MSLane *lane=0) const
Get the vehicle's lateral position on the edge of the given lane (or its current edge if lane == 0) ...
int wantsChangeSublane(int laneOffset, LaneChangeAction alternatives, const MSLeaderDistanceInfo &leaders, const MSLeaderDistanceInfo &followers, const MSLeaderDistanceInfo &blockers, const MSLeaderDistanceInfo &neighLeaders, const MSLeaderDistanceInfo &neighFollowers, const MSLeaderDistanceInfo &neighBlockers, const MSLane &neighLane, const std::vector< MSVehicle::LaneQ > &preb, MSVehicle **lastBlocked, MSVehicle **firstBlocked, double &latDist, double &maneuverDist, int &blocked)
Called to examine whether the vehicle wants to change with the given laneOffset (using the sublane mo...
At the rightmost side of the lane.
double getSpeedLimit() const
Returns the lane's maximum allowed speed.
SUMOTime getCurrentTimeStep() const
Returns the current simulation step.
#define KEEP_RIGHT_ACCEPTANCE
The action is needed to follow the route (navigational lc)
int _wantsChangeSublane(int laneOffset, LaneChangeAction alternatives, const MSLeaderDistanceInfo &leaders, const MSLeaderDistanceInfo &followers, const MSLeaderDistanceInfo &blockers, const MSLeaderDistanceInfo &neighLeaders, const MSLeaderDistanceInfo &neighFollowers, const MSLeaderDistanceInfo &neighBlockers, const MSLane &neighLane, const std::vector< MSVehicle::LaneQ > &preb, MSVehicle **lastBlocked, MSVehicle **firstBlocked, double &latDist, double &maneuverDist, int &blocked)
helper function for doing the actual work
A structure representing the best lanes for continuing the current route starting at 'lane'...
virtual PersonDist nextBlocking(const MSLane *lane, double minPos, double minRight, double maxLeft, double stopTime=0)
returns the next pedestrian beyond minPos that is laterally between minRight and maxLeft or 0 ...
bool hasInfluencer() const
virtual void updateSafeLatDist(const double travelledLatDist)
Updates the value of safe lateral distances (mySafeLatDistLeft and mySafeLatDistRight) during maneuve...
double getMinGap() const
Get the free space in front of vehicles of this class.
double _patchSpeed(const double min, const double wanted, const double max, const MSCFModel &cfModel)
double getMaxDecel() const
Get the vehicle type's maximal comfortable deceleration [m/s^2].
int myOwnState
The current state of the vehicle.
maintain the current alignment
bool isInternal() const
return whether this edge is an internal edge
virtual bool debugVehicle() const
whether the current vehicles shall be debugged
#define LOOK_AHEAD_SPEED_MEMORY
#define LOOK_AHEAD_MIN_SPEED
static CLeaderDist getLongest(const MSLeaderDistanceInfo &ldi)
get the longest vehicle in the given info
double myCooperativeParam
double getLateralPositionOnLane() const
Get the vehicle's lateral position on the lane.
double getRightSideOnEdge() const
static double gapExtrapolation(const double duration, const double currentGap, double v1, double v2, double a1=0, double a2=0, const double maxV1=std::numeric_limits< double >::max(), const double maxV2=std::numeric_limits< double >::max())
return the resulting gap if, starting with gap currentGap, two vehicles continue with constant accele...
std::pair< double, int > Info
information regarding save velocity (unused) and state flags of the ego vehicle
double computeSpeedGain(double latDistSublane, double defaultNextSpeed) const
compute speedGain when moving by the given amount
int getRightmostSublane() const
std::pair< const MSPerson *, double > PersonDist
int checkStrategicChange(int ret, int laneOffset, const std::vector< MSVehicle::LaneQ > &preb, const MSLeaderDistanceInfo &leaders, const MSLeaderDistanceInfo &neighLeaders, int currIdx, int bestLaneOffset, bool changeToBest, double currentDist, double neighDist, double laDist, int roundaboutEdgesAhead, double latLaneDist, double &latDist)
compute strategic lane change actions TODO: Better documentation, refs #2
bool hasStoppedVehicle() const
whether a stopped vehicle is leader
double getMaxSpeedLat() const
Get vehicle's maximum lateral speed [m/s].
double getWidth() const
Get the width which vehicles of this class shall have when being drawn.
MSVehicle & myVehicle
The vehicle this lane-changer belongs to.
std::pair< const MSVehicle *, double > CLeaderDist
Influencer & getInfluencer()
Returns the velocity/lane influencer.
LaneChangeAction
The state of a vehicle's lane-change behavior.
virtual void setOwnState(const int state)
virtual void setMaxDecel(double decel)
Sets a new value for maximal comfortable deceleration [m/s^2].
double occupation
The overall vehicle sum on consecutive lanes which can be passed without a lane change.
const MSVehicleType & getVehicleType() const
Returns the vehicle's type definition.
double patchSpeed(const double min, const double wanted, const double max, const MSCFModel &cfModel)
Called to adapt the speed in order to allow a lane change. It uses information on LC-related desired ...
#define SPEEDGAIN_MEMORY_FACTOR
bool isActive() const
Returns whether the current simulation step is an action point for the vehicle.
double myKeepRightProbability
The vehicle is blocked being overlapping.
int slowDownForBlocked(MSVehicle **blocked, int state)
compute useful slowdowns for blocked vehicles
std::vector< MSLane * > bestContinuations
void getSublaneBorders(int sublane, double latOffset, double &rightSide, double &leftSide) const
static double estimateArrivalTime(double dist, double speed, double maxSpeed, double accel)
Computes the time needed to travel a distance dist given an initial speed and constant acceleration...
double getAcceleration() const
Returns the vehicle's acceleration in m/s (this is computed as the last step's mean acceleration in c...
void informFollower(int blocked, int dir, const CLeaderDist &neighFollow, double remainingSeconds, double plannedSpeed)
decide whether we will try cut in before the follower or allow to be overtaken
double mySpeedLat
the current lateral speed
static double _2double(const E *const data)
converts a char-type array into the double value described by it
No information given; use default.
double getLength() const
Get vehicle's length [m].
#define SPEEDGAIN_DECAY_FACTOR
bool currentDistDisallows(double dist, int laneOffset, double lookForwardDist)
align with the rightmost sublane that allows keeping the current speed
virtual void prepareStep()
bool allowsVehicleClass(SUMOVehicleClass vclass) const
std::vector< double > myLCAccelerationAdvices
vector of LC-related acceleration recommendations Filled in wantsChange() and applied in patchSpeed()...
const double SUMO_const_haltingSpeed
the speed threshold at which vehicles are considered as halting
#define CUT_IN_LEFT_SPEED_THRESHOLD
std::string getParameter(const std::string &key) const
try to retrieve the given parameter from this device. Throw exception for unsupported key ...
The action is due to a TraCI request.
virtual double followSpeedTransient(double duration, const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel) const
Computes the vehicle's follow speed that avoids a collision for the given amount of time...
#define HELP_DECEL_FACTOR
double mySpeedGainProbabilityRight
a value for tracking the probability that a change to the right is beneficial
#define LATGAP_SPEED_THRESHOLD2
static bool gSemiImplicitEulerUpdate
double myCommittedSpeed
the speed when committing to a change maneuver
bool currentDistAllows(double dist, int laneOffset, double lookForwardDist)
MSLane * getShadowLane() const
Returns the lane the vehicle's shadow is on during continuous/sublane lane change.
double getSecureGap(const double speed, const double leaderSpeed, const double leaderMaxDecel) const
Returns the minimum gap to reserve if the leader is braking at maximum (>=0)
bool myDontBrake
flag to prevent speed adaptation by slowing down
#define LATGAP_SPEED_THRESHOLD
const MSLinkCont & getLinkCont() const
returns the container with all links !!!
double getRightSideOnEdge(const MSLane *lane=0) const
Get the vehicle's lateral position on the edge of the given lane (or its current edge if lane == 0) ...
const std::vector< LaneQ > & getBestLanes() const
Returns the description of best lanes to use in order to continue the route.
virtual double getArrivalPos() const
Returns this vehicle's desired arrivalPos for its current route (may change on reroute) ...
double getSpeed() const
Returns the vehicle's current speed.
The vehicle is blocked by right leader.
int influenceChangeDecision(int state)
allow TraCI to influence a lane change decision
double myLastLateralGapLeft
the minimum lateral gaps to other vehicles that were found when last changing to the left and right ...
double getLatDist() const
public emergency vehicles
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 is a wrapper a...
const std::string & getID() const
Returns the name of the vehicle.
Representation of a lane in the micro simulation.
const MSCFModel & myCarFollowModel
The vehicle's car following model.
The vehicle is blocked by right follower.
int computeSublaneShift(const MSEdge *prevEdge, const MSEdge *curEdge)
compute shift so that prevSublane + shift = newSublane
Interface for lane-change models.
virtual double stopSpeed(const MSVehicle *const veh, const double speed, double gap) const =0
Computes the vehicle's safe speed for approaching a non-moving obstacle (no dawdling) ...
SUMOVehicleClass getVehicleClass() const
Get this vehicle type's vehicle class.
void updateExpectedSublaneSpeeds(const MSLeaderDistanceInfo &ahead, int sublaneOffset, int laneIndex)
update expected speeds for each sublane of the current edge
void updateGaps(const MSLeaderDistanceInfo &others, double foeOffset, double oldCenter, double gapFactor, double &surplusGapRight, double &surplusGapLeft, bool saveMinGap=false, double netOverlap=0, double latDist=0, std::vector< CLeaderDist > *collectBlockers=0)
check remaining lateral gaps for the given foe vehicles and optionally update minimum lateral gaps ...
#define SPEED_GAIN_MIN_SECONDS
bool amBlockingFollowerPlusNB()
double getWidth() const
Returns the edges's width (sum over all lanes)