std::pair<MSVehicle* const, SUMOReal> MSLaneChanger::getRealThisLeader(const ChangerIt& target) const { // get the leading vehicle on the lane to change to MSVehicle* leader = target->lead; if (leader == 0) { MSLane* targetLane = target->lane; MSVehicle* predP = targetLane->getPartialOccupator(); if (predP != 0) { return std::pair<MSVehicle*, SUMOReal>(predP, targetLane->getPartialOccupatorEnd() - veh(myCandi)->getPositionOnLane()); } const std::vector<MSLane*>& bestLaneConts = veh(myCandi)->getBestLanesContinuation(); MSLinkCont::const_iterator link = targetLane->succLinkSec(*veh(myCandi), 1, *targetLane, bestLaneConts); if (targetLane->isLinkEnd(link)) { return std::pair<MSVehicle*, SUMOReal>(static_cast<MSVehicle*>(0), -1); } MSLane* nextLane = (*link)->getLane(); if (nextLane == 0) { return std::pair<MSVehicle*, SUMOReal>(static_cast<MSVehicle*>(0), -1); } leader = nextLane->getLastVehicle(); if (leader == 0) { return std::pair<MSVehicle*, SUMOReal>(static_cast<MSVehicle*>(0), -1); } SUMOReal gap = leader->getPositionOnLane() - leader->getVehicleType().getLength() + (myCandi->lane->getLength() - veh(myCandi)->getPositionOnLane() - veh(myCandi)->getVehicleType().getMinGap()); // !!! recheck return std::pair<MSVehicle* const, SUMOReal>(leader, MAX2((SUMOReal) 0, gap)); } else { MSVehicle* candi = veh(myCandi); SUMOReal gap = leader->getPositionOnLane() - leader->getVehicleType().getLength() - candi->getPositionOnLane() - candi->getVehicleType().getMinGap(); return std::pair<MSVehicle* const, SUMOReal>(leader, MAX2((SUMOReal) 0, gap)); } }
bool MSLane::isEmissionSuccess(MSVehicle* aVehicle, SUMOReal speed, SUMOReal pos, bool patchSpeed, size_t startStripId) throw(ProcessError) { // and the speed is not too high (vehicle should decelerate) // try to get a leader on consecutive lanes // we have to do this even if we have found a leader on our lane because it may // be driving into another direction //std::cerr<<"EMISSION speed:"<<speed<<std::endl; std::cerr<<"EMISSION vehicle:"<<aVehicle->getID()<<std::endl; size_t endStripId = startStripId + aVehicle->getWidth() - 1; assert(startStripId >=0 && endStripId < myStrips.size()); aVehicle->getBestLanes(true, this); const MSCFModel &cfModel = aVehicle->getCarFollowModel(); const std::vector<MSLane*> &bestLaneConts = aVehicle->getBestLanesContinuation(this); std::vector<MSLane*>::const_iterator ri = bestLaneConts.begin(); SUMOReal seen = getLength() - pos; SUMOReal dist = cfModel.brakeGap(speed); const MSRoute &r = aVehicle->getRoute(); MSRouteIterator ce = r.begin(); MSLane *currentLane = this; MSLane *nextLane = this; while (seen<dist&&ri!=bestLaneConts.end()&&nextLane!=0/*&&ce!=r.end()*/) { // get the next link used... MSLinkCont::const_iterator link = currentLane->succLinkSec(*aVehicle, 1, *currentLane, bestLaneConts); // ...and the next used lane (including internal) if (!currentLane->isLinkEnd(link) && (*link)->havePriority() && (*link)->getState()!=MSLink::LINKSTATE_TL_RED) { // red may have priority? #ifdef HAVE_INTERNAL_LANES bool nextInternal = false; nextLane = (*link)->getViaLane(); if (nextLane==0) { nextLane = (*link)->getLane(); } else { nextInternal = true; } #else nextLane = (*link)->getLane(); #endif } else { nextLane = 0; } // check how next lane effects the journey if (nextLane!=0) { SUMOReal gap = 0; //TODO: fix get & set partial occupator to strip level MSVehicle * leader = 0;//currentLane->getPartialOccupator(); if (leader!=0) { gap = getPartialOccupatorEnd(); } else { // check leader on next lane leader = nextLane->getLastVehicle(aVehicle->getStrips()); if (leader!=0) { gap = seen+leader->getPositionOnLane()-leader->getVehicleType().getLength(); } } if (leader!=0) { SUMOReal nspeed = gap>=0 ? cfModel.ffeV(aVehicle, speed, gap, leader->getSpeed()) : 0; if (nspeed<speed) { if (patchSpeed) { speed = MIN2(nspeed, speed); dist = cfModel.brakeGap(speed); } else { // we may not drive with the given velocity - we crash into the leader return false; } } } // check next lane's maximum velocity SUMOReal nspeed = nextLane->getMaxSpeed(); if (nspeed<speed) { // patch speed if needed if (patchSpeed) { speed = MIN2(cfModel.ffeV(aVehicle, speed, seen, nspeed), speed); dist = cfModel.brakeGap(speed); } else { // we may not drive with the given velocity - we would be too fast on the next lane return false; } } // check traffic on next junctions const SUMOTime arrivalTime = MSNet::getInstance()->getCurrentTimeStep() + TIME2STEPS(seen / speed); #ifdef HAVE_INTERNAL_LANES const SUMOTime leaveTime = (*link)->getViaLane()==0 ? arrivalTime + TIME2STEPS((*link)->getLength() * speed) : arrivalTime + TIME2STEPS((*link)->getViaLane()->getLength() * speed); #else const SUMOTime leaveTime = arrivalTime + TIME2STEPS((*link)->getLength() * speed); #endif if ((*link)->hasApproachingFoe(arrivalTime, leaveTime)) { SUMOReal nspeed = cfModel.ffeV(aVehicle, speed, seen, 0); if (nspeed<speed) { if (patchSpeed) { speed = MIN2(nspeed, speed); dist = cfModel.brakeGap(speed); } else { // we may not drive with the given velocity - we crash into the leader return false; } } } else { // we can only drive to the end of the current lane... SUMOReal nspeed = cfModel.ffeV(aVehicle, speed, seen, 0); if (nspeed<speed) { if (patchSpeed) { speed = MIN2(nspeed, speed); dist = cfModel.brakeGap(speed); } else { // we may not drive with the given velocity - we crash into the leader return false; } } } seen += nextLane->getLength(); ++ce; ++ri; currentLane = nextLane; } } if (seen<dist) { SUMOReal nspeed = cfModel.ffeV(aVehicle, speed, seen, 0); if (nspeed<speed) { if (patchSpeed) { speed = MIN2(nspeed, speed); dist = cfModel.brakeGap(speed); } else { // we may not drive with the given velocity - we crash into the leader MsgHandler::getErrorInstance()->inform("Vehicle '" + aVehicle->getID() + "' will not be able to emit using given velocity!"); // !!! we probably should do something else... return false; } } } // get the pointer to the vehicle next in front of the given position MSVehicle *pred; std::vector<MSVehicle *> predCont; std::vector<MSVehicle *>::iterator predIt, it; for (unsigned int i=startStripId; i<=endStripId; ++i) { predCont.push_back(myStrips.at(i)->getPredAtPos(pos)); } predIt = predCont.begin(); SUMOReal currMin = -1; if (*predIt != 0) { currMin = (*predIt)->getPositionOnLane(); } else { // signals no leader in front predIt = predCont.end(); } for (it = predCont.begin(); it != predCont.end(); ++it) { if (*it == 0) continue; if ((*it)->getPositionOnLane() < currMin) { predIt = it; currMin = (*it)->getPositionOnLane(); } } if (predIt != predCont.end()) { // ok, there is one (a leader) MSVehicle* leader = *predIt; SUMOReal frontGapNeeded = aVehicle->getCarFollowModel().getSecureGap(speed, leader->getCarFollowModel().getSpeedAfterMaxDecel(leader->getSpeed())); SUMOReal gap = MSVehicle::gap(leader->getPositionOnLane(), leader->getVehicleType().getLength(), pos); if (gap<frontGapNeeded) { // too close to the leader on this lane return false; } } // FIXME: implement look back // check back vehicle if (false/*predIt!=myVehicles.begin()*/) { // there is direct follower on this lane MSVehicle *follower = *(predIt-1); SUMOReal backGapNeeded = follower->getCarFollowModel().getSecureGap(follower->getSpeed(), aVehicle->getCarFollowModel().getSpeedAfterMaxDecel(speed)); SUMOReal gap = MSVehicle::gap(pos, aVehicle->getVehicleType().getLength(), follower->getPositionOnLane()); if (gap<backGapNeeded) { // too close to the follower on this lane return false; } } else if (false) { // check approaching vehicle (consecutive follower) SUMOReal lspeed = getMaxSpeed(); // in order to look back, we'd need the minimum braking ability of vehicles in the net... // we'll assume it to be 4m/s^2 // !!!revisit SUMOReal dist = lspeed * lspeed * SUMOReal(1./2.*4.) + SPEED2DIST(lspeed); std::pair<const MSVehicle * const, SUMOReal> approaching = getFollowerOnConsecutive(dist, 0, speed, pos - aVehicle->getVehicleType().getLength()); if (approaching.first!=0) { const MSVehicle *const follower = approaching.first; SUMOReal backGapNeeded = follower->getCarFollowModel().getSecureGap(follower->getSpeed(), aVehicle->getCarFollowModel().getSpeedAfterMaxDecel(speed)); SUMOReal gap = approaching.second - pos - aVehicle->getVehicleType().getLength(); if (gap<backGapNeeded) { // too close to the consecutive follower return false; } } // check for in-lapping vehicle MSVehicle* leader = getPartialOccupator(); if (leader!=0) { SUMOReal frontGapNeeded = aVehicle->getCarFollowModel().getSecureGap(speed, leader->getCarFollowModel().getSpeedAfterMaxDecel(leader->getSpeed())); SUMOReal gap = getPartialOccupatorEnd() - pos; if (gap<=frontGapNeeded) { // too close to the leader on this lane return false; } } } // may got negative while adaptation if (speed<0) { return false; } // enter //XXX: later change to enterStripAtEmit()? //if (speed < 0.0001) speed += 10.0; StripCont strips; strips.resize(aVehicle->getWidth()); StripCont::iterator start = myStrips.begin() + startStripId; std::copy(start, start + aVehicle->getWidth(), strips.begin()); aVehicle->enterLaneAtEmit(this, pos, speed, strips); bool wasInactive = getVehicleNumber()==0; if (true/*predIt==myVehicles.end()*/) { // vehicle will be the first on the lane //std::cerr<<"startStripId:"<<startStripId<<", NumStrips:"<<strips.size()<<", VehWidth:"<<aVehicle->getWidth()<<std::endl; for (size_t i=startStripId; i<startStripId+strips.size(); ++i) { this->getStrip(i)->pushIntoStrip(aVehicle); this->getStrip(i)->setVehLenSum(this->getStrip(i)->getVehLenSum() + aVehicle->getVehicleType().getLength()); } aVehicle->printDebugMsg("Emitting"); printDebugMsg(); } else { //this->getStrip(0).insert(0, aVehicle); } //myVehicleLengthSum += aVehicle->getVehicleType().getLength(); if (wasInactive) { MSNet::getInstance()->getEdgeControl().gotActive(this); } return true; }