void
TraCIServerAPI_Simulation::writeStage(tcpip::Storage& outputStorage, const libsumo::TraCIStage& stage) {
outputStorage.writeUnsignedByte(TYPE_COMPOUND);
outputStorage.writeInt(6);
outputStorage.writeUnsignedByte(TYPE_INTEGER);
outputStorage.writeInt(stage.type);
outputStorage.writeUnsignedByte(TYPE_STRING);
outputStorage.writeString(stage.line);
outputStorage.writeUnsignedByte(TYPE_STRING);
outputStorage.writeString(stage.destStop);
outputStorage.writeUnsignedByte(TYPE_STRINGLIST);
outputStorage.writeStringList(stage.edges);
outputStorage.writeUnsignedByte(TYPE_DOUBLE);
outputStorage.writeDouble(stage.travelTime);
outputStorage.writeUnsignedByte(TYPE_DOUBLE);
outputStorage.writeDouble(stage.cost);
outputStorage.writeUnsignedByte(TYPE_STRING);
outputStorage.writeString(stage.intended);
outputStorage.writeUnsignedByte(TYPE_DOUBLE);
outputStorage.writeDouble(stage.depart);
}
bool
TraCIServerAPI_Simulation::commandDistanceRequest(TraCIServer& server, tcpip::Storage& inputStorage,
tcpip::Storage& outputStorage, int commandId) {
Position pos1;
Position pos2;
std::pair<const MSLane*, SUMOReal> roadPos1;
std::pair<const MSLane*, SUMOReal> roadPos2;
// read position 1
int posType = inputStorage.readUnsignedByte();
switch (posType) {
case POSITION_ROADMAP:
try {
std::string roadID = inputStorage.readString();
roadPos1.second = inputStorage.readDouble();
roadPos1.first = getLaneChecking(roadID, inputStorage.readUnsignedByte(), roadPos1.second);
pos1 = roadPos1.first->getShape().positionAtOffset(roadPos1.second);
} catch (TraCIException& e) {
server.writeStatusCmd(commandId, RTYPE_ERR, e.what());
return false;
}
break;
case POSITION_2D:
case POSITION_3D: {
SUMOReal p1x = inputStorage.readDouble();
SUMOReal p1y = inputStorage.readDouble();
pos1.set(p1x, p1y);
}
if (posType == POSITION_3D) {
inputStorage.readDouble(); // z value is ignored
}
roadPos1 = convertCartesianToRoadMap(pos1);
break;
default:
server.writeStatusCmd(commandId, RTYPE_ERR, "Unknown position format used for distance request");
return false;
}
// read position 2
posType = inputStorage.readUnsignedByte();
switch (posType) {
case POSITION_ROADMAP:
try {
std::string roadID = inputStorage.readString();
roadPos2.second = inputStorage.readDouble();
roadPos2.first = getLaneChecking(roadID, inputStorage.readUnsignedByte(), roadPos2.second);
pos2 = roadPos2.first->getShape().positionAtOffset(roadPos2.second);
} catch (TraCIException& e) {
server.writeStatusCmd(commandId, RTYPE_ERR, e.what());
return false;
}
break;
case POSITION_2D:
case POSITION_3D: {
SUMOReal p2x = inputStorage.readDouble();
SUMOReal p2y = inputStorage.readDouble();
pos2.set(p2x, p2y);
}
if (posType == POSITION_3D) {
inputStorage.readDouble(); // z value is ignored
}
roadPos2 = convertCartesianToRoadMap(pos2);
break;
default:
server.writeStatusCmd(commandId, RTYPE_ERR, "Unknown position format used for distance request");
return false;
}
// read distance type
int distType = inputStorage.readUnsignedByte();
SUMOReal distance = 0.0;
if (distType == REQUEST_DRIVINGDIST) {
// compute driving distance
if ((roadPos1.first == roadPos2.first) && (roadPos1.second <= roadPos2.second)) {
// same edge
distance = roadPos2.second - roadPos1.second;
} else {
MSEdgeVector newRoute;
MSNet::getInstance()->getRouterTT().compute(
&roadPos1.first->getEdge(), &roadPos2.first->getEdge(), 0, MSNet::getInstance()->getCurrentTimeStep(), newRoute);
MSRoute route("", newRoute, false, 0, std::vector<SUMOVehicleParameter::Stop>());
distance = route.getDistanceBetween(roadPos1.second, roadPos2.second, &roadPos1.first->getEdge(), &roadPos2.first->getEdge());
}
} else {
// compute air distance (default)
distance = pos1.distanceTo(pos2);
}
// write response command
outputStorage.writeUnsignedByte(TYPE_DOUBLE);
outputStorage.writeDouble(distance);
return true;
}
bool
TraCIServerAPI_Simulation::commandPositionConversion(TraCIServer& server, tcpip::Storage& inputStorage,
tcpip::Storage& outputStorage, int commandId) {
std::pair<MSLane*, SUMOReal> roadPos;
Position cartesianPos;
Position geoPos;
SUMOReal z = 0;
// actual position type that will be converted
int srcPosType = inputStorage.readUnsignedByte();
switch (srcPosType) {
case POSITION_2D:
case POSITION_3D:
case POSITION_LON_LAT:
case POSITION_LON_LAT_ALT: {
SUMOReal x = inputStorage.readDouble();
SUMOReal y = inputStorage.readDouble();
if (srcPosType != POSITION_2D && srcPosType != POSITION_LON_LAT) {
z = inputStorage.readDouble();
}
geoPos.set(x, y);
cartesianPos.set(x, y);
if (srcPosType == POSITION_LON_LAT || srcPosType == POSITION_LON_LAT_ALT) {
GeoConvHelper::getFinal().x2cartesian_const(cartesianPos);
} else {
GeoConvHelper::getFinal().cartesian2geo(geoPos);
}
}
break;
case POSITION_ROADMAP: {
std::string roadID = inputStorage.readString();
SUMOReal pos = inputStorage.readDouble();
int laneIdx = inputStorage.readUnsignedByte();
try {
// convert edge,offset,laneIdx to cartesian position
cartesianPos = geoPos = getLaneChecking(roadID, laneIdx, pos)->getShape().positionAtOffset(pos);
z = cartesianPos.z();
GeoConvHelper::getFinal().cartesian2geo(geoPos);
} catch (TraCIException& e) {
server.writeStatusCmd(commandId, RTYPE_ERR, e.what());
return false;
}
}
break;
default:
server.writeStatusCmd(commandId, RTYPE_ERR, "Source position type not supported");
return false;
}
int destPosType = 0;
if (!server.readTypeCheckingUnsignedByte(inputStorage, destPosType)) {
server.writeStatusCmd(commandId, RTYPE_ERR, "Destination position type must be of type ubyte.");
return false;
}
switch (destPosType) {
case POSITION_ROADMAP: {
// convert cartesion position to edge,offset,lane_index
roadPos = convertCartesianToRoadMap(cartesianPos);
// write result that is added to response msg
outputStorage.writeUnsignedByte(POSITION_ROADMAP);
outputStorage.writeString(roadPos.first->getEdge().getID());
outputStorage.writeDouble(roadPos.second);
const std::vector<MSLane*> lanes = roadPos.first->getEdge().getLanes();
outputStorage.writeUnsignedByte((int)distance(lanes.begin(), find(lanes.begin(), lanes.end(), roadPos.first)));
}
break;
case POSITION_2D:
case POSITION_3D:
case POSITION_LON_LAT:
case POSITION_LON_LAT_ALT:
outputStorage.writeUnsignedByte(destPosType);
if (destPosType == POSITION_LON_LAT || destPosType == POSITION_LON_LAT_ALT) {
outputStorage.writeDouble(geoPos.x());
outputStorage.writeDouble(geoPos.y());
} else {
outputStorage.writeDouble(cartesianPos.x());
outputStorage.writeDouble(cartesianPos.y());
}
if (destPosType != POSITION_2D && destPosType != POSITION_LON_LAT) {
outputStorage.writeDouble(z);
}
break;
default:
server.writeStatusCmd(commandId, RTYPE_ERR, "Destination position type not supported");
return false;
}
return true;
}
bool
TraCIServerAPI_VehicleType::getVariable(const int variable, const MSVehicleType& v, tcpip::Storage& tempMsg) {
switch (variable) {
case VAR_LENGTH:
tempMsg.writeUnsignedByte(TYPE_DOUBLE);
tempMsg.writeDouble(v.getLength());
break;
case VAR_MINGAP:
tempMsg.writeUnsignedByte(TYPE_DOUBLE);
tempMsg.writeDouble(v.getMinGap());
break;
case VAR_MAXSPEED:
tempMsg.writeUnsignedByte(TYPE_DOUBLE);
tempMsg.writeDouble(v.getMaxSpeed());
break;
case VAR_ACCEL:
tempMsg.writeUnsignedByte(TYPE_DOUBLE);
tempMsg.writeDouble(v.getCarFollowModel().getMaxAccel());
break;
case VAR_DECEL:
tempMsg.writeUnsignedByte(TYPE_DOUBLE);
tempMsg.writeDouble(v.getCarFollowModel().getMaxDecel());
break;
case VAR_IMPERFECTION:
tempMsg.writeUnsignedByte(TYPE_DOUBLE);
tempMsg.writeDouble(v.getCarFollowModel().getImperfection());
break;
case VAR_TAU:
tempMsg.writeUnsignedByte(TYPE_DOUBLE);
tempMsg.writeDouble(v.getCarFollowModel().getHeadwayTime());
break;
case VAR_SPEED_FACTOR:
tempMsg.writeUnsignedByte(TYPE_DOUBLE);
tempMsg.writeDouble(v.getSpeedFactor());
break;
case VAR_SPEED_DEVIATION:
tempMsg.writeUnsignedByte(TYPE_DOUBLE);
tempMsg.writeDouble(v.getSpeedDeviation());
break;
case VAR_VEHICLECLASS:
tempMsg.writeUnsignedByte(TYPE_STRING);
tempMsg.writeString(toString(v.getVehicleClass()));
break;
case VAR_EMISSIONCLASS:
tempMsg.writeUnsignedByte(TYPE_STRING);
tempMsg.writeString(getVehicleEmissionTypeName(v.getEmissionClass()));
break;
case VAR_SHAPECLASS:
tempMsg.writeUnsignedByte(TYPE_STRING);
tempMsg.writeString(getVehicleShapeName(v.getGuiShape()));
break;
case VAR_WIDTH:
tempMsg.writeUnsignedByte(TYPE_DOUBLE);
tempMsg.writeDouble(v.getGuiWidth());
break;
case VAR_COLOR:
tempMsg.writeUnsignedByte(TYPE_COLOR);
tempMsg.writeUnsignedByte(static_cast<int>(v.getColor().red() * 255. + 0.5));
tempMsg.writeUnsignedByte(static_cast<int>(v.getColor().green() * 255. + 0.5));
tempMsg.writeUnsignedByte(static_cast<int>(v.getColor().blue() * 255. + 0.5));
tempMsg.writeUnsignedByte(255);
break;
default:
break;
}
return true;
}
