void
NLTriggerBuilder::parseAndBuildCalibrator(MSNet& net, const SUMOSAXAttributes& attrs,
const std::string& base) {
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 file name to read further definitions from
MSLane* lane = getLane(attrs, "calibrator", id);
const SUMOReal pos = getPosition(attrs, lane, "calibrator", id);
const SUMOTime freq = attrs.getOptSUMOTimeReporting(SUMO_ATTR_FREQUENCY, id.c_str(), ok, DELTA_T); // !!! no error handling
std::string file = getFileName(attrs, base, true);
std::string outfile = attrs.getOpt<std::string>(SUMO_ATTR_OUTPUT, 0, ok, "");
if (MSGlobals::gUseMesoSim) {
#ifdef HAVE_INTERNAL
METriggeredCalibrator* trigger = buildMECalibrator(net, id, &lane->getEdge(), pos, file, outfile, freq);
if (file == "") {
trigger->registerParent(SUMO_TAG_CALIBRATOR, myHandler);
}
#endif
} else {
MSCalibrator* trigger = buildCalibrator(net, id, &lane->getEdge(), pos, file, outfile, freq);
if (file == "") {
trigger->registerParent(SUMO_TAG_CALIBRATOR, myHandler);
}
}
}
void
MSEdge::closeBuilding() {
myAllowed[0] = new std::vector<MSLane*>();
for (std::vector<MSLane*>::iterator i = myLanes->begin(); i != myLanes->end(); ++i) {
myAllowed[0]->push_back(*i);
const MSLinkCont& lc = (*i)->getLinkCont();
for (MSLinkCont::const_iterator j = lc.begin(); j != lc.end(); ++j) {
MSLane* toL = (*j)->getLane();
if (toL != 0) {
MSEdge& to = toL->getEdge();
//
if (std::find(mySuccessors.begin(), mySuccessors.end(), &to) == mySuccessors.end()) {
mySuccessors.push_back(&to);
}
if (std::find(to.myPredeccesors.begin(), to.myPredeccesors.end(), this) == to.myPredeccesors.end()) {
to.myPredeccesors.push_back(this);
}
//
if (myAllowed.find(&to) == myAllowed.end()) {
myAllowed[&to] = new std::vector<MSLane*>();
}
myAllowed[&to]->push_back(*i);
}
#ifdef HAVE_INTERNAL_LANES
toL = (*j)->getViaLane();
if (toL != 0) {
MSEdge& to = toL->getEdge();
to.myPredeccesors.push_back(this);
}
#endif
}
}
std::sort(mySuccessors.begin(), mySuccessors.end(), by_id_sorter());
rebuildAllowedLanes();
}
bool
TraCIServerAPI_Lane::processSet(TraCIServer& server, tcpip::Storage& inputStorage,
tcpip::Storage& outputStorage) {
std::string warning = ""; // additional description for response
// variable
int variable = inputStorage.readUnsignedByte();
if (variable != VAR_MAXSPEED && variable != VAR_LENGTH && variable != LANE_ALLOWED && variable != LANE_DISALLOWED) {
return server.writeErrorStatusCmd(CMD_SET_LANE_VARIABLE, "Change Lane State: unsupported variable specified", outputStorage);
}
// id
std::string id = inputStorage.readString();
MSLane* l = MSLane::dictionary(id);
if (l == 0) {
return server.writeErrorStatusCmd(CMD_SET_LANE_VARIABLE, "Lane '" + id + "' is not known", outputStorage);
}
// process
switch (variable) {
case VAR_MAXSPEED: {
double value = 0;
if (!server.readTypeCheckingDouble(inputStorage, value)) {
return server.writeErrorStatusCmd(CMD_SET_LANE_VARIABLE, "The speed must be given as a double.", outputStorage);
}
l->setMaxSpeed(value);
}
break;
case VAR_LENGTH: {
double value = 0;
if (!server.readTypeCheckingDouble(inputStorage, value)) {
return server.writeErrorStatusCmd(CMD_SET_LANE_VARIABLE, "The length must be given as a double.", outputStorage);
}
l->setLength(value);
}
break;
case LANE_ALLOWED: {
std::vector<std::string> classes;
if (!server.readTypeCheckingStringList(inputStorage, classes)) {
return server.writeErrorStatusCmd(CMD_SET_LANE_VARIABLE, "Allowed classes must be given as a list of strings.", outputStorage);
}
l->setPermissions(parseVehicleClasses(classes));
l->getEdge().rebuildAllowedLanes();
}
break;
case LANE_DISALLOWED: {
std::vector<std::string> classes;
if (!server.readTypeCheckingStringList(inputStorage, classes)) {
return server.writeErrorStatusCmd(CMD_SET_LANE_VARIABLE, "Not allowed classes must be given as a list of strings.", outputStorage);
}
l->setPermissions(~parseVehicleClasses(classes)); // negation yields allowed
l->getEdge().rebuildAllowedLanes();
}
break;
default:
break;
}
server.writeStatusCmd(CMD_SET_LANE_VARIABLE, RTYPE_OK, warning, outputStorage);
return true;
}
void
MSEdge::closeBuilding() {
myAllowed[0] = new std::vector<MSLane*>();
for (std::vector<MSLane*>::const_iterator i = myLanes->begin(); i != myLanes->end(); ++i) {
myAllowed[0]->push_back(*i);
const MSLinkCont& lc = (*i)->getLinkCont();
for (MSLinkCont::const_iterator j = lc.begin(); j != lc.end(); ++j) {
(*j)->initParallelLinks();
MSLane* toL = (*j)->getLane();
if (toL != 0) {
MSEdge& to = toL->getEdge();
//
if (std::find(mySuccessors.begin(), mySuccessors.end(), &to) == mySuccessors.end()) {
mySuccessors.push_back(&to);
}
if (std::find(to.myPredecessors.begin(), to.myPredecessors.end(), this) == to.myPredecessors.end()) {
to.myPredecessors.push_back(this);
}
//
if (myAllowed.find(&to) == myAllowed.end()) {
myAllowed[&to] = new std::vector<MSLane*>();
}
myAllowed[&to]->push_back(*i);
}
#ifdef HAVE_INTERNAL_LANES
toL = (*j)->getViaLane();
if (toL != 0) {
MSEdge& to = toL->getEdge();
if (std::find(to.myPredecessors.begin(), to.myPredecessors.end(), this) == to.myPredecessors.end()) {
to.myPredecessors.push_back(this);
}
}
#endif
}
}
std::sort(mySuccessors.begin(), mySuccessors.end(), by_id_sorter());
rebuildAllowedLanes();
recalcCache();
// segment building depends on the finished list of successors (for multi-queue)
if (MSGlobals::gUseMesoSim && !myLanes->empty()) {
MSGlobals::gMesoNet->buildSegmentsFor(*this, OptionsCont::getOptions());
}
}
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
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
}
}
int
MSLCM_LC2013::_wantsChange(
int laneOffset,
MSAbstractLaneChangeModel::MSLCMessager& msgPass,
int blocked,
const std::pair<MSVehicle*, SUMOReal>& leader,
const std::pair<MSVehicle*, SUMOReal>& neighLead,
const std::pair<MSVehicle*, SUMOReal>& neighFollow,
const MSLane& neighLane,
const std::vector<MSVehicle::LaneQ>& preb,
MSVehicle** lastBlocked,
MSVehicle** firstBlocked) {
assert(laneOffset == 1 || laneOffset == -1);
const SUMOTime currentTime = MSNet::getInstance()->getCurrentTimeStep();
// compute bestLaneOffset
MSVehicle::LaneQ curr, neigh, best;
int bestLaneOffset = 0;
SUMOReal currentDist = 0;
SUMOReal neighDist = 0;
int currIdx = 0;
MSLane* prebLane = myVehicle.getLane();
if (prebLane->getEdge().getPurpose() == MSEdge::EDGEFUNCTION_INTERNAL) {
// internal edges are not kept inside the bestLanes structure
prebLane = prebLane->getLinkCont()[0]->getLane();
}
for (int p = 0; p < (int) preb.size(); ++p) {
if (preb[p].lane == prebLane && p + laneOffset >= 0) {
assert(p + laneOffset < (int)preb.size());
curr = preb[p];
neigh = preb[p + laneOffset];
currentDist = curr.length;
neighDist = neigh.length;
bestLaneOffset = curr.bestLaneOffset;
if (bestLaneOffset == 0 && preb[p + laneOffset].bestLaneOffset == 0) {
bestLaneOffset = laneOffset;
}
best = preb[p + bestLaneOffset];
currIdx = p;
break;
}
}
// direction specific constants
const bool right = (laneOffset == -1);
const int lca = (right ? LCA_RIGHT : LCA_LEFT);
const int myLca = (right ? LCA_MRIGHT : LCA_MLEFT);
const int lcaCounter = (right ? LCA_LEFT : LCA_RIGHT);
const bool changeToBest = (right && bestLaneOffset < 0) || (!right && bestLaneOffset > 0);
// keep information about being a leader/follower
int ret = (myOwnState & 0xffff0000);
ret = slowDownForBlocked(lastBlocked, ret);
if (lastBlocked != firstBlocked) {
ret = slowDownForBlocked(firstBlocked, ret);
}
// we try to estimate the distance which is necessary to get on a lane
// we have to get on in order to keep our route
// we assume we need something that depends on our velocity
// and compare this with the free space on our wished lane
//
// if the free space is somehow less than the space we need, we should
// definitely try to get to the desired lane
//
// this rule forces our vehicle to change the lane if a lane changing is necessary soon
// lookAheadDistance:
// we do not want the lookahead distance to change all the time so we discrectize the speed a bit
if (myVehicle.getSpeed() > myLookAheadSpeed) {
myLookAheadSpeed = myVehicle.getSpeed();
} else {
myLookAheadSpeed = MAX2(LOOK_AHEAD_MIN_SPEED,
(LOOK_AHEAD_SPEED_MEMORY * myLookAheadSpeed + (1 - LOOK_AHEAD_SPEED_MEMORY) * myVehicle.getSpeed()));
}
SUMOReal laDist = myLookAheadSpeed * (right ? LOOK_FORWARD_RIGHT : LOOK_FORWARD_LEFT);
laDist += myVehicle.getVehicleType().getLengthWithGap() * (SUMOReal) 2.;
// free space that is available for changing
//const SUMOReal neighSpeed = (neighLead.first != 0 ? neighLead.first->getSpeed() :
// neighFollow.first != 0 ? neighFollow.first->getSpeed() :
// best.lane->getSpeedLimit());
// @note: while this lets vehicles change earlier into the correct direction
// it also makes the vehicles more "selfish" and prevents changes which are necessary to help others
int roundaboutEdgesAhead = 0;
for (std::vector<MSLane*>::iterator it = curr.bestContinuations.begin(); it != curr.bestContinuations.end(); ++it) {
if ((*it) != 0 && (*it)->getEdge().isRoundabout()) {
roundaboutEdgesAhead += 1;
} else if (roundaboutEdgesAhead > 0) {
// only check the next roundabout
break;
}
}
int roundaboutEdgesAheadNeigh = 0;
for (std::vector<MSLane*>::iterator it = neigh.bestContinuations.begin(); it != neigh.bestContinuations.end(); ++it) {
if ((*it) != 0 && (*it)->getEdge().isRoundabout()) {
roundaboutEdgesAheadNeigh += 1;
} else if (roundaboutEdgesAheadNeigh > 0) {
// only check the next roundabout
break;
}
}
if (roundaboutEdgesAhead > 1) {
currentDist += roundaboutEdgesAhead * ROUNDABOUT_DIST_BONUS;
neighDist += roundaboutEdgesAheadNeigh * ROUNDABOUT_DIST_BONUS;
}
const SUMOReal usableDist = (currentDist - myVehicle.getPositionOnLane() - best.occupation * JAM_FACTOR);
const SUMOReal maxJam = MAX2(preb[currIdx + laneOffset].occupation, preb[currIdx].occupation);
const SUMOReal neighLeftPlace = MAX2((SUMOReal) 0, neighDist - myVehicle.getPositionOnLane() - maxJam);
if (changeToBest && bestLaneOffset == curr.bestLaneOffset
&& currentDistDisallows(usableDist, bestLaneOffset, laDist)) {
/// @brief we urgently need to change lanes to follow our route
ret = ret | lca | LCA_STRATEGIC | LCA_URGENT;
} else {
if (!myAllowOvertakingRight && !right && !myVehicle.congested() && neighLead.first != 0) {
// check for slower leader on the left. we should not overtake but
// rather move left ourselves (unless congested)
MSVehicle* nv = neighLead.first;
if (nv->getSpeed() < myVehicle.getSpeed()) {
myVSafes.push_back(myCarFollowModel.followSpeed(
&myVehicle, myVehicle.getSpeed(), neighLead.second, nv->getSpeed(), nv->getCarFollowModel().getMaxDecel()));
if (nv->getSpeed() + 5 / 3.6 < myVehicle.getSpeed()) {
mySpeedGainProbability += CHANGE_PROB_THRESHOLD_LEFT / 3;
}
}
}
if (!changeToBest && (currentDistDisallows(neighLeftPlace, abs(bestLaneOffset) + 2, laDist))) {
// the opposite lane-changing direction should be done than the one examined herein
// we'll check whether we assume we could change anyhow and get back in time...
//
// this rule prevents the vehicle from moving in opposite direction of the best lane
// unless the way till the end where the vehicle has to be on the best lane
// is long enough
ret = ret | LCA_STAY | LCA_STRATEGIC;
} else if (bestLaneOffset == 0 && (neighLeftPlace * 2. < laDist)) {
// the current lane is the best and a lane-changing would cause a situation
// of which we assume we will not be able to return to the lane we have to be on.
// this rule prevents the vehicle from leaving the current, best lane when it is
// close to this lane's end
ret = ret | LCA_STAY | LCA_STRATEGIC;
}
}
// check for overriding TraCI requests
ret = myVehicle.influenceChangeDecision(ret);
if ((ret & lcaCounter) != 0) {
// we are not interested in traci requests for the opposite direction here
ret &= ~(LCA_TRACI | lcaCounter | LCA_URGENT);
}
if ((ret & LCA_STAY) != 0) {
return ret;
}
if ((ret & LCA_URGENT) != 0) {
// prepare urgent lane change maneuver
// save the left space
myLeftSpace = currentDist - myVehicle.getPositionOnLane();
if (changeToBest && abs(bestLaneOffset) > 1) {
// there might be a vehicle which needs to counter-lane-change one lane further and we cannot see it yet
myLeadingBlockerLength = MAX2((SUMOReal)(right ? 20.0 : 40.0), myLeadingBlockerLength);
}
// letting vehicles merge in at the end of the lane in case of counter-lane change, step#1
// if there is a leader and he wants to change to the opposite direction
saveBlockerLength(neighLead.first, lcaCounter);
if (*firstBlocked != neighLead.first) {
saveBlockerLength(*firstBlocked, lcaCounter);
}
const SUMOReal remainingSeconds = ((ret & LCA_TRACI) == 0 ?
MAX2((SUMOReal)STEPS2TIME(TS), myLeftSpace / myLookAheadSpeed / abs(bestLaneOffset) / URGENCY) :
myVehicle.getInfluencer().changeRequestRemainingSeconds(currentTime));
const SUMOReal plannedSpeed = informLeader(msgPass, blocked, myLca, neighLead, remainingSeconds);
if (plannedSpeed >= 0) {
// maybe we need to deal with a blocking follower
informFollower(msgPass, blocked, myLca, neighFollow, remainingSeconds, plannedSpeed);
}
return ret;
}
if (roundaboutEdgesAhead > 1) {
// try to use the inner lanes of a roundabout to increase throughput
// unless we are approaching the exit
if (lca == LCA_LEFT) {
return ret | lca | LCA_COOPERATIVE;
} else {
return ret | LCA_STAY | LCA_COOPERATIVE;
}
}
// let's also regard the case where the vehicle is driving on a highway...
// in this case, we do not want to get to the dead-end of an on-ramp
if (right) {
if (bestLaneOffset == 0 && myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle) > 80. / 3.6 && myLookAheadSpeed > SUMO_const_haltingSpeed) {
return ret | LCA_STAY | LCA_STRATEGIC;
}
}
// --------
// -------- make place on current lane if blocking follower
//if (amBlockingFollowerPlusNB()) {
// std::cout << myVehicle.getID() << ", " << currentDistAllows(neighDist, bestLaneOffset, laDist)
// << " neighDist=" << neighDist
// << " currentDist=" << currentDist
// << "\n";
//}
if (amBlockingFollowerPlusNB()
&& (changeToBest || currentDistAllows(neighDist, abs(bestLaneOffset) + 1, laDist))) {
return ret | lca | LCA_COOPERATIVE | LCA_URGENT ;//| LCA_CHANGE_TO_HELP;
}
// --------
//// -------- security checks for krauss
//// (vsafe fails when gap<0)
//if ((blocked & LCA_BLOCKED) != 0) {
// return ret;
//}
//// --------
// -------- higher speed
//if ((congested(neighLead.first) && neighLead.second < 20) || predInteraction(leader.first)) { //!!!
// return ret;
//}
SUMOReal thisLaneVSafe = myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle);
SUMOReal neighLaneVSafe = neighLane.getVehicleMaxSpeed(&myVehicle);
if (neighLead.first == 0) {
neighLaneVSafe = MIN2(neighLaneVSafe, myCarFollowModel.followSpeed(&myVehicle, myVehicle.getSpeed(), neighDist, 0, 0));
} else {
// @todo: what if leader is below safe gap?!!!
neighLaneVSafe = MIN2(neighLaneVSafe, myCarFollowModel.followSpeed(
&myVehicle, myVehicle.getSpeed(), neighLead.second, neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()));
}
if (leader.first == 0) {
thisLaneVSafe = MIN2(thisLaneVSafe, myCarFollowModel.followSpeed(&myVehicle, myVehicle.getSpeed(), currentDist, 0, 0));
} else {
// @todo: what if leader is below safe gap?!!!
thisLaneVSafe = MIN2(thisLaneVSafe, myCarFollowModel.followSpeed(&myVehicle, myVehicle.getSpeed(), leader.second, leader.first->getSpeed(), leader.first->getCarFollowModel().getMaxDecel()));
}
thisLaneVSafe = MIN3(thisLaneVSafe, myVehicle.getVehicleType().getMaxSpeed(), myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle));
neighLaneVSafe = MIN3(neighLaneVSafe, myVehicle.getVehicleType().getMaxSpeed(), neighLane.getVehicleMaxSpeed(&myVehicle));
const SUMOReal relativeGain = (neighLaneVSafe - thisLaneVSafe) / neighLaneVSafe;
if (right) {
// ONLY FOR CHANGING TO THE RIGHT
if (thisLaneVSafe - 5 / 3.6 > neighLaneVSafe) {
// ok, the current lane is faster than the right one...
if (mySpeedGainProbability < 0) {
mySpeedGainProbability /= 2.0;
//myKeepRightProbability /= 2.0;
}
} else {
// ok, the current lane is not faster than the right one
mySpeedGainProbability -= relativeGain;
// honor the obligation to keep right (Rechtsfahrgebot)
// XXX consider fast approaching followers on the current lane
//const SUMOReal vMax = myLookAheadSpeed;
const SUMOReal vMax = MIN2(myVehicle.getVehicleType().getMaxSpeed(), myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle));
const SUMOReal acceptanceTime = KEEP_RIGHT_ACCEPTANCE * vMax * MAX2((SUMOReal)1, myVehicle.getSpeed()) / myVehicle.getLane()->getSpeedLimit();
SUMOReal fullSpeedGap = MAX2((SUMOReal)0, neighDist - myVehicle.getCarFollowModel().brakeGap(vMax));
SUMOReal fullSpeedDrivingSeconds = MIN2(acceptanceTime, fullSpeedGap / vMax);
if (neighLead.first != 0 && neighLead.first->getSpeed() < vMax) {
fullSpeedGap = MAX2((SUMOReal)0, MIN2(fullSpeedGap,
neighLead.second - myVehicle.getCarFollowModel().getSecureGap(
vMax, neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel())));
fullSpeedDrivingSeconds = MIN2(fullSpeedDrivingSeconds, fullSpeedGap / (vMax - neighLead.first->getSpeed()));
}
const SUMOReal deltaProb = (CHANGE_PROB_THRESHOLD_RIGHT
* STEPS2TIME(DELTA_T)
* (fullSpeedDrivingSeconds / acceptanceTime) / KEEP_RIGHT_TIME);
myKeepRightProbability -= deltaProb;
if (gDebugFlag2) {
std::cout << STEPS2TIME(currentTime)
<< " veh=" << myVehicle.getID()
<< " vMax=" << vMax
<< " neighDist=" << neighDist
<< " brakeGap=" << myVehicle.getCarFollowModel().brakeGap(myVehicle.getSpeed())
<< " leaderSpeed=" << (neighLead.first == 0 ? -1 : neighLead.first->getSpeed())
<< " secGap=" << (neighLead.first == 0 ? -1 : myVehicle.getCarFollowModel().getSecureGap(
myVehicle.getSpeed(), neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()))
<< " acceptanceTime=" << acceptanceTime
<< " fullSpeedGap=" << fullSpeedGap
<< " fullSpeedDrivingSeconds=" << fullSpeedDrivingSeconds
<< " dProb=" << deltaProb
<< "\n";
}
if (myKeepRightProbability < -CHANGE_PROB_THRESHOLD_RIGHT) {
return ret | lca | LCA_KEEPRIGHT;
}
}
if (mySpeedGainProbability < -CHANGE_PROB_THRESHOLD_RIGHT
&& neighDist / MAX2((SUMOReal) .1, myVehicle.getSpeed()) > 20.) { //./MAX2((SUMOReal) .1, myVehicle.getSpeed())) { // -.1
return ret | lca | LCA_SPEEDGAIN;
}
} else {
// ONLY FOR CHANGING TO THE LEFT
if (thisLaneVSafe > neighLaneVSafe) {
// this lane is better
if (mySpeedGainProbability > 0) {
mySpeedGainProbability /= 2.0;
}
} else {
// left lane is better
mySpeedGainProbability += relativeGain;
}
if (mySpeedGainProbability > CHANGE_PROB_THRESHOLD_LEFT && neighDist / MAX2((SUMOReal) .1, myVehicle.getSpeed()) > 20.) { // .1
return ret | lca | LCA_SPEEDGAIN;
}
}
// --------
if (changeToBest && bestLaneOffset == curr.bestLaneOffset
&& (right ? mySpeedGainProbability < 0 : mySpeedGainProbability > 0)) {
// change towards the correct lane, speedwise it does not hurt
return ret | lca | LCA_STRATEGIC;
}
return ret;
}
void
MSRailSignal::init(NLDetectorBuilder&) {
assert(myLanes.size() > 0);
myConflictLanes.resize(myLinks.size());
myConflictLinks.resize(myLinks.size());
myRouteConflictLanes.resize(myLinks.size());
myRouteConflictLinks.resize(myLinks.size());
myLastRerouteAttempt.resize(myLinks.size(), std::make_pair(nullptr, -1));
if (OptionsCont::getOptions().isSet("railsignal-block-output")) {
OutputDevice& od = OutputDevice::getDeviceByOption("railsignal-block-output");
od.openTag("railSignal");
od.writeAttr(SUMO_ATTR_ID, getID());
}
for (LinkVector& links : myLinks) { //for every link index
// collect lanes and links that are relevant for setting this signal
// for each index we collect
// - conflictLanes (signal must be red if any conflict lane is occupied)
// - conflictLinks (signal must be red if any conflict link is approached by a vehicle
// - that cannot break in time (arrivalSpeedBraking > 0)
// - approached by a vehicle with higher switching priority (see #3941)
//
// forwardBlock
// - search forward recursive from outgoing lane until controlled railSignal link found
// -> add all found lanes
//
// bidiBlock (if any forwardBlock edge edge has bidi edge)
// - search bidi backward recursive until first switch
// - from switch search backward recursive all other incoming until controlled rail signal link
// -> add final links
//
// backwardBlock
// - search backward recursive from incoming lanes (not bidi for current outgoing lane)
// until controlled railSignal link found
// -> add all found lanes
// -> add final links
//
// conditionalBlocks
// - for each conflict link (always signalized) that enters from a
// bidirectional track
// - search bidi backward recursive until first switch that is
// accessible from the bidi-direction
// - from switch search bidi backward recursive until controlled rail signal link
// -> add all found lanes
// -> add final links
std::vector<MSLink*> conflictLinks;
LaneSet visited;
std::vector<MSLane*> forwardBlock;
std::vector<MSLane*> bidiBlock;
std::vector<MSLane*> backwardBlock;
// there should be only one link per index for railSignal
if (links.size() > 1) {
throw ProcessError("At railSignal '" + getID() + "' found " + toString(links.size())
+ " links controlled by index " + toString(links[0]->getTLIndex()));
}
for (MSLink* link : links) {
MSLane* toLane = link->getViaLaneOrLane(); //the lane this link is leading to
MSLane* fromBidi = link->getLaneBefore()->getBidiLane();
if (fromBidi != nullptr) {
// do not extend to forward block beyond the entering track (in case of a loop)
visited.insert(fromBidi);
}
collectForwardBlock(toLane, 0., forwardBlock, visited);
#ifdef DEBUG_FORWARD_BLOCK
if (DEBUG_COND) {
std::cout << "railSignal=" << getID() << " index=" << link->getTLIndex() << " forwardBlock=" << toString(forwardBlock) << "\n";
}
#endif
for (MSLane* forward : forwardBlock) {
if (forward->getEdge().getBidiEdge() != nullptr) {
MSLane* bidi = forward->getBidiLane();
for (MSLink* bidiOutLink : bidi->getLinkCont()) {
if (bidiOutLink->getViaLane() != nullptr) {
bidiBlock.push_back(bidiOutLink->getViaLane());
}
}
collectBidiBlock(bidi, 0., false, bidiBlock, visited);
// assume bidirectional patches are continuous
break;
}
}
#ifdef DEBUG_BIDI_BLOCK
if (DEBUG_COND) {
std::cout << "railSignal=" << getID() << " index=" << link->getTLIndex() << " bidiBlock=" << toString(bidiBlock) << "\n";
}
#endif
// compute conflict links
for (MSLane* cl : forwardBlock) {
collectConflictLinks(cl, 0, backwardBlock, conflictLinks, visited, true);
}
for (MSLane* cl : bidiBlock) {
collectConflictLinks(cl, 0, backwardBlock, conflictLinks, visited, false);
}
auto thisLinkIt = std::find(conflictLinks.begin(), conflictLinks.end(), link);
if (thisLinkIt != conflictLinks.end()) {
conflictLinks.erase(thisLinkIt);
} else {
WRITE_WARNING("At railSignal junction '" + getID() + "' link " + toString(link->getTLIndex()) + " with direction " + toString(link->getDirection()) + " should be uncontrolled");
}
#ifdef DEBUG_BACKWARD_BLOCK
if (DEBUG_COND) {
std::cout << "railSignal=" << getID() << " index=" << link->getTLIndex() << " backwardBlock=" << toString(backwardBlock);
std::cout << "railSignal=" << getID() << " index=" << link->getTLIndex() << " conflictLinks=";
for (MSLink* cl : conflictLinks) {
std::cout << toString(cl->getViaLaneOrLane()->getID()) << " ";
}
std::cout << "\n";
}
#endif
// compute conditional conflict lanes and links
for (MSLink* cl : conflictLinks) {
std::vector<MSLane*> routeConflictLanes;
std::vector<MSLink*> routeConflictLinks;
MSLane* in = const_cast<MSLane*>(cl->getLaneBefore());
LaneSet rCVisited = visited;
// only collect if
// 1) the in-edge is bidirectional
// 2) the foe has no alternative track before reach meeting the end of the forwardBlock
// 3) the forward block has no alternative track between the end of the forward block and the conflict link
if (in->getEdge().getBidiEdge() != nullptr
&& !hasAlternativeTrack(cl)
&& !hasAlternativeTrackBetween(forwardBlock, cl)) {
collectBidiBlock(in, 0., false, routeConflictLanes, rCVisited);
std::vector<MSLane*> rCBackwardBlock;
for (MSLane* rCLane : routeConflictLanes) {
collectConflictLinks(rCLane, 0, rCBackwardBlock, routeConflictLinks, rCVisited);
}
}
myRouteConflictLanes[link->getTLIndex()].push_back(routeConflictLanes);
myRouteConflictLinks[link->getTLIndex()].push_back(routeConflictLinks);
}
if (OptionsCont::getOptions().isSet("railsignal-block-output")) {
OutputDevice& od = OutputDevice::getDeviceByOption("railsignal-block-output");
od.openTag("link");
od.writeAttr(SUMO_ATTR_TLLINKINDEX, link->getTLIndex());
od.writeAttr(SUMO_ATTR_FROM, link->getLaneBefore()->getID());
od.writeAttr(SUMO_ATTR_TO, link->getViaLaneOrLane()->getID());
od.openTag("forwardBlock");
od.writeAttr(SUMO_ATTR_LANES, toString(forwardBlock));
od.closeTag();
od.openTag("bidiBlock");
od.writeAttr(SUMO_ATTR_LANES, toString(bidiBlock));
od.closeTag();
od.openTag("backwardBlock");
od.writeAttr(SUMO_ATTR_LANES, toString(backwardBlock));
od.closeTag();
od.openTag("conflictLinks");
std::vector<std::string> conflictLinkIDs; // railSignalID_tlIndex
for (MSLink* cl : conflictLinks) {
conflictLinkIDs.push_back(getTLLinkID(cl));
}
od.writeAttr("logicIndex", toString(conflictLinkIDs));
for (int i = 0; i < (int)conflictLinks.size(); i++) {
const std::vector<MSLane*>& rCLanes = myRouteConflictLanes[link->getTLIndex()][i];
const std::vector<MSLink*>& rCLinks = myRouteConflictLinks[link->getTLIndex()][i];
if (rCLanes.size() > 0 || rCLinks.size() > 0) {
od.openTag("conflictLink");
od.writeAttr("logicIndex", getTLLinkID(conflictLinks[i]));
if (rCLanes.size() > 0) {
od.writeAttr("lanes", toString(rCLanes));
}
if (rCLinks.size() > 0) {
std::vector<std::string> rCLinkIDs;
for (MSLink* rcl : rCLinks) {
rCLinkIDs.push_back(getTLLinkID(rcl));
}
od.writeAttr("links", toString(rCLinkIDs));
}
od.closeTag();
}
}
od.closeTag(); // conflictLinks
od.closeTag(); // link
}
std::vector<MSLane*> conflictLanes;
conflictLanes.insert(conflictLanes.end(), forwardBlock.begin(), forwardBlock.end());
conflictLanes.insert(conflictLanes.end(), bidiBlock.begin(), bidiBlock.end());
conflictLanes.insert(conflictLanes.end(), backwardBlock.begin(), backwardBlock.end());
myConflictLanes[link->getTLIndex()] = conflictLanes;
myConflictLinks[link->getTLIndex()] = conflictLinks;
}
}
if (OptionsCont::getOptions().isSet("railsignal-block-output")) {
OutputDevice& od = OutputDevice::getDeviceByOption("railsignal-block-output");
od.closeTag();
}
updateCurrentPhase(); //check if this is necessary or if will be done already at another stage
setTrafficLightSignals(MSNet::getInstance()->getCurrentTimeStep());
}
void
MSRailSignal::collectBidiBlock(MSLane* toLane, double length, bool foundSwitch, std::vector<MSLane*>& bidiBlock, LaneSet& visited) {
while (toLane != nullptr) {
//std::cout << " toLane=" << toLane->getID() << " length=" << length << " foundSwitch=" << foundSwitch << "\n";
if (visited.count(toLane) != 0) {
return;
}
visited.insert(toLane);
bidiBlock.push_back(toLane);
length += toLane->getLength();
if (length > MAX_BLOCK_LENGTH) {
if (myNumWarnings < MAX_SIGNAL_WARNINGS) {
WRITE_WARNING("Bidirectional block after rail signal junction '" + getID() +
"' exceeds maximum length (stopped searching after lane '" + toLane->getID() + "' (length=" + toString(length) + "m).");
}
myNumWarnings++;
return;
}
const auto& incomingLaneInfos = toLane->getIncomingLanes();
MSLane* prev = toLane;
toLane = nullptr;
for (const auto& ili : incomingLaneInfos) {
if (ili.viaLink->getDirection() == LINKDIR_TURN) {
continue;
}
if (ili.viaLink->getTLLogic() != nullptr) {
if (!foundSwitch && bidiBlock.size() > 1) {
// check wether this node is switch (first edge doesn't count)
for (MSLink* link : ili.lane->getLinkCont()) {
if (link->getDirection() == LINKDIR_TURN) {
continue;
}
if (link->getViaLaneOrLane() == prev) {
continue;
}
//std::cout << " ili.lane=" << ili.lane->getID()
// << " prev=" << prev->getID()
// << " linkDir=" << ili.viaLink->getDirection()
// << " linkIndex=" << ili.viaLink->getTLIndex()
// << "\n";
foundSwitch = true;
break;
}
}
if (foundSwitch) {
return;
}
}
if (toLane == nullptr) {
toLane = ili.lane;
} else {
foundSwitch = true;
collectBidiBlock(ili.lane, length, true, bidiBlock, visited);
}
}
if (toLane != nullptr && !foundSwitch && prev->getEdge().getBidiEdge() != nullptr) {
// check switch in the other direction
MSLane* prevBidi = prev->getBidiLane();
for (MSLink* link : prevBidi->getLinkCont()) {
if (link->getDirection() == LINKDIR_TURN) {
continue;
}
if (link->getViaLaneOrLane()->getEdge().getBidiEdge() != &toLane->getEdge()) {
//std::cout << " toLane=" << toLane->getID()
// << " prev=" << prev->getID()
// << " prevBidi=" << prevBidi->getID()
// << " linkLane=" << link->getViaLaneOrLane()->getEdge().getID()
// << " linkLaneBidi=" << Named::getIDSecure(link->getViaLaneOrLane()->getEdge().getBidiEdge())
// << "\n";
foundSwitch = true;
break;
}
}
}
}
}
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;
}
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;
}
// ===========================================================================
// 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
TraCIServerAPI_Lane::processSet(TraCIServer &server, tcpip::Storage &inputStorage,
tcpip::Storage &outputStorage) {
std::string warning = ""; // additional description for response
// variable
int variable = inputStorage.readUnsignedByte();
if (variable!=VAR_MAXSPEED&&variable!=VAR_LENGTH&&variable!=LANE_ALLOWED&&variable!=LANE_DISALLOWED) {
server.writeStatusCmd(CMD_SET_LANE_VARIABLE, RTYPE_ERR, "Change Lane State: unsupported variable specified", outputStorage);
return false;
}
// id
std::string id = inputStorage.readString();
MSLane *l = MSLane::dictionary(id);
if (l==0) {
server.writeStatusCmd(CMD_SET_LANE_VARIABLE, RTYPE_ERR, "Lane '" + id + "' is not known", outputStorage);
return false;
}
// process
int valueDataType = inputStorage.readUnsignedByte();
switch (variable) {
case VAR_MAXSPEED: {
// speed
if (valueDataType!=TYPE_FLOAT) {
server.writeStatusCmd(CMD_SET_LANE_VARIABLE, RTYPE_ERR, "The speed must be given as a float.", outputStorage);
return false;
}
SUMOReal val = inputStorage.readFloat();
l->setMaxSpeed(val);
}
break;
case VAR_LENGTH: {
// speed
if (valueDataType!=TYPE_FLOAT) {
server.writeStatusCmd(CMD_SET_LANE_VARIABLE, RTYPE_ERR, "The length must be given as a float.", outputStorage);
return false;
}
SUMOReal val = inputStorage.readFloat();
l->setLength(val);
}
break;
case LANE_ALLOWED: {
if (valueDataType!=TYPE_STRINGLIST) {
server.writeStatusCmd(CMD_SET_LANE_VARIABLE, RTYPE_ERR, "Allowed classes must be given as a list of strings.", outputStorage);
return false;
}
std::vector<SUMOVehicleClass> allowed;
parseVehicleClasses(inputStorage.readStringList(), allowed);
l->setAllowedClasses(allowed);
l->getEdge().rebuildAllowedLanes();
}
break;
case LANE_DISALLOWED: {
if (valueDataType!=TYPE_STRINGLIST) {
server.writeStatusCmd(CMD_SET_LANE_VARIABLE, RTYPE_ERR, "Not allowed classes must be given as a list of strings.", outputStorage);
return false;
}
std::vector<SUMOVehicleClass> disallowed;
parseVehicleClasses(inputStorage.readStringList(), disallowed);
l->setNotAllowedClasses(disallowed);
l->getEdge().rebuildAllowedLanes();
}
break;
default:
break;
}
server.writeStatusCmd(CMD_SET_LANE_VARIABLE, RTYPE_OK, warning, outputStorage);
return true;
}
// ===========================================================================
// 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;
}
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;
}
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;
}
}
}
}
// ===========================================================================
// member method definitions
// ===========================================================================
bool
GUISelectionLoader::loadSelection(const std::string &file, std::string &msg) throw() {
// ok, load all
std::map<std::string, int> typeMap;
typeMap["edge"] = GLO_EDGE;
typeMap["induct loop"] = GLO_DETECTOR;
typeMap["junction"] = GLO_JUNCTION;
typeMap["speedtrigger"] = GLO_TRIGGER;
typeMap["lane"] = GLO_LANE;
typeMap["tl-logic"] = GLO_TLLOGIC;
typeMap["vehicle"] = GLO_VEHICLE;
std::ifstream strm(file.c_str());
if (!strm.good()) {
msg = "Could not open '" + file + "'.";
return false;
}
while (strm.good()) {
std::string line;
strm >> line;
if (line.length()==0) {
continue;
}
size_t idx = line.find(':');
if (idx!=std::string::npos) {
std::string type = line.substr(0, idx);
std::string name = line.substr(idx+1);
if (typeMap.find(type)==typeMap.end()) {
msg = "Unknown type '" + type + "' occured.";
continue;
}
int itype = typeMap[type];
int oid = -1;
switch (itype) {
case GLO_VEHICLE: {}
break;
case GLO_TLLOGIC: {}
break;
case GLO_DETECTOR: {}
break;
case GLO_EMITTER: {}
break;
case GLO_LANE: {
MSLane *l = MSLane::dictionary(name);
if (l!=0) {
oid = static_cast<GUIEdge&>(l->getEdge()).getLaneGeometry(l).getGlID();
}
}
break;
case GLO_EDGE: {
MSEdge *e = MSEdge::dictionary(name);
if (e!=0) {
oid = static_cast<const GUIEdge * const>(e)->getGlID();
}
}
break;
case GLO_JUNCTION: {}
break;
case GLO_TRIGGER: {}
break;
}
if (oid>=0) {
gSelected.select(itype, oid, false);
} else {
msg = "Item '" + line + "' not found";
continue;
}
} else {
msg = "Could not parse entry while loading selection.";
continue;
}
}
return true;
}
