bool
MSLaneChanger::continueChange(MSVehicle* vehicle, ChangerIt& from) {
MSAbstractLaneChangeModel& lcm = vehicle->getLaneChangeModel();
const int direction = lcm.getLaneChangeDirection();
const bool pastMidpoint = lcm.updateCompletion();
vehicle->myState.myPosLat += lcm.getLateralSpeed();
vehicle->myCachedPosition = Position::INVALID;
ChangerIt shadow;
if (pastMidpoint) {
ChangerIt to = from + direction;
MSLane* source = myCandi->lane;
MSLane* target = to->lane;
vehicle->myState.myPosLat -= direction * 0.5 * (source->getWidth() + target->getWidth());
lcm.primaryLaneChanged(source, target, direction);
to->lane->myTmpVehicles.insert(to->lane->myTmpVehicles.begin(), vehicle);
to->dens += vehicle->getVehicleType().getLengthWithGap();
to->hoppedVeh = vehicle;
shadow = from;
} else {
from->lane->myTmpVehicles.insert(from->lane->myTmpVehicles.begin(), vehicle);
from->dens += vehicle->getVehicleType().getLengthWithGap();
from->hoppedVeh = vehicle;
shadow = from + lcm.getShadowDirection();
}
if (!lcm.isChangingLanes()) {
vehicle->myState.myPosLat = 0;
lcm.endLaneChangeManeuver();
}
lcm.updateShadowLane();
if (lcm.getShadowLane() != 0) {
// set as hoppedVeh on the shadow lane so it is found as leader on both lanes
shadow->hoppedVeh = vehicle;
}
vehicle->myAngle = vehicle->computeAngle();
#ifdef DEBUG_CONTINUE_CHANGE
if(DEBUG_COND){
std::cout << SIMTIME
<< " continueChange veh=" << vehicle->getID()
<< " from=" << Named::getIDSecure(from->lane)
<< " dir=" << direction
<< " pastMidpoint=" << pastMidpoint
<< " posLat=" << vehicle->getLateralPositionOnLane()
//<< " completion=" << lcm.getLaneChangeCompletion()
<< " shadowLane=" << Named::getIDSecure(lcm.getShadowLane())
<< " shadowHopped=" << Named::getIDSecure(shadow->lane)
<< "\n";
}
#endif
return pastMidpoint;
}
// ===========================================================================
// 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();
}
// ===========================================================================
// method definitions
// ===========================================================================
GUIParkingArea::GUIParkingArea(const std::string& id, const std::vector<std::string>& lines, MSLane& lane,
SUMOReal frompos, SUMOReal topos, unsigned int capacity,
SUMOReal width, SUMOReal length, SUMOReal angle)
: MSParkingArea(id, lines, lane, frompos, topos, capacity, width, length, angle),
GUIGlObject_AbstractAdd("parkingArea", GLO_TRIGGER, id) {
myShapeRotations.reserve(myShape.size() - 1);
myShapeLengths.reserve(myShape.size() - 1);
int e = (int) myShape.size() - 1;
for (int i = 0; i < e; ++i) {
const Position& f = myShape[i];
const Position& s = myShape[i + 1];
myShapeLengths.push_back(f.distanceTo(s));
myShapeRotations.push_back((SUMOReal) atan2((s.x() - f.x()), (f.y() - s.y())) * (SUMOReal) 180.0 / (SUMOReal) PI);
}
PositionVector tmp = myShape;
tmp.move2side(lane.getWidth() + myWidth);
mySignPos = tmp.getLineCenter();
mySignRot = 0;
if (tmp.length() != 0) {
mySignRot = myShape.rotationDegreeAtOffset(SUMOReal((myShape.length() / 2.)));
mySignRot -= 90;
}
}
// ===========================================================================
// method definitions
// ===========================================================================
GUIParkingArea::GUIParkingArea(const std::string& id, const std::vector<std::string>& lines, MSLane& lane,
double frompos, double topos, unsigned int capacity,
double width, double length, double angle, const std::string& name) :
MSParkingArea(id, lines, lane, frompos, topos, capacity, width, length, angle, name),
GUIGlObject_AbstractAdd(GLO_PARKING_AREA, id) {
const double offsetSign = MSNet::getInstance()->lefthand() ? -1 : 1;
myShapeRotations.reserve(myShape.size() - 1);
myShapeLengths.reserve(myShape.size() - 1);
int e = (int) myShape.size() - 1;
for (int i = 0; i < e; ++i) {
const Position& f = myShape[i];
const Position& s = myShape[i + 1];
myShapeLengths.push_back(f.distanceTo(s));
myShapeRotations.push_back((double) atan2((s.x() - f.x()), (f.y() - s.y())) * (double) 180.0 / (double) M_PI);
}
PositionVector tmp = myShape;
tmp.move2side((lane.getWidth() + myWidth) * offsetSign);
mySignPos = tmp.getLineCenter();
mySignRot = 0;
if (tmp.length() != 0) {
mySignRot = myShape.rotationDegreeAtOffset(double((myShape.length() / 2.)));
mySignRot -= 90;
}
}
// ===========================================================================
// 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) + ".");
}
}
}
