bool
TraCIServerAPI_Route::processSet(TraCIServer& server, tcpip::Storage& inputStorage,
tcpip::Storage& outputStorage) {
std::string warning = ""; // additional description for response
// variable
int variable = inputStorage.readUnsignedByte();
if (variable != ADD && variable != VAR_PARAMETER) {
return server.writeErrorStatusCmd(CMD_SET_ROUTE_VARIABLE, "Change Route State: unsupported variable " + toHex(variable, 2) + " specified", outputStorage);
}
// id
std::string id = inputStorage.readString();
const MSRoute* r = 0;
if (variable != ADD) {
r = MSRoute::dictionary(id);
if (r == 0) {
return server.writeErrorStatusCmd(CMD_SET_ROUTE_VARIABLE, "Route '" + id + "' is not known", outputStorage);
}
}
// process
switch (variable) {
case ADD: {
std::vector<std::string> edgeIDs;
if (!server.readTypeCheckingStringList(inputStorage, edgeIDs)) {
return server.writeErrorStatusCmd(CMD_SET_ROUTE_VARIABLE, "A string list is needed for adding a new route.", outputStorage);
}
//read itemNo
ConstMSEdgeVector edges;
for (std::vector<std::string>::const_iterator i = edgeIDs.begin(); i != edgeIDs.end(); ++i) {
MSEdge* edge = MSEdge::dictionary(*i);
if (edge == 0) {
return server.writeErrorStatusCmd(CMD_SET_ROUTE_VARIABLE, "Unknown edge '" + *i + "' in route.", outputStorage);
}
edges.push_back(edge);
}
const std::vector<SUMOVehicleParameter::Stop> stops;
if (!MSRoute::dictionary(id, new MSRoute(id, edges, true, 0, stops))) {
return server.writeErrorStatusCmd(CMD_SET_ROUTE_VARIABLE, "Could not add route.", outputStorage);
}
}
break;
case VAR_PARAMETER: {
if (inputStorage.readUnsignedByte() != TYPE_COMPOUND) {
return server.writeErrorStatusCmd(CMD_SET_ROUTE_VARIABLE, "A compound object is needed for setting a parameter.", outputStorage);
}
//readt itemNo
inputStorage.readInt();
std::string name;
if (!server.readTypeCheckingString(inputStorage, name)) {
return server.writeErrorStatusCmd(CMD_SET_ROUTE_VARIABLE, "The name of the parameter must be given as a string.", outputStorage);
}
std::string value;
if (!server.readTypeCheckingString(inputStorage, value)) {
return server.writeErrorStatusCmd(CMD_SET_ROUTE_VARIABLE, "The value of the parameter must be given as a string.", outputStorage);
}
((MSRoute*) r)->addParameter(name, value);
}
break;
default:
break;
}
server.writeStatusCmd(CMD_SET_ROUTE_VARIABLE, RTYPE_OK, warning, outputStorage);
return true;
}
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) + ".");
}
}
}
void
GUIVehicle::rerouteDRTStop(MSStoppingPlace* busStop) {
SUMOTime intermediateDuration = TIME2STEPS(20);
SUMOTime finalDuration = SUMOTime_MAX;
if (myParameter->stops.size() >= 2) {
// copy durations from the original stops
intermediateDuration = myParameter->stops.front().duration;
finalDuration = myParameter->stops.back().duration;
}
// if the stop is already in the list of stops, cancel all stops that come
// after it and set the stop duration
std::string line = "";
int destinations = 0;
bool add = true;
for (auto it = myStops.begin(); it != myStops.end(); it++) {
if (!it->reached && destinations < 2 && it->busstop != nullptr) {
line += it->busstop->getID();
destinations++;
}
if (it->busstop == busStop) {
it->duration = finalDuration;
myStops.erase(++it, myStops.end());
add = false;
break;
} else {
it->duration = MIN2(it->duration, intermediateDuration);
}
}
if (destinations < 2) {
line += busStop->getID();
}
if (add) {
// create new stop
SUMOVehicleParameter::Stop stopPar;
stopPar.busstop = busStop->getID();
stopPar.lane = busStop->getLane().getID();
stopPar.startPos = busStop->getBeginLanePosition();
stopPar.endPos = busStop->getEndLanePosition();
stopPar.duration = finalDuration;
stopPar.until = -1;
stopPar.triggered = false;
stopPar.containerTriggered = false;
stopPar.parking = false;
stopPar.index = STOP_INDEX_FIT;
stopPar.parametersSet = STOP_START_SET | STOP_END_SET;
// clean up prior route to improve visualisation, ensure that the stop can be added immediately
ConstMSEdgeVector edges = myRoute->getEdges();
edges.erase(edges.begin(), edges.begin() + getRoutePosition());
edges.push_back(&busStop->getLane().getEdge());
replaceRouteEdges(edges, -1, 0, "DRT.tmp", false, false, false);
std::string errorMsg;
// add stop
addStop(stopPar, errorMsg);
}
const bool hasReroutingDevice = getDevice(typeid(MSDevice_Routing)) != nullptr;
SUMOAbstractRouter<MSEdge, SUMOVehicle>& router = hasReroutingDevice
? MSRoutingEngine::getRouterTT()
: MSNet::getInstance()->getRouterTT();
// reroute to ensure the new stop is reached
reroute(MSNet::getInstance()->getCurrentTimeStep(), "DRT", router);
myParameter->line = line;
assert(haveValidStopEdges());
}
