void
MSBaseVehicle::reroute(SUMOTime t, SUMOAbstractRouter<MSEdge, SUMOVehicle>& router, const bool onInit, const bool withTaz) {
// check whether to reroute
const MSEdge* source = withTaz && onInit ? MSEdge::dictionary(myParameter->fromTaz + "-source") : getRerouteOrigin();
if (source == 0) {
source = getRerouteOrigin();
}
const MSEdge* sink = withTaz ? MSEdge::dictionary(myParameter->toTaz + "-sink") : myRoute->getLastEdge();
if (sink == 0) {
sink = myRoute->getLastEdge();
}
ConstMSEdgeVector edges;
const ConstMSEdgeVector stops = getStopEdges();
for (MSRouteIterator s = stops.begin(); s != stops.end(); ++s) {
if (*s != source) {
// !!! need to adapt t here
router.compute(source, *s, this, t, edges);
source = *s;
edges.pop_back();
}
}
router.compute(source, sink, this, t, edges);
if (!edges.empty() && edges.front()->getPurpose() == MSEdge::EDGEFUNCTION_DISTRICT) {
edges.erase(edges.begin());
}
if (!edges.empty() && edges.back()->getPurpose() == MSEdge::EDGEFUNCTION_DISTRICT) {
edges.pop_back();
}
replaceRouteEdges(edges, onInit);
}
void
Person::rerouteTraveltime(const std::string& personID) {
MSPerson* p = getPerson(personID);
if (p->getNumRemainingStages() == 0) {
throw TraCIException("Person '" + personID + "' has no remaining stages.");
}
const MSEdge* from = p->getEdge();
double departPos = p->getEdgePos();
// reroute to the start of the next-non-walking stage
int firstIndex;
if (p->getCurrentStageType() == MSTransportable::MOVING_WITHOUT_VEHICLE) {
firstIndex = 0;
} else if (p->getCurrentStageType() == MSTransportable::WAITING) {
if (p->getNumRemainingStages() < 2 || p->getStageType(1) != MSTransportable::MOVING_WITHOUT_VEHICLE) {
throw TraCIException("Person '" + personID + "' cannot reroute after the current stop.");
}
firstIndex = 1;
} else {
throw TraCIException("Person '" + personID + "' cannot reroute in stage type '" + toString(p->getCurrentStageType()) + "'.");
}
int nextIndex = firstIndex + 1;
for (; nextIndex < p->getNumRemainingStages(); nextIndex++) {
if (p->getStageType(nextIndex) != MSTransportable::MOVING_WITHOUT_VEHICLE) {
break;
}
}
MSTransportable::Stage* destStage = p->getNextStage(nextIndex - 1);
const MSEdge* to = destStage->getEdges().back();
double arrivalPos = destStage->getArrivalPos();
double speed = p->getVehicleType().getMaxSpeed();
ConstMSEdgeVector newEdges;
MSNet::getInstance()->getPedestrianRouter().compute(from, to, departPos, arrivalPos, speed, 0, nullptr, newEdges);
if (newEdges.empty()) {
throw TraCIException("Could not find new route for person '" + personID + "'.");
}
ConstMSEdgeVector oldEdges = p->getEdges(firstIndex);
assert(!oldEdges.empty());
if (oldEdges.front()->getFunction() != EDGEFUNC_NORMAL) {
oldEdges.erase(oldEdges.begin());
}
//std::cout << " remainingStages=" << p->getNumRemainingStages() << " oldEdges=" << toString(oldEdges) << " newEdges=" << toString(newEdges) << " firstIndex=" << firstIndex << " nextIndex=" << nextIndex << "\n";
if (newEdges == oldEdges && (firstIndex + 1 == nextIndex)) {
return;
}
if (newEdges.front() != from) {
// @note: maybe this should be done automatically by the router
newEdges.insert(newEdges.begin(), from);
}
p->reroute(newEdges, departPos, firstIndex, nextIndex);
}
void
Person::appendWalkingStage(const std::string& personID, const std::vector<std::string>& edgeIDs, double arrivalPos, double duration, double speed, const std::string& stopID) {
MSTransportable* p = getPerson(personID);
ConstMSEdgeVector edges;
try {
MSEdge::parseEdgesList(edgeIDs, edges, "<unknown>");
} catch (ProcessError& e) {
throw TraCIException(e.what());
}
if (edges.empty()) {
throw TraCIException("Empty edge list for walking stage of person '" + personID + "'.");
}
if (fabs(arrivalPos) > edges.back()->getLength()) {
throw TraCIException("Invalid arrivalPos for walking stage of person '" + personID + "'.");
}
if (arrivalPos < 0) {
arrivalPos += edges.back()->getLength();
}
if (speed < 0) {
speed = p->getVehicleType().getMaxSpeed();
}
MSStoppingPlace* bs = nullptr;
if (stopID != "") {
bs = MSNet::getInstance()->getStoppingPlace(stopID, SUMO_TAG_BUS_STOP);
if (bs == nullptr) {
throw TraCIException("Invalid stopping place id '" + stopID + "' for person: '" + personID + "'");
}
}
p->appendStage(new MSPerson::MSPersonStage_Walking(p->getID(), edges, bs, TIME2STEPS(duration), speed, p->getArrivalPos(), arrivalPos, 0));
}
bool
MSBaseVehicle::replaceRouteEdges(ConstMSEdgeVector& edges, bool onInit) {
if (edges.empty()) {
WRITE_WARNING("No route for vehicle '" + getID() + "' found.");
return false;
}
// build a new id, first
std::string id = getID();
if (id[0] != '!') {
id = "!" + id;
}
if (myRoute->getID().find("!var#") != std::string::npos) {
id = myRoute->getID().substr(0, myRoute->getID().rfind("!var#") + 5) + toString(getNumberReroutes() + 1);
} else {
id = id + "!var#1";
}
int oldSize = (int)edges.size();
if (!onInit) {
const MSEdge* const origin = getRerouteOrigin();
if (origin != *myCurrEdge && edges.front() == origin) {
edges.insert(edges.begin(), *myCurrEdge);
oldSize = (int)edges.size();
}
edges.insert(edges.begin(), myRoute->begin(), myCurrEdge);
}
if (edges == myRoute->getEdges()) {
if (onInit) {
// if edges = 'from to' we still need to calculate the arrivalPos once
calculateArrivalParams();
}
return true;
}
const RGBColor& c = myRoute->getColor();
MSRoute* newRoute = new MSRoute(id, edges, false, &c == &RGBColor::DEFAULT_COLOR ? 0 : new RGBColor(c), myRoute->getStops());
if (!MSRoute::dictionary(id, newRoute)) {
delete newRoute;
return false;
}
if (!replaceRoute(newRoute, onInit, (int)edges.size() - oldSize)) {
newRoute->addReference();
newRoute->release();
return false;
}
calculateArrivalParams();
return true;
}