bool MSLaneChangerSublane::startChangeSublane(MSVehicle* vehicle, ChangerIt& from, SUMOReal latDist) { //gDebugFlag4 = vehicle->getID() == "Togliatti_80_26"; // 1) update vehicles lateral position according to latDist and target lane vehicle->myState.myPosLat += latDist; vehicle->myCachedPosition = Position::INVALID; // 2) distinguish several cases // a) vehicle moves completely within the same lane // b) vehicle intersects another lane // - vehicle must be moved to the lane where it's midpoint is (either old or new) // - shadow vehicle must be created/moved to the other lane if the vehicle intersects it // 3) updated dens of all lanes that hold the vehicle or its shadow const int direction = vehicle->getLateralPositionOnLane() < 0 ? -1 : 1; ChangerIt to = from; if (mayChange(direction)) { to = from + direction; } else { /// XXX assert(false); } const bool changedToNewLane = to != from && fabs(vehicle->getLateralPositionOnLane()) > 0.5 * vehicle->getLane()->getWidth() && mayChange(direction); if (changedToNewLane) { vehicle->myState.myPosLat -= direction * 0.5 * (from->lane->getWidth() + to->lane->getWidth()); to->lane->myTmpVehicles.insert(to->lane->myTmpVehicles.begin(), vehicle); to->dens += vehicle->getVehicleType().getLengthWithGap(); vehicle->getLaneChangeModel().startLaneChangeManeuver(from->lane, to->lane, direction); to->ahead.addLeader(vehicle, false, 0); } else { registerUnchanged(vehicle); from->ahead.addLeader(vehicle, false, 0); } MSLane* oldShadowLane = vehicle->getLaneChangeModel().getShadowLane(); vehicle->getLaneChangeModel().updateShadowLane(); MSLane* shadowLane = vehicle->getLaneChangeModel().getShadowLane(); if (shadowLane != 0 && shadowLane != oldShadowLane) { assert(to != from); const SUMOReal latOffset = vehicle->getLane()->getRightSideOnEdge() - shadowLane->getRightSideOnEdge(); (myChanger.begin() + shadowLane->getIndex())->ahead.addLeader(vehicle, false, latOffset); } if (gDebugFlag4) std::cout << SIMTIME << " startChangeSublane shadowLane" << " old=" << Named::getIDSecure(oldShadowLane) << " new=" << Named::getIDSecure(vehicle->getLaneChangeModel().getShadowLane()) << "\n"; // compute new angle of the vehicle from the x- and y-distances travelled within last time step // (should happen last because primaryLaneChanged() also triggers angle computation) // this part of the angle comes from the orientation of our current lane SUMOReal laneAngle = vehicle->getLane()->getShape().rotationAtOffset(vehicle->getLane()->interpolateLanePosToGeometryPos(vehicle->getPositionOnLane())) ; // this part of the angle comes from the vehicle's lateral movement SUMOReal changeAngle = 0; // avoid flicker if (fabs(latDist) > NUMERICAL_EPS) { // avoid extreme angles by using vehicle length as a proxy for turning radius changeAngle = atan2(latDist, SPEED2DIST(MAX2(vehicle->getVehicleType().getLength(), vehicle->getSpeed()))); } vehicle->setAngle(laneAngle + changeAngle); return changedToNewLane; }
bool MSLaneChangerSublane::change() { // variant of change() for the sublane case myCandi = findCandidate(); MSVehicle* vehicle = veh(myCandi); #ifdef DEBUG_VEHICLE_GUI_SELECTION if (gDebugSelectedVehicle == vehicle->getID()) { int bla = 0; } #endif assert(vehicle->getLane() == (*myCandi).lane); assert(!vehicle->getLaneChangeModel().isChangingLanes()); #ifndef NO_TRACI if (vehicle->isRemoteControlled()) { return false; // !!! temporary; just because it broke, here } #endif vehicle->updateBestLanes(); // needed? for (int i = 0; i < (int) myChanger.size(); ++i) { vehicle->adaptBestLanesOccupation(i, myChanger[i].dens); } // update expected speeds int sublaneIndex = 0; for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) { vehicle->getLaneChangeModel().updateExpectedSublaneSpeeds(ce->ahead, sublaneIndex, ce->lane->getIndex()); sublaneIndex += ce->ahead.numSublanes(); } StateAndDist right = checkChangeHelper(vehicle, -1); StateAndDist left = checkChangeHelper(vehicle, 1); StateAndDist current = checkChangeHelper(vehicle, 0); StateAndDist decision = vehicle->getLaneChangeModel().decideDirection(current, vehicle->getLaneChangeModel().decideDirection(right, left)); if ((decision.state & LCA_WANTS_LANECHANGE) != 0 && (decision.state & LCA_BLOCKED) == 0) { // change if the vehicle wants to and is allowed to change if (vehicle->getLaneChangeModel().debugVehicle()) { std::cout << SIMTIME << " decision=" << toString((LaneChangeAction)decision.state) << " latDist=" << decision.latDist << "\n"; } vehicle->getLaneChangeModel().setOwnState(decision.state); return startChangeSublane(vehicle, myCandi, decision.latDist); } if ((right.state & (LCA_URGENT)) != 0 && (left.state & (LCA_URGENT)) != 0) { // ... wants to go to the left AND to the right // just let them go to the right lane... left.state = 0; } vehicle->getLaneChangeModel().setOwnState(right.state | left.state | current.state); registerUnchanged(vehicle); 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. myCandi = findCandidate(); MSVehicle* vehicle = veh(myCandi); #ifdef DEBUG_VEHICLE_GUI_SELECTION if (gDebugSelectedVehicle == vehicle->getID()) { int bla = 0; } #endif if (vehicle->getLaneChangeModel().isChangingLanes()) { return continueChange(vehicle, myCandi); } if (!myAllowsChanging || vehicle->getLaneChangeModel().alreadyChanged()) { registerUnchanged(vehicle); return false; } std::pair<MSVehicle* const, SUMOReal> leader = getRealLeader(myCandi); if (myChanger.size() == 1 || vehicle->getLaneChangeModel().isOpposite()) { if (changeOpposite(leader)) { return true; } registerUnchanged(vehicle); return false; } #ifndef NO_TRACI if (vehicle->isRemoteControlled()) { return false; // !!! temporary; just because it broke, here } #endif 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 state1 = 0; if (mayChange(-1)) { state1 = checkChangeWithinEdge(-1, leader, preb); bool changingAllowed1 = (state1 & LCA_BLOCKED) == 0; // change if the vehicle wants to and is allowed to change if ((state1 & LCA_RIGHT) != 0 && changingAllowed1) { vehicle->getLaneChangeModel().setOwnState(state1); startChange(vehicle, myCandi, -1); return true; } if ((state1 & LCA_RIGHT) != 0 && (state1 & LCA_URGENT) != 0) { (myCandi - 1)->lastBlocked = vehicle; if ((myCandi - 1)->firstBlocked == 0) { (myCandi - 1)->firstBlocked = vehicle; } } } // check whether the vehicle wants and is able to change to left lane int state2 = 0; if (mayChange(1)) { state2 = checkChangeWithinEdge(1, leader, preb); bool changingAllowed2 = (state2 & LCA_BLOCKED) == 0; // change if the vehicle wants to and is allowed to change if ((state2 & LCA_LEFT) != 0 && changingAllowed2) { vehicle->getLaneChangeModel().setOwnState(state2); startChange(vehicle, myCandi, 1); return true; } if ((state2 & LCA_LEFT) != 0 && (state2 & LCA_URGENT) != 0) { (myCandi + 1)->lastBlocked = vehicle; if ((myCandi + 1)->firstBlocked == 0) { (myCandi + 1)->firstBlocked = vehicle; } } } if ((state1 & (LCA_URGENT)) != 0 && (state2 & (LCA_URGENT)) != 0) { // ... wants to go to the left AND to the right // just let them go to the right lane... state2 = 0; } vehicle->getLaneChangeModel().setOwnState(state2 | state1); // only emergency vehicles should change to the opposite side on a // multi-lane road if (vehicle->getVehicleType().getVehicleClass() == SVC_EMERGENCY && changeOpposite(leader)) { return true; } else { registerUnchanged(vehicle); 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. myCandi = findCandidate(); MSVehicle* vehicle = veh(myCandi); #ifdef DEBUG_VEHICLE_GUI_SELECTION if (gDebugSelectedVehicle == vehicle->getID()) { int bla = 0; } #endif if (vehicle->getLaneChangeModel().isChangingLanes()) { return continueChange(vehicle, myCandi); } if (!myAllowsChanging || vehicle->getLaneChangeModel().alreadyChanged()) { registerUnchanged(vehicle); return false; } std::pair<MSVehicle* const, SUMOReal> leader = getRealLeader(myCandi); if (myChanger.size() == 1) { if (changeOpposite(leader)) { return true; } registerUnchanged(vehicle); return false; } #ifndef NO_TRACI if (vehicle->isRemoteControlled()) { return false; // !!! temporary; just because it broke, here } #endif 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 state1 = 0; if (mayChange(-1)) { state1 = checkChangeWithinEdge(-1, leader, preb); bool changingAllowed1 = (state1 & LCA_BLOCKED) == 0; // change if the vehicle wants to and is allowed to change if ((state1 & LCA_RIGHT) != 0 && changingAllowed1) { vehicle->getLaneChangeModel().setOwnState(state1); startChange(vehicle, myCandi, -1); return true; } if ((state1 & LCA_RIGHT) != 0 && (state1 & LCA_URGENT) != 0) { (myCandi - 1)->lastBlocked = vehicle; if ((myCandi - 1)->firstBlocked == 0) { (myCandi - 1)->firstBlocked = vehicle; } } } // check whether the vehicle wants and is able to change to left lane int state2 = 0; if (mayChange(1)) { state2 = checkChangeWithinEdge(1, leader, preb); bool changingAllowed2 = (state2 & LCA_BLOCKED) == 0; // change if the vehicle wants to and is allowed to change if ((state2 & LCA_LEFT) != 0 && changingAllowed2) { vehicle->getLaneChangeModel().setOwnState(state2); startChange(vehicle, myCandi, 1); return true; } if ((state2 & LCA_LEFT) != 0 && (state2 & LCA_URGENT) != 0) { (myCandi + 1)->lastBlocked = vehicle; if ((myCandi + 1)->firstBlocked == 0) { (myCandi + 1)->firstBlocked = vehicle; } } } if ((state1 & (LCA_URGENT)) != 0 && (state2 & (LCA_URGENT)) != 0) { // ... wants to go to the left AND to the right // just let them go to the right lane... state2 = 0; } vehicle->getLaneChangeModel().setOwnState(state2 | state1); // check whether the vehicles should be swapped if (myAllowsSwap && ((state1 & (LCA_URGENT)) != 0 || (state2 & (LCA_URGENT)) != 0)) { // get the direction ... ChangerIt target; int direction = 0; if ((state1 & (LCA_URGENT)) != 0) { // ... wants to go right target = myCandi - 1; direction = -1; } if ((state2 & (LCA_URGENT)) != 0) { // ... wants to go left target = myCandi + 1; direction = 1; } MSVehicle* prohibitor = target->lead; if (target->hoppedVeh != 0) { SUMOReal hoppedPos = target->hoppedVeh->getPositionOnLane(); if (prohibitor == 0 || (hoppedPos > vehicle->getPositionOnLane() && prohibitor->getPositionOnLane() > hoppedPos)) { prohibitor = 0;// !!! vehicles should not jump over more than one lanetarget->hoppedVeh; } } if (prohibitor != 0 && ((prohibitor->getLaneChangeModel().getOwnState() & (LCA_URGENT/*|LCA_SPEEDGAIN*/)) != 0 && (prohibitor->getLaneChangeModel().getOwnState() & (LCA_LEFT | LCA_RIGHT)) != (vehicle->getLaneChangeModel().getOwnState() & (LCA_LEFT | LCA_RIGHT)) ) ) { // check for position and speed if (prohibitor->getVehicleType().getLengthWithGap() == vehicle->getVehicleType().getLengthWithGap()) { // ok, may be swapped // remove vehicle to swap with MSLane::VehCont::iterator i = find(target->lane->myTmpVehicles.begin(), target->lane->myTmpVehicles.end(), prohibitor); if (i != target->lane->myTmpVehicles.end()) { assert(*i == prohibitor); target->lane->myTmpVehicles.erase(i); startChange(vehicle, myCandi, direction); startChange(prohibitor, target, -direction); std::swap(vehicle->myState, prohibitor->myState); myCandi->lead = prohibitor; target->lead = vehicle; return true; } } } } if (!changeOpposite(leader)) { registerUnchanged(vehicle); return false; } else { return true; } }