void
NLDetectorBuilder::buildInductLoop(const std::string &id,
const std::string &lane, SUMOReal pos, int splInterval,
OutputDevice& device, bool friendlyPos) throw(InvalidArgument) {
if (splInterval<0) {
throw InvalidArgument("Negative sampling frequency (in e1-detector '" + id + "').");
}
if (splInterval==0) {
throw InvalidArgument("Sampling frequency must not be zero (in e1-detector '" + id + "').");
}
// get and check the lane
MSLane *clane = getLaneChecking(lane, id);
if (pos<0) {
pos = clane->getLength() + pos;
}
#ifdef HAVE_MESOSIM
if (!MSGlobals::gUseMesoSim) {
#endif
// get and check the position
pos = getPositionChecking(pos, clane, friendlyPos, id);
// build the loop
MSInductLoop *loop = createInductLoop(id, clane, pos);
// add the file output
myNet.getDetectorControl().add(loop, device, splInterval);
#ifdef HAVE_MESOSIM
} else {
if (pos<0) {
pos = clane->getLength() + pos;
}
MESegment *s = MSGlobals::gMesoNet->getSegmentForEdge(clane->getEdge());
MESegment *prev = s;
SUMOReal cpos = 0;
while (cpos+prev->getLength()<pos&&s!=0) {
prev = s;
cpos += s->getLength();
s = s->getNextSegment();
}
SUMOReal rpos = pos-cpos;//-prev->getLength();
if (rpos>prev->getLength()||rpos<0) {
if (friendlyPos) {
rpos = prev->getLength() - (SUMOReal) 0.1;
} else {
throw InvalidArgument("The position of detector '" + id + "' lies beyond the lane's '" + lane + "' length.");
}
}
MEInductLoop *loop =
createMEInductLoop(id, prev, rpos);
myNet.getDetectorControl().add(loop, device, splInterval);
}
#endif
}
void
MSRailSignal::collectConflictLinks(MSLane* toLane, double length,
std::vector<MSLane*>& backwardBlock,
std::vector<MSLink*>& conflictLinks,
LaneSet& visited,
bool checkFoes) {
while (toLane != nullptr) {
//std::cout << "collectConflictLinks " << getID() << " toLane=" << toLane->getID() << " length=" << length
// << " backward=" << toString(backwardBlock)
// << " conflictLinks=" << conflictLinks.size()
// << " visited=" << visited.size()
// << " checkFoes=" << checkFoes
// << "\n";
const auto& incomingLaneInfos = toLane->getIncomingLanes();
MSLane* orig = toLane;
toLane = nullptr;
for (const auto& ili : incomingLaneInfos) {
if (ili.viaLink->getDirection() == LINKDIR_TURN) {
continue;
}
if (visited.count(ili.lane) != 0) {
continue;
}
if (ili.viaLink->getTLLogic() != nullptr) {
conflictLinks.push_back(ili.viaLink);
continue;
}
backwardBlock.push_back(ili.lane);
visited.insert(ili.lane);
length += orig->getLength();
if (length > MAX_BLOCK_LENGTH) {
if (myNumWarnings < MAX_SIGNAL_WARNINGS) {
WRITE_WARNING("incoming conflict block after rail signal junction '" + getID() +
"' exceeds maximum length (stopped searching after lane '" + orig->getID() + "' (length=" + toString(length) + "m).");
}
myNumWarnings++;
return;
}
if (toLane == nullptr) {
toLane = ili.lane;
} else {
collectConflictLinks(ili.lane, length, backwardBlock, conflictLinks, visited, false);
}
}
if (checkFoes && orig->isInternal()) {
// check for crossed tracks
MSLink* link = orig->getIncomingLanes().front().viaLink;
if (link->getDirection() != LINKDIR_TURN) {
for (const MSLane* foeConst : link->getFoeLanes()) {
MSLane* foe = const_cast<MSLane*>(foeConst);
if (visited.count(foe) == 0) {
backwardBlock.push_back(foe);
visited.insert(foe);
collectConflictLinks(foe, length, backwardBlock, conflictLinks, visited, false);
}
}
}
}
}
}
void
NLHandler::addPOI(const SUMOSAXAttributes& attrs) {
bool ok = true;
const SUMOReal INVALID_POSITION(-1000000);
std::string id = attrs.get<std::string>(SUMO_ATTR_ID, 0, ok);
SUMOReal x = attrs.getOpt<SUMOReal>(SUMO_ATTR_X, id.c_str(), ok, INVALID_POSITION);
SUMOReal y = attrs.getOpt<SUMOReal>(SUMO_ATTR_Y, id.c_str(), ok, INVALID_POSITION);
SUMOReal lon = attrs.getOpt<SUMOReal>(SUMO_ATTR_LON, id.c_str(), ok, INVALID_POSITION);
SUMOReal lat = attrs.getOpt<SUMOReal>(SUMO_ATTR_LAT, id.c_str(), ok, INVALID_POSITION);
SUMOReal lanePos = attrs.getOpt<SUMOReal>(SUMO_ATTR_POSITION, id.c_str(), ok, INVALID_POSITION);
SUMOReal layer = attrs.getOpt<SUMOReal>(SUMO_ATTR_LAYER, id.c_str(), ok, (SUMOReal)GLO_POI);
std::string type = attrs.getOpt<std::string>(SUMO_ATTR_TYPE, id.c_str(), ok, "");
std::string laneID = attrs.getOpt<std::string>(SUMO_ATTR_LANE, id.c_str(), ok, "");
RGBColor color = attrs.hasAttribute(SUMO_ATTR_COLOR) ? attrs.get<RGBColor>(SUMO_ATTR_COLOR, id.c_str(), ok) : RGBColor::RED;
SUMOReal angle = attrs.getOpt<SUMOReal>(SUMO_ATTR_ANGLE, id.c_str(), ok, Shape::DEFAULT_ANGLE);
std::string imgFile = attrs.getOpt<std::string>(SUMO_ATTR_IMGFILE, id.c_str(), ok, Shape::DEFAULT_IMG_FILE);
if (imgFile != "" && !FileHelpers::isAbsolute(imgFile)) {
imgFile = FileHelpers::getConfigurationRelative(getFileName(), imgFile);
}
SUMOReal width = attrs.getOpt<SUMOReal>(SUMO_ATTR_WIDTH, id.c_str(), ok, Shape::DEFAULT_IMG_WIDTH);
SUMOReal height = attrs.getOpt<SUMOReal>(SUMO_ATTR_HEIGHT, id.c_str(), ok, Shape::DEFAULT_IMG_HEIGHT);
if (!ok) {
return;
}
Position pos(x, y);
if (x == INVALID_POSITION || y == INVALID_POSITION) {
// try computing x,y from lane,pos
if (laneID != "") {
MSLane* lane = MSLane::dictionary(laneID);
if (lane == 0) {
WRITE_ERROR("Lane '" + laneID + "' to place a poi '" + id + "'on is not known.");
return;
}
if (lanePos < 0) {
lanePos = lane->getLength() + lanePos;
}
pos = lane->geometryPositionAtOffset(lanePos);
} else {
// try computing x,y from lon,lat
if (lat == INVALID_POSITION || lon == INVALID_POSITION) {
WRITE_ERROR("Either (x,y), (lon,lat) or (lane,pos) must be specified for poi '" + id + "'.");
return;
} else if (!GeoConvHelper::getFinal().usingGeoProjection()) {
WRITE_ERROR("(lon, lat) is specified for poi '" + id + "' but no geo-conversion is specified for the network.");
return;
}
pos.set(lon, lat);
GeoConvHelper::getFinal().x2cartesian_const(pos);
}
}
if (!myNet.getShapeContainer().addPOI(id, type, color, layer, angle, imgFile, pos, width, height)) {
WRITE_ERROR("PoI '" + id + "' already exists.");
}
}
void
NLTriggerBuilder::parseAndBuildBusStop(MSNet& net, const SUMOSAXAttributes& attrs) {
bool ok = true;
// get the id, throw if not given or empty...
std::string id = attrs.get<std::string>(SUMO_ATTR_ID, 0, ok);
if (!ok) {
throw ProcessError();
}
// get the lane
MSLane* lane = getLane(attrs, "busStop", id);
// get the positions
SUMOReal frompos = attrs.getOpt<SUMOReal>(SUMO_ATTR_STARTPOS, id.c_str(), ok, 0);
SUMOReal topos = attrs.getOpt<SUMOReal>(SUMO_ATTR_ENDPOS, id.c_str(), ok, lane->getLength());
const bool friendlyPos = attrs.getOpt<bool>(SUMO_ATTR_FRIENDLY_POS, id.c_str(), ok, false);
if (!ok || !myHandler->checkStopPos(frompos, topos, lane->getLength(), POSITION_EPS, friendlyPos)) {
throw InvalidArgument("Invalid position for bus stop '" + id + "'.");
}
// get the lines
std::vector<std::string> lines;
SUMOSAXAttributes::parseStringVector(attrs.getOpt<std::string>(SUMO_ATTR_LINES, id.c_str(), ok, "", false), lines);
// build the bus stop
buildBusStop(net, id, lines, lane, frompos, topos);
}
void
MSQueueExport::writeLane(OutputDevice& of, const MSLane& lane) {
// maximum of all vehicle waiting times
SUMOReal queueing_time = 0.0;
// back of last stopped vehicle (XXX does not check for continuous queue)
SUMOReal queueing_length = 0.0;
// back of last slow vehicle (XXX does not check for continuous queue)
SUMOReal queueing_length2 = 0.0;
const SUMOReal threshold_velocity = 5 / 3.6; // slow
if (!lane.empty()) {
for (MSLane::VehCont::const_iterator it_veh = lane.myVehicles.begin(); it_veh != lane.myVehicles.end(); ++it_veh) {
const MSVehicle& veh = **it_veh;
if (!veh.isOnRoad()) {
continue;
}
if (veh.getWaitingSeconds() > 0) {
queueing_time = MAX2(veh.getWaitingSeconds(), queueing_time);
const SUMOReal veh_back_to_lane_end = (lane.getLength() - veh.getPositionOnLane()) + veh.getVehicleType().getLength();
queueing_length = MAX2(veh_back_to_lane_end, queueing_length);
}
//Experimental
if (veh.getSpeed() < (threshold_velocity) && (veh.getPositionOnLane() > (veh.getLane()->getLength()) * 0.25)) {
const SUMOReal veh_back_to_lane_end = (lane.getLength() - veh.getPositionOnLane()) + veh.getVehicleType().getLength();
queueing_length2 = MAX2(veh_back_to_lane_end, queueing_length2);
}
}
}
//Output
if (queueing_length > 1 || queueing_length2 > 1) {
of.openTag("lane").writeAttr("id", lane.getID()).writeAttr("queueing_time", queueing_time).writeAttr("queueing_length", queueing_length);
of.writeAttr("queueing_length_experimental", queueing_length2).closeTag();
}
}
Position2D
NLGeomShapeBuilder::getPointPosition(SUMOReal x, SUMOReal y,
const std::string &laneID,
SUMOReal posOnLane) const throw(InvalidArgument) {
if (x!=INVALID_POSITION&&y!=INVALID_POSITION) {
return Position2D(x,y);
}
MSLane *lane = MSLane::dictionary(laneID);
if (lane==0) {
throw InvalidArgument("Lane '" + laneID + "' to place a poi on is not known.");
}
if (posOnLane<0) {
posOnLane = lane->getLength() + posOnLane;
}
return lane->getShape().positionAtLengthPosition(posOnLane);
}
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;
}
void
NLDetectorBuilder::buildInductLoop(const std::string& id,
const std::string& lane, SUMOReal pos, int splInterval,
const std::string& device, bool friendlyPos, bool splitByType) {
checkSampleInterval(splInterval, SUMO_TAG_E1DETECTOR, id);
// get and check the lane
MSLane* clane = getLaneChecking(lane, SUMO_TAG_E1DETECTOR, id);
if (!MSGlobals::gUseMesoSim) {
// get and check the position
pos = getPositionChecking(pos, clane, friendlyPos, id);
// build the loop
MSDetectorFileOutput* loop = createInductLoop(id, clane, pos, splitByType);
// add the file output
myNet.getDetectorControl().add(SUMO_TAG_INDUCTION_LOOP, loop, device, splInterval);
} else {
#ifdef HAVE_INTERNAL
if (pos < 0) {
pos = clane->getLength() + pos;
}
MESegment* s = MSGlobals::gMesoNet->getSegmentForEdge(clane->getEdge());
MESegment* prev = s;
SUMOReal cpos = 0;
while (cpos + prev->getLength() < pos && s != 0) {
prev = s;
cpos += s->getLength();
s = s->getNextSegment();
}
SUMOReal rpos = pos - cpos; //-prev->getLength();
if (rpos > prev->getLength() || rpos < 0) {
if (friendlyPos) {
rpos = prev->getLength() - (SUMOReal) 0.1;
} else {
throw InvalidArgument("The position of detector '" + id + "' lies beyond the lane's '" + lane + "' length.");
}
}
MEInductLoop* loop =
createMEInductLoop(id, prev, rpos);
myNet.getDetectorControl().add(SUMO_TAG_INDUCTION_LOOP, loop, device, splInterval);
#endif
}
}
void
MSActuatedTrafficLightLogic::init(NLDetectorBuilder& nb) {
MSTrafficLightLogic::init(nb);
assert(myLanes.size() > 0);
// change values for setting the loops and lanestate-detectors, here
//SUMOTime inductLoopInterval = 1; //
LaneVectorVector::const_iterator i2;
LaneVector::const_iterator i;
// build the induct loops
double maxDetectorGap = 0;
for (i2 = myLanes.begin(); i2 != myLanes.end(); ++i2) {
const LaneVector& lanes = *i2;
for (i = lanes.begin(); i != lanes.end(); i++) {
MSLane* lane = (*i);
if (noVehicles(lane->getPermissions())) {
// do not build detectors on green verges or sidewalks
continue;
}
double length = lane->getLength();
double speed = lane->getSpeedLimit();
double inductLoopPosition = myDetectorGap * speed;
// check whether the lane is long enough
double ilpos = length - inductLoopPosition;
if (ilpos < 0) {
ilpos = 0;
}
// Build the induct loop and set it into the container
std::string id = "TLS" + myID + "_" + myProgramID + "_InductLoopOn_" + lane->getID();
if (myInductLoops.find(lane) == myInductLoops.end()) {
myInductLoops[lane] = nb.createInductLoop(id, lane, ilpos, myVehicleTypes, myShowDetectors);
MSNet::getInstance()->getDetectorControl().add(SUMO_TAG_INDUCTION_LOOP, myInductLoops[lane], myFile, myFreq);
}
maxDetectorGap = MAX2(maxDetectorGap, length - ilpos);
}
}
// warn if the minGap is insufficient to clear vehicles between stop line and detector
SUMOTime minMinDur = getMinimumMinDuration();
if (floor(floor(maxDetectorGap / DEFAULT_LENGTH_WITH_GAP) * myPassingTime) > STEPS2TIME(minMinDur)) {
WRITE_WARNING("At actuated tlLogic '" + getID() + "', minDur " + time2string(minMinDur) + " is too short for a detector gap of " + toString(maxDetectorGap) + "m.");
}
}
void
NLDetectorBuilder::buildMsgDetector(const std::string &id,
const std::string &lane, SUMOReal pos, int splInterval,
const std::string &msg,
OutputDevice& device, bool friendlyPos) throw(InvalidArgument) {
if (splInterval<0) {
throw InvalidArgument("Negative sampling frequency (in e4-detector '" + id + "').");
}
if (splInterval==0) {
throw InvalidArgument("Sampling frequency must not be zero (in e4-detector '" + id + "').");
}
if (msg == "") {
throw InvalidArgument("No Message given (in e4-detector '" + id + "').");
}
MSLane *clane = getLaneChecking(lane, id);
if (pos<0) {
pos = clane->getLength() + pos;
}
pos = getPositionChecking(pos, clane, friendlyPos, id);
MSMsgInductLoop *msgloop = createMsgInductLoop(id, msg, clane, pos);
myNet.getDetectorControl().add(msgloop, device, splInterval);
}
// ===========================================================================
// 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;
}
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;
}
void
NLHandler::addConnection(const SUMOSAXAttributes& attrs) {
bool ok = true;
std::string fromID = attrs.get<std::string>(SUMO_ATTR_FROM, 0, ok);
if (!MSGlobals::gUsingInternalLanes && fromID[0] == ':') {
return;
}
try {
bool ok = true;
const std::string toID = attrs.get<std::string>(SUMO_ATTR_TO, 0, ok);
const int fromLaneIdx = attrs.get<int>(SUMO_ATTR_FROM_LANE, 0, ok);
const int toLaneIdx = attrs.get<int>(SUMO_ATTR_TO_LANE, 0, ok);
LinkDirection dir = parseLinkDir(attrs.get<std::string>(SUMO_ATTR_DIR, 0, ok));
LinkState state = parseLinkState(attrs.get<std::string>(SUMO_ATTR_STATE, 0, ok));
std::string tlID = attrs.getOpt<std::string>(SUMO_ATTR_TLID, 0, ok, "");
#ifdef HAVE_INTERNAL_LANES
std::string viaID = attrs.getOpt<std::string>(SUMO_ATTR_VIA, 0, ok, "");
#endif
MSEdge* from = MSEdge::dictionary(fromID);
if (from == 0) {
WRITE_ERROR("Unknown from-edge '" + fromID + "' in connection");
return;
}
MSEdge* to = MSEdge::dictionary(toID);
if (to == 0) {
WRITE_ERROR("Unknown to-edge '" + toID + "' in connection");
return;
}
if (fromLaneIdx < 0 || static_cast<unsigned int>(fromLaneIdx) >= from->getLanes().size() ||
toLaneIdx < 0 || static_cast<unsigned int>(toLaneIdx) >= to->getLanes().size()) {
WRITE_ERROR("Invalid lane index in connection from '" + from->getID() + "' to '" + to->getID() + "'.");
return;
}
MSLane* fromLane = from->getLanes()[fromLaneIdx];
MSLane* toLane = to->getLanes()[toLaneIdx];
assert(fromLane);
assert(toLane);
int tlLinkIdx = -1;
if (tlID != "") {
tlLinkIdx = attrs.get<int>(SUMO_ATTR_TLLINKINDEX, 0, ok);
// make sure that the index is in range
MSTrafficLightLogic* logic = myJunctionControlBuilder.getTLLogic(tlID).getActive();
if (tlLinkIdx < 0 || tlLinkIdx >= (int)logic->getCurrentPhaseDef().getState().size()) {
WRITE_ERROR("Invalid " + toString(SUMO_ATTR_TLLINKINDEX) + " '" + toString(tlLinkIdx) +
"' in connection controlled by '" + tlID + "'");
return;
}
if (!ok) {
return;
}
}
SUMOReal length = fromLane->getShape()[-1].distanceTo(toLane->getShape()[0]);
MSLink* link = 0;
// build the link
#ifdef HAVE_INTERNAL_LANES
MSLane* via = 0;
if (viaID != "" && MSGlobals::gUsingInternalLanes) {
via = MSLane::dictionary(viaID);
if (via == 0) {
WRITE_ERROR("An unknown lane ('" + viaID +
"') should be set as a via-lane for lane '" + toLane->getID() + "'.");
return;
}
length = via->getLength();
}
link = new MSLink(toLane, via, dir, state, length);
if (via != 0) {
via->addIncomingLane(fromLane, link);
} else {
toLane->addIncomingLane(fromLane, link);
}
#else
link = new MSLink(toLane, dir, state, length);
toLane->addIncomingLane(fromLane, link);
#endif
toLane->addApproachingLane(fromLane);
// if a traffic light is responsible for it, inform the traffic light
// check whether this link is controlled by a traffic light
if (tlID != "") {
myJunctionControlBuilder.getTLLogic(tlID).addLink(link, fromLane, tlLinkIdx);
}
// add the link
fromLane->addLink(link);
} catch (InvalidArgument& e) {
WRITE_ERROR(e.what());
}
}
void
MS_E2_ZS_CollectorOverLanes::extendTo(SUMOReal length) throw() {
bool done = false;
while (!done) {
done = true;
LengthVector::iterator leni = myLengths.begin();
LaneVectorVector::iterator lanei = myLaneCombinations.begin();
DetectorVectorVector::iterator deti = myDetectorCombinations.begin();
for (; leni!=myLengths.end(); leni++, lanei++, deti++) {
if ((*leni)<length) {
done = false;
// copy current values
LaneVector lv = *lanei;
DetectorVector dv = *deti;
SUMOReal clength = *leni;
assert(lv.size()>0);
assert(dv.size()>0);
// erase previous elements
assert(leni!=myLengths.end());
myLengths.erase(leni);
myLaneCombinations.erase(lanei);
myDetectorCombinations.erase(deti);
// get the lane to look before
MSLane *toExtend = lv.back();
// and her predecessors
std::vector<MSLane*> predeccessors = getLanePredeccessorLanes(toExtend);
if (predeccessors.size()==0) {
int off = 1;
MSEdge &e = toExtend->getEdge();
const std::vector<MSLane*> &lanes = e.getLanes();
int idx = (int) distance(lanes.begin(), find(lanes.begin(), lanes.end(), toExtend));
while (predeccessors.size()==0) {
if (idx-off>=0) {
MSLane *tryMe = lanes[idx-off];
predeccessors = getLanePredeccessorLanes(tryMe);
}
if (predeccessors.size()==0&&idx+off<(int) lanes.size()) {
MSLane *tryMe = lanes[idx+off];
predeccessors = getLanePredeccessorLanes(tryMe);
}
off++;
}
}
/* LaneContinuations::const_iterator conts =
laneContinuations.find(toExtend->id());
assert(conts!=laneContinuations.end());
const std::vector<std::string> &predeccessors =
(*conts).second;*/
// go through the predeccessors and extend the detector
for (std::vector<MSLane*>::const_iterator i=predeccessors.begin(); i!=predeccessors.end(); i++) {
// get the lane
MSLane *l = *i;
// compute detector length
SUMOReal lanelen = length - clength;
if (lanelen>l->getLength()) {
lanelen = l->getLength() - (SUMOReal) 0.2;
}
// build new info
LaneVector nlv = lv;
nlv.push_back(l);
DetectorVector ndv = dv;
MSE2Collector *coll = 0;
if (myAlreadyBuild.find(l)==myAlreadyBuild.end()) {
coll = buildCollector(0, 0, l, (SUMOReal) 0.1, lanelen);
} else {
coll = myAlreadyBuild.find(l)->second;
}
myAlreadyBuild[l] = coll;
ndv.push_back(coll);
// store new info
myLaneCombinations.push_back(nlv);
myDetectorCombinations.push_back(ndv);
myLengths.push_back(clength+lanelen);
}
// restart
leni = myLengths.end() - 1;
}
}
}
}
// ===========================================================================
// method definitions
// ===========================================================================
bool
TraCIServerAPI_Lane::processGet(TraCIServer& server, tcpip::Storage& inputStorage,
tcpip::Storage& outputStorage) {
// 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 != VAR_WAITING_TIME
&& 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 && variable != VAR_WIDTH && variable != ID_COUNT
) {
return server.writeErrorStatusCmd(CMD_GET_LANE_VARIABLE, "Get Lane Variable: unsupported variable specified", outputStorage);
}
// begin response building
tcpip::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 if (variable == ID_COUNT) {
std::vector<std::string> ids;
MSLane::insertIDs(ids);
tempMsg.writeUnsignedByte(TYPE_INTEGER);
tempMsg.writeInt((int) ids.size());
} else {
MSLane* lane = MSLane::dictionary(id);
if (lane == 0) {
return server.writeErrorStatusCmd(CMD_GET_LANE_VARIABLE, "Lane '" + id + "' is not known", outputStorage);
}
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_DOUBLE);
tempMsg.writeDouble(lane->getLength());
break;
case VAR_MAXSPEED:
tempMsg.writeUnsignedByte(TYPE_DOUBLE);
tempMsg.writeDouble(lane->getSpeedLimit());
break;
case LANE_LINKS: {
tempMsg.writeUnsignedByte(TYPE_COMPOUND);
tcpip::Storage tempContent;
unsigned int cnt = 0;
tempContent.writeUnsignedByte(TYPE_INTEGER);
const MSLinkCont& links = lane->getLinkCont();
tempContent.writeInt((int) links.size());
++cnt;
const SUMOTime currTime = MSNet::getInstance()->getCurrentTimeStep();
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);
const SUMOReal speed = MIN2(lane->getSpeedLimit(), link->getLane()->getSpeedLimit());
tempContent.writeUnsignedByte(link->opened(currTime, speed, speed, DEFAULT_VEH_LENGTH, 0.0, DEFAULT_VEH_DECEL, 0) ? 1 : 0);
++cnt;
// approaching foe
tempContent.writeUnsignedByte(TYPE_UBYTE);
tempContent.writeUnsignedByte(link->hasApproachingFoe(currTime, currTime, 0) ? 1 : 0);
++cnt;
// state (not implemented, yet)
tempContent.writeUnsignedByte(TYPE_STRING);
tempContent.writeString(SUMOXMLDefinitions::LinkStates.getString(link->getState()));
++cnt;
// direction
tempContent.writeUnsignedByte(TYPE_STRING);
tempContent.writeString(SUMOXMLDefinitions::LinkDirections.getString(link->getDirection()));
++cnt;
// length
tempContent.writeUnsignedByte(TYPE_DOUBLE);
tempContent.writeDouble(link->getLength());
++cnt;
}
tempMsg.writeInt((int) cnt);
tempMsg.writeStorage(tempContent);
}
break;
case LANE_ALLOWED: {
tempMsg.writeUnsignedByte(TYPE_STRINGLIST);
SVCPermissions permissions = lane->getPermissions();
if (permissions == SVCFreeForAll) { // special case: write nothing
permissions = 0;
}
tempMsg.writeStringList(getAllowedVehicleClassNamesList(permissions));
}
case LANE_DISALLOWED: {
tempMsg.writeUnsignedByte(TYPE_STRINGLIST);
tempMsg.writeStringList(getAllowedVehicleClassNamesList(~(lane->getPermissions()))); // negation yields disallowed
}
break;
case VAR_SHAPE:
tempMsg.writeUnsignedByte(TYPE_POLYGON);
tempMsg.writeUnsignedByte((int)MIN2(static_cast<size_t>(255), lane->getShape().size()));
for (unsigned int iPoint = 0; iPoint < MIN2(static_cast<size_t>(255), lane->getShape().size()); ++iPoint) {
tempMsg.writeDouble(lane->getShape()[iPoint].x());
tempMsg.writeDouble(lane->getShape()[iPoint].y());
}
break;
case VAR_CO2EMISSION:
tempMsg.writeUnsignedByte(TYPE_DOUBLE);
tempMsg.writeDouble(lane->getCO2Emissions());
break;
case VAR_COEMISSION:
tempMsg.writeUnsignedByte(TYPE_DOUBLE);
tempMsg.writeDouble(lane->getCOEmissions());
break;
case VAR_HCEMISSION:
tempMsg.writeUnsignedByte(TYPE_DOUBLE);
tempMsg.writeDouble(lane->getHCEmissions());
break;
case VAR_PMXEMISSION:
tempMsg.writeUnsignedByte(TYPE_DOUBLE);
tempMsg.writeDouble(lane->getPMxEmissions());
break;
case VAR_NOXEMISSION:
tempMsg.writeUnsignedByte(TYPE_DOUBLE);
tempMsg.writeDouble(lane->getNOxEmissions());
break;
case VAR_FUELCONSUMPTION:
tempMsg.writeUnsignedByte(TYPE_DOUBLE);
tempMsg.writeDouble(lane->getFuelConsumption());
break;
case VAR_NOISEEMISSION:
tempMsg.writeUnsignedByte(TYPE_DOUBLE);
tempMsg.writeDouble(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_DOUBLE);
tempMsg.writeDouble(lane->getMeanSpeed());
break;
case LAST_STEP_VEHICLE_ID_LIST: {
std::vector<std::string> vehIDs;
const MSLane::VehCont& vehs = lane->getVehiclesSecure();
for (MSLane::VehCont::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_DOUBLE);
tempMsg.writeDouble(lane->getNettoOccupancy());
break;
case LAST_STEP_VEHICLE_HALTING_NUMBER: {
int halting = 0;
const MSLane::VehCont& vehs = lane->getVehiclesSecure();
for (MSLane::VehCont::const_iterator j = vehs.begin(); j != vehs.end(); ++j) {
if ((*j)->getSpeed() < SUMO_const_haltingSpeed) {
++halting;
}
}
lane->releaseVehicles();
tempMsg.writeUnsignedByte(TYPE_INTEGER);
tempMsg.writeInt(halting);
}
break;
case LAST_STEP_LENGTH: {
SUMOReal lengthSum = 0;
const MSLane::VehCont& vehs = lane->getVehiclesSecure();
for (MSLane::VehCont::const_iterator j = vehs.begin(); j != vehs.end(); ++j) {
lengthSum += (*j)->getVehicleType().getLength();
}
tempMsg.writeUnsignedByte(TYPE_DOUBLE);
if (vehs.size() == 0) {
tempMsg.writeDouble(0);
} else {
tempMsg.writeDouble(lengthSum / (SUMOReal) vehs.size());
}
lane->releaseVehicles();
}
break;
case VAR_WAITING_TIME: {
tempMsg.writeUnsignedByte(TYPE_DOUBLE);
tempMsg.writeDouble(lane->getWaitingSeconds());
}
break;
case VAR_CURRENT_TRAVELTIME: {
SUMOReal meanSpeed = lane->getMeanSpeed();
tempMsg.writeUnsignedByte(TYPE_DOUBLE);
if (meanSpeed != 0) {
tempMsg.writeDouble(lane->getLength() / meanSpeed);
} else {
tempMsg.writeDouble(1000000.);
}
}
break;
case VAR_WIDTH:
tempMsg.writeUnsignedByte(TYPE_DOUBLE);
tempMsg.writeDouble(lane->getWidth());
break;
default:
break;
}
}
server.writeStatusCmd(CMD_GET_LANE_VARIABLE, RTYPE_OK, "", outputStorage);
server.writeResponseWithLength(outputStorage, tempMsg);
return true;
}
// ===========================================================================
// method definitions
// ===========================================================================
MSCalibrator::MSCalibrator(const std::string& id,
const MSEdge* const edge,
MSLane* lane,
const double pos,
const std::string& aXMLFilename,
const std::string& outputFilename,
const SUMOTime freq, const double length,
const MSRouteProbe* probe,
bool addLaneMeanData) :
MSTrigger(id),
MSRouteHandler(aXMLFilename, true),
myEdge(edge),
myLane(lane),
myPos(pos), myProbe(probe),
myEdgeMeanData(nullptr, length, false, nullptr),
myCurrentStateInterval(myIntervals.begin()),
myOutput(nullptr), myFrequency(freq), myRemoved(0),
myInserted(0), myClearedInJam(0),
mySpeedIsDefault(true), myDidSpeedAdaption(false), myDidInit(false),
myDefaultSpeed(myLane == nullptr ? myEdge->getSpeedLimit() : myLane->getSpeedLimit()),
myHaveWarnedAboutClearingJam(false),
myAmActive(false) {
if (outputFilename != "") {
myOutput = &OutputDevice::getDevice(outputFilename);
myOutput->writeXMLHeader("calibratorstats", "calibratorstats_file.xsd");
}
if (aXMLFilename != "") {
XMLSubSys::runParser(*this, aXMLFilename);
if (!myDidInit) {
init();
}
}
if (addLaneMeanData) {
// disabled for METriggeredCalibrator
for (int i = 0; i < (int)myEdge->getLanes().size(); ++i) {
MSLane* lane = myEdge->getLanes()[i];
if (myLane == nullptr || myLane == lane) {
//std::cout << " cali=" << getID() << " myLane=" << Named::getIDSecure(myLane) << " checkLane=" << i << "\n";
MSMeanData_Net::MSLaneMeanDataValues* laneData = new MSMeanData_Net::MSLaneMeanDataValues(lane, lane->getLength(), true, nullptr);
laneData->setDescription("meandata_calibrator_" + lane->getID());
LeftoverReminders.push_back(laneData);
myLaneMeanData.push_back(laneData);
VehicleRemover* remover = new VehicleRemover(lane, (int)i, this);
LeftoverReminders.push_back(remover);
myVehicleRemovers.push_back(remover);
}
}
}
}
void
MSQueueExport::writeLane(OutputDevice& of, const MSLane& lane) {
//Fahrzeug mit der höchsten Wartezeit
//Fahrzeug am Ende des Rückstaus
double queueing_time = 0.0;
double queueing_length = 0.0;
double queueing_length2 = 0.0;
if (lane.getVehicleNumber() != 0) {
for (std::vector<MSVehicle*>::const_iterator veh = lane.myVehBuffer.begin(); veh != lane.myVehBuffer.end(); ++veh) {
const MSVehicle& veh_tmp = **veh;
if (veh_tmp.isOnRoad()) {
if (veh_tmp.getWaitingSeconds() > 0) {
if (veh_tmp.getWaitingSeconds() > queueing_time) {
queueing_time = veh_tmp.getWaitingSeconds();
}
double tmp_length = (lane.getLength() - veh_tmp.getPositionOnLane()) + veh_tmp.getVehicleType().getLengthWithGap();
if (tmp_length > queueing_length) {
queueing_length = tmp_length;
}
}
}
}
for (MSLane::VehCont::const_iterator veh = lane.myVehicles.begin(); veh != lane.myVehicles.end(); ++veh) {
const MSVehicle& veh_tmp = **veh;
if (veh_tmp.isOnRoad()) {
if (veh_tmp.getWaitingSeconds() > 0) {
if (veh_tmp.getWaitingSeconds() > queueing_time) {
queueing_time = veh_tmp.getWaitingSeconds();
}
double tmp_length = (lane.getLength() - veh_tmp.getPositionOnLane()) + veh_tmp.getVehicleType().getLengthWithGap();
if (tmp_length > queueing_length) {
queueing_length = tmp_length;
}
}
}
}
//Experimental
double tmp_length2 = 0.0;
for (MSLane::VehCont::const_iterator veh = lane.myVehicles.begin(); veh != lane.myVehicles.end(); ++veh) {
//wenn Fahrzeug langsamer als 5 km/h fährt = Rückstau
double threshold_velocity = 5 / 3.6;
const MSVehicle& veh_tmp = **veh;
if (veh_tmp.isOnRoad()) {
if (veh_tmp.getSpeed() < (threshold_velocity) && (veh_tmp.getPositionOnLane() > (veh_tmp.getLane()->getLength()) * 0.25))
{
tmp_length2 = (lane.getLength() - veh_tmp.getPositionOnLane()) + veh_tmp.getVehicleType().getLengthWithGap();
}
if (tmp_length2 > queueing_length2) {
queueing_length2 = tmp_length2;
}
}
}
}
//Output
if (queueing_length > 1 || queueing_length2 > 1) {
of.openTag("lane") << " id=\"" << lane.getID() << "\"";
of << " queueing_time=\"" << queueing_time << "\" queueing_length=\"" << queueing_length << "\" queueing_length_experimental=\"" << queueing_length2 << "\"";
of.closeTag(true);
}
}
void
NLSucceedingLaneBuilder::addSuccLane(bool yield, const std::string &laneId,
#ifdef HAVE_INTERNAL_LANES
const std::string &viaID,
SUMOReal pass,
#endif
MSLink::LinkDirection dir,
MSLink::LinkState state,
bool internalEnd,
const std::string &tlid, unsigned int linkNo) throw(InvalidArgument) {
// check whether the link is a dead link
if (laneId=="SUMO_NO_DESTINATION") {
// build the dead link and add it to the container
#ifdef HAVE_INTERNAL_LANES
MSLink *link = new MSLink(0, 0, yield, MSLink::LINKDIR_NODIR, MSLink::LINKSTATE_DEADEND, false, 0.);
#else
MSLink *link = new MSLink(0, yield, MSLink::LINKDIR_NODIR, MSLink::LINKSTATE_DEADEND, 0.);
#endif
mySuccLanes->push_back(link);
if (tlid!="") {
MSTLLogicControl::TLSLogicVariants &logics = myJunctionControlBuilder.getTLLogic(tlid);
MSLane *current = MSLane::dictionary(myCurrentLane);
if (current==0) {
throw InvalidArgument("An unknown lane ('" + myCurrentLane + "') should be assigned to a tl-logic.");
}
logics.addLink(link, current, linkNo);
}
return;
}
// get the lane the link belongs to
MSLane *lane = MSLane::dictionary(laneId);
if (lane==0) {
throw InvalidArgument("An unknown lane ('" + laneId + "') should be set as a follower for lane '" + myCurrentLane + "'.");
}
#ifdef HAVE_INTERNAL_LANES
MSLane *via = 0;
if (viaID!="" && MSGlobals::gUsingInternalLanes) {
via = MSLane::dictionary(viaID);
if (via==0) {
throw InvalidArgument("An unknown lane ('" + viaID + "') should be set as a via-lane for lane '" + myCurrentLane + "'.");
}
}
if (pass>=0) {
static_cast<MSInternalLane*>(lane)->setPassPosition(pass);
}
#endif
MSLane *orig = MSLane::dictionary(myCurrentLane);
if (orig==0) {
return;
}
// build the link
SUMOReal length = orig!=0&&lane!=0
? orig->getShape()[-1].distanceTo(lane->getShape()[0])
: 0;
#ifdef HAVE_INTERNAL_LANES
if (via!=0) {
length = via->getLength();
}
MSLink *link = new MSLink(lane, via, yield, dir, state, internalEnd, length);
#else
MSLink *link = new MSLink(lane, yield, dir, state, length);
#endif
if (MSLane::dictionary(myCurrentLane)!=0) {
#ifdef HAVE_INTERNAL_LANES
if (via!=0) {
// from a normal in to a normal out via
// --> via incomes in out
lane->addIncomingLane(via, link);
// --> in incomes in via
via->addIncomingLane(MSLane::dictionary(myCurrentLane), link);
} else {
if (myCurrentLane[0]!=':') {
// internal not wished; other case already set
lane->addIncomingLane(MSLane::dictionary(myCurrentLane), link);
}
}
#else
lane->addIncomingLane(MSLane::dictionary(myCurrentLane), link);
#endif
}
// if a traffic light is responsible for it, inform the traffic light
// check whether this link is controlled by a traffic light
if (tlid!="") {
MSTLLogicControl::TLSLogicVariants &logics = myJunctionControlBuilder.getTLLogic(tlid);
MSLane *current = MSLane::dictionary(myCurrentLane);
if (current==0) {
throw InvalidArgument("An unknown lane ('" + myCurrentLane + "') should be assigned to a tl-logic.");
}
logics.addLink(link, current, linkNo);
}
// add the link to the container
mySuccLanes->push_back(link);
}
void
GUIVehicle::drawAction_drawCarriageClass(const GUIVisualizationSettings& s, bool asImage) const {
RGBColor current = GLHelper::getColor();
RGBColor darker = current.changedBrightness(-51);
const double exaggeration = s.vehicleSize.getExaggeration(s, this);
const double totalLength = getVType().getLength();
double upscaleLength = exaggeration;
if (exaggeration > 1 && totalLength > 5) {
// reduce the length/width ratio because this is not usefull at high zoom
upscaleLength = MAX2(1.0, upscaleLength * (5 + sqrt(totalLength - 5)) / totalLength);
}
const double locomotiveLength = getVehicleType().getParameter().locomotiveLength * upscaleLength;
if (exaggeration == 0) {
return;
}
const double defaultLength = getVehicleType().getParameter().carriageLength * upscaleLength;
const double carriageGap = getVehicleType().getParameter().carriageGap * upscaleLength;
const double length = totalLength * upscaleLength;
const double halfWidth = getVehicleType().getWidth() / 2.0 * exaggeration;
glPopMatrix(); // undo initial translation and rotation
const double xCornerCut = 0.3 * exaggeration;
const double yCornerCut = 0.4 * exaggeration;
// round to closest integer
const int numCarriages = MAX2(1, 1 + (int)((length - locomotiveLength) / (defaultLength + carriageGap) + 0.5));
assert(numCarriages > 0);
double carriageLengthWithGap = length / numCarriages;
double carriageLength = carriageLengthWithGap - carriageGap;
double firstCarriageLength = carriageLength;
if (defaultLength != locomotiveLength && numCarriages > 1) {
firstCarriageLength = locomotiveLength;
carriageLengthWithGap = (length - locomotiveLength) / (numCarriages - 1);
carriageLength = carriageLengthWithGap - carriageGap;
}
const int firstPassengerCarriage = defaultLength == locomotiveLength || numCarriages == 1 ? 0 : 1;
const int totalSeats = getVType().getPersonCapacity() + getVType().getContainerCapacity();
const int seatsPerCarriage = (int)ceil(totalSeats / (numCarriages - firstPassengerCarriage));
// lane on which the carriage front is situated
MSLane* lane = myLane;
int routeIndex = getRoutePosition();
// lane on which the carriage back is situated
MSLane* backLane = myLane;
int backRouteIndex = routeIndex;
// offsets of front and back
double carriageOffset = myState.pos();
double carriageBackOffset = myState.pos() - firstCarriageLength;
// handle seats
int requiredSeats = getNumPassengers() + getNumContainers();
if (requiredSeats > 0) {
mySeatPositions.clear();
}
Position front, back;
double angle = 0.;
// draw individual carriages
double curCLength = firstCarriageLength;
//std::cout << SIMTIME << " veh=" << getID() << " curCLength=" << curCLength << " loc=" << locomotiveLength << " car=" << carriageLength << " tlen=" << totalLength << " len=" << length << "\n";
for (int i = 0; i < numCarriages; ++i) {
if (i > 0) {
curCLength = carriageLength;
}
while (carriageOffset < 0) {
MSLane* prev = getPreviousLane(lane, routeIndex);
if (prev != lane) {
carriageOffset += prev->getLength();
} else {
// no lane available for drawing.
carriageOffset = 0;
}
lane = prev;
}
while (carriageBackOffset < 0) {
MSLane* prev = getPreviousLane(backLane, backRouteIndex);
if (prev != backLane) {
carriageBackOffset += prev->getLength();
} else {
// no lane available for drawing.
carriageBackOffset = 0;
}
backLane = prev;
}
front = lane->geometryPositionAtOffset(carriageOffset);
back = backLane->geometryPositionAtOffset(carriageBackOffset);
if (front == back) {
// no place for drawing available
continue;
}
const double drawnCarriageLength = front.distanceTo2D(back);
angle = atan2((front.x() - back.x()), (back.y() - front.y())) * (double) 180.0 / (double) M_PI;
if (i >= firstPassengerCarriage) {
computeSeats(front, back, seatsPerCarriage, exaggeration, requiredSeats);
}
glPushMatrix();
glTranslated(front.x(), front.y(), getType());
glRotated(angle, 0, 0, 1);
if (!asImage || !GUIBaseVehicleHelper::drawAction_drawVehicleAsImage(s, getVType().getImgFile(), this, getVType().getWidth(), curCLength)) {
switch (getVType().getGuiShape()) {
case SVS_TRUCK_SEMITRAILER:
case SVS_TRUCK_1TRAILER:
if (i == 0) {
GUIBaseVehicleHelper::drawAction_drawVehicleAsPoly(s, getVType().getGuiShape(), getVType().getWidth() * exaggeration, curCLength, i);
} else {
GLHelper::setColor(current);
GLHelper::drawBoxLine(Position(0, 0), 180, curCLength, halfWidth);
}
break;
default: {
if (i == 0) {
GLHelper::setColor(darker);
} else {
GLHelper::setColor(current);
}
// generic rail carriage
glBegin(GL_TRIANGLE_FAN);
glVertex2d(-halfWidth + xCornerCut, 0);
glVertex2d(-halfWidth, yCornerCut);
glVertex2d(-halfWidth, drawnCarriageLength - yCornerCut);
glVertex2d(-halfWidth + xCornerCut, drawnCarriageLength);
glVertex2d(halfWidth - xCornerCut, drawnCarriageLength);
glVertex2d(halfWidth, drawnCarriageLength - yCornerCut);
glVertex2d(halfWidth, yCornerCut);
glVertex2d(halfWidth - xCornerCut, 0);
glEnd();
}
}
}
glPopMatrix();
carriageOffset -= (curCLength + carriageGap);
carriageBackOffset -= carriageLengthWithGap;
}
if (getVType().getGuiShape() == SVS_RAIL_CAR) {
glPushMatrix();
glTranslated(front.x(), front.y(), getType());
glRotated(angle, 0, 0, 1);
drawAction_drawVehicleBlinker(curCLength);
drawAction_drawVehicleBrakeLight(curCLength);
glPopMatrix();
}
// restore matrix
glPushMatrix();
front = getPosition();
glTranslated(front.x(), front.y(), getType());
const double degAngle = RAD2DEG(getAngle() + M_PI / 2.);
glRotated(degAngle, 0, 0, 1);
glScaled(exaggeration, upscaleLength, 1);
if (mySeatPositions.size() == 0) {
mySeatPositions.push_back(back);
}
}
void
NLDetectorBuilder::buildE2Detector(const std::string& id, std::vector<MSLane*> lanes, double pos, double endPos,
const std::string& device, SUMOTime frequency,
SUMOTime haltingTimeThreshold, double haltingSpeedThreshold, double jamDistThreshold,
const std::string& vTypes, bool friendlyPos, bool showDetector,
MSTLLogicControl::TLSLogicVariants* tlls, MSLane* toLane) {
bool tlsGiven = tlls != 0;
bool toLaneGiven = toLane != 0;
assert(pos != std::numeric_limits<double>::max());
assert(endPos != std::numeric_limits<double>::max());
assert(lanes.size() != 0);
MSLane* firstLane = lanes[0];
MSLane* lastLane = lanes[lanes.size() - 1];
// Check positioning
if (pos >= firstLane->getLength() || (pos < 0 && -pos > firstLane->getLength())) {
std::stringstream ss;
ss << "The given position (=" << pos << ") for detector '" << id
<< "' does not lie on the given lane '" << firstLane->getID()
<< "' with length " << firstLane->getLength();
if (friendlyPos) {
double newPos = pos > 0 ? firstLane->getLength() - POSITION_EPS : 0.;
ss << " (adjusting to new position " << newPos;
WRITE_WARNING(ss.str());
pos = newPos;
} else {
ss << " (0 <= pos < lane->getLength() is required)";
throw InvalidArgument(ss.str());
}
}
if (endPos > lastLane->getLength() || (endPos <= 0 && -endPos >= lastLane->getLength())) {
std::stringstream ss;
ss << "The given end position (=" << endPos << ") for detector '" << id
<< "' does not lie on the given lane '" << lastLane->getID()
<< "' with length " << lastLane->getLength();
if (friendlyPos) {
double newEndPos = endPos > 0 ? lastLane->getLength() : POSITION_EPS;
ss << " (adjusting to new position " << newEndPos;
WRITE_WARNING(ss.str());
pos = newEndPos;
} else {
ss << " (0 <= pos < lane->getLength() is required)";
throw InvalidArgument(ss.str());
}
}
MSE2Collector* det = 0;
if (tlsGiven) {
// Detector connected to TLS
det = createE2Detector(id, DU_USER_DEFINED, lanes, pos, endPos, haltingTimeThreshold, haltingSpeedThreshold, jamDistThreshold, vTypes, showDetector);
myNet.getDetectorControl().add(SUMO_TAG_LANE_AREA_DETECTOR, det);
// add the file output (XXX: Where's the corresponding delete?)
if (toLaneGiven) {
// Detector also associated to specific link
MSLane* lastLane = det->getLastLane();
MSLink* link = MSLinkContHelper::getConnectingLink(*lastLane, *toLane);
if (link == 0) {
throw InvalidArgument(
"The detector '" + id + "' cannot be build as no connection between lanes '"
+ lastLane->getID() + "' and '" + toLane->getID() + "' exists.");
}
new Command_SaveTLCoupledLaneDet(*tlls, det, myNet.getCurrentTimeStep(), OutputDevice::getDevice(device), link);
} else {
// detector for tls but without specific link
new Command_SaveTLCoupledDet(*tlls, det, myNet.getCurrentTimeStep(), OutputDevice::getDevice(device));
}
} else {
// User specified detector for xml-output
checkSampleInterval(frequency, SUMO_TAG_E2DETECTOR, id);
det = createE2Detector(id, DU_USER_DEFINED, lanes, pos, endPos, haltingTimeThreshold, haltingSpeedThreshold, jamDistThreshold, vTypes, showDetector);
myNet.getDetectorControl().add(SUMO_TAG_LANE_AREA_DETECTOR, det, device, frequency);
}
}
bool
MSLaneChanger::changeOpposite(std::pair<MSVehicle*, SUMOReal> leader) {
if (!myChangeToOpposite) {
return false;
}
myCandi = findCandidate();
MSVehicle* vehicle = veh(myCandi);
MSLane* source = vehicle->getLane();
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();
if (!isOpposite && leader.first == 0) {
// 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 (!source->getEdge().canChangeToOpposite()) {
return false;
}
MSLane* opposite = source->getOpposite();
if (opposite == 0) {
return false;
}
// changing into the opposite direction is always to the left (XXX except for left-hand networkds)
int direction = vehicle->getLaneChangeModel().isOpposite() ? -1 : 1;
std::pair<MSVehicle*, SUMOReal> neighLead((MSVehicle*)0, -1);
// preliminary sanity checks for overtaking space
if (!isOpposite) {
assert(leader.first != 0);
// find a leader vehicle with sufficient space ahead for merging back
const SUMOReal overtakingSpeed = source->getVehicleMaxSpeed(vehicle); // just a guess
const SUMOReal mergeBrakeGap = vehicle->getCarFollowModel().brakeGap(overtakingSpeed);
const SUMOReal maxLookAhead = 150; // just a guess
std::pair<MSVehicle*, SUMOReal> columnLeader = leader;
SUMOReal egoGap = leader.second;
bool foundSpaceAhead = false;
SUMOReal seen = leader.second + leader.first->getVehicleType().getLengthWithGap();
std::vector<MSLane*> conts = vehicle->getBestLanesContinuation();
while (!foundSpaceAhead) {
const SUMOReal requiredSpaceAfterLeader = (columnLeader.first->getCarFollowModel().getSecureGap(
columnLeader.first->getSpeed(), overtakingSpeed, vehicle->getCarFollowModel().getMaxDecel())
+ vehicle->getVehicleType().getLengthWithGap());
std::pair<MSVehicle* const, SUMOReal> leadLead = columnLeader.first->getLane()->getLeader(
columnLeader.first, columnLeader.first->getPositionOnLane(), conts, requiredSpaceAfterLeader + mergeBrakeGap, true);
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " leadLead=" << Named::getIDSecure(leadLead.first) << " gap=" << leadLead.second << "\n";
}
#endif
if (leadLead.first == 0) {
foundSpaceAhead = true;
} else {
const SUMOReal requiredSpace = (requiredSpaceAfterLeader
+ vehicle->getCarFollowModel().getSecureGap(overtakingSpeed, leadLead.first->getSpeed(), leadLead.first->getCarFollowModel().getMaxDecel()));
if (leadLead.second > requiredSpace) {
foundSpaceAhead = true;
} else {
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " not enough space after columnLeader=" << leadLead.first->getID() << " gap=" << leadLead.second << " required=" << requiredSpace << "\n";
}
#endif
seen += 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=" << leadLead.first->getID() << ")\n";
}
#endif
return false;
}
// 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
}
}
}
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " compute time/space to overtake for columnLeader=" << columnLeader.first->getID() << " gap=" << columnLeader.second << "\n";
}
#endif
SUMOReal timeToOvertake;
SUMOReal spaceToOvertake;
computeOvertakingTime(vehicle, columnLeader.first, egoGap, timeToOvertake, spaceToOvertake);
// 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;
}
neighLead = opposite->getOppositeLeader(vehicle, timeToOvertake * opposite->getSpeedLimit() * 2 + spaceToOvertake);
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << SIMTIME
<< " veh=" << vehicle->getID()
<< " changeOpposite opposite=" << opposite->getID()
<< " lead=" << Named::getIDSecure(leader.first)
<< " oncoming=" << Named::getIDSecure(neighLead.first)
<< " timeToOvertake=" << timeToOvertake
<< " spaceToOvertake=" << spaceToOvertake
<< "\n";
}
#endif
// check for dangerous oncoming leader
if (!vehicle->getLaneChangeModel().isOpposite() && neighLead.first != 0) {
const MSVehicle* oncoming = neighLead.first;
/// XXX what about overtaking multiple vehicles?
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << SIMTIME
<< " timeToOvertake=" << timeToOvertake
<< " spaceToOvertake=" << spaceToOvertake
<< " oncomingGap=" << neighLead.second
<< " leaderGap=" << leader.second
<< "\n";
}
#endif
if (neighLead.second - spaceToOvertake - timeToOvertake * oncoming->getSpeed() < 0) {
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " cannot changeOpposite due to dangerous oncoming\n";
}
#endif
return false;
}
}
// check for sufficient space on the opposite side
seen = source->getLength() - vehicle->getPositionOnLane();
if (!vehicle->getLaneChangeModel().isOpposite() && seen < spaceToOvertake) {
const std::vector<MSLane*>& bestLaneConts = vehicle->getBestLanesContinuation();
assert(bestLaneConts.size() >= 1);
std::vector<MSLane*>::const_iterator it = bestLaneConts.begin() + 1;
while (seen < spaceToOvertake && it != bestLaneConts.end()) {
if ((*it)->getOpposite() == 0) {
break;
}
// do not overtake past a minor link
if (*(it - 1) != 0) {
MSLink* link = MSLinkContHelper::getConnectingLink(**(it - 1), **it);
if (link == 0 || !link->havePriority() || link->getState() == LINKSTATE_ZIPPER) {
break;
}
}
seen += (*it)->getLength();
}
if (seen < spaceToOvertake) {
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " cannot changeOpposite due to insufficient space (seen=" << seen << " spaceToOvertake=" << spaceToOvertake << ")\n";
}
#endif
return false;
}
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " seen=" << seen << " spaceToOvertake=" << spaceToOvertake << " timeToOvertake=" << timeToOvertake << "\n";
}
#endif
}
} else {
/// XXX compute sensible distance
leader = source->getOppositeLeader(vehicle, 200);
neighLead = opposite->getOppositeLeader(vehicle, -1);
}
// compute wish to change
std::vector<MSVehicle::LaneQ> preb = vehicle->getBestLanes();
if (isOpposite && leader.first != 0) {
MSVehicle::LaneQ& laneQ = preb[preb.size() - 1];
/// XXX compute sensible usable dist
laneQ.length -= MIN2(laneQ.length, opposite->getOppositePos(vehicle->getPositionOnLane()) + leader.second / 2);
leader.first = 0; // ignore leader
}
std::pair<MSVehicle* const, SUMOReal> neighFollow = opposite->getOppositeFollower(vehicle);
int state = checkChange(direction, opposite, leader, neighLead, neighFollow, preb);
bool changingAllowed = (state & LCA_BLOCKED) == 0;
// change if the vehicle wants to and is allowed to change
if ((state & LCA_WANTS_LANECHANGE) != 0 && changingAllowed) {
vehicle->getLaneChangeModel().startLaneChangeManeuver(source, opposite, direction);
/// XXX use a dedicated transformation function
vehicle->myState.myPos = source->getOppositePos(vehicle->myState.myPos);
vehicle->myState.myBackPos = source->getOppositePos(vehicle->myState.myBackPos);
/// XXX compute a bette lateral position
opposite->forceVehicleInsertion(vehicle, vehicle->getPositionOnLane(), MSMoveReminder::NOTIFICATION_LANE_CHANGE, 0);
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << SIMTIME << " changing to opposite veh=" << vehicle->getID() << " dir=" << direction << " opposite=" << Named::getIDSecure(opposite) << " state=" << state << "\n";
}
#endif
return true;
}
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << SIMTIME << " not changing to opposite veh=" << vehicle->getID() << " dir=" << direction << " opposite=" << Named::getIDSecure(opposite) << " state=" << state << "\n";
}
#endif
return false;
}
int
MSLaneChanger::checkChange(
int laneOffset,
const MSLane* targetLane,
const std::pair<MSVehicle* const, SUMOReal>& leader,
const std::pair<MSVehicle* const, SUMOReal>& neighLead,
const std::pair<MSVehicle* const, SUMOReal>& neighFollow,
const std::vector<MSVehicle::LaneQ>& preb) const {
MSVehicle* vehicle = veh(myCandi);
// Debug (Leo)
#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 != 0 && 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 != 0 && neighLead.second < 0) {
blocked |= (blockedByLeader | LCA_OVERLAPPING);
// Debug (Leo)
#ifdef DEBUG_CHECK_CHANGE
if (DEBUG_COND) {
std::cout << SIMTIME
<< " overlapping with leader..."
<< std::endl;
}
#endif
}
// safe back gap
if ((blocked & blockedByFollower) == 0 && neighFollow.first != 0) {
// !!! eigentlich: vsafe braucht die Max. Geschwindigkeit beider Spuren
if (neighFollow.second < neighFollow.first->getCarFollowModel().getSecureGap(neighFollow.first->getSpeed(), vehicle->getSpeed(), vehicle->getCarFollowModel().getMaxDecel())) {
blocked |= blockedByFollower;
// Debug (Leo)
#ifdef DEBUG_CHECK_CHANGE
if (DEBUG_COND) {
std::cout << SIMTIME
<< " back gap unsafe: "
<< "gap = " << neighFollow.second
<< ", secureGap = "
<< neighFollow.first->getCarFollowModel().getSecureGap(neighFollow.first->getSpeed(),
vehicle->getSpeed(), vehicle->getCarFollowModel().getMaxDecel())
<< std::endl;
}
#endif
}
}
// safe front gap
if ((blocked & blockedByLeader) == 0 && neighLead.first != 0) {
// !!! eigentlich: vsafe braucht die Max. Geschwindigkeit beider Spuren
if (neighLead.second < vehicle->getCarFollowModel().getSecureGap(vehicle->getSpeed(), neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel())) {
blocked |= blockedByLeader;
// Debug (Leo)
#ifdef DEBUG_CHECK_CHANGE
if (DEBUG_COND) {
std::cout << SIMTIME
<< " front gap unsafe: "
<< "gap = " << neighLead.second
<< ", secureGap = "
<< vehicle->getCarFollowModel().getSecureGap(vehicle->getSpeed(),
neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel())
<< std::endl;
}
#endif
}
}
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 != 0) {
// do are more carefull (but expensive) check to ensure that a
// safety-critical leader is not being overloocked
const SUMOReal seen = myCandi->lane->getLength() - vehicle->getPositionOnLane();
const SUMOReal speed = vehicle->getSpeed();
const SUMOReal dist = vehicle->getCarFollowModel().brakeGap(speed) + vehicle->getVehicleType().getMinGap();
if (seen < dist) {
std::pair<MSVehicle* const, SUMOReal> neighLead2 = targetLane->getCriticalLeader(dist, seen, speed, *vehicle);
if (neighLead2.first != 0 && neighLead2.first != neighLead.first
&& (neighLead2.second < vehicle->getCarFollowModel().getSecureGap(
vehicle->getSpeed(), neighLead2.first->getSpeed(), neighLead2.first->getCarFollowModel().getMaxDecel()))) {
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
SUMOReal seen = myCandi->lane->getLength() - vehicle->getPositionOnLane();
const SUMOReal decel = vehicle->getCarFollowModel().getMaxDecel() * STEPS2TIME(MSGlobals::gLaneChangeDuration);
const SUMOReal avgSpeed = 0.5 * (
MAX2((SUMOReal)0, vehicle->getSpeed() - ACCEL2SPEED(vehicle->getCarFollowModel().getMaxDecel())) +
MAX2((SUMOReal)0, vehicle->getSpeed() - decel));
const SUMOReal space2change = avgSpeed * STEPS2TIME(MSGlobals::gLaneChangeDuration);
// 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;
MSLane* nextLane = vehicle->getLane();
MSLinkCont::const_iterator link = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
while (!nextLane->isLinkEnd(link) && seen <= space2change) {
if ((*link)->getDirection() == LINKDIR_LEFT || (*link)->getDirection() == LINKDIR_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;
}
#ifdef HAVE_INTERNAL_LANES
if ((*link)->getViaLane() == 0) {
view++;
}
#else
view++;
#endif
nextLane = (*link)->getViaLaneOrLane();
seen += nextLane->getLength();
// get the next link used
link = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
}
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 SUMOReal speed = vehicle->getSpeed();
seen = myCandi->lane->getLength() - vehicle->getPositionOnLane();
nextLane = vehicle->getLane();
view = 1;
const SUMOReal dist = vehicle->getCarFollowModel().brakeGap(speed) + vehicle->getVehicleType().getMinGap();
MSLinkCont::const_iterator link = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
while (!nextLane->isLinkEnd(link) && seen <= space2change && seen <= dist) {
nextLane = (*link)->getViaLaneOrLane();
MSLane* targetLane = nextLane->getParallelLane(laneOffset);
if (targetLane == 0) {
state |= LCA_INSUFFICIENT_SPACE;
break;
} else {
std::pair<MSVehicle* const, SUMOReal> neighLead2 = targetLane->getLeader(vehicle, -seen, std::vector<MSLane*>());
if (neighLead2.first != 0 && neighLead2.first != neighLead.first
&& (neighLead2.second < vehicle->getCarFollowModel().getSecureGap(
vehicle->getSpeed(), neighLead2.first->getSpeed(), neighLead2.first->getCarFollowModel().getMaxDecel()))) {
state |= blockedByLeader;
break;
}
}
#ifdef HAVE_INTERNAL_LANES
if ((*link)->getViaLane() == 0) {
view++;
}
#else
view++;
#endif
seen += nextLane->getLength();
// get the next link used
link = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
}
}
}
#ifndef NO_TRACI
#ifdef DEBUG_CHECK_CHANGE
const int oldstate = state;
#endif
// let TraCI influence the wish to change lanes and the security to take
state = vehicle->influenceChangeDecision(state);
#endif
#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
return state;
}
bool
MSLaneChanger::changeOpposite(std::pair<MSVehicle*, SUMOReal> leader) {
if (!myChangeToOpposite) {
return false;
}
myCandi = findCandidate();
MSVehicle* vehicle = veh(myCandi);
MSLane* source = vehicle->getLane();
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();
if (!isOpposite && leader.first == 0) {
// 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;
}
MSLane* opposite = source->getOpposite();
if (opposite == 0) {
return false;
}
// changing into the opposite direction is always to the left (XXX except for left-hand networkds)
int direction = isOpposite ? -1 : 1;
std::pair<MSVehicle*, SUMOReal> neighLead((MSVehicle*)0, -1);
// preliminary sanity checks for overtaking space
SUMOReal timeToOvertake;
SUMOReal spaceToOvertake;
if (!isOpposite) {
assert(leader.first != 0);
// find a leader vehicle with sufficient space ahead for merging back
const SUMOReal overtakingSpeed = source->getVehicleMaxSpeed(vehicle); // just a guess
const SUMOReal mergeBrakeGap = vehicle->getCarFollowModel().brakeGap(overtakingSpeed);
std::pair<MSVehicle*, SUMOReal> columnLeader = leader;
SUMOReal egoGap = leader.second;
bool foundSpaceAhead = false;
SUMOReal seen = leader.second + leader.first->getVehicleType().getLengthWithGap();
std::vector<MSLane*> conts = vehicle->getBestLanesContinuation();
while (!foundSpaceAhead) {
const SUMOReal requiredSpaceAfterLeader = (columnLeader.first->getCarFollowModel().getSecureGap(
columnLeader.first->getSpeed(), overtakingSpeed, vehicle->getCarFollowModel().getMaxDecel())
+ 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());
std::pair<MSVehicle* const, SUMOReal> leadLead = columnLeader.first->getLane()->getLeader(
columnLeader.first, columnLeader.first->getPositionOnLane(), conts, requiredSpaceAfterLeader + mergeBrakeGap,
checkTmpVehicles);
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " leadLead=" << Named::getIDSecure(leadLead.first) << " gap=" << leadLead.second << "\n";
}
#endif
if (leadLead.first == 0) {
foundSpaceAhead = true;
} else {
const SUMOReal requiredSpace = (requiredSpaceAfterLeader
+ vehicle->getCarFollowModel().getSecureGap(overtakingSpeed, leadLead.first->getSpeed(), leadLead.first->getCarFollowModel().getMaxDecel()));
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((SUMOReal)0, leadLead.second) + leadLead.first->getVehicleType().getLengthWithGap();
if (seen > OPPOSITE_OVERTAKING_MAX_LOOKAHEAD) {
#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 false;
}
// 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
}
}
}
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " compute time/space to overtake for columnLeader=" << columnLeader.first->getID() << " gap=" << columnLeader.second << "\n";
}
#endif
computeOvertakingTime(vehicle, columnLeader.first, egoGap, timeToOvertake, spaceToOvertake);
// 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;
}
neighLead = opposite->getOppositeLeader(vehicle, timeToOvertake * opposite->getSpeedLimit() * 2 + spaceToOvertake, true);
#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
// check for dangerous oncoming leader
if (neighLead.first != 0) {
const MSVehicle* oncoming = neighLead.first;
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << SIMTIME
<< " oncoming=" << oncoming->getID()
<< " oncomingGap=" << neighLead.second
<< " leaderGap=" << leader.second
<< "\n";
}
#endif
if (neighLead.second - spaceToOvertake - timeToOvertake * oncoming->getSpeed() < 0) {
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " cannot changeOpposite due to dangerous oncoming\n";
}
#endif
return false;
}
}
} else {
timeToOvertake = -1;
// look forward as far as possible
spaceToOvertake = std::numeric_limits<SUMOReal>::max();
leader = source->getOppositeLeader(vehicle, OPPOSITE_OVERTAKING_ONCOMING_LOOKAHEAD, true);
// -1 will use getMaximumBrakeDist() as look-ahead distance
neighLead = opposite->getOppositeLeader(vehicle, -1, false);
}
// compute remaining space on the opposite side
// 1. the part that remains on the current lane
SUMOReal 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() == 0) {
// opposite lane ends
break;
}
// do not overtake past a minor link or turn
if (*(it - 1) != 0) {
MSLink* link = MSLinkContHelper::getConnectingLink(**(it - 1), **it);
if (link == 0 || !link->havePriority() || link->getState() == LINKSTATE_ZIPPER || link->getDirection() != LINKDIR_STRAIGHT) {
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 drive 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 SUMOReal 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);
// consider oncoming leaders
if (leader.first != 0) {
laneQ.length = MIN2(laneQ.length, leader.second / 2 + forwardPos);
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << SIMTIME << " found oncoming leader=" << leader.first->getID() << " gap=" << leader.second << "\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, SUMOReal> neighFollow = opposite->getOppositeFollower(vehicle);
int state = checkChange(direction, opposite, leader, neighLead, neighFollow, preb);
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)) {
vehicle->getLaneChangeModel().startLaneChangeManeuver(source, opposite, direction);
/// XXX use a dedicated transformation function
vehicle->myState.myPos = source->getOppositePos(vehicle->myState.myPos);
/// XXX compute a better lateral position
opposite->forceVehicleInsertion(vehicle, vehicle->getPositionOnLane(), MSMoveReminder::NOTIFICATION_LANE_CHANGE, 0);
if (!isOpposite) {
vehicle->myState.myBackPos = source->getOppositePos(vehicle->myState.myBackPos);
}
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << SIMTIME << " changing to opposite veh=" << vehicle->getID() << " dir=" << direction << " opposite=" << Named::getIDSecure(opposite) << " state=" << state << "\n";
}
#endif
return true;
}
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << SIMTIME << " not changing to opposite veh=" << vehicle->getID() << " dir=" << direction
<< " opposite=" << Named::getIDSecure(opposite) << " state=" << toString((LaneChangeAction)state) << "\n";
}
#endif
return false;
}
