void
GUIVehicle::drawAction_drawLinkItems(const GUIVisualizationSettings& s) const {
glTranslated(0, 0, getType() + .2); // draw on top of cars
for (DriveItemVector::const_iterator i = myLFLinkLanes.begin(); i != myLFLinkLanes.end(); ++i) {
if ((*i).myLink == nullptr) {
continue;
}
MSLink* link = (*i).myLink;
MSLane* via = link->getViaLaneOrLane();
if (via != nullptr) {
Position p = via->getShape()[0];
if ((*i).mySetRequest) {
glColor3d(0, .8, 0);
} else {
glColor3d(.8, 0, 0);
}
const SUMOTime leaveTime = (*i).myLink->getLeaveTime(
(*i).myArrivalTime, (*i).myArrivalSpeed, (*i).getLeaveSpeed(), getVehicleType().getLength());
drawLinkItem(p, (*i).myArrivalTime, leaveTime, s.vehicleName.size / s.scale);
// the time slot that ego vehicle uses when checking opened may
// differ from the one it requests in setApproaching
MSLink::ApproachingVehicleInformation avi = (*i).myLink->getApproaching(this);
assert(avi.arrivalTime == (*i).myArrivalTime && avi.leavingTime == leaveTime);
UNUSED_PARAMETER(avi); // only used for assertion
}
}
glTranslated(0, 0, getType() - .2); // draw on top of cars
}
// ===========================================================================
// method definitions
// ===========================================================================
GUIContainerStop::GUIContainerStop(const std::string& id, const std::vector<std::string>& lines, MSLane& lane,
SUMOReal frompos, SUMOReal topos)
: MSStoppingPlace(id, lines, lane, frompos, topos),
GUIGlObject_AbstractAdd("containerStop", GLO_TRIGGER, id) {
myFGShape = lane.getShape();
myFGShape.move2side((SUMOReal) 1.65);
myFGShape = myFGShape.getSubpart(frompos, topos);
myFGShapeRotations.reserve(myFGShape.size() - 1);
myFGShapeLengths.reserve(myFGShape.size() - 1);
int e = (int) myFGShape.size() - 1;
for (int i = 0; i < e; ++i) {
const Position& f = myFGShape[i];
const Position& s = myFGShape[i + 1];
myFGShapeLengths.push_back(f.distanceTo(s));
myFGShapeRotations.push_back((SUMOReal) atan2((s.x() - f.x()), (f.y() - s.y())) * (SUMOReal) 180.0 / (SUMOReal) PI);
}
PositionVector tmp = myFGShape;
tmp.move2side(1.5);
myFGSignPos = tmp.getLineCenter();
myFGSignRot = 0;
if (tmp.length() != 0) {
myFGSignRot = myFGShape.rotationDegreeAtOffset(SUMOReal((myFGShape.length() / 2.)));
myFGSignRot -= 90;
}
}
// ===========================================================================
// method definitions
// ===========================================================================
MSParkingArea::MSParkingArea(const std::string& id,
const std::vector<std::string>& lines,
MSLane& lane,
double begPos, double endPos,
unsigned int capacity,
double width, double length, double angle, const std::string& name) :
MSStoppingPlace(id, lines, lane, begPos, endPos, name),
myCapacity(capacity),
myWidth(width),
myLength(length),
myAngle(angle) {
// initialize unspecified defaults
if (myWidth == 0) {
myWidth = SUMO_const_laneWidth;
}
if (myLength == 0) {
myLength = getSpaceDim();
}
const double offset = MSNet::getInstance()->lefthand() ? -1 : 1;
myShape = lane.getShape().getSubpart(
lane.interpolateLanePosToGeometryPos(begPos),
lane.interpolateLanePosToGeometryPos(endPos));
myShape.move2side((lane.getWidth() / 2. + myWidth / 2.) * offset);
// Initialize space occupancies if there is a road-side capacity
// The overall number of lots is fixed and each lot accepts one vehicle regardless of size
if (myCapacity > 0) {
for (int i = 1; i <= myCapacity; ++i) {
mySpaceOccupancies[i] = LotSpaceDefinition();
mySpaceOccupancies[i].index = i;
mySpaceOccupancies[i].vehicle = 0;
mySpaceOccupancies[i].myWidth = myWidth;
mySpaceOccupancies[i].myLength = myLength;
mySpaceOccupancies[i].myEndPos = myBegPos + getSpaceDim() * i;
const Position& f = myShape.positionAtOffset(getSpaceDim() * (i - 1));
const Position& s = myShape.positionAtOffset(getSpaceDim() * (i));
double lot_angle = ((double) atan2((s.x() - f.x()), (f.y() - s.y())) * (double) 180.0 / (double) M_PI) + myAngle;
mySpaceOccupancies[i].myRotation = lot_angle;
if (myAngle == 0) {
// parking parallel to the road
mySpaceOccupancies[i].myPosition = s;
} else {
// angled parking
mySpaceOccupancies[i].myPosition = (f + s) * 0.5;
}
}
}
computeLastFreePos();
}
Position2D
NLGeomShapeBuilder::getPointPosition(SUMOReal x, SUMOReal y,
const std::string &laneID,
SUMOReal posOnLane) const throw(InvalidArgument) {
if (x!=INVALID_POSITION&&y!=INVALID_POSITION) {
return Position2D(x,y);
}
MSLane *lane = MSLane::dictionary(laneID);
if (lane==0) {
throw InvalidArgument("Lane '" + laneID + "' to place a poi on is not known.");
}
if (posOnLane<0) {
posOnLane = lane->getLength() + posOnLane;
}
return lane->getShape().positionAtLengthPosition(posOnLane);
}
// ===========================================================================
// method definitions
// ===========================================================================
GUIChargingStation::GUIChargingStation(const std::string& id, MSLane& lane, SUMOReal frompos, SUMOReal topos, SUMOReal chargingPower, SUMOReal efficiency, bool chargeInTransit, int chargeDelay) :
MSChargingStation(id, lane, frompos, topos, chargingPower, efficiency, chargeInTransit, chargeDelay),
GUIGlObject_AbstractAdd("chargingStation", GLO_TRIGGER, id) {
myFGShape = lane.getShape();
myFGShape = myFGShape.getSubpart(frompos, topos);
myFGShapeRotations.reserve(myFGShape.size() - 1);
myFGShapeLengths.reserve(myFGShape.size() - 1);
int e = (int) myFGShape.size() - 1;
for (int i = 0; i < e; ++i) {
const Position& f = myFGShape[i];
const Position& s = myFGShape[i + 1];
myFGShapeLengths.push_back(f.distanceTo(s));
myFGShapeRotations.push_back((SUMOReal) atan2((s.x() - f.x()), (f.y() - s.y())) * (SUMOReal) 180.0 / (SUMOReal) PI);
}
PositionVector tmp = myFGShape;
tmp.move2side(1.5);
myFGSignPos = tmp.getLineCenter();
myFGSignRot = 0;
if (tmp.length() != 0) {
myFGSignRot = myFGShape.rotationDegreeAtOffset(SUMOReal((myFGShape.length() / 2.)));
myFGSignRot -= 90;
}
}
void
NLHandler::addConnection(const SUMOSAXAttributes& attrs) {
bool ok = true;
std::string fromID = attrs.get<std::string>(SUMO_ATTR_FROM, 0, ok);
if (!MSGlobals::gUsingInternalLanes && fromID[0] == ':') {
return;
}
try {
bool ok = true;
const std::string toID = attrs.get<std::string>(SUMO_ATTR_TO, 0, ok);
const int fromLaneIdx = attrs.get<int>(SUMO_ATTR_FROM_LANE, 0, ok);
const int toLaneIdx = attrs.get<int>(SUMO_ATTR_TO_LANE, 0, ok);
LinkDirection dir = parseLinkDir(attrs.get<std::string>(SUMO_ATTR_DIR, 0, ok));
LinkState state = parseLinkState(attrs.get<std::string>(SUMO_ATTR_STATE, 0, ok));
std::string tlID = attrs.getOpt<std::string>(SUMO_ATTR_TLID, 0, ok, "");
#ifdef HAVE_INTERNAL_LANES
std::string viaID = attrs.getOpt<std::string>(SUMO_ATTR_VIA, 0, ok, "");
#endif
MSEdge* from = MSEdge::dictionary(fromID);
if (from == 0) {
WRITE_ERROR("Unknown from-edge '" + fromID + "' in connection");
return;
}
MSEdge* to = MSEdge::dictionary(toID);
if (to == 0) {
WRITE_ERROR("Unknown to-edge '" + toID + "' in connection");
return;
}
if (fromLaneIdx < 0 || static_cast<unsigned int>(fromLaneIdx) >= from->getLanes().size() ||
toLaneIdx < 0 || static_cast<unsigned int>(toLaneIdx) >= to->getLanes().size()) {
WRITE_ERROR("Invalid lane index in connection from '" + from->getID() + "' to '" + to->getID() + "'.");
return;
}
MSLane* fromLane = from->getLanes()[fromLaneIdx];
MSLane* toLane = to->getLanes()[toLaneIdx];
assert(fromLane);
assert(toLane);
int tlLinkIdx = -1;
if (tlID != "") {
tlLinkIdx = attrs.get<int>(SUMO_ATTR_TLLINKINDEX, 0, ok);
// make sure that the index is in range
MSTrafficLightLogic* logic = myJunctionControlBuilder.getTLLogic(tlID).getActive();
if (tlLinkIdx < 0 || tlLinkIdx >= (int)logic->getCurrentPhaseDef().getState().size()) {
WRITE_ERROR("Invalid " + toString(SUMO_ATTR_TLLINKINDEX) + " '" + toString(tlLinkIdx) +
"' in connection controlled by '" + tlID + "'");
return;
}
if (!ok) {
return;
}
}
SUMOReal length = fromLane->getShape()[-1].distanceTo(toLane->getShape()[0]);
MSLink* link = 0;
// build the link
#ifdef HAVE_INTERNAL_LANES
MSLane* via = 0;
if (viaID != "" && MSGlobals::gUsingInternalLanes) {
via = MSLane::dictionary(viaID);
if (via == 0) {
WRITE_ERROR("An unknown lane ('" + viaID +
"') should be set as a via-lane for lane '" + toLane->getID() + "'.");
return;
}
length = via->getLength();
}
link = new MSLink(toLane, via, dir, state, length);
if (via != 0) {
via->addIncomingLane(fromLane, link);
} else {
toLane->addIncomingLane(fromLane, link);
}
#else
link = new MSLink(toLane, dir, state, length);
toLane->addIncomingLane(fromLane, link);
#endif
toLane->addApproachingLane(fromLane);
// if a traffic light is responsible for it, inform the traffic light
// check whether this link is controlled by a traffic light
if (tlID != "") {
myJunctionControlBuilder.getTLLogic(tlID).addLink(link, fromLane, tlLinkIdx);
}
// add the link
fromLane->addLink(link);
} catch (InvalidArgument& e) {
WRITE_ERROR(e.what());
}
}
// ===========================================================================
// 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;
}
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
NLSucceedingLaneBuilder::addSuccLane(bool yield, const std::string &laneId,
#ifdef HAVE_INTERNAL_LANES
const std::string &viaID,
SUMOReal pass,
#endif
MSLink::LinkDirection dir,
MSLink::LinkState state,
bool internalEnd,
const std::string &tlid, unsigned int linkNo) throw(InvalidArgument) {
// check whether the link is a dead link
if (laneId=="SUMO_NO_DESTINATION") {
// build the dead link and add it to the container
#ifdef HAVE_INTERNAL_LANES
MSLink *link = new MSLink(0, 0, yield, MSLink::LINKDIR_NODIR, MSLink::LINKSTATE_DEADEND, false, 0.);
#else
MSLink *link = new MSLink(0, yield, MSLink::LINKDIR_NODIR, MSLink::LINKSTATE_DEADEND, 0.);
#endif
mySuccLanes->push_back(link);
if (tlid!="") {
MSTLLogicControl::TLSLogicVariants &logics = myJunctionControlBuilder.getTLLogic(tlid);
MSLane *current = MSLane::dictionary(myCurrentLane);
if (current==0) {
throw InvalidArgument("An unknown lane ('" + myCurrentLane + "') should be assigned to a tl-logic.");
}
logics.addLink(link, current, linkNo);
}
return;
}
// get the lane the link belongs to
MSLane *lane = MSLane::dictionary(laneId);
if (lane==0) {
throw InvalidArgument("An unknown lane ('" + laneId + "') should be set as a follower for lane '" + myCurrentLane + "'.");
}
#ifdef HAVE_INTERNAL_LANES
MSLane *via = 0;
if (viaID!="" && MSGlobals::gUsingInternalLanes) {
via = MSLane::dictionary(viaID);
if (via==0) {
throw InvalidArgument("An unknown lane ('" + viaID + "') should be set as a via-lane for lane '" + myCurrentLane + "'.");
}
}
if (pass>=0) {
static_cast<MSInternalLane*>(lane)->setPassPosition(pass);
}
#endif
MSLane *orig = MSLane::dictionary(myCurrentLane);
if (orig==0) {
return;
}
// build the link
SUMOReal length = orig!=0&&lane!=0
? orig->getShape()[-1].distanceTo(lane->getShape()[0])
: 0;
#ifdef HAVE_INTERNAL_LANES
if (via!=0) {
length = via->getLength();
}
MSLink *link = new MSLink(lane, via, yield, dir, state, internalEnd, length);
#else
MSLink *link = new MSLink(lane, yield, dir, state, length);
#endif
if (MSLane::dictionary(myCurrentLane)!=0) {
#ifdef HAVE_INTERNAL_LANES
if (via!=0) {
// from a normal in to a normal out via
// --> via incomes in out
lane->addIncomingLane(via, link);
// --> in incomes in via
via->addIncomingLane(MSLane::dictionary(myCurrentLane), link);
} else {
if (myCurrentLane[0]!=':') {
// internal not wished; other case already set
lane->addIncomingLane(MSLane::dictionary(myCurrentLane), link);
}
}
#else
lane->addIncomingLane(MSLane::dictionary(myCurrentLane), link);
#endif
}
// if a traffic light is responsible for it, inform the traffic light
// check whether this link is controlled by a traffic light
if (tlid!="") {
MSTLLogicControl::TLSLogicVariants &logics = myJunctionControlBuilder.getTLLogic(tlid);
MSLane *current = MSLane::dictionary(myCurrentLane);
if (current==0) {
throw InvalidArgument("An unknown lane ('" + myCurrentLane + "') should be assigned to a tl-logic.");
}
logics.addLink(link, current, linkNo);
}
// add the link to the container
mySuccLanes->push_back(link);
}
// ===========================================================================
// 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;
}
