Permalink
Cannot retrieve contributors at this time
| /****************************************************************************/ | |
| // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo | |
| // Copyright (C) 2002-2021 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 | |
| /****************************************************************************/ | |
| /// @file MSLaneChanger.cpp | |
| /// @author Christian Roessel | |
| /// @author Daniel Krajzewicz | |
| /// @author Laura Bieker | |
| /// @author Michael Behrisch | |
| /// @author Friedemann Wesner | |
| /// @author Jakob Erdmann | |
| /// @date Fri, 01 Feb 2002 | |
| /// | |
| // Performs lane changing of vehicles | |
| /****************************************************************************/ | |
| #include <config.h> | |
| #include "MSLaneChanger.h" | |
| #include "MSNet.h" | |
| #include "MSLink.h" | |
| #include "MSVehicle.h" | |
| #include "MSVehicleType.h" | |
| #include "MSVehicleTransfer.h" | |
| #include "MSGlobals.h" | |
| #include <cassert> | |
| #include <iterator> | |
| #include <cstdlib> | |
| #include <cmath> | |
| #include <microsim/lcmodels/MSAbstractLaneChangeModel.h> | |
| #include <microsim/transportables/MSTransportableControl.h> | |
| #include <microsim/transportables/MSPModel.h> | |
| #include <utils/common/MsgHandler.h> | |
| #define OPPOSITE_OVERTAKING_SAFE_TIMEGAP 0.0 | |
| #define OPPOSITE_OVERTAKING_SAFETYGAP_HEADWAY_FACTOR 0.0 | |
| #define OPPOSITE_OVERTAKING_SAFETY_FACTOR 1.2 | |
| // XXX maxLookAhead should be higher if all leaders are stopped and lower when they are jammed/queued | |
| #define OPPOSITE_OVERTAKING_MAX_LOOKAHEAD 150.0 // just a guess | |
| #define OPPOSITE_OVERTAKING_MAX_LOOKAHEAD_EMERGENCY 1000.0 // just a guess | |
| // this is used for finding oncoming vehicles while driving in the opposite direction | |
| #define OPPOSITE_OVERTAKING_ONCOMING_LOOKAHEAD 1000.0 // just a guess | |
| // do not attempt overtaking maneuvers that would exceed this distance | |
| #define OPPOSITE_OVERTAKING_MAX_SPACE_TO_OVERTAKE 1000.0 // just a guess | |
| // =========================================================================== | |
| // debug defines | |
| // =========================================================================== | |
| //#define DEBUG_CONTINUE_CHANGE | |
| //#define DEBUG_CHECK_CHANGE | |
| //#define DEBUG_SURROUNDING_VEHICLES // debug getRealFollower() and getRealLeader() | |
| //#define DEBUG_CHANGE_OPPOSITE | |
| //#define DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME | |
| //#define DEBUG_ACTIONSTEPS | |
| //#define DEBUG_STATE | |
| //#define DEBUG_CANDIDATE | |
| //#define DEBUG_COND (vehicle->getLaneChangeModel().debugVehicle()) | |
| #define DEBUG_COND (vehicle->isSelected()) | |
| //#define DEBUG_COND (true) | |
| // =========================================================================== | |
| // ChangeElem member method definitions | |
| // =========================================================================== | |
| MSLaneChanger::ChangeElem::ChangeElem(MSLane* _lane) : | |
| lead(nullptr), | |
| lane(_lane), | |
| hoppedVeh(nullptr), | |
| lastBlocked(nullptr), | |
| firstBlocked(nullptr), | |
| ahead(lane), | |
| aheadNext(lane, nullptr, 0) { | |
| } | |
| void | |
| MSLaneChanger::ChangeElem::registerHop(MSVehicle* vehicle) { | |
| //std::cout << SIMTIME << " registerHop lane=" << lane->getID() << " veh=" << vehicle->getID() << "\n"; | |
| lane->myTmpVehicles.insert(lane->myTmpVehicles.begin(), vehicle); | |
| dens += vehicle->getVehicleType().getLengthWithGap(); | |
| hoppedVeh = vehicle; | |
| } | |
| // =========================================================================== | |
| // member method definitions | |
| // =========================================================================== | |
| MSLaneChanger::MSLaneChanger(const std::vector<MSLane*>* lanes, bool allowChanging) : | |
| myAllowsChanging(allowChanging), | |
| myChangeToOpposite(lanes->front()->getEdge().canChangeToOpposite()) { | |
| // Fill the changer with the lane-data. | |
| myChanger.reserve(lanes->size()); | |
| for (std::vector<MSLane*>::const_iterator lane = lanes->begin(); lane != lanes->end(); ++lane) { | |
| myChanger.push_back(ChangeElem(*lane)); | |
| myChanger.back().mayChangeRight = lane != lanes->begin(); | |
| myChanger.back().mayChangeLeft = (lane + 1) != lanes->end(); | |
| if ((*lane)->isInternal()) { | |
| // avoid changing on internal sibling lane | |
| if (myChanger.back().mayChangeRight && (*lane)->getLogicalPredecessorLane() == (*(lane - 1))->getLogicalPredecessorLane()) { | |
| myChanger.back().mayChangeRight = false; | |
| } | |
| if (myChanger.back().mayChangeLeft && (*lane)->getLogicalPredecessorLane() == (*(lane + 1))->getLogicalPredecessorLane()) { | |
| myChanger.back().mayChangeLeft = false; | |
| } | |
| // avoid changing if lanes have different lengths | |
| if (myChanger.back().mayChangeRight && (*lane)->getLength() != (*(lane - 1))->getLength()) { | |
| //std::cout << " cannot change right from lane=" << (*lane)->getID() << " len=" << (*lane)->getLength() << " to=" << (*(lane - 1))->getID() << " len2=" << (*(lane - 1))->getLength() << "\n"; | |
| myChanger.back().mayChangeRight = false; | |
| } | |
| if (myChanger.back().mayChangeLeft && (*lane)->getLength() != (*(lane + 1))->getLength()) { | |
| //std::cout << " cannot change left from lane=" << (*lane)->getID() << " len=" << (*lane)->getLength() << " to=" << (*(lane + 1))->getID() << " len2=" << (*(lane + 1))->getLength() << "\n"; | |
| myChanger.back().mayChangeLeft = false; | |
| } | |
| } | |
| } | |
| } | |
| MSLaneChanger::~MSLaneChanger() { | |
| } | |
| void | |
| MSLaneChanger::laneChange(SUMOTime t) { | |
| // This is what happens in one timestep. After initialization of the | |
| // changer, each vehicle will try to change. After that the changer | |
| // needs an update to prevent multiple changes of one vehicle. | |
| // Finally, the change-result has to be given back to the lanes. | |
| initChanger(); | |
| try { | |
| while (vehInChanger()) { | |
| const bool haveChanged = change(); | |
| updateChanger(haveChanged); | |
| } | |
| updateLanes(t); | |
| } catch (const ProcessError&) { | |
| // clean up locks or the gui may hang | |
| for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) { | |
| ce->lane->releaseVehicles(); | |
| } | |
| throw; | |
| } | |
| } | |
| void | |
| MSLaneChanger::initChanger() { | |
| // Prepare myChanger with a safe state. | |
| for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) { | |
| ce->lead = nullptr; | |
| ce->hoppedVeh = nullptr; | |
| ce->lastBlocked = nullptr; | |
| ce->firstBlocked = nullptr; | |
| ce->dens = 0; | |
| ce->lane->getVehiclesSecure(); | |
| //std::cout << SIMTIME << " initChanger lane=" << ce->lane->getID() << " vehicles=" << toString(ce->lane->myVehicles) << "\n"; | |
| } | |
| } | |
| void | |
| MSLaneChanger::updateChanger(bool vehHasChanged) { | |
| assert(veh(myCandi) != 0); | |
| // "Push" the vehicles to the back, i.e. follower becomes vehicle, | |
| // vehicle becomes leader, and leader becomes predecessor of vehicle, | |
| // if it exists. | |
| if (!vehHasChanged) { | |
| //std::cout << SIMTIME << " updateChanger: lane=" << myCandi->lane->getID() << " has new lead=" << veh(myCandi)->getID() << "\n"; | |
| myCandi->lead = veh(myCandi); | |
| } | |
| MSLane::VehCont& vehicles = myCandi->lane->myVehicles; | |
| vehicles.pop_back(); | |
| //std::cout << SIMTIME << " updateChanger lane=" << myCandi->lane->getID() << " vehicles=" << toString(myCandi->lane->myVehicles) << "\n"; | |
| } | |
| void | |
| MSLaneChanger::updateLanes(SUMOTime t) { | |
| // Update the lane's vehicle-container. | |
| // First: it is bad style to change other classes members, but for | |
| // this release, other attempts were too time-consuming. In a next | |
| // release we will change from this lane-centered design to a vehicle- | |
| // centered. This will solve many problems. | |
| // Second: this swap would be faster if vehicle-containers would have | |
| // been pointers, but then I had to change too much of the MSLane code. | |
| for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) { | |
| //std::cout << SIMTIME << " updateLanes lane=" << ce->lane->getID() << " myVehicles=" << toString(ce->lane->myVehicles) << " myTmpVehicles=" << toString(ce->lane->myTmpVehicles) << "\n"; | |
| ce->lane->swapAfterLaneChange(t); | |
| ce->lane->releaseVehicles(); | |
| } | |
| } | |
| MSLaneChanger::ChangerIt | |
| MSLaneChanger::findCandidate() { | |
| // Find the vehicle in myChanger with the largest position. If there | |
| // is no vehicle in myChanger (shouldn't happen) , return myChanger.end(). | |
| ChangerIt max = myChanger.end(); | |
| #ifdef DEBUG_CANDIDATE | |
| std::cout << SIMTIME << " findCandidate() on edge " << myChanger.begin()->lane->getEdge().getID() << std::endl; | |
| #endif | |
| for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) { | |
| if (veh(ce) == nullptr) { | |
| continue; | |
| } | |
| #ifdef DEBUG_CANDIDATE | |
| std::cout << " lane = " << ce->lane->getID() << "\n"; | |
| std::cout << " check vehicle=" << veh(ce)->getID() << " pos=" << veh(ce)->getPositionOnLane() << " lane=" << ce->lane->getID() << " isFrontOnLane=" << veh(ce)->isFrontOnLane(ce->lane) << "\n"; | |
| #endif | |
| if (max == myChanger.end()) { | |
| #ifdef DEBUG_CANDIDATE | |
| std::cout << " new max vehicle=" << veh(ce)->getID() << " pos=" << veh(ce)->getPositionOnLane() << " lane=" << ce->lane->getID() << " isFrontOnLane=" << veh(ce)->isFrontOnLane(ce->lane) << "\n"; | |
| #endif | |
| max = ce; | |
| continue; | |
| } | |
| assert(veh(ce) != 0); | |
| assert(veh(max) != 0); | |
| if (veh(max)->getPositionOnLane() < veh(ce)->getPositionOnLane()) { | |
| #ifdef DEBUG_CANDIDATE | |
| std::cout << " new max vehicle=" << veh(ce)->getID() << " pos=" << veh(ce)->getPositionOnLane() << " lane=" << ce->lane->getID() << " isFrontOnLane=" << veh(ce)->isFrontOnLane(ce->lane) << " oldMaxPos=" << veh(max)->getPositionOnLane() << "\n"; | |
| #endif | |
| max = ce; | |
| } | |
| } | |
| assert(max != myChanger.end()); | |
| assert(veh(max) != 0); | |
| return max; | |
| } | |
| bool | |
| MSLaneChanger::mayChange(int direction) const { | |
| if (direction == 0) { | |
| return true; | |
| } | |
| if (!myAllowsChanging) { | |
| return false; | |
| } | |
| SUMOVehicleClass svc = veh(myCandi)->getVehicleType().getVehicleClass(); | |
| if (direction == -1) { | |
| return myCandi->mayChangeRight && (myCandi - 1)->lane->allowsVehicleClass(svc) && myCandi->lane->allowsChangingRight(svc); | |
| } else if (direction == 1) { | |
| return myCandi->mayChangeLeft && (myCandi + 1)->lane->allowsVehicleClass(svc) && myCandi->lane->allowsChangingLeft(svc); | |
| } else { | |
| return false; | |
| } | |
| } | |
| bool | |
| MSLaneChanger::change() { | |
| // Find change-candidate. If it is on an allowed lane, try to change | |
| // to the right (there is a rule in Germany that you have to change | |
| // to the right, unless you are overtaking). If change to the right | |
| // isn't possible, check if there is a possibility to overtake (on the | |
| // left. | |
| // If candidate isn't on an allowed lane, changing to an allowed has | |
| // priority. | |
| #ifdef DEBUG_ACTIONSTEPS | |
| // std::cout<< "\nCHANGE" << std::endl; | |
| #endif | |
| myCandi = findCandidate(); | |
| MSVehicle* vehicle = veh(myCandi); | |
| vehicle->getLaneChangeModel().clearNeighbors(); | |
| if (vehicle->getLaneChangeModel().isChangingLanes() && !vehicle->getLaneChangeModel().alreadyChanged()) { | |
| return continueChange(vehicle, myCandi); | |
| } | |
| vehicle->getLaneChangeModel().setSpeedLat(0); | |
| if (!myAllowsChanging || vehicle->getLaneChangeModel().alreadyChanged() || vehicle->isStoppedOnLane()) { | |
| registerUnchanged(vehicle); | |
| return false; | |
| } | |
| if (!vehicle->isActive()) { | |
| #ifdef DEBUG_ACTIONSTEPS | |
| if (DEBUG_COND) { | |
| std::cout << SIMTIME << " veh '" << vehicle->getID() << "' skips regular change checks." << std::endl; | |
| } | |
| #endif | |
| bool changed = false; | |
| const int oldstate = vehicle->getLaneChangeModel().getOwnState(); | |
| // let TraCI influence the wish to change lanes during non-actionsteps | |
| checkTraCICommands(vehicle); | |
| if (oldstate != vehicle->getLaneChangeModel().getOwnState()) { | |
| changed = applyTraCICommands(vehicle); | |
| } | |
| if (!changed) { | |
| registerUnchanged(vehicle); | |
| } | |
| return changed; | |
| } | |
| // Check for changes to the opposite lane if vehicle is active | |
| std::pair<MSVehicle* const, double> leader = getRealLeader(myCandi); | |
| if (myChanger.size() == 1 || vehicle->getLaneChangeModel().isOpposite() || (!mayChange(-1) && !mayChange(1))) { | |
| if (changeOpposite(vehicle, leader)) { | |
| return true; | |
| } | |
| registerUnchanged(vehicle); | |
| return false; | |
| } | |
| vehicle->updateBestLanes(); // needed? | |
| for (int i = 0; i < (int) myChanger.size(); ++i) { | |
| vehicle->adaptBestLanesOccupation(i, myChanger[i].dens); | |
| } | |
| const std::vector<MSVehicle::LaneQ>& preb = vehicle->getBestLanes(); | |
| // check whether the vehicle wants and is able to change to right lane | |
| int stateRight = 0; | |
| if (mayChange(-1)) { | |
| stateRight = checkChangeWithinEdge(-1, leader, preb); | |
| // change if the vehicle wants to and is allowed to change | |
| if ((stateRight & LCA_RIGHT) != 0 && (stateRight & LCA_BLOCKED) == 0) { | |
| vehicle->getLaneChangeModel().setOwnState(stateRight); | |
| return startChange(vehicle, myCandi, -1); | |
| } | |
| if ((stateRight & LCA_RIGHT) != 0 && (stateRight & LCA_URGENT) != 0) { | |
| (myCandi - 1)->lastBlocked = vehicle; | |
| if ((myCandi - 1)->firstBlocked == nullptr) { | |
| (myCandi - 1)->firstBlocked = vehicle; | |
| } | |
| } | |
| } | |
| // check whether the vehicle wants and is able to change to left lane | |
| int stateLeft = 0; | |
| if (mayChange(1)) { | |
| stateLeft = checkChangeWithinEdge(1, leader, preb); | |
| // change if the vehicle wants to and is allowed to change | |
| if ((stateLeft & LCA_LEFT) != 0 && (stateLeft & LCA_BLOCKED) == 0) { | |
| vehicle->getLaneChangeModel().setOwnState(stateLeft); | |
| return startChange(vehicle, myCandi, 1); | |
| } | |
| if ((stateLeft & LCA_LEFT) != 0 && (stateLeft & LCA_URGENT) != 0) { | |
| (myCandi + 1)->lastBlocked = vehicle; | |
| if ((myCandi + 1)->firstBlocked == nullptr) { | |
| (myCandi + 1)->firstBlocked = vehicle; | |
| } | |
| } | |
| } | |
| if ((stateRight & LCA_URGENT) != 0 && (stateLeft & LCA_URGENT) != 0) { | |
| // ... wants to go to the left AND to the right | |
| // just let them go to the right lane... | |
| stateLeft = 0; | |
| } | |
| vehicle->getLaneChangeModel().setOwnState(stateRight | stateLeft); | |
| // only emergency vehicles should change to the opposite side on a | |
| // multi-lane road | |
| if (vehicle->getVehicleType().getVehicleClass() == SVC_EMERGENCY | |
| && changeOpposite(vehicle, leader)) { | |
| return true; | |
| } | |
| registerUnchanged(vehicle); | |
| return false; | |
| } | |
| void | |
| MSLaneChanger::registerUnchanged(MSVehicle* vehicle) { | |
| //std::cout << SIMTIME << " registerUnchanged lane=" << myCandi->lane->getID() << " veh=" << vehicle->getID() << "\n"; | |
| myCandi->lane->myTmpVehicles.insert(myCandi->lane->myTmpVehicles.begin(), veh(myCandi)); | |
| myCandi->dens += vehicle->getVehicleType().getLengthWithGap(); | |
| vehicle->getLaneChangeModel().unchanged(); | |
| } | |
| void | |
| MSLaneChanger::checkTraCICommands(MSVehicle* vehicle) { | |
| #ifdef DEBUG_STATE | |
| const int oldstate = vehicle->getLaneChangeModel().getOwnState(); | |
| #endif | |
| vehicle->getLaneChangeModel().checkTraCICommands(); | |
| #ifdef DEBUG_STATE | |
| if (DEBUG_COND) { | |
| const int newstate = vehicle->getLaneChangeModel().getOwnState(); | |
| std::cout << SIMTIME | |
| << " veh=" << vehicle->getID() | |
| << " oldState=" << toString((LaneChangeAction) oldstate) | |
| << " newState=" << toString((LaneChangeAction) newstate) | |
| << ((newstate & LCA_BLOCKED) != 0 ? " (blocked)" : "") | |
| << ((newstate & LCA_OVERLAPPING) != 0 ? " (overlap)" : "") | |
| << "\n"; | |
| } | |
| #endif | |
| } | |
| bool | |
| MSLaneChanger::applyTraCICommands(MSVehicle* vehicle) { | |
| // Execute request if not blocked | |
| bool changed = false; | |
| const int state = vehicle->getLaneChangeModel().getOwnState(); | |
| const int dir = (state & LCA_RIGHT) != 0 ? -1 : ((state & LCA_LEFT) != 0 ? 1 : 0); | |
| const bool execute = dir != 0 && ((state & LCA_BLOCKED) == 0); | |
| if (execute) { | |
| ChangerIt to = myCandi + dir; | |
| bool continuous = vehicle->getLaneChangeModel().startLaneChangeManeuver(myCandi->lane, to->lane, dir); | |
| if (continuous) { | |
| changed = continueChange(vehicle, myCandi); | |
| } else { | |
| // insert vehicle into target lane | |
| to->registerHop(vehicle); | |
| changed = true; | |
| } | |
| } | |
| return changed; | |
| } | |
| bool | |
| MSLaneChanger::startChange(MSVehicle* vehicle, ChangerIt& from, int direction) { | |
| if (vehicle->isRemoteControlled()) { | |
| registerUnchanged(vehicle); | |
| return false; | |
| } | |
| ChangerIt to = from + direction; | |
| // @todo delay entering the target lane until the vehicle intersects it | |
| // physically (considering lane width and vehicle width) | |
| //if (to->lane->getID() == "beg_1") std::cout << SIMTIME << " startChange to lane=" << to->lane->getID() << " myTmpVehiclesBefore=" << toString(to->lane->myTmpVehicles) << "\n"; | |
| const bool continuous = vehicle->getLaneChangeModel().startLaneChangeManeuver(from->lane, to->lane, direction); | |
| if (continuous) { | |
| return continueChange(vehicle, myCandi); | |
| } else { | |
| to->registerHop(vehicle); | |
| to->lane->requireCollisionCheck(); | |
| return true; | |
| } | |
| } | |
| bool | |
| MSLaneChanger::continueChange(MSVehicle* vehicle, ChangerIt& from) { | |
| MSAbstractLaneChangeModel& lcm = vehicle->getLaneChangeModel(); | |
| const int direction = lcm.isOpposite() ? 1 : lcm.getLaneChangeDirection(); | |
| const bool pastMidpoint = lcm.updateCompletion(); // computes lcm.mySpeedLat as a side effect | |
| const double speedLat = lcm.isOpposite() ? -lcm.getSpeedLat() : lcm.getSpeedLat(); | |
| vehicle->myState.myPosLat += SPEED2DIST(speedLat); | |
| vehicle->myCachedPosition = Position::INVALID; | |
| //std::cout << SIMTIME << " veh=" << vehicle->getID() << " dir=" << direction << " pm=" << pastMidpoint << " speedLat=" << speedLat << " posLat=" << vehicle->myState.myPosLat << "\n"; | |
| if (pastMidpoint) { | |
| MSLane* source = myCandi->lane; | |
| MSLane* target = source->getParallelLane(direction); | |
| vehicle->myState.myPosLat -= direction * 0.5 * (source->getWidth() + target->getWidth()); | |
| lcm.primaryLaneChanged(source, target, direction); | |
| if (&source->getEdge() == &target->getEdge()) { | |
| ChangerIt to = from + direction; | |
| to->registerHop(vehicle); | |
| } | |
| target->requireCollisionCheck(); | |
| } else { | |
| from->registerHop(vehicle); | |
| from->lane->requireCollisionCheck(); | |
| } | |
| if (!lcm.isChangingLanes()) { | |
| vehicle->myState.myPosLat = 0; | |
| lcm.endLaneChangeManeuver(); | |
| } | |
| lcm.updateShadowLane(); | |
| if (lcm.getShadowLane() != nullptr && &lcm.getShadowLane()->getEdge() == &vehicle->getLane()->getEdge()) { | |
| // set as hoppedVeh on the shadow lane so it is found as leader on both lanes | |
| ChangerIt shadow = pastMidpoint ? from : from + lcm.getShadowDirection(); | |
| shadow->hoppedVeh = vehicle; | |
| lcm.getShadowLane()->requireCollisionCheck(); | |
| } | |
| vehicle->myAngle = vehicle->computeAngle(); | |
| if (lcm.isOpposite()) { | |
| vehicle->myAngle += M_PI; | |
| } | |
| #ifdef DEBUG_CONTINUE_CHANGE | |
| if (DEBUG_COND) { | |
| std::cout << SIMTIME | |
| << " continueChange veh=" << vehicle->getID() | |
| << " from=" << Named::getIDSecure(from->lane) | |
| << " dir=" << direction | |
| << " speedLat=" << speedLat | |
| << " pastMidpoint=" << pastMidpoint | |
| << " posLat=" << vehicle->getLateralPositionOnLane() | |
| //<< " completion=" << lcm.getLaneChangeCompletion() | |
| << " shadowLane=" << Named::getIDSecure(lcm.getShadowLane()) | |
| //<< " shadowHopped=" << Named::getIDSecure(shadow->lane) | |
| << "\n"; | |
| } | |
| #endif | |
| return pastMidpoint && lcm.getShadowLane() == nullptr; | |
| } | |
| std::pair<MSVehicle* const, double> | |
| MSLaneChanger::getRealLeader(const ChangerIt& target) const { | |
| assert(veh(myCandi) != 0); | |
| MSVehicle* vehicle = veh(myCandi); | |
| #ifdef DEBUG_SURROUNDING_VEHICLES | |
| if (DEBUG_COND) { | |
| std::cout << SIMTIME << " veh '" << vehicle->getID() << "' looks for leader on lc-target lane '" << target->lane->getID() << "'." << std::endl; | |
| } | |
| #endif | |
| // get the leading vehicle on the lane to change to | |
| MSVehicle* neighLead = target->lead; | |
| #ifdef DEBUG_SURROUNDING_VEHICLES | |
| if (DEBUG_COND) { | |
| if (neighLead != 0) { | |
| std::cout << "Considering '" << neighLead->getID() << "' at position " << neighLead->getPositionOnLane() << std::endl; | |
| } | |
| } | |
| #endif | |
| // check whether the hopped vehicle became the leader | |
| if (target->hoppedVeh != nullptr) { | |
| double hoppedPos = target->hoppedVeh->getPositionOnLane(); | |
| #ifdef DEBUG_SURROUNDING_VEHICLES | |
| if (DEBUG_COND) { | |
| std::cout << "Considering hopped vehicle '" << target->hoppedVeh->getID() << "' at position " << hoppedPos << std::endl; | |
| } | |
| #endif | |
| if (hoppedPos > vehicle->getPositionOnLane() && (neighLead == nullptr || neighLead->getPositionOnLane() > hoppedPos)) { | |
| neighLead = target->hoppedVeh; | |
| //if (vehicle->getID() == "flow.21") std::cout << SIMTIME << " neighLead=" << Named::getIDSecure(neighLead) << " (422)\n"; | |
| } | |
| } | |
| if (neighLead == nullptr) { | |
| #ifdef DEBUG_SURROUNDING_VEHICLES | |
| if (DEBUG_COND) { | |
| std::cout << "Looking for leader on consecutive lanes." << std::endl; | |
| } | |
| #endif | |
| // There's no leader on the target lane. Look for leaders on consecutive lanes. | |
| // (there might also be partial leaders due to continuous lane changing) | |
| MSLane* targetLane = target->lane; | |
| const double egoBack = vehicle->getBackPositionOnLane(); | |
| double leaderBack = targetLane->getLength(); | |
| for (MSVehicle* pl : targetLane->myPartialVehicles) { | |
| double plBack = pl->getBackPositionOnLane(targetLane); | |
| if (plBack < leaderBack && | |
| pl->getPositionOnLane(targetLane) + pl->getVehicleType().getMinGap() >= egoBack) { | |
| neighLead = pl; | |
| leaderBack = plBack; | |
| } | |
| } | |
| if (neighLead != nullptr) { | |
| #ifdef DEBUG_SURROUNDING_VEHICLES | |
| if (DEBUG_COND) { | |
| std::cout << " found leader=" << neighLead->getID() << " (partial)\n"; | |
| } | |
| #endif | |
| return std::pair<MSVehicle*, double>(neighLead, leaderBack - vehicle->getPositionOnLane() - vehicle->getVehicleType().getMinGap()); | |
| } | |
| double seen = myCandi->lane->getLength() - vehicle->getPositionOnLane(); | |
| double speed = vehicle->getSpeed(); | |
| double dist = vehicle->getCarFollowModel().brakeGap(speed) + vehicle->getVehicleType().getMinGap(); | |
| // always check for link leaders while on an internal lane | |
| if (seen > dist && !myCandi->lane->isInternal()) { | |
| #ifdef DEBUG_SURROUNDING_VEHICLES | |
| if (DEBUG_COND) { | |
| std::cout << " found no leader within dist=" << dist << "\n"; | |
| } | |
| #endif | |
| return std::pair<MSVehicle* const, double>(static_cast<MSVehicle*>(nullptr), -1); | |
| } | |
| const std::vector<MSLane*>& bestLaneConts = vehicle->getBestLanesContinuation(targetLane); | |
| std::pair<MSVehicle* const, double> result = target->lane->getLeaderOnConsecutive(dist, seen, speed, *vehicle, bestLaneConts); | |
| #ifdef DEBUG_SURROUNDING_VEHICLES | |
| if (DEBUG_COND) { | |
| std::cout << " found consecutiveLeader=" << Named::getIDSecure(result.first) << "\n"; | |
| } | |
| #endif | |
| return result; | |
| } else { | |
| #ifdef DEBUG_SURROUNDING_VEHICLES | |
| if (DEBUG_COND) { | |
| std::cout << " found leader=" << neighLead->getID() << "\n"; | |
| } | |
| #endif | |
| return std::pair<MSVehicle* const, double>(neighLead, neighLead->getBackPositionOnLane(target->lane) - vehicle->getPositionOnLane() - vehicle->getVehicleType().getMinGap()); | |
| } | |
| } | |
| std::pair<MSVehicle* const, double> | |
| MSLaneChanger::getRealFollower(const ChangerIt& target) const { | |
| assert(veh(myCandi) != 0); | |
| MSVehicle* vehicle = veh(myCandi); | |
| #ifdef DEBUG_SURROUNDING_VEHICLES | |
| if (DEBUG_COND) { | |
| std::cout << SIMTIME << " veh '" << vehicle->getID() << "' looks for follower on lc-target lane '" << target->lane->getID() << "'." << std::endl; | |
| } | |
| #endif | |
| const double candiPos = vehicle->getPositionOnLane(); | |
| MSVehicle* neighFollow = veh(target); | |
| #ifdef DEBUG_SURROUNDING_VEHICLES | |
| if (DEBUG_COND) { | |
| if (neighFollow != 0) { | |
| std::cout << "veh(target) returns '" << neighFollow->getID() << "' at position " << neighFollow->getPositionOnLane() << std::endl; | |
| } else { | |
| std::cout << "veh(target) returns none." << std::endl; | |
| } | |
| } | |
| #endif | |
| #ifdef DEBUG_SURROUNDING_VEHICLES | |
| if (DEBUG_COND) { | |
| if (getCloserFollower(candiPos, neighFollow, target->hoppedVeh) != neighFollow) { | |
| std::cout << "Hopped vehicle '" << target->hoppedVeh->getID() << "' at position " << target->hoppedVeh->getPositionOnLane() << " is closer." << std::endl; | |
| } | |
| } | |
| #endif | |
| // check whether the hopped vehicle became the follower | |
| neighFollow = getCloserFollower(candiPos, neighFollow, target->hoppedVeh); | |
| #ifdef DEBUG_SURROUNDING_VEHICLES | |
| if (DEBUG_COND) { | |
| MSVehicle* partialBehind = getCloserFollower(candiPos, neighFollow, target->lane->getPartialBehind(vehicle)); | |
| if (partialBehind != 0 && partialBehind != neighFollow) { | |
| std::cout << "'Partial behind'-vehicle '" << target->lane->getPartialBehind(vehicle)->getID() << "' at position " << partialBehind->getPositionOnLane() << " is closer." << std::endl; | |
| } | |
| } | |
| #endif | |
| // or a follower which is partially lapping into the target lane | |
| neighFollow = getCloserFollower(candiPos, neighFollow, target->lane->getPartialBehind(vehicle)); | |
| if (neighFollow == nullptr) { | |
| CLeaderDist consecutiveFollower = target->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true)[0]; | |
| #ifdef DEBUG_SURROUNDING_VEHICLES | |
| if (DEBUG_COND) { | |
| if (consecutiveFollower.first == 0) { | |
| std::cout << "no follower found." << std::endl; | |
| } else { | |
| std::cout << "found follower '" << consecutiveFollower.first->getID() << "' on consecutive lanes." << std::endl; | |
| } | |
| } | |
| #endif | |
| return std::make_pair(const_cast<MSVehicle*>(consecutiveFollower.first), consecutiveFollower.second); | |
| } else { | |
| #ifdef DEBUG_SURROUNDING_VEHICLES | |
| if (DEBUG_COND) { | |
| std::cout << "found follower '" << neighFollow->getID() << "'." << std::endl; | |
| } | |
| #endif | |
| return std::pair<MSVehicle* const, double>(neighFollow, | |
| vehicle->getPositionOnLane() - vehicle->getVehicleType().getLength() - neighFollow->getPositionOnLane() - neighFollow->getVehicleType().getMinGap()); | |
| } | |
| } | |
| MSVehicle* | |
| MSLaneChanger::getCloserFollower(const double maxPos, MSVehicle* follow1, MSVehicle* follow2) { | |
| if (follow1 == nullptr || follow1->getPositionOnLane() > maxPos) { | |
| return follow2; | |
| } else if (follow2 == nullptr || follow2->getPositionOnLane() > maxPos) { | |
| return follow1; | |
| } else { | |
| if (follow1->getPositionOnLane() > follow2->getPositionOnLane()) { | |
| return follow1; | |
| } else { | |
| return follow2; | |
| } | |
| } | |
| } | |
| int | |
| MSLaneChanger::checkChangeWithinEdge( | |
| int laneOffset, | |
| const std::pair<MSVehicle* const, double>& leader, | |
| const std::vector<MSVehicle::LaneQ>& preb) const { | |
| std::pair<MSVehicle* const, double> neighLead = getRealLeader(myCandi + laneOffset); | |
| std::pair<MSVehicle*, double> neighFollow = getRealFollower(myCandi + laneOffset); | |
| if (neighLead.first != nullptr && neighLead.first == neighFollow.first) { | |
| // vehicles should not be leader and follower at the same time to avoid | |
| // contradictory behavior | |
| neighFollow.first = 0; | |
| } | |
| ChangerIt target = myCandi + laneOffset; | |
| return checkChange(laneOffset, target->lane, leader, neighLead, neighFollow, preb); | |
| } | |
| int | |
| MSLaneChanger::checkChange( | |
| int laneOffset, | |
| const MSLane* targetLane, | |
| const std::pair<MSVehicle* const, double>& leader, | |
| const std::pair<MSVehicle* const, double>& neighLead, | |
| const std::pair<MSVehicle* const, double>& neighFollow, | |
| const std::vector<MSVehicle::LaneQ>& preb) const { | |
| MSVehicle* vehicle = veh(myCandi); | |
| #ifdef DEBUG_CHECK_CHANGE | |
| if (DEBUG_COND) { | |
| std::cout | |
| << "\n" << SIMTIME << " checkChange() for vehicle '" << vehicle->getID() << "'" | |
| << std::endl; | |
| } | |
| #endif | |
| int blocked = 0; | |
| int blockedByLeader = (laneOffset == -1 ? LCA_BLOCKED_BY_RIGHT_LEADER : LCA_BLOCKED_BY_LEFT_LEADER); | |
| int blockedByFollower = (laneOffset == -1 ? LCA_BLOCKED_BY_RIGHT_FOLLOWER : LCA_BLOCKED_BY_LEFT_FOLLOWER); | |
| // overlap | |
| if (neighFollow.first != nullptr && neighFollow.second < 0) { | |
| blocked |= (blockedByFollower | LCA_OVERLAPPING); | |
| // Debug (Leo) | |
| #ifdef DEBUG_CHECK_CHANGE | |
| if (DEBUG_COND) { | |
| std::cout << SIMTIME | |
| << " overlapping with follower..." | |
| << std::endl; | |
| } | |
| #endif | |
| } | |
| if (neighLead.first != nullptr && neighLead.second < 0) { | |
| blocked |= (blockedByLeader | LCA_OVERLAPPING); | |
| #ifdef DEBUG_CHECK_CHANGE | |
| if (DEBUG_COND) { | |
| std::cout << SIMTIME | |
| << " overlapping with leader..." | |
| << std::endl; | |
| } | |
| #endif | |
| } | |
| double secureFrontGap = MSAbstractLaneChangeModel::NO_NEIGHBOR; | |
| double secureBackGap = MSAbstractLaneChangeModel::NO_NEIGHBOR; | |
| double secureOrigFrontGap = MSAbstractLaneChangeModel::NO_NEIGHBOR; | |
| const double tauRemainder = vehicle->getActionStepLength() == DELTA_T ? 0 : MAX2(vehicle->getCarFollowModel().getHeadwayTime() - TS, 0.); | |
| // safe back gap | |
| if ((blocked & blockedByFollower) == 0 && neighFollow.first != nullptr) { | |
| // Calculate secure gap conservatively with vNextFollower / vNextLeader as | |
| // extrapolated speeds after the driver's expected reaction time (tau). | |
| // NOTE: there exists a possible source for collisions if the follower and the leader | |
| // have desynchronized action steps as the extrapolated speeds can be exceeded in this case | |
| // Expected reaction time (tau) for the follower-vehicle. | |
| // (substracted TS since at this point the vehicles' states are already updated) | |
| const double vNextFollower = neighFollow.first->getSpeed() + MAX2(0., tauRemainder * neighFollow.first->getAcceleration()); | |
| const double vNextLeader = vehicle->getSpeed() + MIN2(0., tauRemainder * vehicle->getAcceleration()); | |
| // !!! eigentlich: vsafe braucht die Max. Geschwindigkeit beider Spuren | |
| secureBackGap = neighFollow.first->getCarFollowModel().getSecureGap(neighFollow.first, vehicle, vNextFollower, | |
| vNextLeader, vehicle->getCarFollowModel().getMaxDecel()); | |
| #ifdef DEBUG_CHECK_CHANGE | |
| if (DEBUG_COND) { | |
| std::cout << SIMTIME | |
| << " backGap=" << neighFollow.second | |
| << " vNextFollower=" << vNextFollower | |
| << " vNextEgo=" << vNextLeader | |
| << " secureGap=" << secureBackGap | |
| << " safetyFactor=" << vehicle->getLaneChangeModel().getSafetyFactor() | |
| << " blocked=" << (neighFollow.second < secureBackGap * vehicle->getLaneChangeModel().getSafetyFactor()) | |
| << "\n"; | |
| } | |
| #endif | |
| if (neighFollow.second < secureBackGap * vehicle->getLaneChangeModel().getSafetyFactor()) { | |
| blocked |= blockedByFollower; | |
| } | |
| } | |
| // safe front gap | |
| if ((blocked & blockedByLeader) == 0 && neighLead.first != nullptr) { | |
| // Calculate secure gap conservatively with vNextFollower / vNextLeader as | |
| // extrapolated speeds after the driver's expected reaction time (tau). | |
| // NOTE: there exists a possible source for collisions if the follower and the leader | |
| // have desynchronized action steps as the extrapolated speeds can be exceeded in this case | |
| // Expected reaction time (tau) for the follower-vehicle. | |
| // (substracted TS since at this point the vehicles' states are already updated) | |
| const double vNextFollower = vehicle->getSpeed() + MAX2(0., tauRemainder * vehicle->getAcceleration()); | |
| const double vNextLeader = neighLead.first->getSpeed() + MIN2(0., tauRemainder * neighLead.first->getAcceleration()); | |
| // !!! eigentlich: vsafe braucht die Max. Geschwindigkeit beider Spuren | |
| secureFrontGap = vehicle->getCarFollowModel().getSecureGap(vehicle, neighLead.first, vNextFollower, | |
| vNextLeader, neighLead.first->getCarFollowModel().getMaxDecel()); | |
| #ifdef DEBUG_CHECK_CHANGE | |
| if (DEBUG_COND) { | |
| std::cout << SIMTIME | |
| << " frontGap=" << neighFollow.second | |
| << " vNextEgo=" << vNextFollower | |
| << " vNextLeader=" << vNextLeader | |
| << " secureGap=" << secureFrontGap | |
| << " safetyFactor=" << vehicle->getLaneChangeModel().getSafetyFactor() | |
| << " blocked=" << (neighLead.second < secureFrontGap * vehicle->getLaneChangeModel().getSafetyFactor()) | |
| << "\n"; | |
| } | |
| #endif | |
| if (neighLead.second < secureFrontGap * vehicle->getLaneChangeModel().getSafetyFactor()) { | |
| blocked |= blockedByLeader; | |
| } | |
| } | |
| if (blocked == 0 && targetLane->hasPedestrians()) { | |
| PersonDist nextLeader = targetLane->nextBlocking(vehicle->getBackPositionOnLane(), | |
| vehicle->getRightSideOnLane(), vehicle->getRightSideOnLane() + vehicle->getVehicleType().getWidth(), | |
| ceil(vehicle->getSpeed() / vehicle->getCarFollowModel().getMaxDecel())); | |
| if (nextLeader.first != 0) { | |
| const double brakeGap = vehicle->getCarFollowModel().brakeGap(vehicle->getSpeed()); | |
| // returned gap value is relative to backPosition | |
| const double gap = nextLeader.second - vehicle->getVehicleType().getLengthWithGap(); | |
| #ifdef DEBUG_CHECK_CHANGE | |
| if (DEBUG_COND) { | |
| std::cout << SIMTIME << " pedestrian on road " + leader.first->getID() << " gap=" << gap << " brakeGap=" << brakeGap << "\n"; | |
| } | |
| #endif | |
| if (brakeGap > gap) { | |
| blocked |= blockedByLeader; | |
| #ifdef DEBUG_CHECK_CHANGE | |
| if (DEBUG_COND) { | |
| std::cout << SIMTIME << " blocked by pedestrian " + leader.first->getID() << "\n"; | |
| } | |
| #endif | |
| } | |
| } | |
| } | |
| if (leader.first != nullptr) { | |
| secureOrigFrontGap = vehicle->getCarFollowModel().getSecureGap(vehicle, leader.first, vehicle->getSpeed(), leader.first->getSpeed(), leader.first->getCarFollowModel().getMaxDecel()); | |
| } | |
| MSAbstractLaneChangeModel::MSLCMessager msg(leader.first, neighLead.first, neighFollow.first); | |
| int state = blocked | vehicle->getLaneChangeModel().wantsChange( | |
| laneOffset, msg, blocked, leader, neighLead, neighFollow, *targetLane, preb, &(myCandi->lastBlocked), &(myCandi->firstBlocked)); | |
| if (blocked == 0 && (state & LCA_WANTS_LANECHANGE) != 0 && neighLead.first != nullptr) { | |
| // do a more careful (but expensive) check to ensure that a | |
| // safety-critical leader is not being overlooked | |
| // while changing on an intersection, it is not sufficient to abort the | |
| // search with a leader on the current lane because all linkLeaders must | |
| // be considered as well | |
| const double seen = myCandi->lane->getLength() - vehicle->getPositionOnLane(); | |
| const double speed = vehicle->getSpeed(); | |
| const double dist = vehicle->getCarFollowModel().brakeGap(speed) + vehicle->getVehicleType().getMinGap(); | |
| if (seen < dist || myCandi->lane->isInternal()) { | |
| std::pair<MSVehicle* const, double> neighLead2 = targetLane->getCriticalLeader(dist, seen, speed, *vehicle); | |
| if (neighLead2.first != nullptr && neighLead2.first != neighLead.first) { | |
| const double secureGap = vehicle->getCarFollowModel().getSecureGap(vehicle, neighLead2.first, vehicle->getSpeed(), | |
| neighLead2.first->getSpeed(), neighLead2.first->getCarFollowModel().getMaxDecel()); | |
| const double secureGap2 = secureGap * vehicle->getLaneChangeModel().getSafetyFactor(); | |
| #ifdef DEBUG_SURROUNDING_VEHICLES | |
| if (DEBUG_COND) { | |
| std::cout << SIMTIME << " found critical leader=" << neighLead2.first->getID() | |
| << " gap=" << neighLead2.second << " secGap=" << secureGap << " secGap2=" << secureGap2 << "\n"; | |
| } | |
| #endif | |
| if (neighLead2.second < secureGap2) { | |
| state |= blockedByLeader; | |
| } | |
| } | |
| } | |
| } | |
| if (blocked == 0 && (state & LCA_WANTS_LANECHANGE)) { | |
| // ensure that merging is safe for any upcoming zipper links after changing | |
| if (vehicle->unsafeLinkAhead(targetLane)) { | |
| state |= blockedByLeader; | |
| } | |
| } | |
| if ((state & LCA_BLOCKED) == 0 && (state & LCA_WANTS_LANECHANGE) != 0 && MSGlobals::gLaneChangeDuration > DELTA_T) { | |
| // Ensure that a continuous lane change manoeuvre can be completed before the next turning movement. | |
| // Assume lateral position == 0. (If this should change in the future add + laneOffset*vehicle->getLateralPositionOnLane() to distToNeighLane) | |
| const double distToNeighLane = 0.5 * (vehicle->getLane()->getWidth() + targetLane->getWidth()); | |
| // Extrapolate the LC duration if operating with speed dependent lateral speed. | |
| const MSAbstractLaneChangeModel& lcm = vehicle->getLaneChangeModel(); | |
| const double assumedDecel = lcm.getAssumedDecelForLaneChangeDuration(); | |
| const double estimatedLCDuration = lcm.estimateLCDuration(vehicle->getSpeed(), distToNeighLane, assumedDecel); | |
| if (estimatedLCDuration == -1) { | |
| // Can't guarantee that LC will succeed if vehicle is braking -> assert(lcm.myMaxSpeedLatStanding==0) | |
| #ifdef DEBUG_CHECK_CHANGE | |
| if (DEBUG_COND) { | |
| std::cout << SIMTIME << " checkChange() too slow to guarantee completion of continuous lane change." | |
| << "\nestimatedLCDuration=" << estimatedLCDuration | |
| << "\ndistToNeighLane=" << distToNeighLane | |
| << std::endl; | |
| } | |
| #endif | |
| state |= LCA_INSUFFICIENT_SPEED; | |
| } else { | |
| // Compute covered distance, when braking for the whole lc duration | |
| const double decel = vehicle->getCarFollowModel().getMaxDecel() * estimatedLCDuration; | |
| const double avgSpeed = 0.5 * ( | |
| MAX2(0., vehicle->getSpeed() - ACCEL2SPEED(vehicle->getCarFollowModel().getMaxDecel())) + | |
| MAX2(0., vehicle->getSpeed() - decel)); | |
| // Distance required for lane change. | |
| const double space2change = avgSpeed * estimatedLCDuration; | |
| // Available distance for LC maneuver (distance till next turn) | |
| double seen = myCandi->lane->getLength() - vehicle->getPositionOnLane(); | |
| #ifdef DEBUG_CHECK_CHANGE | |
| if (DEBUG_COND) { | |
| std::cout << SIMTIME << " checkChange() checking continuous lane change..." | |
| << "\ndistToNeighLane=" << distToNeighLane | |
| << " estimatedLCDuration=" << estimatedLCDuration | |
| << " space2change=" << space2change | |
| << " avgSpeed=" << avgSpeed | |
| << std::endl; | |
| } | |
| #endif | |
| // for finding turns it doesn't matter whether we look along the current lane or the target lane | |
| const std::vector<MSLane*>& bestLaneConts = vehicle->getBestLanesContinuation(); | |
| int view = 1; | |
| const MSLane* nextLane = vehicle->getLane(); | |
| std::vector<MSLink*>::const_iterator link = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts); | |
| while (!nextLane->isLinkEnd(link) && seen <= space2change) { | |
| if ((*link)->getDirection() == LinkDirection::LEFT || (*link)->getDirection() == LinkDirection::RIGHT | |
| // the lanes after an internal junction are on different | |
| // edges and do not allow lane-changing | |
| || (nextLane->getEdge().isInternal() && (*link)->getViaLaneOrLane()->getEdge().isInternal()) | |
| ) { | |
| state |= LCA_INSUFFICIENT_SPACE; | |
| break; | |
| } | |
| if ((*link)->getViaLane() == nullptr) { | |
| view++; | |
| } | |
| nextLane = (*link)->getViaLaneOrLane(); | |
| seen += nextLane->getLength(); | |
| // get the next link used | |
| link = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts); | |
| } | |
| #ifdef DEBUG_CHECK_CHANGE | |
| if (DEBUG_COND) { | |
| std::cout << " available distance=" << seen << std::endl; | |
| } | |
| #endif | |
| if (nextLane->isLinkEnd(link) && seen < space2change) { | |
| #ifdef DEBUG_CHECK_CHANGE | |
| if (DEBUG_COND) { | |
| std::cout << SIMTIME << " checkChange insufficientSpace: seen=" << seen << " space2change=" << space2change << "\n"; | |
| } | |
| #endif | |
| state |= LCA_INSUFFICIENT_SPACE; | |
| } | |
| if ((state & LCA_BLOCKED) == 0) { | |
| // check for dangerous leaders in case the target lane changes laterally between | |
| // now and the lane-changing midpoint | |
| const double speed = vehicle->getSpeed(); | |
| seen = myCandi->lane->getLength() - vehicle->getPositionOnLane(); | |
| nextLane = vehicle->getLane(); | |
| view = 1; | |
| const double dist = vehicle->getCarFollowModel().brakeGap(speed) + vehicle->getVehicleType().getMinGap(); | |
| std::vector<MSLink*>::const_iterator nextLink = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts); | |
| while (!nextLane->isLinkEnd(nextLink) && seen <= space2change && seen <= dist) { | |
| nextLane = (*nextLink)->getViaLaneOrLane(); | |
| const MSLane* const parallelLane = nextLane->getParallelLane(laneOffset); | |
| if (parallelLane == nullptr) { | |
| state |= LCA_INSUFFICIENT_SPACE; | |
| break; | |
| } else { | |
| std::pair<MSVehicle* const, double> neighLead2 = parallelLane->getLeader(vehicle, -seen, std::vector<MSLane*>()); | |
| if (neighLead2.first != nullptr && neighLead2.first != neighLead.first | |
| && (neighLead2.second < vehicle->getCarFollowModel().getSecureGap(vehicle, neighLead2.first, | |
| vehicle->getSpeed(), neighLead2.first->getSpeed(), neighLead2.first->getCarFollowModel().getMaxDecel()))) { | |
| state |= blockedByLeader; | |
| break; | |
| } | |
| } | |
| if ((*nextLink)->getViaLane() == nullptr) { | |
| view++; | |
| } | |
| seen += nextLane->getLength(); | |
| // get the next link used | |
| nextLink = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts); | |
| } | |
| } | |
| } | |
| } | |
| const int oldstate = state; | |
| // let TraCI influence the wish to change lanes and the security to take | |
| state = vehicle->influenceChangeDecision(state); | |
| #ifdef DEBUG_CHECK_CHANGE | |
| if (DEBUG_COND) { | |
| std::cout << SIMTIME | |
| << " veh=" << vehicle->getID() | |
| << " oldState=" << toString((LaneChangeAction)oldstate) | |
| << " newState=" << toString((LaneChangeAction)state) | |
| << ((blocked & LCA_BLOCKED) ? " (blocked)" : "") | |
| << ((blocked & LCA_OVERLAPPING) ? " (overlap)" : "") | |
| << "\n"; | |
| } | |
| #endif | |
| vehicle->getLaneChangeModel().saveLCState(laneOffset, oldstate, state); | |
| if (blocked == 0 && (state & LCA_WANTS_LANECHANGE)) { | |
| // this lane change will be executed, save gaps | |
| vehicle->getLaneChangeModel().setFollowerGaps(neighFollow, secureBackGap); | |
| vehicle->getLaneChangeModel().setLeaderGaps(neighLead, secureFrontGap); | |
| vehicle->getLaneChangeModel().setOrigLeaderGaps(leader, secureOrigFrontGap); | |
| } | |
| if (laneOffset != 0) { | |
| vehicle->getLaneChangeModel().saveNeighbors(laneOffset, neighFollow, neighLead); | |
| } | |
| return state; | |
| } | |
| bool | |
| MSLaneChanger::changeOpposite(MSVehicle* vehicle, std::pair<MSVehicle*, double> leader) { | |
| // Evaluate lane-changing between opposite direction lanes | |
| if (!myChangeToOpposite) { | |
| return false; | |
| } | |
| MSLane* source = vehicle->getMutableLane(); | |
| #ifdef DEBUG_CHANGE_OPPOSITE | |
| gDebugFlag5 = DEBUG_COND; | |
| if (DEBUG_COND) { | |
| std::cout << SIMTIME << " veh=" << vehicle->getID() << " considerChangeOpposite source=" << source->getID() << " opposite=" << Named::getIDSecure(source->getOpposite()) << " lead=" << Named::getIDSecure(leader.first) << "\n"; | |
| } | |
| #endif | |
| if (vehicle->isStopped()) { | |
| // stopped vehicles obviously should not change lanes. Usually this is | |
| // prevent by appropriate bestLane distances | |
| return false; | |
| } | |
| const bool isOpposite = vehicle->getLaneChangeModel().isOpposite(); | |
| int ret = 0; | |
| ret = vehicle->influenceChangeDecision(ret); | |
| bool oppositeChangeByTraci = false; | |
| // Check whether a lane change to the opposite direction was requested via TraCI | |
| if ((ret & (LCA_TRACI)) != 0) { | |
| if (isOpposite && (ret & LCA_LEFT) != 0) { | |
| // stay on the opposite side | |
| return false; | |
| } | |
| oppositeChangeByTraci = true; | |
| } | |
| if (!isOpposite && !oppositeChangeByTraci && !source->allowsChangingLeft(vehicle->getVClass())) { | |
| // lane changing explicitly forbidden from this lane | |
| #ifdef DEBUG_CHANGE_OPPOSITE | |
| if (DEBUG_COND) { | |
| std::cout << " not overtaking due to changeLeft restriction\n"; | |
| } | |
| #endif | |
| return false; | |
| } | |
| if (!isOpposite && leader.first == 0 && !oppositeChangeByTraci) { | |
| // no reason to change unless there is a leader | |
| // or we are changing back to the propper direction | |
| // XXX also check whether the leader is so far away as to be irrelevant | |
| return false; | |
| } | |
| if (!isOpposite && !oppositeChangeByTraci | |
| && vehicle->getVClass() != SVC_EMERGENCY | |
| && leader.first != 0) { | |
| if (leader.first->signalSet(MSGlobals::gLefthand | |
| ? MSVehicle::VEH_SIGNAL_BLINKER_RIGHT : MSVehicle::VEH_SIGNAL_BLINKER_LEFT)) { | |
| // do not try to overtake a vehicle that is about to turn left or wants | |
| // to change left itself | |
| #ifdef DEBUG_CHANGE_OPPOSITE | |
| if (DEBUG_COND) { | |
| std::cout << " not overtaking leader " << leader.first->getID() << " that has blinker set\n"; | |
| } | |
| #endif | |
| return false; | |
| } else if (leader.second < 0) { | |
| // leaders is either a junction leader (that cannot be overtaken) or something else is wrong | |
| #ifdef DEBUG_CHANGE_OPPOSITE | |
| if (DEBUG_COND) { | |
| std::cout << " not overtaking leader " << leader.first->getID() << " with gap " << leader.second << "\n"; | |
| } | |
| #endif | |
| return false; | |
| } | |
| } | |
| MSLane* opposite = source->getOpposite(); | |
| //There is no lane for opposite driving | |
| if (opposite == nullptr || !opposite->allowsVehicleClass(vehicle->getVClass())) { | |
| return false; | |
| } | |
| const MSLane* oncomingLane = isOpposite ? source : opposite; | |
| // changing into the opposite direction is always to the left (XXX except for left-hand networkds) | |
| int direction = isOpposite ? -1 : 1; | |
| std::pair<MSVehicle*, double> neighLead((MSVehicle*)nullptr, -1); | |
| // checks for overtaking space | |
| double timeToOvertake = std::numeric_limits<double>::max(); | |
| double spaceToOvertake = std::numeric_limits<double>::max(); | |
| // distance that can safely be driven on the opposite side | |
| double surplusGap = std::numeric_limits<double>::max(); | |
| // we need to find two vehicles: | |
| // 1) the leader that shall be overtaken (not necessarily the current leader but one of its leaders that has enough space in front) | |
| // 2) the oncoming vehicle (we need to look past vehicles that are currently overtaking through the opposite direction themselves) | |
| // | |
| // if the vehicle is driving normally, then the search for 1) starts on the current lane and 2) on the opposite lane | |
| // if the vehicle is driving on the opposite side then 1) is found on the neighboring lane and 2) on the current lane | |
| std::pair<MSVehicle*, double> overtaken; | |
| std::pair<MSVehicle*, double> oncoming; | |
| // the maximum speed while overtaking (may be lowered if slow vehicles are | |
| // currently overtaking ahead of vehicle) | |
| double vMax = vehicle->getLane()->getVehicleMaxSpeed(vehicle); | |
| double oncomingSpeed = oncomingLane->getSpeedLimit(); | |
| if (oppositeChangeByTraci) { | |
| timeToOvertake = STEPS2TIME(vehicle->getInfluencer().getLaneTimeLineDuration());//todo discuss concept | |
| spaceToOvertake = timeToOvertake * vehicle->getLane()->getVehicleMaxSpeed(vehicle); | |
| } else { | |
| if (isOpposite) { | |
| // -1 will use getMaximumBrakeDist() as look-ahead distance | |
| neighLead = opposite->getOppositeLeader(vehicle, -1, false); | |
| // make sure that overlapping vehicles on the neighboring lane are found by starting search at the back position | |
| overtaken = opposite->getLeader(vehicle, opposite->getOppositePos(vehicle->getBackPositionOnLane()), vehicle->getBestLanesContinuation(opposite)); | |
| overtaken.second -= vehicle->getVehicleType().getLength(); | |
| if (overtaken.first == nullptr && neighLead.first != nullptr) { | |
| overtaken = neighLead; | |
| } | |
| if (overtaken.first != nullptr) { | |
| overtaken = getColumnleader(vehicle, overtaken); | |
| } | |
| #ifdef DEBUG_CHANGE_OPPOSITE | |
| if (DEBUG_COND) { | |
| std::cout << " leaderOnSource=" << Named::getIDSecure(oncoming.first) << " gap=" << oncoming.second << "\n"; | |
| std::cout << " leaderOnTarget=" << Named::getIDSecure(neighLead.first) << " gap=" << neighLead.second << "\n"; | |
| std::cout << " overtaken=" << Named::getIDSecure(overtaken.first) << " gap=" << overtaken.second << "\n"; | |
| } | |
| #endif | |
| } else { | |
| overtaken = getColumnleader(vehicle, leader); | |
| } | |
| if (overtaken.first == 0) { | |
| if (!isOpposite) { | |
| // no reason to change to the opposite side | |
| return false; | |
| } | |
| } else { | |
| #ifdef DEBUG_CHANGE_OPPOSITE | |
| if (DEBUG_COND) { | |
| std::cout << " compute time/space to overtake for columnLeader=" << overtaken.first->getID() << " egoGap=" << overtaken.second << "\n"; | |
| } | |
| #endif | |
| // there might be leader vehicles on the opposite side that also drive | |
| // against the flow which are slower than ego (must be factored into | |
| // overtaking time) | |
| computeOvertakingTime(vehicle, vMax, overtaken.first, overtaken.second, timeToOvertake, spaceToOvertake); | |
| #ifdef DEBUG_CHANGE_OPPOSITE | |
| if (DEBUG_COND) { | |
| std::cout << SIMTIME | |
| << " veh=" << vehicle->getID() | |
| << " changeOpposite opposite=" << opposite->getID() | |
| << " lead=" << Named::getIDSecure(leader.first) | |
| << " timeToOvertake=" << timeToOvertake | |
| << " spaceToOvertake=" << spaceToOvertake | |
| << "\n"; | |
| } | |
| #endif | |
| if (!isOpposite && spaceToOvertake > OPPOSITE_OVERTAKING_MAX_SPACE_TO_OVERTAKE) { | |
| #ifdef DEBUG_CHANGE_OPPOSITE | |
| if (DEBUG_COND) { | |
| std::cout << " cannot changeOpposite (cannot overtake fast leader " << Named::getIDSecure(overtaken.first) << " v=" << overtaken.first->getSpeed() << ")\n"; | |
| } | |
| #endif | |
| return false; | |
| } | |
| } | |
| if (!isOpposite) { | |
| assert(timeToOvertake != std::numeric_limits<double>::max()); | |
| assert(spaceToOvertake != std::numeric_limits<double>::max()); | |
| // we keep neighLead disticnt from oncoiming because it determines blocking on the neigh lane | |
| // but also look for an oncoming leader to compute safety constraint | |
| double searchDist = timeToOvertake * oncomingLane->getSpeedLimit() * 2 + spaceToOvertake; | |
| neighLead = oncomingLane->getOppositeLeader(vehicle, searchDist, true); | |
| oncoming = getOncomingVehicle(oncomingLane, neighLead, searchDist, vMax); | |
| } else { | |
| double searchDist = OPPOSITE_OVERTAKING_ONCOMING_LOOKAHEAD; | |
| oncoming = oncomingLane->getOppositeLeader(vehicle, searchDist, true); | |
| oncoming = getOncomingVehicle(oncomingLane, oncoming, searchDist, vMax); | |
| } | |
| if (overtaken.first != nullptr && vMax != vehicle->getLane()->getVehicleMaxSpeed(vehicle)) { | |
| // recompute overtaking time with slow opposite leader | |
| computeOvertakingTime(vehicle, vMax, overtaken.first, overtaken.second, timeToOvertake, spaceToOvertake); | |
| #ifdef DEBUG_CHANGE_OPPOSITE | |
| if (DEBUG_COND) { | |
| std::cout << " recomputed overtaking time with vMax=" << vMax | |
| << " timeToOvertake=" << timeToOvertake | |
| << " spaceToOvertake=" << spaceToOvertake | |
| << "\n"; | |
| } | |
| #endif | |
| } | |
| if (!isOpposite) { | |
| if (spaceToOvertake > OPPOSITE_OVERTAKING_MAX_SPACE_TO_OVERTAKE) { | |
| #ifdef DEBUG_CHANGE_OPPOSITE | |
| if (DEBUG_COND) { | |
| std::cout << " cannot changeOpposite (cannot overtake fast leader " << Named::getIDSecure(overtaken.first) << " v=" << overtaken.first->getSpeed() << ")\n"; | |
| } | |
| #endif | |
| return false; | |
| } | |
| // check for upcoming stops | |
| if (vehicle->nextStopDist() < spaceToOvertake) { | |
| #ifdef DEBUG_CHANGE_OPPOSITE | |
| if (DEBUG_COND) { | |
| std::cout << " cannot changeOpposite due to upcoming stop (dist=" << vehicle->nextStopDist() << " spaceToOvertake=" << spaceToOvertake << ")\n"; | |
| } | |
| #endif | |
| return false; | |
| } | |
| assert(timeToOvertake != std::numeric_limits<double>::max()); | |
| assert(spaceToOvertake != std::numeric_limits<double>::max()); | |
| } | |
| MSVehicle* oncomingVeh = oncoming.first; | |
| // check for dangerous oncoming leader | |
| if (oncomingVeh != 0 && !oncomingVeh->getLaneChangeModel().isOpposite() && oncomingVeh->getLaneChangeModel().getShadowLane() != opposite) { | |
| // conservative: assume that the oncoming vehicle accelerates to its maximum speed | |
| oncomingSpeed = oncomingVeh->isStopped() ? 0 : oncomingVeh->getLane()->getVehicleMaxSpeed(oncomingVeh); | |
| const double safetyGap = ((oncomingSpeed + vehicle->getLane()->getVehicleMaxSpeed(vehicle)) | |
| * vehicle->getCarFollowModel().getHeadwayTime() | |
| * OPPOSITE_OVERTAKING_SAFETYGAP_HEADWAY_FACTOR); | |
| surplusGap = oncoming.second - spaceToOvertake - timeToOvertake * oncomingSpeed - safetyGap; | |
| #ifdef DEBUG_CHANGE_OPPOSITE | |
| if (DEBUG_COND) { | |
| std::cout << SIMTIME | |
| << " oncoming=" << oncomingVeh->getID() | |
| << " oncomingGap=" << oncoming.second | |
| << " leaderGap=" << leader.second | |
| << " safetyGap=" << safetyGap | |
| << " surplusGap=" << surplusGap | |
| << "\n"; | |
| } | |
| #endif | |
| if (!isOpposite && surplusGap < 0) { | |
| #ifdef DEBUG_CHANGE_OPPOSITE | |
| if (DEBUG_COND) { | |
| std::cout << " cannot changeOpposite due to dangerous oncoming (surplusGap=" << surplusGap << ")\n"; | |
| } | |
| #endif | |
| #ifdef DEBUG_CHANGE_OPPOSITE | |
| if (DEBUG_COND) { | |
| if (oncomingVeh->getLaneChangeModel().isOpposite()) { | |
| std::cout << SIMTIME << " ego=" << vehicle->getID() << " does not changeOpposite due to dangerous oncoming " << oncomingVeh->getID() << " (but the leader is also opposite)\n"; | |
| } | |
| } | |
| #endif | |
| return false; | |
| } | |
| } | |
| } | |
| // compute remaining space on the opposite side | |
| // 1. the part that remains on the current lane | |
| double usableDist = isOpposite ? vehicle->getPositionOnLane() : source->getLength() - vehicle->getPositionOnLane(); | |
| if (usableDist < spaceToOvertake) { | |
| // look forward along the next lanes | |
| const std::vector<MSLane*>& bestLaneConts = vehicle->getBestLanesContinuation(); | |
| assert(bestLaneConts.size() >= 1); | |
| std::vector<MSLane*>::const_iterator it = bestLaneConts.begin() + 1; | |
| while (usableDist < spaceToOvertake && it != bestLaneConts.end()) { | |
| #ifdef DEBUG_CHANGE_OPPOSITE | |
| if (DEBUG_COND) { | |
| std::cout << " usableDist=" << usableDist << " opposite=" << Named::getIDSecure((*it)->getOpposite()) << "\n"; | |
| } | |
| #endif | |
| if ((*it)->getOpposite() == nullptr || !(*it)->getOpposite()->allowsVehicleClass(vehicle->getVClass())) { | |
| // opposite lane ends | |
| break; | |
| } | |
| // do not overtake past a minor link or turn | |
| const MSLane* const prev = *(it - 1); | |
| if (prev != nullptr) { | |
| const MSLink* link = prev->getLinkTo(*it); | |
| if (link == nullptr || link->getState() == LINKSTATE_ZIPPER | |
| || (link->getDirection() != LinkDirection::STRAIGHT && vehicle->getVehicleType().getVehicleClass() != SVC_EMERGENCY) | |
| || (!link->havePriority() | |
| // consider traci-influence | |
| && (!vehicle->hasInfluencer() || vehicle->getInfluencer().getRespectJunctionPriority()) | |
| // consider junction model parameters | |
| && ((!link->haveRed() && !link->haveYellow()) || !vehicle->ignoreRed(link, true)))) { | |
| #ifdef DEBUG_CHANGE_OPPOSITE | |
| if (DEBUG_COND) { | |
| std::cout << " stop lookahead at link=" << (link == 0 ? "NULL" : link->getViaLaneOrLane()->getID()) << " state=" << (link == 0 ? "?" : toString(link->getState())) << " ignoreRed=" << vehicle->ignoreRed(link, true) << "\n"; | |
| } | |
| #endif | |
| break; | |
| } | |
| } | |
| usableDist += (*it)->getLength(); | |
| ++it; | |
| } | |
| } | |
| if (!isOpposite && usableDist < spaceToOvertake) { | |
| #ifdef DEBUG_CHANGE_OPPOSITE | |
| if (DEBUG_COND) { | |
| std::cout << " cannot changeOpposite due to insufficient space (seen=" << usableDist << " spaceToOvertake=" << spaceToOvertake << ")\n"; | |
| } | |
| #endif | |
| return false; | |
| } | |
| #ifdef DEBUG_CHANGE_OPPOSITE | |
| if (DEBUG_COND) { | |
| std::cout << " usableDist=" << usableDist << " spaceToOvertake=" << spaceToOvertake << " timeToOvertake=" << timeToOvertake << "\n"; | |
| } | |
| #endif | |
| // compute wish to change | |
| std::vector<MSVehicle::LaneQ> preb = vehicle->getBestLanes(); | |
| if (isOpposite) { | |
| // compute the remaining distance that can be driven on the opposite side | |
| // this value will put into LaneQ.length of the leftmost lane | |
| // @note: length counts from the start of the current lane | |
| // @note: see MSLCM_LC2013::_wantsChange @1092 (isOpposite() | |
| MSVehicle::LaneQ& laneQ = preb[preb.size() - 1]; | |
| // position on the target lane | |
| const double forwardPos = source->getOppositePos(vehicle->getPositionOnLane()); | |
| // consider usableDist (due to minor links or end of opposite lanes) | |
| laneQ.length = MIN2(laneQ.length, usableDist + forwardPos); | |
| // consider upcoming stops | |
| laneQ.length = MIN2(laneQ.length, vehicle->nextStopDist() + forwardPos); | |
| #ifdef DEBUG_CHANGE_OPPOSITE | |
| if (DEBUG_COND) { | |
| std::cout << " laneQLength=" << laneQ.length << " usableDist=" << usableDist << " forwardPos=" << forwardPos << " stopDist=" << vehicle->nextStopDist() << "\n"; | |
| } | |
| #endif | |
| // consider oncoming leaders | |
| const MSVehicle* oncomingVeh = oncoming.first; | |
| if (oncomingVeh != 0) { | |
| if (!oncomingVeh->getLaneChangeModel().isOpposite() && oncomingVeh->getLaneChangeModel().getShadowLane() != source) { | |
| const double egoSpeedFraction = MIN2(0.5, vMax / (vMax + oncomingSpeed)); | |
| laneQ.length = MIN2(laneQ.length, forwardPos + oncoming.second * egoSpeedFraction); | |
| #ifdef DEBUG_CHANGE_OPPOSITE | |
| if (DEBUG_COND) { | |
| std::cout << SIMTIME << " found oncoming leader=" << oncomingVeh->getID() << " gap=" << oncoming.second | |
| << " egoSpeedFraction=" << egoSpeedFraction << " newDist=" << laneQ.length << "\n"; | |
| } | |
| #endif | |
| } else { | |
| #ifdef DEBUG_CHANGE_OPPOSITE | |
| if (DEBUG_COND) { | |
| std::cout << SIMTIME << " opposite leader=" << oncomingVeh->getID() << " gap=" << oncoming.second << " is driving against the flow\n"; | |
| } | |
| #endif | |
| } | |
| if (neighLead.first != 0) { | |
| if (overtaken.first == 0) { | |
| #ifdef DEBUG_CHANGE_OPPOSITE | |
| if (DEBUG_COND) { | |
| std::cout << SIMTIME << " ego=" << vehicle->getID() << " did not find columnleader to overtake\n"; | |
| } | |
| #endif | |
| } else { | |
| if (surplusGap > 0) { | |
| // exaggerate remaining dist so that the vehicle continues | |
| // overtaking (otherwise the lane change model might abort prematurely) | |
| laneQ.length += 1000; | |
| } else { | |
| // return from the opposite ahead of the unpassable column leader (unless overlapping) | |
| if (overtaken.second > 0) { | |
| laneQ.length = MIN2(laneQ.length, forwardPos + overtaken.second); | |
| } | |
| // (don't set the distance so low as to imply emergency braking) | |
| laneQ.length = MAX2(laneQ.length, forwardPos + vehicle->getCarFollowModel().brakeGap(vehicle->getSpeed())); | |
| } | |
| #ifdef DEBUG_CHANGE_OPPOSITE | |
| if (DEBUG_COND) { | |
| std::cout << SIMTIME << " ego=" << vehicle->getID() << " is overtaking " << overtaken.first->getID() | |
| << " surplusGap=" << surplusGap | |
| << " spaceToOvertake=" << spaceToOvertake | |
| << " timeToOvertake=" << timeToOvertake | |
| << " timeToOvertake=" << timeToOvertake | |
| << " final laneQLength=" << laneQ.length | |
| << "\n"; | |
| } | |
| #endif | |
| } | |
| } | |
| } | |
| leader.first = 0; // ignore leader after this | |
| #ifdef DEBUG_CHANGE_OPPOSITE | |
| if (DEBUG_COND) { | |
| std::cout << SIMTIME << " veh=" << vehicle->getID() << " remaining dist=" << laneQ.length - forwardPos << " forwardPos=" << forwardPos << " laneQ.length=" << laneQ.length << "\n"; | |
| } | |
| #endif | |
| } | |
| std::pair<MSVehicle* const, double> neighFollow = opposite->getOppositeFollower(vehicle); | |
| return checkChangeOpposite(vehicle, direction, opposite, leader, neighLead, neighFollow, preb); | |
| } | |
| std::pair<MSVehicle* const, double> | |
| MSLaneChanger::getOncomingVehicle(const MSLane* opposite, std::pair<MSVehicle*, double> oncoming, double searchDist, double& vMax) { | |
| double gap = oncoming.second; | |
| while (oncoming.first != nullptr && (oncoming.first->getLaneChangeModel().isOpposite() || oncoming.first->getLaneChangeModel().getShadowLane() == opposite)) { | |
| searchDist -= (oncoming.first->getVehicleType().getLengthWithGap() + MAX2(0.0, oncoming.second)); | |
| // leader is itself overtaking through the opposite side. find real oncoming vehicle | |
| gap += oncoming.first->getVehicleType().getLengthWithGap(); | |
| vMax = MIN2(vMax, oncoming.first->getSpeed()); | |
| #ifdef DEBUG_CHANGE_OPPOSITE | |
| if (gDebugFlag5) { | |
| std::cout << SIMTIME << " oncoming=" << oncoming.first->getID() << " isOpposite gap=" << oncoming.second | |
| << " totalGap=" << gap << " searchDist=" << searchDist << " vMax=" << vMax << "\n"; | |
| } | |
| #endif | |
| if (searchDist < 0) { | |
| break; | |
| } | |
| // getOppositeLeader resets the search postion by ego length and may thus create cycles | |
| if (oncoming.first->getLaneChangeModel().getShadowLane() != opposite) { | |
| opposite = oncoming.first->getLane(); | |
| } | |
| oncoming = opposite->getFollower(oncoming.first, oncoming.first->getPositionOnLane(opposite), searchDist, true); | |
| if (oncoming.first != nullptr) { | |
| gap += oncoming.second; | |
| } | |
| } | |
| oncoming.second = gap; | |
| return oncoming; | |
| } | |
| bool | |
| MSLaneChanger::checkChangeOpposite( | |
| MSVehicle* vehicle, | |
| int laneOffset, | |
| MSLane* targetLane, | |
| const std::pair<MSVehicle* const, double>& leader, | |
| const std::pair<MSVehicle* const, double>& neighLead, | |
| const std::pair<MSVehicle* const, double>& neighFollow, | |
| const std::vector<MSVehicle::LaneQ>& preb) { | |
| const bool isOpposite = vehicle->getLaneChangeModel().isOpposite(); | |
| MSLane* source = vehicle->getMutableLane(); | |
| int state = checkChange(laneOffset, targetLane, leader, neighLead, neighFollow, preb); | |
| vehicle->getLaneChangeModel().setOwnState(state); | |
| bool changingAllowed = (state & LCA_BLOCKED) == 0; | |
| // change if the vehicle wants to and is allowed to change | |
| if ((state & LCA_WANTS_LANECHANGE) != 0 && changingAllowed | |
| // do not change to the opposite direction for cooperative reasons | |
| && (isOpposite || (state & LCA_COOPERATIVE) == 0)) { | |
| const bool continuous = vehicle->getLaneChangeModel().startLaneChangeManeuver(source, targetLane, laneOffset); | |
| #ifdef DEBUG_CHANGE_OPPOSITE | |
| if (DEBUG_COND) { | |
| std::cout << SIMTIME << " changing to opposite veh=" << vehicle->getID() << " dir=" << laneOffset << " opposite=" << Named::getIDSecure(targetLane) << " state=" << state << "\n"; | |
| } | |
| #endif | |
| if (continuous) { | |
| continueChange(vehicle, myCandi); | |
| } | |
| return true; | |
| } | |
| #ifdef DEBUG_CHANGE_OPPOSITE | |
| if (DEBUG_COND) { | |
| std::cout << SIMTIME << " not changing to opposite veh=" << vehicle->getID() << " dir=" << laneOffset | |
| << " opposite=" << Named::getIDSecure(targetLane) << " state=" << toString((LaneChangeAction)state) << "\n"; | |
| } | |
| #endif | |
| return false; | |
| } | |
| void | |
| MSLaneChanger::computeOvertakingTime(const MSVehicle* vehicle, double vMax, const MSVehicle* leader, double gap, double& timeToOvertake, double& spaceToOvertake) { | |
| // Assumptions: | |
| // - leader maintains the current speed | |
| // - vehicle merges with maxSpeed ahead of leader | |
| // XXX affected by ticket #860 (the formula is invalid for the current position update rule) | |
| // first compute these values for the case where vehicle is accelerating | |
| // without upper bound on speed | |
| const double v = vehicle->getSpeed(); | |
| const double u = leader->getAcceleration() > 0 ? leader->getLane()->getVehicleMaxSpeed(leader) : leader->getSpeed(); | |
| const double a = vehicle->getCarFollowModel().getMaxAccel(); | |
| const double d = vehicle->getCarFollowModel().getMaxDecel(); | |
| const double g = MAX2(0.0, ( | |
| // drive up to the rear of leader | |
| gap + vehicle->getVehicleType().getMinGap() | |
| // drive head-to-head with the leader | |
| + leader->getVehicleType().getLengthWithGap() | |
| // drive past the leader | |
| + vehicle->getVehicleType().getLength() | |
| // allow for safe gap between leader and vehicle | |
| + leader->getCarFollowModel().getSecureGap(leader, vehicle, u, vMax, d)) | |
| // time to move between lanes | |
| + (MSGlobals::gSublane ? vMax * vehicle->getLane()->getWidth() / vehicle->getVehicleType().getMaxSpeedLat() : 0)); | |
| const double sign = -1; // XXX recheck | |
| // v*t + t*t*a*0.5 = g + u*t | |
| // solve t | |
| // t = ((u - v - (((((2.0*(u - v))**2.0) + (8.0*a*g))**(1.0/2.0))*sign/2.0))/a) | |
| double t = (u - v - sqrt(4 * (u - v) * (u - v) + 8 * a * g) * sign * 0.5) / a; | |
| #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME | |
| if (DEBUG_COND) { | |
| std::cout << " computeOvertakingTime v=" << v << " vMax=" << vMax << " u=" << u << " a=" << a << " d=" << d << " gap=" << gap << " g=" << g << " t=" << t | |
| << " distEgo=" << v* t + t* t* a * 0.5 << " distLead=" << g + u* t | |
| << "\n"; | |
| } | |
| #endif | |
| assert(t >= 0); | |
| if (vMax <= u) { | |
| // do not try to overtake faster leader | |
| timeToOvertake = std::numeric_limits<double>::max(); | |
| spaceToOvertake = std::numeric_limits<double>::max(); | |
| return; | |
| } | |
| // allow for a safety time gap | |
| t += OPPOSITE_OVERTAKING_SAFE_TIMEGAP; | |
| // round to multiples of step length (TS) | |
| t = ceil(t / TS) * TS; | |
| /// XXX ignore speed limit when overtaking through the opposite lane? | |
| const double timeToMaxSpeed = (vMax - v) / a; | |
| #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME | |
| if (DEBUG_COND) { | |
| std::cout << " t=" << t << " tvMax=" << timeToMaxSpeed << "\n"; | |
| } | |
| #endif | |
| if (t <= timeToMaxSpeed) { | |
| timeToOvertake = t; | |
| spaceToOvertake = v * t + t * t * a * 0.5; | |
| #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME | |
| if (DEBUG_COND) { | |
| std::cout << " sto=" << spaceToOvertake << "\n"; | |
| } | |
| #endif | |
| } else { | |
| // space until max speed is reached | |
| const double s = v * timeToMaxSpeed + timeToMaxSpeed * timeToMaxSpeed * a * 0.5; | |
| const double m = timeToMaxSpeed; | |
| // s + (t-m) * vMax = g + u*t | |
| // solve t | |
| t = (g - s + m * vMax) / (vMax - u); | |
| if (t < 0) { | |
| // cannot overtake in time | |
| #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME | |
| if (DEBUG_COND) { | |
| std::cout << " t2=" << t << "\n"; | |
| } | |
| #endif | |
| timeToOvertake = std::numeric_limits<double>::max(); | |
| spaceToOvertake = std::numeric_limits<double>::max(); | |
| return; | |
| } else { | |
| // allow for a safety time gap | |
| t += OPPOSITE_OVERTAKING_SAFE_TIMEGAP; | |
| // round to multiples of step length (TS) | |
| t = ceil(t / TS) * TS; | |
| timeToOvertake = t; | |
| spaceToOvertake = s + (t - m) * vMax; | |
| #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME | |
| if (DEBUG_COND) { | |
| std::cout << " t2=" << t << " s=" << s << " sto=" << spaceToOvertake << " m=" << m << "\n"; | |
| } | |
| #endif | |
| } | |
| } | |
| const double safetyFactor = OPPOSITE_OVERTAKING_SAFETY_FACTOR * vehicle->getLaneChangeModel().getOppositeSafetyFactor(); | |
| timeToOvertake *= safetyFactor; | |
| spaceToOvertake *= safetyFactor; | |
| #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME | |
| if (DEBUG_COND) { | |
| if (safetyFactor != 1) { | |
| std::cout << " applying safetyFactor=" << safetyFactor | |
| << " tto=" << timeToOvertake << " sto=" << spaceToOvertake << "\n"; | |
| } | |
| } | |
| #endif | |
| } | |
| std::pair<MSVehicle*, double> | |
| MSLaneChanger::getColumnleader(MSVehicle* vehicle, std::pair<MSVehicle*, double> leader, double maxLookAhead) { | |
| assert(leader.first != 0); | |
| const MSLane* source = vehicle->getLane(); | |
| // find a leader vehicle with sufficient space ahead for merging back | |
| const double overtakingSpeed = source->getVehicleMaxSpeed(vehicle); // just a guess | |
| const double mergeBrakeGap = vehicle->getCarFollowModel().brakeGap(overtakingSpeed); | |
| std::pair<MSVehicle*, double> columnLeader = leader; | |
| double egoGap = leader.second; | |
| bool foundSpaceAhead = false; | |
| double seen = leader.second + leader.first->getVehicleType().getLengthWithGap(); | |
| std::vector<MSLane*> conts = vehicle->getBestLanesContinuation(); | |
| if (maxLookAhead == std::numeric_limits<double>::max()) { | |
| maxLookAhead = (vehicle->getVehicleType().getVehicleClass() == SVC_EMERGENCY | |
| ? OPPOSITE_OVERTAKING_MAX_LOOKAHEAD_EMERGENCY | |
| : OPPOSITE_OVERTAKING_MAX_LOOKAHEAD); | |
| } | |
| #ifdef DEBUG_CHANGE_OPPOSITE | |
| if (DEBUG_COND) { | |
| std::cout << " getColumnleader vehicle=" << vehicle->getID() << " leader=" << leader.first->getID() << " gap=" << leader.second << " maxLookAhead=" << maxLookAhead << "\n"; | |
| } | |
| #endif | |
| const double safetyFactor = OPPOSITE_OVERTAKING_SAFETY_FACTOR * vehicle->getLaneChangeModel().getOppositeSafetyFactor(); | |
| while (!foundSpaceAhead) { | |
| const double requiredSpaceAfterLeader = (columnLeader.first->getCarFollowModel().getSecureGap( | |
| columnLeader.first, vehicle, | |
| columnLeader.first->getSpeed(), overtakingSpeed, vehicle->getCarFollowModel().getMaxDecel()) | |
| + columnLeader.first->getVehicleType().getMinGap() | |
| + vehicle->getVehicleType().getLengthWithGap()); | |
| // all leader vehicles on the current laneChanger edge are already moved into MSLane::myTmpVehicles | |
| const bool checkTmpVehicles = (&columnLeader.first->getLane()->getEdge() == &source->getEdge()); | |
| double searchStart = columnLeader.first->getPositionOnLane(); | |
| std::pair<MSVehicle*, double> leadLead = columnLeader.first->getLane()->getLeader( | |
| columnLeader.first, searchStart, conts, requiredSpaceAfterLeader + mergeBrakeGap, | |
| checkTmpVehicles); | |
| std::set<MSVehicle*> seenLeaders; | |
| while (leadLead.first != nullptr && leadLead.first->getLaneChangeModel().isOpposite()) { | |
| #ifdef DEBUG_CHANGE_OPPOSITE | |
| if (DEBUG_COND) { | |
| std::cout << " skipping opposite leadLead=" << leadLead.first->getID() << " gap=" << leadLead.second << "\n"; | |
| } | |
| #endif | |
| if (leadLead.second + seen > maxLookAhead || seenLeaders.count(leadLead.first) > 0) { | |
| leadLead.first = nullptr; | |
| break; | |
| } | |
| seenLeaders.insert(leadLead.first); | |
| // found via shadow lane, skip it | |
| const double searchStart2 = searchStart + MAX2(0.0, leadLead.second) + leadLead.first->getVehicleType().getLengthWithGap(); | |
| leadLead = columnLeader.first->getLane()->getLeader( | |
| columnLeader.first, searchStart2, conts, requiredSpaceAfterLeader + mergeBrakeGap, | |
| checkTmpVehicles); | |
| leadLead.second += (searchStart2 - searchStart); | |
| } | |
| if (leadLead.first == nullptr) { | |
| double availableSpace = columnLeader.first->getLane()->getLength() - columnLeader.first->getPositionOnLane(); | |
| const double requiredSpace = safetyFactor * (requiredSpaceAfterLeader | |
| + vehicle->getCarFollowModel().brakeGap(overtakingSpeed)); | |
| #ifdef DEBUG_CHANGE_OPPOSITE | |
| if (DEBUG_COND) { | |
| std::cout << " no direct leader found after columnLeader " << columnLeader.first->getID() | |
| << " availableSpace=" << availableSpace | |
| << " req1=" << requiredSpaceAfterLeader | |
| << " req2=" << requiredSpace / safetyFactor | |
| << " req3=" << requiredSpace | |
| << "\n"; | |
| } | |
| #endif | |
| if (availableSpace > requiredSpace) { | |
| foundSpaceAhead = true; | |
| } else { | |
| // maybe the columnleader is stopped before a junction or takes a different turn. | |
| // try to find another columnleader on successive lanes | |
| const MSLane* next = getLaneAfter(columnLeader.first->getLane(), conts); | |
| #ifdef DEBUG_CHANGE_OPPOSITE | |
| if (DEBUG_COND) { | |
| std::cout << " look for another leader on lane " << Named::getIDSecure(next) << "\n"; | |
| } | |
| #endif | |
| while (next != nullptr && seen < maxLookAhead) { | |
| seen += next->getLength(); | |
| MSVehicle* cand = next->getLastAnyVehicle(); | |
| if (cand == nullptr) { | |
| availableSpace += next->getLength(); | |
| if (availableSpace > requiredSpace) { | |
| foundSpaceAhead = true; | |
| break; | |
| } | |
| } else { | |
| availableSpace += cand->getBackPositionOnLane(); | |
| if (availableSpace > requiredSpace) { | |
| foundSpaceAhead = true; | |
| break; | |
| } else { | |
| return getColumnleader(vehicle, std::make_pair(cand, availableSpace + cand->getPositionOnLane()), maxLookAhead - seen); | |
| } | |
| } | |
| } | |
| if (!foundSpaceAhead) { | |
| return std::make_pair(nullptr, -1); | |
| } | |
| } | |
| } else { | |
| const double requiredSpace = safetyFactor * (requiredSpaceAfterLeader | |
| + vehicle->getCarFollowModel().getSecureGap(vehicle, leadLead.first, | |
| overtakingSpeed, leadLead.first->getSpeed(), leadLead.first->getCarFollowModel().getMaxDecel())); | |
| #ifdef DEBUG_CHANGE_OPPOSITE | |
| if (DEBUG_COND) { | |
| std::cout << " leader's leader " << leadLead.first->getID() << " space=" << leadLead.second | |
| << " req1=" << requiredSpaceAfterLeader | |
| << " req2=" << requiredSpace / safetyFactor | |
| << " req3=" << requiredSpace | |
| << "\n"; | |
| } | |
| #endif | |
| if (leadLead.second > requiredSpace) { | |
| foundSpaceAhead = true; | |
| } else { | |
| #ifdef DEBUG_CHANGE_OPPOSITE | |
| if (DEBUG_COND) { | |
| std::cout << " not enough space after columnLeader=" << columnLeader.first->getID() << " required=" << requiredSpace << "\n"; | |
| } | |
| #endif | |
| seen += MAX2(0., leadLead.second) + leadLead.first->getVehicleType().getLengthWithGap(); | |
| if (seen > maxLookAhead) { | |
| #ifdef DEBUG_CHANGE_OPPOSITE | |
| if (DEBUG_COND) { | |
| std::cout << " cannot changeOpposite due to insufficient free space after columnLeader (seen=" << seen << " columnLeader=" << columnLeader.first->getID() << ")\n"; | |
| } | |
| #endif | |
| return std::make_pair(nullptr, -1); | |
| } | |
| // see if merging after leadLead is possible | |
| egoGap += columnLeader.first->getVehicleType().getLengthWithGap() + leadLead.second; | |
| columnLeader = leadLead; | |
| #ifdef DEBUG_CHANGE_OPPOSITE | |
| if (DEBUG_COND) { | |
| std::cout << " new columnLeader=" << columnLeader.first->getID() << "\n"; | |
| } | |
| #endif | |
| } | |
| } | |
| } | |
| columnLeader.second = egoGap; | |
| return columnLeader; | |
| } | |
| MSLane* | |
| MSLaneChanger::getLaneAfter(const MSLane* lane, const std::vector<MSLane*>& conts) { | |
| for (auto it = conts.begin(); it != conts.end(); ++it) { | |
| if (*it == lane) { | |
| if (it + 1 != conts.end()) { | |
| return *(it + 1); | |
| } else { | |
| return nullptr; | |
| } | |
| } | |
| } | |
| return nullptr; | |
| } | |
| /****************************************************************************/ |