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);
}
void
TraCIServer::writeStatusCmd(int commandId, int status, const std::string& description, tcpip::Storage& outputStorage) {
if (status == RTYPE_ERR) {
WRITE_ERROR("Answered with error to command " + toString(commandId) + ": " + description);
} else if (status == RTYPE_NOTIMPLEMENTED) {
WRITE_ERROR("Requested command not implemented (" + toString(commandId) + "): " + description);
}
outputStorage.writeUnsignedByte(1 + 1 + 1 + 4 + static_cast<int>(description.length())); // command length
outputStorage.writeUnsignedByte(commandId); // command type
outputStorage.writeUnsignedByte(status); // status
outputStorage.writeString(description); // description
}
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;
}
bool
TraCIServer::processSingleSubscription(const Subscription& s, tcpip::Storage& writeInto,
std::string& errors) {
bool ok = true;
tcpip::Storage outputStorage;
const int getCommandId = s.contextVars ? s.contextDomain : s.commandId - 0x30;
std::set<std::string> objIDs;
if (s.contextVars) {
PositionVector shape;
if (!findObjectShape(s.commandId, s.id, shape)) {
return false;
}
collectObjectsInRange(s.contextDomain, shape, s.range, objIDs);
} else {
objIDs.insert(s.id);
}
const int numVars = s.contextVars && s.variables.size() == 1 && s.variables[0] == ID_LIST ? 0 : (int)s.variables.size();
for (std::set<std::string>::iterator j = objIDs.begin(); j != objIDs.end(); ++j) {
if (s.contextVars) {
outputStorage.writeString(*j);
}
if (numVars > 0) {
for (std::vector<int>::const_iterator i = s.variables.begin(); i != s.variables.end(); ++i) {
tcpip::Storage message;
message.writeUnsignedByte(*i);
message.writeString(*j);
tcpip::Storage tmpOutput;
if (myExecutors.find(getCommandId) != myExecutors.end()) {
ok &= myExecutors[getCommandId](*this, message, tmpOutput);
} else {
writeStatusCmd(s.commandId, RTYPE_NOTIMPLEMENTED, "Unsupported command specified", tmpOutput);
ok = false;
}
// copy response part
if (ok) {
int length = tmpOutput.readUnsignedByte();
while (--length > 0) {
tmpOutput.readUnsignedByte();
}
int lengthLength = 1;
length = tmpOutput.readUnsignedByte();
if (length == 0) {
lengthLength = 5;
length = tmpOutput.readInt();
}
//read responseType
tmpOutput.readUnsignedByte();
int variable = tmpOutput.readUnsignedByte();
std::string id = tmpOutput.readString();
outputStorage.writeUnsignedByte(variable);
outputStorage.writeUnsignedByte(RTYPE_OK);
length -= (lengthLength + 1 + 4 + (int)id.length());
while (--length > 0) {
outputStorage.writeUnsignedByte(tmpOutput.readUnsignedByte());
}
} else {
//read length
tmpOutput.readUnsignedByte();
//read cmd
tmpOutput.readUnsignedByte();
//read status
tmpOutput.readUnsignedByte();
std::string msg = tmpOutput.readString();
outputStorage.writeUnsignedByte(*i);
outputStorage.writeUnsignedByte(RTYPE_ERR);
outputStorage.writeUnsignedByte(TYPE_STRING);
outputStorage.writeString(msg);
errors = errors + msg;
}
}
}
}
unsigned int length = (1 + 4) + 1 + (4 + (int)(s.id.length())) + 1 + (int)outputStorage.size();
if (s.contextVars) {
length += 4;
}
writeInto.writeUnsignedByte(0); // command length -> extended
writeInto.writeInt(length);
writeInto.writeUnsignedByte(s.commandId + 0x10);
writeInto.writeString(s.id);
if (s.contextVars) {
writeInto.writeUnsignedByte(s.contextDomain);
}
writeInto.writeUnsignedByte(numVars);
if (s.contextVars) {
writeInto.writeInt((int)objIDs.size());
}
if (!s.contextVars || objIDs.size() != 0) {
writeInto.writeStorage(outputStorage);
}
return ok;
}