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));
}
void
Person::removeStage(const std::string& personID, int nextStageIndex) {
MSTransportable* p = getPerson(personID);
if (nextStageIndex >= p->getNumRemainingStages()) {
throw TraCIException("The stage index must be lower than the number of remaining stages.");
}
if (nextStageIndex < 0) {
throw TraCIException("The stage index may not be negative.");
}
p->removeStage(nextStageIndex);
}
int
Person::getStage(const std::string& personID, int nextStageIndex) {
MSTransportable* p = getPerson(personID);
if (nextStageIndex >= p->getNumRemainingStages()) {
throw TraCIException("The stage index must be lower than the number of remaining stages.");
}
if (nextStageIndex < (p->getNumRemainingStages() - p->getNumStages())) {
throw TraCIException("The negative stage index must refer to a valid previous stage.");
}
return p->getStageType(nextStageIndex);
}
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::appendWaitingStage(const std::string& personID, double duration, const std::string& description, const std::string& stopID) {
MSTransportable* p = getPerson(personID);
if (duration < 0) {
throw TraCIException("Duration for person: '" + personID + "' must not be negative");
}
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 MSTransportable::Stage_Waiting(p->getArrivalEdge(), nullptr, TIME2STEPS(duration), 0, p->getArrivalPos(), description, false));
}
const MSLane*
TraCIServerAPI_Simulation::getLaneChecking(std::string roadID, int laneIndex, SUMOReal pos) {
const MSEdge* edge = MSEdge::dictionary(roadID);
if (edge == 0) {
throw TraCIException("Unknown edge " + roadID);
}
if (laneIndex < 0 || laneIndex >= (int)edge->getLanes().size()) {
throw TraCIException("Invalid lane index for " + roadID);
}
const MSLane* lane = edge->getLanes()[laneIndex];
if (pos < 0 || pos > lane->getLength()) {
throw TraCIException("Position on lane invalid");
}
return lane;
}
MSTLLogicControl::TLSLogicVariants&
TrafficLight::getTLS(const std::string& id) {
if (!MSNet::getInstance()->getTLSControl().knows(id)) {
throw TraCIException("Traffic light '" + id + "' is not known");
}
return MSNet::getInstance()->getTLSControl().get(id);
}
void
TrafficLight::setProgram(const std::string& tlsID, const std::string& programID) {
try {
getTLS(tlsID).switchTo(MSNet::getInstance()->getTLSControl(), programID);
} catch (ProcessError& e) {
throw TraCIException(e.what());
}
}
void
Person::setType(const std::string& personID, const std::string& typeID) {
MSVehicleType* vehicleType = MSNet::getInstance()->getVehicleControl().getVType(typeID);
if (vehicleType == nullptr) {
throw TraCIException("The vehicle type '" + typeID + "' is not known.");
}
getPerson(personID)->replaceVehicleType(vehicleType);
}
std::vector<std::string>
Person::getEdges(const std::string& personID, int nextStageIndex) {
MSTransportable* p = getPerson(personID);
if (nextStageIndex >= p->getNumRemainingStages()) {
throw TraCIException("The stage index must be lower than the number of remaining stages.");
}
if (nextStageIndex < (p->getNumRemainingStages() - p->getNumStages())) {
throw TraCIException("The negative stage index must refer to a valid previous stage.");
}
std::vector<std::string> edgeIDs;
for (auto& e : p->getEdges(nextStageIndex)) {
if (e != nullptr) {
edgeIDs.push_back(e->getID());
}
}
return edgeIDs;
}
MSVehicleType*
VehicleType::getVType(std::string id) {
MSVehicleType* t = MSNet::getInstance()->getVehicleControl().getVType(id);
if (t == nullptr) {
throw TraCIException("Vehicle type '" + id + "' is not known");
}
return t;
}
MSE2Collector*
LaneArea::getDetector(const std::string& id) {
MSE2Collector* e2 = dynamic_cast<MSE2Collector*>(MSNet::getInstance()->getDetectorControl().getTypedDetectors(SUMO_TAG_LANE_AREA_DETECTOR).get(id));
if (e2 == 0) {
throw TraCIException("Lane area detector '" + id + "' is not known");
}
return e2;
}
MSPerson*
Person::getPerson(const std::string& personID) {
MSTransportableControl& c = MSNet::getInstance()->getPersonControl();
MSPerson* p = dynamic_cast<MSPerson*>(c.get(personID));
if (p == nullptr) {
throw TraCIException("Person '" + personID + "' is not known");
}
return p;
}
void
Person::appendDrivingStage(const std::string& personID, const std::string& toEdge, const std::string& lines, const std::string& stopID) {
MSTransportable* p = getPerson(personID);
const MSEdge* edge = MSEdge::dictionary(toEdge);
if (!edge) {
throw TraCIException("Invalid edge '" + toEdge + "' for person: '" + personID + "'");
}
if (lines.size() == 0) {
return throw TraCIException("Empty lines parameter for person: '" + personID + "'");
}
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_Driving(edge, bs, -NUMERICAL_EPS, StringTokenizer(lines).getVector()));
}
void
TrafficLight::setPhase(const std::string& tlsID, const int index) {
MSTrafficLightLogic* const active = getTLS(tlsID).getActive();
if (index < 0 || active->getPhaseNumber() <= index) {
throw TraCIException("The phase index " + toString(index) + " is not in the allowed range [0,"
+ toString(active->getPhaseNumber() - 1) + "].");
}
const SUMOTime cTime = MSNet::getInstance()->getCurrentTimeStep();
const SUMOTime duration = active->getPhase(index).duration;
active->changeStepAndDuration(MSNet::getInstance()->getTLSControl(), cTime, index, duration);
}
void
Person::moveTo(const std::string& personID, const std::string& edgeID, double /* position */) {
MSPerson* p = getPerson(personID);
MSEdge* e = MSEdge::dictionary(edgeID);
if (e == nullptr) {
throw TraCIException("Unknown edge '" + edgeID + "'.");
}
/*
switch (p->getStageType(0)) {
case MSTransportable::MOVING_WITHOUT_VEHICLE: {
MSPerson::MSPersonStage_Walking* s = dynamic_cast<MSPerson::MSPersonStage_Walking*>(p->getCurrentStage());
assert(s != 0);
const std::string error = s->moveTo(p, Simulation::getCurrentTime());
if (error != "") {
throw TraCIException("Command moveTo failed for person '" + personID + "' (" + error + ").");
}
break;
}
default:
*/
throw TraCIException("Command moveTo is not supported for person '" + personID + "' while " + p->getCurrentStageDescription() + ".");
//}
}
void
TrafficLight::setCompleteRedYellowGreenDefinition(const std::string& tlsID, const TraCILogic& logic) {
MSTLLogicControl::TLSLogicVariants& vars = getTLS(tlsID);
// make sure index and phaseNo are consistent
if (logic.currentPhaseIndex >= (int)logic.phases.size()) {
throw TraCIException("set program: parameter index must be less than parameter phase number.");
}
std::vector<MSPhaseDefinition*> phases;
for (TraCIPhase phase : logic.phases) {
phases.push_back(new MSPhaseDefinition(TIME2STEPS(phase.duration), phase.state, TIME2STEPS(phase.minDur), TIME2STEPS(phase.maxDur), phase.next, phase.name));
}
if (vars.getLogic(logic.programID) == nullptr) {
MSTrafficLightLogic* mslogic = new MSSimpleTrafficLightLogic(MSNet::getInstance()->getTLSControl(), tlsID, logic.programID, TLTYPE_STATIC, phases, logic.currentPhaseIndex, 0, logic.subParameter);
vars.addLogic(logic.programID, mslogic, true, true);
} else {
static_cast<MSSimpleTrafficLightLogic*>(vars.getLogic(logic.programID))->setPhases(phases, logic.currentPhaseIndex);
}
}
void
Person::add(const std::string& personID, const std::string& edgeID, double pos, double departInSecs, const std::string typeID) {
MSTransportable* p;
try {
p = getPerson(personID);
} catch (TraCIException&) {
p = nullptr;
}
if (p != nullptr) {
throw TraCIException("The person " + personID + " to add already exists.");
}
SUMOTime depart = TIME2STEPS(departInSecs);
SUMOVehicleParameter vehicleParams;
vehicleParams.id = personID;
MSVehicleType* vehicleType = MSNet::getInstance()->getVehicleControl().getVType(typeID);
if (!vehicleType) {
throw TraCIException("Invalid type '" + typeID + "' for person '" + personID + "'");
}
const MSEdge* edge = MSEdge::dictionary(edgeID);
if (!edge) {
throw TraCIException("Invalid edge '" + edgeID + "' for person: '" + personID + "'");
}
if (departInSecs < 0.) {
const int proc = (int) - departInSecs;
if (proc >= static_cast<int>(DEPART_DEF_MAX)) {
throw TraCIException("Invalid departure time." + toString(depart) + " " + toString(proc));
}
vehicleParams.departProcedure = (DepartDefinition)proc;
vehicleParams.depart = MSNet::getInstance()->getCurrentTimeStep();
} else if (depart < MSNet::getInstance()->getCurrentTimeStep()) {
vehicleParams.depart = MSNet::getInstance()->getCurrentTimeStep();
WRITE_WARNING("Departure time " + toString(departInSecs) + " for person '" + personID
+ "' is in the past; using current time " + time2string(vehicleParams.depart) + " instead.");
} else {
vehicleParams.depart = depart;
}
vehicleParams.departPosProcedure = DEPART_POS_GIVEN;
if (fabs(pos) > edge->getLength()) {
throw TraCIException("Invalid departure position.");
}
if (pos < 0) {
pos += edge->getLength();
}
vehicleParams.departPos = pos;
SUMOVehicleParameter* params = new SUMOVehicleParameter(vehicleParams);
MSTransportable::MSTransportablePlan* plan = new MSTransportable::MSTransportablePlan();
plan->push_back(new MSTransportable::Stage_Waiting(edge, nullptr, 0, depart, pos, "awaiting departure", true));
try {
MSTransportable* person = MSNet::getInstance()->getPersonControl().buildPerson(params, vehicleType, plan, nullptr);
MSNet::getInstance()->getPersonControl().add(person);
} catch (ProcessError& e) {
delete params;
delete plan;
throw TraCIException(e.what());
}
}
void
Person::moveToXY(const std::string& personID, const std::string& edgeID, const double x, const double y, double angle, const int keepRouteFlag) {
MSPerson* p = getPerson(personID);
bool keepRoute = (keepRouteFlag == 1);
bool mayLeaveNetwork = (keepRouteFlag == 2);
Position pos(x, y);
#ifdef DEBUG_MOVEXY
const double origAngle = angle;
#endif
// angle must be in [0,360] because it will be compared against those returned by naviDegree()
// angle set to INVALID_DOUBLE_VALUE is ignored in the evaluated and later set to the angle of the matched lane
if (angle != INVALID_DOUBLE_VALUE) {
while (angle >= 360.) {
angle -= 360.;
}
while (angle < 0.) {
angle += 360.;
}
}
Position currentPos = p->getPosition();
#ifdef DEBUG_MOVEXY
std::cout << std::endl << "begin person " << p->getID() << " lanePos:" << p->getEdgePos() << " edge:" << Named::getIDSecure(p->getEdge()) << "\n";
std::cout << " want pos:" << pos << " edgeID:" << edgeID << " origAngle:" << origAngle << " angle:" << angle << " keepRoute:" << keepRoute << std::endl;
#endif
ConstMSEdgeVector edges;
MSLane* lane = nullptr;
double lanePos;
double lanePosLat = 0;
double bestDistance = std::numeric_limits<double>::max();
int routeOffset = 0;
bool found = false;
double maxRouteDistance = 100;
ConstMSEdgeVector ev;
ev.push_back(p->getEdge());
int routeIndex = 0;
MSLane* currentLane = const_cast<MSLane*>(getSidewalk<MSEdge, MSLane>(p->getEdge()));
switch (p->getStageType(0)) {
case MSTransportable::MOVING_WITHOUT_VEHICLE: {
MSPerson::MSPersonStage_Walking* s = dynamic_cast<MSPerson::MSPersonStage_Walking*>(p->getCurrentStage());
assert(s != 0);
ev = s->getEdges();
routeIndex = (int)(s->getRouteStep() - s->getRoute().begin());
}
break;
default:
break;
}
if (keepRoute) {
// case a): vehicle is on its earlier route
// we additionally assume it is moving forward (SUMO-limit);
// note that the route ("edges") is not changed in this case
found = Helper::moveToXYMap_matchingRoutePosition(pos, edgeID,
ev, routeIndex,
bestDistance, &lane, lanePos, routeOffset);
} else {
double speed = pos.distanceTo2D(p->getPosition()); // !!!veh->getSpeed();
found = Helper::moveToXYMap(pos, maxRouteDistance, mayLeaveNetwork, edgeID, angle,
speed, ev, routeIndex, currentLane, p->getEdgePos(), true,
bestDistance, &lane, lanePos, routeOffset, edges);
}
if ((found && bestDistance <= maxRouteDistance) || mayLeaveNetwork) {
// compute lateral offset
if (found) {
const double perpDist = lane->getShape().distance2D(pos, false);
if (perpDist != GeomHelper::INVALID_OFFSET) {
lanePosLat = perpDist;
if (!mayLeaveNetwork) {
lanePosLat = MIN2(lanePosLat, 0.5 * (lane->getWidth() + p->getVehicleType().getWidth()));
}
// figure out whether the offset is to the left or to the right
PositionVector tmp = lane->getShape();
try {
tmp.move2side(-lanePosLat); // moved to left
} catch (ProcessError&) {
WRITE_WARNING("Could not determine position on lane '" + lane->getID() + " at lateral position " + toString(-lanePosLat) + ".");
}
//std::cout << " lane=" << lane->getID() << " posLat=" << lanePosLat << " shape=" << lane->getShape() << " tmp=" << tmp << " tmpDist=" << tmp.distance2D(pos) << "\n";
if (tmp.distance2D(pos) > perpDist) {
lanePosLat = -lanePosLat;
}
}
}
if (found && !mayLeaveNetwork && MSGlobals::gLateralResolution < 0) {
// mapped position may differ from pos
pos = lane->geometryPositionAtOffset(lanePos, -lanePosLat);
}
assert((found && lane != 0) || (!found && lane == 0));
if (angle == INVALID_DOUBLE_VALUE) {
if (lane != nullptr) {
angle = GeomHelper::naviDegree(lane->getShape().rotationAtOffset(lanePos));
} else {
// compute angle outside road network from old and new position
angle = GeomHelper::naviDegree(p->getPosition().angleTo2D(pos));
}
}
switch (p->getStageType(0)) {
case MSTransportable::MOVING_WITHOUT_VEHICLE: {
Helper::setRemoteControlled(p, pos, lane, lanePos, lanePosLat, angle, routeOffset, edges, MSNet::getInstance()->getCurrentTimeStep());
break;
}
default:
throw TraCIException("Command moveToXY is not supported for person '" + personID + "' while " + p->getCurrentStageDescription() + ".");
}
} else {
if (lane == nullptr) {
throw TraCIException("Could not map person '" + personID + "' no road found within " + toString(maxRouteDistance) + "m.");
} else {
throw TraCIException("Could not map person '" + personID + "' distance to road is " + toString(bestDistance) + ".");
}
}
}
