Line data    Source code

       1             : /****************************************************************************/
       2             : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
       3             : // Copyright (C) 2013-2024 German Aerospace Center (DLR) and others.
       4             : // This program and the accompanying materials are made available under the
       5             : // terms of the Eclipse Public License 2.0 which is available at
       6             : // https://www.eclipse.org/legal/epl-2.0/
       7             : // This Source Code may also be made available under the following Secondary
       8             : // Licenses when the conditions for such availability set forth in the Eclipse
       9             : // Public License 2.0 are satisfied: GNU General Public License, version 2
      10             : // or later which is available at
      11             : // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
      12             : // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
      13             : /****************************************************************************/
      14             : /// @file    MSLCHelper.cpp
      15             : /// @author  Jakob Erdmann
      16             : /// @date    Fri, 19.06.2020
      17             : ///
      18             : // Common functions for lane change models
      19             : /****************************************************************************/
      20             : 
      21             : #include <microsim/MSEdge.h>
      22             : #include <microsim/MSLane.h>
      23             : #include <microsim/MSLink.h>
      24             : #include <microsim/MSVehicle.h>
      25             : #include <microsim/lcmodels/MSAbstractLaneChangeModel.h>
      26             : #include "MSLCHelper.h"
      27             : 
      28             : // ===========================================================================
      29             : // Debug flags
      30             : // ===========================================================================
      31             : //#define DEBUG_WANTS_CHANGE
      32             : //#define DEBUG_SAVE_BLOCKER_LENGTH
      33             : 
      34             : #define DEBUG_COND (veh.isSelected())
      35             : //#define DEBUG_COND (true)
      36             : 
      37             : 
      38             : // ===========================================================================
      39             : // member method definitions
      40             : // ===========================================================================
      41             : 
      42             : double
      43   214733144 : MSLCHelper::getRoundaboutDistBonus(const MSVehicle& veh,
      44             :                                    double bonusParam,
      45             :                                    const MSVehicle::LaneQ& curr,
      46             :                                    const MSVehicle::LaneQ& neigh,
      47             :                                    const MSVehicle::LaneQ& best) {
      48   214733144 :     if (veh.getLaneChangeModel().isOpposite()) {
      49             :         return 0;
      50             :     }
      51   214345476 :     const MSVehicle::LaneQ& inner = neigh.lane->getIndex() > curr.lane->getIndex() ? neigh : curr;
      52             : #ifdef DEBUG_WANTS_CHANGE
      53             :     const bool debugVehicle = veh.getLaneChangeModel().debugVehicle();
      54             :     if (debugVehicle) {
      55             :         std::cout << SIMTIME << " veh=" << veh.getID() << " getRoundaboutDistBonus bonusParam=" << bonusParam
      56             :                   << " curr=" << curr.lane->getID()
      57             :                   << " neigh=" << neigh.lane->getID()
      58             :                   << " inner=" << inner.lane->getID()
      59             :                   << " best=" << best.lane->getID()
      60             :                   << "\n   innerCont=" << toString(inner.bestContinuations)
      61             :                   << "\n   bestCont=" << toString(best.bestContinuations)
      62             :                   << "\n";
      63             :     }
      64             : #endif
      65             : 
      66             :     int roundaboutJunctionsAhead = 0;
      67             :     bool enteredRoundabout = false;
      68   214345476 :     double seen = -veh.getPositionOnLane();
      69             : 
      70             :     // first check using only normal lanes
      71   621016555 :     for (int i = 0; i < (int)best.bestContinuations.size(); i++) {
      72   548321997 :         const MSLane* lane = best.bestContinuations[i];
      73   548321997 :         if (lane == nullptr) {
      74     3232394 :             lane = veh.getLane();
      75             :         }
      76   548321997 :         if ((!enteredRoundabout || lane->getEdge().isRoundabout()) && i >= (int)inner.bestContinuations.size()) {
      77             :             // no bonus if we cannot continue on the inner lane until leaving the roundabout
      78             : #ifdef DEBUG_WANTS_CHANGE
      79             :             if (debugVehicle) {
      80             :                 std::cout << "   noBonus: inner does not continue (lane=" << lane->getID() << ")\n";
      81             :             }
      82             : #endif
      83             :             return 0;
      84             :         }
      85   530629389 :         if (seen > 300) {
      86             :             // avoid long look-ahead
      87             : #ifdef DEBUG_WANTS_CHANGE
      88             :             if (debugVehicle) {
      89             :                 std::cout << "   noBonus: seen=" << seen << " (lane=" << lane->getID() << ")\n";
      90             :             }
      91             : #endif
      92             :             return 0;
      93             :         }
      94             :         const MSJunction* junction = lane->getEdge().getToJunction();
      95   432631690 :         if (lane->getEdge().isRoundabout()) {
      96             :             enteredRoundabout = true;
      97    53940645 :             if (junction->getIncoming().size() + junction->getOutgoing().size() > 2) {
      98    53716207 :                 roundaboutJunctionsAhead++;
      99             :             }
     100   378691045 :         } else if (enteredRoundabout) {
     101             :             // only check the first roundabout
     102             :             break;
     103             :         }
     104   406671079 :         seen += lane->getLength();
     105             :     }
     106             :     // no bonus if we want to take the next exit
     107    98655169 :     if (roundaboutJunctionsAhead < 2) {
     108             :         return 0;
     109             :     }
     110             : 
     111             :     // compute bonus value based on jamming and exact distances (taking into
     112             :     // account internal lanes)
     113             :     double occupancyOuter = 0;
     114             :     double occupancyInner = 0;
     115             :     double distanceInRoundabout = 0;
     116             :     MSLane* prevNormal = nullptr;
     117             :     MSLane* prevInner = nullptr;
     118             :     enteredRoundabout = false;
     119    72082720 :     for (int i = 0; i < (int)best.bestContinuations.size(); i++) {
     120    71926024 :         MSLane* lane = best.bestContinuations[i];
     121    71926024 :         if (lane == nullptr) {
     122      626609 :             continue;
     123             :         }
     124    71299415 :         if (lane->getEdge().isRoundabout()) {
     125             :             enteredRoundabout = true;
     126    31078220 :         } else if (enteredRoundabout) {
     127             :             // only check the first roundabout
     128             :             break;
     129             :         }
     130             :         MSLane* via = nullptr;
     131    55146984 :         if (prevNormal != nullptr) {
     132   120261699 :             for (MSLink* link : prevNormal->getLinkCont()) {
     133    81423842 :                 if (link->getLane() == lane) {
     134             :                     via = link->getViaLane();
     135             :                 }
     136             :             }
     137             :         }
     138    55146984 :         if (enteredRoundabout) {
     139    40221195 :             distanceInRoundabout += lane->getLength();
     140    40221195 :             if (via != nullptr) {
     141    38714036 :                 distanceInRoundabout += via->getLength();
     142             :             }
     143             :         }
     144             :         // discount vehicles that are upstream from ego
     145    55146984 :         const double upstreamDiscount = &lane->getEdge() == &veh.getLane()->getEdge()
     146    55146984 :                                         ? (lane->getLength() - veh.getPositionOnLane()) / lane->getLength() : 1;
     147             :         prevNormal = lane;
     148    55146984 :         occupancyOuter += upstreamDiscount * lane->getBruttoVehLenSum();
     149             : #ifdef DEBUG_WANTS_CHANGE
     150             :         if (debugVehicle) {
     151             :             std::cout << " lane=" << lane->getID() << " occ=" << lane->getBruttoVehLenSum() << " discount=" << upstreamDiscount << " outer=" << occupancyOuter << "\n";
     152             :         }
     153             : #endif
     154    55146984 :         if (via != nullptr) {
     155    38773728 :             occupancyOuter += via->getBruttoVehLenSum();
     156             : #ifdef DEBUG_WANTS_CHANGE
     157             :             if (debugVehicle) {
     158             :                 std::cout << " via=" << via->getID() << " occ=" << via->getBruttoVehLenSum() << " outer=" << occupancyOuter << "\n";
     159             :             }
     160             : #endif
     161             :         }
     162    55146984 :         if (i < (int)inner.bestContinuations.size()) {
     163    55146984 :             MSLane* innerLane = inner.bestContinuations[i];
     164    55146984 :             occupancyInner += upstreamDiscount * innerLane->getBruttoVehLenSum();
     165             : #ifdef DEBUG_WANTS_CHANGE
     166             :             if (debugVehicle) {
     167             :                 std::cout << " inner=" << innerLane->getID() << " occ=" << innerLane->getBruttoVehLenSum() << " discount=" << upstreamDiscount << " inner=" << occupancyInner << "\n";
     168             :             }
     169             : #endif
     170    55146984 :             if (prevInner != nullptr) {
     171    85386304 :                 for (MSLink* link : prevInner->getLinkCont()) {
     172    46548447 :                     if (link->getLane() == innerLane && link->getViaLane() != nullptr) {
     173    38773728 :                         occupancyInner += link->getViaLane()->getBruttoVehLenSum();
     174             : #ifdef DEBUG_WANTS_CHANGE
     175             :                         if (debugVehicle) {
     176             :                             std::cout << " innerVia=" << link->getViaLane()->getID() << " occ=" << link->getViaLane()->getBruttoVehLenSum() << " inner=" << occupancyInner << "\n";
     177             :                         }
     178             : #endif
     179             :                     }
     180             :                 }
     181             :             }
     182             :             prevInner = innerLane;
     183             :         }
     184             :     }
     185             : 
     186             : #ifdef DEBUG_WANTS_CHANGE
     187             :     if (debugVehicle) {
     188             :         std::cout << "   distanceInRoundabout=" << distanceInRoundabout
     189             :                   << " roundaboutJunctionsAhead=" << roundaboutJunctionsAhead
     190             :                   << " occupancyInner=" << occupancyInner
     191             :                   << " occupancyOuter=" << occupancyOuter
     192             :                   << "\n";
     193             :     }
     194             : #endif
     195             : 
     196             :     const double maxOccupancy = MAX2(occupancyInner, occupancyOuter);
     197    16309127 :     if (maxOccupancy == 0) {
     198             :         // no bonues if the roundabout is empty
     199             :         return 0;
     200             :     }
     201             :     // give some bonus for using the inside lane at equal occupancy
     202    16162695 :     const double bonus = roundaboutJunctionsAhead * 7.5;
     203    16162695 :     const double relativeJam = (occupancyOuter - occupancyInner + bonus) / (maxOccupancy + bonus);
     204             :     // no bonus if the inner lane or the left lane entering the roundabout is jammed
     205             :     double jamFactor = MAX2(0.0, relativeJam);
     206    16162695 :     if (veh.getLane()->getEdge().isRoundabout() && curr.lane->getIndex() > neigh.lane->getIndex()) {
     207             :         // only use jamFactor when deciding to move to the inside lane but prefer
     208             :         // staying inside if the distance allows it
     209             :         jamFactor = 1;
     210             :     }
     211    16162695 :     const double result = distanceInRoundabout * jamFactor * bonusParam * 9; // the 9 is abitrary and only there for backward compatibility
     212             : #ifdef DEBUG_WANTS_CHANGE
     213             :     if (debugVehicle) {
     214             :         std::cout << "   relativeJam=" << relativeJam
     215             :                   << " jamFactor=" << jamFactor
     216             :                   << " distanceBonus=" << result
     217             :                   << "\n";
     218             :     }
     219             : #endif
     220    16162695 :     return result;
     221             : }
     222             : 
     223             : 
     224             : bool
     225    10961693 : MSLCHelper::updateBlockerLength(const MSVehicle& veh,  MSVehicle* blocker, int lcaCounter, double leftSpace, bool reliefConnection, double& leadingBlockerLength) {
     226             : #ifdef DEBUG_SAVE_BLOCKER_LENGTH
     227             :     if (DEBUG_COND) {
     228             :         std::cout << SIMTIME
     229             :                   << " veh=" << veh.getID()
     230             :                   << " saveBlockerLength blocker=" << Named::getIDSecure(blocker)
     231             :                   << " bState=" << (blocker == 0 ? "None" : toString((LaneChangeAction)blocker->getLaneChangeModel().getOwnState()))
     232             :                   << "\n";
     233             :     }
     234             : #endif
     235    10961693 :     if (blocker != nullptr && (blocker->getLaneChangeModel().getOwnState() & lcaCounter) != 0) {
     236             :         // is there enough space in front of us for the blocker?
     237      372761 :         const double potential = leftSpace - veh.getCarFollowModel().brakeGap(
     238      372761 :                                      veh.getSpeed(), veh.getCarFollowModel().getMaxDecel(), 0);
     239      372761 :         if (blocker->getVehicleType().getLengthWithGap() <= potential) {
     240             :             // save at least his length in myLeadingBlockerLength
     241      692504 :             leadingBlockerLength = MAX2(blocker->getVehicleType().getLengthWithGap(), leadingBlockerLength);
     242             : #ifdef DEBUG_SAVE_BLOCKER_LENGTH
     243             :             if (DEBUG_COND) {
     244             :                 std::cout << SIMTIME
     245             :                           << " veh=" << veh.getID()
     246             :                           << " blocker=" << Named::getIDSecure(blocker)
     247             :                           << " saving myLeadingBlockerLength=" << leadingBlockerLength
     248             :                           << "\n";
     249             :             }
     250             : #endif
     251             :         } else {
     252             :             // we cannot save enough space for the blocker. It needs to save
     253             :             // space for ego instead
     254       26509 :             const bool canReserve = blocker->getLaneChangeModel().saveBlockerLength(veh.getVehicleType().getLengthWithGap(), leftSpace);
     255             :             //reliefConnection ? std::numeric_limits<double>::max() : leftSpace);
     256             : #ifdef DEBUG_SAVE_BLOCKER_LENGTH
     257             :             if (DEBUG_COND) {
     258             :                 std::cout << SIMTIME
     259             :                           << " veh=" << veh.getID()
     260             :                           << " blocker=" << Named::getIDSecure(blocker)
     261             :                           << " cannot save space=" << blocker->getVehicleType().getLengthWithGap()
     262             :                           << " potential=" << potential
     263             :                           << " myReserved=" << leadingBlockerLength
     264             :                           << " canReserve=" << canReserve
     265             :                           << " reliefConnection=" << reliefConnection
     266             :                           << "\n";
     267             :             }
     268             : #endif
     269       26509 :             if (!canReserve && !reliefConnection) {
     270        3646 :                 const int blockerState = blocker->getLaneChangeModel().getOwnState();
     271        3646 :                 if ((blockerState & LCA_STRATEGIC) != 0
     272        3646 :                         && (blockerState & LCA_URGENT) != 0) {
     273             :                     // reserve anyway and try to avoid deadlock with emergency deceleration
     274        4302 :                     leadingBlockerLength = MAX2(blocker->getVehicleType().getLengthWithGap(), leadingBlockerLength);
     275             : #ifdef DEBUG_SAVE_BLOCKER_LENGTH
     276             :                     if (DEBUG_COND) {
     277             :                         std::cout << "   reserving anyway to avoid deadlock (will cause emergency braking)\n";
     278             :                     }
     279             : #endif
     280             :                 }
     281             :             }
     282       26509 :             return canReserve;
     283             :         }
     284             :     }
     285             :     return true;
     286             : }
     287             : 
     288             : 
     289             : bool
     290      196046 : MSLCHelper::canSaveBlockerLength(const MSVehicle& veh, double requested, double leftSpace) {
     291      196046 :     const double potential = leftSpace - veh.getCarFollowModel().brakeGap(veh.getSpeed(), veh.getCarFollowModel().getMaxDecel(), veh.getActionStepLengthSecs());
     292             : #ifdef DEBUG_SAVE_BLOCKER_LENGTH
     293             :     if (DEBUG_COND) {
     294             :         std::cout << SIMTIME << " canSaveBlockerLength veh=" << veh.getID() << " requested=" << requested << " leftSpace=" << leftSpace << " potential=" << potential << "\n";
     295             :     }
     296             : #endif
     297      196046 :     return potential >= requested;
     298             : }
     299             : 
     300             : 
     301             : bool
     302    12321029 : MSLCHelper::divergentRoute(const MSVehicle& v1, const MSVehicle& v2) {
     303             :     // a sufficient, but not necessary condition for divergence
     304    12481580 :     return (v1.getLane()->isInternal() && v2.getLane()->isInternal()
     305      100312 :             && v1.getLane()->getEdge().getFromJunction() == v2.getLane()->getEdge().getFromJunction()
     306    12416283 :             && &v1.getLane()->getEdge() != &v2.getLane()->getEdge());
     307             : }
     308             : 
     309             : 
     310             : double
     311     1600786 : MSLCHelper::getSpeedPreservingSecureGap(const MSVehicle& leader, const MSVehicle& follower, double currentGap, double leaderPlannedSpeed) {
     312             :     // whatever speed the follower choses in the next step, it will change both
     313             :     // the secureGap and the required followSpeed.
     314             :     // Let's assume the leader maintains speed
     315     1600786 :     const double nextGap = currentGap + SPEED2DIST(leaderPlannedSpeed - follower.getSpeed());
     316     1600786 :     double sGap = follower.getCarFollowModel().getSecureGap(&follower, &leader, follower.getSpeed(), leaderPlannedSpeed, leader.getCarFollowModel().getMaxDecel());
     317     1600786 :     if (nextGap >= sGap) {
     318             :         // follower may still accelerate
     319     1425328 :         const double nextGapMin = currentGap + SPEED2DIST(leaderPlannedSpeed - follower.getCarFollowModel().maxNextSpeed(follower.getSpeed(), &follower));
     320     1425328 :         const double vSafe = follower.getCarFollowModel().followSpeed(
     321     1425328 :                                  &follower, follower.getSpeed(), nextGapMin, leaderPlannedSpeed, leader.getCarFollowModel().getMaxDecel());
     322     1425328 :         return MAX2(vSafe, follower.getSpeed());
     323             :     } else {
     324             :         // follower must brake. The following brakes conservatively since the actual gap will be lower due to braking.
     325      175458 :         const double vSafe = follower.getCarFollowModel().followSpeed(
     326      175458 :                                  &follower, follower.getSpeed(), nextGap, leaderPlannedSpeed, leader.getCarFollowModel().getMaxDecel());
     327             :         // avoid emergency deceleration
     328      175458 :         return MAX2(vSafe, follower.getCarFollowModel().minNextSpeed(follower.getSpeed(), &follower));
     329             :     }
     330             : }
     331             : 
     332             : 
     333             : bool
     334      665538 : MSLCHelper::isBidiLeader(const MSVehicle* leader, const std::vector<MSLane*>& cont) {
     335      665538 :     if (leader == nullptr) {
     336             :         return false;
     337             :     }
     338      425076 :     const MSLane* lane1 = leader->getLane()->getNormalSuccessorLane()->getBidiLane();
     339      425076 :     const MSLane* lane2 = leader->getLane()->getNormalPredecessorLane()->getBidiLane();
     340      425076 :     if (lane1 == nullptr && lane2 == nullptr) {
     341             :         return false;
     342             :     }
     343      416766 :     bool result = std::find(cont.begin(), cont.end(), lane1) != cont.end();
     344      416766 :     if (!result && lane1 != lane2 && lane2 != nullptr) {
     345       76191 :         result = std::find(cont.begin(), cont.end(), lane2) != cont.end();
     346             :     }
     347             :     return result;
     348             : }
     349             : 
     350             : 
     351             : bool
     352       24954 : MSLCHelper::isBidiFollower(const MSVehicle* ego, const MSVehicle* follower) {
     353       24954 :     if (follower == nullptr) {
     354             :         return false;
     355             :     }
     356             :     bool result = false;
     357       19992 :     const MSLane* lane1 = follower->getLane()->getNormalSuccessorLane()->getBidiLane();
     358       19992 :     const MSLane* lane2 = follower->getLane()->getNormalPredecessorLane()->getBidiLane();
     359       19992 :     const ConstMSEdgeVector& route = ego->getRoute().getEdges();
     360       19992 :     if (lane1 != nullptr) {
     361       19524 :         result = std::find(route.begin(), route.end(), &lane1->getEdge()) != route.end();
     362             :     }
     363       19992 :     if (!result && lane1 != lane2 && lane2 != nullptr) {
     364        2277 :         result = std::find(route.begin(), route.end(), &lane2->getEdge()) != route.end();
     365             :     }
     366             :     return result;
     367             : }
     368             : 
     369             : /****************************************************************************/