// =========================================================================== // method definitions // =========================================================================== GUILaneWrapper::GUILaneWrapper(GUIGlObjectStorage &idStorage, MSLane &lane, const Position2DVector &shape) throw() : GUIGlObject(idStorage, "lane:"+lane.getID()), myLane(lane), myShape(shape) { SUMOReal x1 = shape[0].x(); SUMOReal y1 = shape[0].y(); SUMOReal x2 = shape[-1].x(); SUMOReal y2 = shape[-1].y(); SUMOReal length = myLane.getLength(); // also the virtual length is set in here myVisLength = sqrt((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2)); // check maximum speed if (myAllMaxSpeed<lane.getMaxSpeed()) { myAllMaxSpeed = lane.getMaxSpeed(); } // myShapeRotations.reserve(myShape.size()-1); myShapeLengths.reserve(myShape.size()-1); int e = (int) myShape.size() - 1; for (int i=0; i<e; ++i) { const Position2D &f = myShape[i]; const Position2D &s = myShape[i+1]; myShapeLengths.push_back(f.distanceTo(s)); myShapeRotations.push_back((SUMOReal) atan2((s.x()-f.x()), (f.y()-s.y()))*(SUMOReal) 180.0/(SUMOReal) PI); } }
SUMOReal getMaxSpeedRegardingNextLanes(MSVehicle& veh, SUMOReal speed, SUMOReal pos) { MSRouteIterator next = veh.getRoute().begin(); const MSCFModel &cfModel = veh.getCarFollowModel(); MSLane *currentLane = (*next)->getLanes()[0]; SUMOReal seen = currentLane->getLength() - pos; SUMOReal dist = SPEED2DIST(speed) + cfModel.brakeGap(speed); SUMOReal tspeed = speed; while (seen<dist&&next!=veh.getRoute().end()-1) { ++next; MSLane *nextLane = (*next)->getLanes()[0]; tspeed = MIN2(cfModel.ffeV(&veh, tspeed, seen, nextLane->getMaxSpeed()), nextLane->getMaxSpeed()); dist = SPEED2DIST(tspeed) + cfModel.brakeGap(tspeed); seen += nextLane->getMaxSpeed(); } return tspeed; }
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; }
// =========================================================================== // method definitions // =========================================================================== bool TraCIServerAPI_Lane::processGet(TraCIServer &server, tcpip::Storage &inputStorage, tcpip::Storage &outputStorage) { Storage tmpResult; std::string warning = ""; // additional description for response // variable int variable = inputStorage.readUnsignedByte(); std::string id = inputStorage.readString(); // check variable if (variable!=ID_LIST&&variable!=LANE_LINK_NUMBER&&variable!=LANE_EDGE_ID&&variable!=VAR_LENGTH &&variable!=VAR_MAXSPEED&&variable!=LANE_LINKS&&variable!=VAR_SHAPE &&variable!=VAR_CO2EMISSION&&variable!=VAR_COEMISSION&&variable!=VAR_HCEMISSION&&variable!=VAR_PMXEMISSION &&variable!=VAR_NOXEMISSION&&variable!=VAR_FUELCONSUMPTION&&variable!=VAR_NOISEEMISSION &&variable!=LAST_STEP_MEAN_SPEED&&variable!=LAST_STEP_VEHICLE_NUMBER &&variable!=LAST_STEP_VEHICLE_ID_LIST&&variable!=LAST_STEP_OCCUPANCY&&variable!=LAST_STEP_VEHICLE_HALTING_NUMBER &&variable!=LAST_STEP_LENGTH&&variable!=VAR_CURRENT_TRAVELTIME &&variable!=LANE_ALLOWED&&variable!=LANE_DISALLOWED) { server.writeStatusCmd(CMD_GET_LANE_VARIABLE, RTYPE_ERR, "Get Lane Variable: unsupported variable specified", outputStorage); return false; } // begin response building Storage tempMsg; // response-code, variableID, objectID tempMsg.writeUnsignedByte(RESPONSE_GET_LANE_VARIABLE); tempMsg.writeUnsignedByte(variable); tempMsg.writeString(id); if (variable==ID_LIST) { std::vector<std::string> ids; MSLane::insertIDs(ids); tempMsg.writeUnsignedByte(TYPE_STRINGLIST); tempMsg.writeStringList(ids); } else { MSLane *lane = MSLane::dictionary(id); if (lane==0) { server.writeStatusCmd(CMD_GET_LANE_VARIABLE, RTYPE_ERR, "Lane '" + id + "' is not known", outputStorage); return false; } switch (variable) { case LANE_LINK_NUMBER: tempMsg.writeUnsignedByte(TYPE_UBYTE); tempMsg.writeUnsignedByte((int) lane->getLinkCont().size()); break; case LANE_EDGE_ID: tempMsg.writeUnsignedByte(TYPE_STRING); tempMsg.writeString(lane->getEdge().getID()); break; case VAR_LENGTH: tempMsg.writeUnsignedByte(TYPE_FLOAT); tempMsg.writeFloat(lane->getLength()); break; case VAR_MAXSPEED: tempMsg.writeUnsignedByte(TYPE_FLOAT); tempMsg.writeFloat(lane->getMaxSpeed()); break; case LANE_LINKS: { tempMsg.writeUnsignedByte(TYPE_COMPOUND); Storage tempContent; unsigned int cnt = 0; tempContent.writeUnsignedByte(TYPE_INTEGER); const MSLinkCont &links = lane->getLinkCont(); tempContent.writeInt((int) links.size()); ++cnt; for (MSLinkCont::const_iterator i=links.begin(); i!=links.end(); ++i) { MSLink *link = (*i); // approached non-internal lane (if any) tempContent.writeUnsignedByte(TYPE_STRING); tempContent.writeString(link->getLane()!=0 ? link->getLane()->getID() : ""); ++cnt; // approached "via", internal lane (if any) tempContent.writeUnsignedByte(TYPE_STRING); #ifdef HAVE_INTERNAL_LANES tempContent.writeString(link->getViaLane()!=0 ? link->getViaLane()->getID() : ""); #else tempContent.writeString(""); #endif ++cnt; // priority tempContent.writeUnsignedByte(TYPE_UBYTE); tempContent.writeUnsignedByte(link->havePriority() ? 1 : 0); ++cnt; // opened tempContent.writeUnsignedByte(TYPE_UBYTE); tempContent.writeUnsignedByte(link->opened(MSNet::getInstance()->getCurrentTimeStep(), MSNet::getInstance()->getCurrentTimeStep(), 0.) ? 1 : 0); ++cnt; // approaching foe tempContent.writeUnsignedByte(TYPE_UBYTE); tempContent.writeUnsignedByte(link->hasApproachingFoe(MSNet::getInstance()->getCurrentTimeStep(), MSNet::getInstance()->getCurrentTimeStep()) ? 1 : 0); ++cnt; // state (not implemented, yet) tempContent.writeUnsignedByte(TYPE_STRING); tempContent.writeString(""); ++cnt; // direction (not implemented, yet) tempContent.writeUnsignedByte(TYPE_STRING); tempContent.writeString(""); ++cnt; // length tempContent.writeUnsignedByte(TYPE_FLOAT); tempContent.writeFloat(link->getLength()); ++cnt; } tempMsg.writeInt((int) cnt); tempMsg.writeStorage(tempContent); } break; case LANE_ALLOWED: { const std::vector<SUMOVehicleClass> &allowed = lane->getAllowedClasses(); std::vector<std::string> allowedS; for (std::vector<SUMOVehicleClass>::const_iterator i=allowed.begin(); i!=allowed.end(); ++i) { allowedS.push_back(getVehicleClassName(*i)); } tempMsg.writeUnsignedByte(TYPE_STRINGLIST); tempMsg.writeStringList(allowedS); } case LANE_DISALLOWED: { const std::vector<SUMOVehicleClass> &disallowed = lane->getNotAllowedClasses(); std::vector<std::string> disallowedS; for (std::vector<SUMOVehicleClass>::const_iterator i=disallowed.begin(); i!=disallowed.end(); ++i) { disallowedS.push_back(getVehicleClassName(*i)); } tempMsg.writeUnsignedByte(TYPE_STRINGLIST); tempMsg.writeStringList(disallowedS); } break; case VAR_SHAPE: tempMsg.writeUnsignedByte(TYPE_POLYGON); tempMsg.writeUnsignedByte(MIN2(static_cast<size_t>(255),lane->getShape().size())); for (int iPoint=0; iPoint < MIN2(static_cast<size_t>(255),lane->getShape().size()); ++iPoint) { tempMsg.writeFloat(lane->getShape()[iPoint].x()); tempMsg.writeFloat(lane->getShape()[iPoint].y()); } break; case VAR_CO2EMISSION: tempMsg.writeUnsignedByte(TYPE_FLOAT); tempMsg.writeFloat(lane->getHBEFA_CO2Emissions()); break; case VAR_COEMISSION: tempMsg.writeUnsignedByte(TYPE_FLOAT); tempMsg.writeFloat(lane->getHBEFA_COEmissions()); break; case VAR_HCEMISSION: tempMsg.writeUnsignedByte(TYPE_FLOAT); tempMsg.writeFloat(lane->getHBEFA_HCEmissions()); break; case VAR_PMXEMISSION: tempMsg.writeUnsignedByte(TYPE_FLOAT); tempMsg.writeFloat(lane->getHBEFA_PMxEmissions()); break; case VAR_NOXEMISSION: tempMsg.writeUnsignedByte(TYPE_FLOAT); tempMsg.writeFloat(lane->getHBEFA_NOxEmissions()); break; case VAR_FUELCONSUMPTION: tempMsg.writeUnsignedByte(TYPE_FLOAT); tempMsg.writeFloat(lane->getHBEFA_FuelConsumption()); break; case VAR_NOISEEMISSION: tempMsg.writeUnsignedByte(TYPE_FLOAT); tempMsg.writeFloat(lane->getHarmonoise_NoiseEmissions()); break; case LAST_STEP_VEHICLE_NUMBER: tempMsg.writeUnsignedByte(TYPE_INTEGER); tempMsg.writeInt((int) lane->getVehicleNumber()); break; case LAST_STEP_MEAN_SPEED: tempMsg.writeUnsignedByte(TYPE_FLOAT); tempMsg.writeFloat(lane->getMeanSpeed()); break; case LAST_STEP_VEHICLE_ID_LIST: { std::vector<std::string> vehIDs; const std::deque<MSVehicle*> &vehs = lane->getVehiclesSecure(); for (std::deque<MSVehicle*>::const_iterator j=vehs.begin(); j!=vehs.end(); ++j) { vehIDs.push_back((*j)->getID()); } lane->releaseVehicles(); tempMsg.writeUnsignedByte(TYPE_STRINGLIST); tempMsg.writeStringList(vehIDs); } break; case LAST_STEP_OCCUPANCY: tempMsg.writeUnsignedByte(TYPE_FLOAT); tempMsg.writeFloat(lane->getOccupancy()); break; case LAST_STEP_VEHICLE_HALTING_NUMBER: { int halting = 0; const std::deque<MSVehicle*> &vehs = lane->getVehiclesSecure(); for (std::deque<MSVehicle*>::const_iterator j=vehs.begin(); j!=vehs.end(); ++j) { if ((*j)->getSpeed()<0.1) { ++halting; } } lane->releaseVehicles(); tempMsg.writeUnsignedByte(TYPE_INTEGER); tempMsg.writeInt(halting); } break; case LAST_STEP_LENGTH: { SUMOReal lengthSum = 0; const std::deque<MSVehicle*> &vehs = lane->getVehiclesSecure(); for (std::deque<MSVehicle*>::const_iterator j=vehs.begin(); j!=vehs.end(); ++j) { lengthSum += (*j)->getVehicleType().getLength(); } tempMsg.writeUnsignedByte(TYPE_FLOAT); if (vehs.size()==0) { tempMsg.writeFloat(0); } else { tempMsg.writeFloat(lengthSum / (SUMOReal) vehs.size()); } lane->releaseVehicles(); } break; case VAR_CURRENT_TRAVELTIME: { SUMOReal meanSpeed = lane->getMeanSpeed(); tempMsg.writeUnsignedByte(TYPE_FLOAT); if (meanSpeed!=0) { tempMsg.writeFloat(lane->getLength() / meanSpeed); } else { tempMsg.writeFloat(1000000.); } } break; default: break; } } server.writeStatusCmd(CMD_GET_LANE_VARIABLE, RTYPE_OK, warning, outputStorage); // send response outputStorage.writeUnsignedByte(0); // command length -> extended outputStorage.writeInt(1 + 4 + tempMsg.size()); outputStorage.writeStorage(tempMsg); return true; }