MSLCHelper.cpp

Go to the documentation of this file.

/****************************************************************************/
// Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
// Copyright (C) 2013-2026 German Aerospace Center (DLR) and others.
// This program and the accompanying materials are made available under the
// terms of the Eclipse Public License 2.0 which is available at
// https://www.eclipse.org/legal/epl-2.0/
// This Source Code may also be made available under the following Secondary
// Licenses when the conditions for such availability set forth in the Eclipse
// Public License 2.0 are satisfied: GNU General Public License, version 2
// or later which is available at
// https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
// SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
/****************************************************************************/
// Common functions for lane change models
/****************************************************************************/
 
#include <microsim/MSEdge.h>
#include <microsim/MSLane.h>
#include <microsim/MSLink.h>
#include <microsim/MSVehicle.h>
#include <microsim/lcmodels/MSAbstractLaneChangeModel.h>
#include "MSLCHelper.h"
 
// ===========================================================================
// Debug flags
// ===========================================================================
//#define DEBUG_WANTS_CHANGE
//#define DEBUG_SAVE_BLOCKER_LENGTH
//#define DEBUG_UNWILLING_TO_HELP
 
#define DEBUG_COND (veh.isSelected())
//#define DEBUG_COND (true)
 
 
// ===========================================================================
// member method definitions
// ===========================================================================
 
double
MSLCHelper::getRoundaboutDistBonus(const MSVehicle& veh,
                                   double bonusParam,
                                   const MSVehicle::LaneQ& curr,
                                   const MSVehicle::LaneQ& neigh,
                                   const MSVehicle::LaneQ& best) {
    if (veh.getLaneChangeModel().isOpposite()) {
        return 0;
    }
    const MSVehicle::LaneQ& inner = neigh.lane->getIndex() > curr.lane->getIndex() ? neigh : curr;
#ifdef DEBUG_WANTS_CHANGE
    const bool debugVehicle = veh.getLaneChangeModel().debugVehicle();
    if (debugVehicle) {
        std::cout << SIMTIME << " veh=" << veh.getID() << " getRoundaboutDistBonus bonusParam=" << bonusParam
                  << " curr=" << curr.lane->getID()
                  << " neigh=" << neigh.lane->getID()
                  << " inner=" << inner.lane->getID()
                  << " best=" << best.lane->getID()
                  << "\n   innerCont=" << toString(inner.bestContinuations)
                  << "\n   bestCont=" << toString(best.bestContinuations)
                  << "\n";
    }
#endif
    if (neigh.lane == inner.lane && curr.bestContinuations.size() < neigh.bestContinuations.size()) {
        // the current lane does not continue to the roundabout and we need a strategic change first.
        return 0;
    }
 
    int roundaboutJunctionsAhead = 0;
    bool enteredRoundabout = false;
    double seen = -veh.getPositionOnLane();
 
    // first check using only normal lanes
    for (int i = 0; i < (int)best.bestContinuations.size(); i++) {
        const MSLane* lane = best.bestContinuations[i];
        if (lane == nullptr) {
            lane = veh.getLane();
        }
        if ((!enteredRoundabout || lane->getEdge().isRoundabout()) && i >= (int)inner.bestContinuations.size()) {
            // no bonus if we cannot continue on the inner lane until leaving the roundabout
#ifdef DEBUG_WANTS_CHANGE
            if (debugVehicle) {
                std::cout << "   noBonus: inner does not continue (lane=" << lane->getID() << ")\n";
            }
#endif
            return 0;
        }
        if (seen > 300) {
            // avoid long look-ahead
#ifdef DEBUG_WANTS_CHANGE
            if (debugVehicle) {
                std::cout << "   noBonus: seen=" << seen << " (lane=" << lane->getID() << ")\n";
            }
#endif
            return 0;
        }
        const MSJunction* junction = lane->getEdge().getToJunction();
        if (lane->getEdge().isRoundabout()) {
            enteredRoundabout = true;
            if (junction->getIncoming().size() + junction->getOutgoing().size() > 2) {
                roundaboutJunctionsAhead++;
            }
        } else if (enteredRoundabout) {
            // only check the first roundabout
            break;
        }
        seen += lane->getLength();
    }
    // no bonus if we want to take the next exit
    if (roundaboutJunctionsAhead < 2) {
#ifdef DEBUG_WANTS_CHANGE
        if (debugVehicle) {
            std::cout << "   noBonus: roundaboutJunctionsAhead=" << roundaboutJunctionsAhead << "\n";
        }
#endif
        return 0;
    }
    // compute bonus value based on jamming and exact distances (taking into
    // account internal lanes)
    double occupancyOuter = 0;
    double occupancyInner = 0;
    double distanceInRoundabout = 0;
    MSLane* prevNormal = nullptr;
    MSLane* prevInner = nullptr;
    enteredRoundabout = false;
    for (int i = 0; i < (int)best.bestContinuations.size(); i++) {
        MSLane* lane = best.bestContinuations[i];
        if (lane == nullptr) {
            continue;
        }
        if (lane->getEdge().isRoundabout()) {
            enteredRoundabout = true;
        } else if (enteredRoundabout) {
            // only check the first roundabout
            break;
        }
        MSLane* via = nullptr;
        if (prevNormal != nullptr) {
            for (MSLink* link : prevNormal->getLinkCont()) {
                if (link->getLane() == lane) {
                    via = link->getViaLane();
                }
            }
        }
        if (enteredRoundabout) {
            distanceInRoundabout += lane->getLength();
            if (via != nullptr) {
                distanceInRoundabout += via->getLength();
            }
        }
        // discount vehicles that are upstream from ego
        const double upstreamDiscount = &lane->getEdge() == &veh.getLane()->getEdge()
                                        ? (lane->getLength() - veh.getPositionOnLane()) / lane->getLength() : 1;
        prevNormal = lane;
        occupancyOuter += upstreamDiscount * lane->getBruttoVehLenSum();
#ifdef DEBUG_WANTS_CHANGE
        if (debugVehicle) {
            std::cout << " lane=" << lane->getID() << " occ=" << lane->getBruttoVehLenSum() << " discount=" << upstreamDiscount << " outer=" << occupancyOuter << "\n";
        }
#endif
        if (via != nullptr) {
            occupancyOuter += via->getBruttoVehLenSum();
#ifdef DEBUG_WANTS_CHANGE
            if (debugVehicle) {
                std::cout << " via=" << via->getID() << " occ=" << via->getBruttoVehLenSum() << " outer=" << occupancyOuter << "\n";
            }
#endif
        }
        if (i < (int)inner.bestContinuations.size()) {
            MSLane* innerLane = inner.bestContinuations[i];
            occupancyInner += upstreamDiscount * innerLane->getBruttoVehLenSum();
#ifdef DEBUG_WANTS_CHANGE
            if (debugVehicle) {
                std::cout << " inner=" << innerLane->getID() << " occ=" << innerLane->getBruttoVehLenSum() << " discount=" << upstreamDiscount << " inner=" << occupancyInner << "\n";
            }
#endif
            if (prevInner != nullptr) {
                for (MSLink* link : prevInner->getLinkCont()) {
                    if (link->getLane() == innerLane && link->getViaLane() != nullptr) {
                        occupancyInner += link->getViaLane()->getBruttoVehLenSum();
#ifdef DEBUG_WANTS_CHANGE
                        if (debugVehicle) {
                            std::cout << " innerVia=" << link->getViaLane()->getID() << " occ=" << link->getViaLane()->getBruttoVehLenSum() << " inner=" << occupancyInner << "\n";
                        }
#endif
                    }
                }
            }
            prevInner = innerLane;
        }
    }
 
#ifdef DEBUG_WANTS_CHANGE
    if (debugVehicle) {
        std::cout << "   distanceInRoundabout=" << distanceInRoundabout
                  << " roundaboutJunctionsAhead=" << roundaboutJunctionsAhead
                  << " occupancyInner=" << occupancyInner
                  << " occupancyOuter=" << occupancyOuter
                  << "\n";
    }
#endif
    if (abs(curr.bestLaneOffset) > 1 && enteredRoundabout) {
        const double bGap = veh.getCarFollowModel().brakeGap(veh.getSpeed() + ACCEL2SPEED(veh.getCarFollowModel().getMaxAccel()), veh.getCarFollowModel().getMaxDecel(), veh.getActionStepLengthSecs());
        const double reservation = veh.getLaneChangeModel().getExtraReservation(curr.bestLaneOffset);
        const double leftSpace = distanceInRoundabout - reservation - veh.getPositionOnLane();
#ifdef DEBUG_WANTS_CHANGE
        if (debugVehicle) {
            std::cout << "   bGap=" << bGap << " reserving=" << reservation << " leftover=" << (leftSpace - bGap) << "\n";
        }
#endif
        if (bGap > leftSpace) {
            return 0;
        }
    }
 
    const double maxOccupancy = MAX2(occupancyInner, occupancyOuter);
    // give some bonus for using the inside lane at equal occupancy
    const double bonus = roundaboutJunctionsAhead * 7.5;
    const double relativeJam = (occupancyOuter - occupancyInner + bonus) / (maxOccupancy + bonus);
    // no bonus if the inner lane or the left lane entering the roundabout is jammed
    double jamFactor = MAX2(0.0, relativeJam);
    if (veh.getLane()->getEdge().isRoundabout() && curr.lane->getIndex() > neigh.lane->getIndex()) {
        // only use jamFactor when deciding to move to the inside lane but prefer
        // staying inside if the distance allows it
        jamFactor = 1;
    }
    const double result = distanceInRoundabout * jamFactor * bonusParam * 9; // the 9 is abitrary and only there for backward compatibility
#ifdef DEBUG_WANTS_CHANGE
    if (debugVehicle) {
        std::cout << "   relativeJam=" << relativeJam
                  << " jamFactor=" << jamFactor
                  << " distanceBonus=" << result
                  << "\n";
    }
#endif
    return result;
}
 
 
bool
MSLCHelper::updateBlockerLength(const MSVehicle& veh,  MSVehicle* blocker, int lcaCounter, double leftSpace, bool reliefConnection, double& leadingBlockerLength) {
#ifdef DEBUG_SAVE_BLOCKER_LENGTH
    if (DEBUG_COND) {
        std::cout << SIMTIME
                  << " veh=" << veh.getID()
                  << " saveBlockerLength blocker=" << Named::getIDSecure(blocker)
                  << " bState=" << (blocker == 0 ? "None" : toString((LaneChangeAction)blocker->getLaneChangeModel().getOwnState()))
                  << "\n";
    }
#endif
    if (blocker != nullptr && (blocker->getLaneChangeModel().getOwnState() & lcaCounter) != 0) {
        // is there enough space in front of us for the blocker?
        const double required = blocker->getVehicleType().getLengthWithGap() + veh.getVehicleType().getMinGap();
        const double potential = leftSpace - veh.getCarFollowModel().brakeGap(
                                     veh.getSpeed(), veh.getCarFollowModel().getMaxDecel(), 0);
        if (required <= potential) {
            // save at least his length in myLeadingBlockerLength
            leadingBlockerLength = MAX2(required, leadingBlockerLength);
#ifdef DEBUG_SAVE_BLOCKER_LENGTH
            if (DEBUG_COND) {
                std::cout << SIMTIME
                          << " veh=" << veh.getID()
                          << " required=" << required
                          << " potential=" << potential
                          << " blocker=" << Named::getIDSecure(blocker)
                          << " saving myLeadingBlockerLength=" << leadingBlockerLength
                          << "\n";
            }
#endif
        } else {
            // we cannot save enough space for the blocker. It needs to save
            // space for ego instead
            const double required2 = veh.getVehicleType().getLengthWithGap() + blocker->getVehicleType().getMinGap();
            const double foeLeftSpace = leftSpace - veh.getPositionOnLane() + blocker->getPositionOnLane() - POSITION_EPS;
            const bool canReserve = blocker->getLaneChangeModel().saveBlockerLength(required2, foeLeftSpace);
            //reliefConnection ? std::numeric_limits<double>::max() : leftSpace);
#ifdef DEBUG_SAVE_BLOCKER_LENGTH
            if (DEBUG_COND) {
                std::cout << SIMTIME
                          << " veh=" << veh.getID()
                          << " required=" << required
                          << " potential=" << potential
                          << " blocker=" << Named::getIDSecure(blocker)
                          << " required2=" << required2
                          << " foeCanReserve=" << canReserve
                          << " myReserved=" << leadingBlockerLength
                          << " reliefConnection=" << reliefConnection
                          << "\n";
            }
#endif
            if (!canReserve && !reliefConnection) {
                const int blockerState = blocker->getLaneChangeModel().getOwnState();
                if ((blockerState & LCA_STRATEGIC) != 0
                        && (blockerState & LCA_URGENT) != 0) {
                    // reserve anyway and try to avoid deadlock with emergency deceleration
                    leadingBlockerLength = MAX2(required, leadingBlockerLength);
#ifdef DEBUG_SAVE_BLOCKER_LENGTH
                    if (DEBUG_COND) {
                        std::cout << "   reserving anyway to avoid deadlock (will cause emergency braking)\n";
                    }
#endif
                }
            }
            return canReserve;
        }
    }
    return true;
}
 
 
bool
MSLCHelper::canSaveBlockerLength(const MSVehicle& veh, double requested, double leftSpace) {
    const double potential = leftSpace - veh.getCarFollowModel().brakeGap(veh.getSpeed(), veh.getCarFollowModel().getMaxDecel(), veh.getActionStepLengthSecs());
#ifdef DEBUG_SAVE_BLOCKER_LENGTH
    if (DEBUG_COND) {
        std::cout << SIMTIME << " canSaveBlockerLength veh=" << veh.getID() << " requested=" << requested << " leftSpace=" << leftSpace << " potential=" << potential << "\n";
    }
#endif
    return potential >= requested;
}
 
 
bool
MSLCHelper::divergentRoute(const MSVehicle& v1, const MSVehicle& v2) {
    // a sufficient, but not necessary condition for divergence
    return (v1.getLane()->isInternal() && v2.getLane()->isInternal()
            && v1.getLane()->getEdge().getFromJunction() == v2.getLane()->getEdge().getFromJunction()
            && &v1.getLane()->getEdge() != &v2.getLane()->getEdge());
}
 
 
double
MSLCHelper::getSpeedPreservingSecureGap(const MSVehicle& leader, const MSVehicle& follower, double currentGap, double leaderPlannedSpeed) {
    // whatever speed the follower choses in the next step, it will change both
    // the secureGap and the required followSpeed.
    // Let's assume the leader maintains speed
    const double nextGap = currentGap + SPEED2DIST(leaderPlannedSpeed - follower.getSpeed());
    double sGap = follower.getCarFollowModel().getSecureGap(&follower, &leader, follower.getSpeed(), leaderPlannedSpeed, leader.getCarFollowModel().getMaxDecel());
    if (nextGap >= sGap) {
        // follower may still accelerate
        const double nextGapMin = currentGap + SPEED2DIST(leaderPlannedSpeed - follower.getCarFollowModel().maxNextSpeed(follower.getSpeed(), &follower));
        const double vSafe = follower.getCarFollowModel().followSpeed(
                                 &follower, follower.getSpeed(), nextGapMin, leaderPlannedSpeed, leader.getCarFollowModel().getMaxDecel());
        return MAX2(vSafe, follower.getSpeed());
    } else {
        // follower must brake. The following brakes conservatively since the actual gap will be lower due to braking.
        const double vSafe = follower.getCarFollowModel().followSpeed(
                                 &follower, follower.getSpeed(), nextGap, leaderPlannedSpeed, leader.getCarFollowModel().getMaxDecel());
        // avoid emergency deceleration
        return MAX2(vSafe, follower.getCarFollowModel().minNextSpeed(follower.getSpeed(), &follower));
    }
}
 
 
bool
MSLCHelper::isBidiLeader(const MSVehicle* leader, const std::vector<MSLane*>& cont) {
    if (leader == nullptr) {
        return false;
    }
    const MSLane* lane1 = leader->getLane()->getNormalSuccessorLane()->getBidiLane();
    const MSLane* lane2 = leader->getLane()->getNormalPredecessorLane()->getBidiLane();
    if (lane1 == nullptr && lane2 == nullptr) {
        return false;
    }
    bool result = std::find(cont.begin(), cont.end(), lane1) != cont.end();
    if (!result && lane1 != lane2 && lane2 != nullptr) {
        result = std::find(cont.begin(), cont.end(), lane2) != cont.end();
    }
    return result;
}
 
 
bool
MSLCHelper::isBidiFollower(const MSVehicle* ego, const MSVehicle* follower) {
    if (follower == nullptr) {
        return false;
    }
    bool result = false;
    const MSLane* lane1 = follower->getLane()->getNormalSuccessorLane()->getBidiLane();
    const MSLane* lane2 = follower->getLane()->getNormalPredecessorLane()->getBidiLane();
    const ConstMSEdgeVector& route = ego->getRoute().getEdges();
    if (lane1 != nullptr) {
        result = std::find(route.begin(), route.end(), &lane1->getEdge()) != route.end();
    }
    if (!result && lane1 != lane2 && lane2 != nullptr) {
        result = std::find(route.begin(), route.end(), &lane2->getEdge()) != route.end();
    }
    return result;
}
 
 
bool
MSLCHelper::unwillingToHelp(const MSVehicle& ego, double plannedSpeed, const MSVehicle& nv) {
    if (nv.getLaneChangeModel().getCooperativeHelpTime() < 0 || ego.getWaitingTime() < nv.getLaneChangeModel().getCooperativeHelpTime()) {
        // ego vehicle has not been waiting long enough to be eligible for unconditional help
        if (nv.getLaneChangeModel().getCooperativeHelpThreshold() >= 0 && (nv.getSpeed() - plannedSpeed) > nv.getLaneChangeModel().getCooperativeHelpThreshold()) {
            // neighhbor not willing to help because ego is much slower
#ifdef DEBUG_UNWILLING_TO_HELP
            if (DEBUG_COND) {
                std::cout << "\n nv=" << nv.getID() << " not willing to help because ego is much slower\n";
            }
#endif
            return true;
        }
        const double nvLaneMax = nv.getLane()->getVehicleMaxSpeed(&nv);
        if (nv.getSpeed() / nvLaneMax < nv.getLaneChangeModel().getCooperativeMinSpeed()) {
            // neighbor not willing to help because it is already to slow and does not want to disturb the flow
#ifdef DEBUG_UNWILLING_TO_HELP
            if (DEBUG_COND) {
                std::cout << "\n nv=" << nv.getID() << " not willing to help because it is already too slow\n";
            }
#endif
            return true;
        }
    }
    return false;
}
/****************************************************************************/