void Query::ReadGetResponse(tcpip::Storage & content) {
int position = 0;
int length2 = 0;
int extLength = 0;
int length = 0;
int commandId = 0;
int varId = 0;
string objectId = "";
int variableType = 0;
try {
position = content.position();
length2 = content.size();
extLength = content.readUnsignedByte();
if (extLength == 0) {
length = content.readInt();
}
commandId = content.readUnsignedByte();
varId = content.readUnsignedByte();
objectId = content.readString();
variableType = content.readUnsignedByte();
int listLen = 0;
int count = 0;
switch (variableType) {
case TYPE_STRING:
stringValue = content.readString();
break;
case TYPE_INTEGER:
intValue = content.readInt();
break;
case TYPE_DOUBLE:
doubleValue = content.readDouble();
break;
case TYPE_STRINGLIST:
listLen = content.readInt();
stringListValue.clear();
for (int i=0; i<listLen; i++) {
stringListValue.push_back(content.readString());
}
break;
case POSITION_2D:
positionValue.x = content.readDouble();
positionValue.y = content.readDouble();
break;
case TYPE_BOUNDINGBOX:
count = content.size()/sizeof(double);
for (int i = 0; i < count-1; i++) {
vectorValue.push_back(content.readDouble());
}
break;
default:
break;
}
}
catch (exception & ex) {
cout << "can't read response for object " << objectId << ex.what() << endl;
cout<<position<<" "<<length2 << " "<<extLength<<" "<<length<<" "<<commandId<<" "<<varId<<" "<<objectId<<" "<<variableType<< "\n";
}
}
bool
TraCIServer::readTypeCheckingBoundary(tcpip::Storage& inputStorage, Boundary& into) {
if (inputStorage.readUnsignedByte() != TYPE_BOUNDINGBOX) {
return false;
}
const SUMOReal xmin = inputStorage.readDouble();
const SUMOReal ymin = inputStorage.readDouble();
const SUMOReal xmax = inputStorage.readDouble();
const SUMOReal ymax = inputStorage.readDouble();
into.set(xmin, ymin, xmax, ymax);
return true;
}
bool
TraCIServer::readTypeCheckingDouble(tcpip::Storage& inputStorage, double& into) {
if (inputStorage.readUnsignedByte() != TYPE_DOUBLE) {
return false;
}
into = inputStorage.readDouble();
return true;
}
bool
TraCIServer::readTypeCheckingPosition2D(tcpip::Storage& inputStorage, Position& into) {
if (inputStorage.readUnsignedByte() != POSITION_2D) {
return false;
}
SUMOReal x = inputStorage.readDouble();
SUMOReal y = inputStorage.readDouble();
into.set(x, y, 0);
return true;
}
bool
TraCIServer::readTypeCheckingPolygon(tcpip::Storage& inputStorage, PositionVector& into) {
if (inputStorage.readUnsignedByte() != TYPE_POLYGON) {
return false;
}
into.clear();
unsigned int noEntries = inputStorage.readUnsignedByte();
PositionVector shape;
for (unsigned int i = 0; i < noEntries; ++i) {
SUMOReal x = inputStorage.readDouble();
SUMOReal y = inputStorage.readDouble();
into.push_back(Position(x, y));
}
return true;
}
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::setVariable(const int cmd, const int variable, const int valueDataType,
MSVehicleType& v, traci::TraCIServer& server,
tcpip::Storage& inputStorage, tcpip::Storage& outputStorage) {
switch (variable) {
case VAR_LENGTH: {
if (valueDataType != TYPE_DOUBLE) {
server.writeStatusCmd(cmd, RTYPE_ERR, "Setting length requires a double.", outputStorage);
return false;
}
double val = inputStorage.readDouble();
if (val == 0.0 || fabs(val) == std::numeric_limits<double>::infinity()) {
server.writeStatusCmd(cmd, RTYPE_ERR, "Invalid length.", outputStorage);
return false;
}
v.setLength(val);
}
break;
case VAR_MAXSPEED: {
if (valueDataType != TYPE_DOUBLE) {
server.writeStatusCmd(cmd, RTYPE_ERR, "Setting maximum speed requires a double.", outputStorage);
return false;
}
double val = inputStorage.readDouble();
if (val == 0.0 || fabs(val) == std::numeric_limits<double>::infinity()) {
server.writeStatusCmd(cmd, RTYPE_ERR, "Invalid maximum speed.", outputStorage);
return false;
}
v.setMaxSpeed(val);
}
break;
case VAR_VEHICLECLASS: {
if (valueDataType != TYPE_STRING) {
server.writeStatusCmd(cmd, RTYPE_ERR, "Setting vehicle class requires a string.", outputStorage);
return false;
}
v.setVClass(getVehicleClassID(inputStorage.readString()));
}
break;
case VAR_SPEED_FACTOR: {
if (valueDataType != TYPE_DOUBLE) {
server.writeStatusCmd(cmd, RTYPE_ERR, "Setting speed factor requires a double.", outputStorage);
return false;
}
v.setSpeedFactor(inputStorage.readDouble());
}
break;
case VAR_SPEED_DEVIATION: {
if (valueDataType != TYPE_DOUBLE) {
server.writeStatusCmd(cmd, RTYPE_ERR, "Setting speed deviation requires a double.", outputStorage);
return false;
}
v.setSpeedDeviation(inputStorage.readDouble());
}
break;
case VAR_EMISSIONCLASS: {
if (valueDataType != TYPE_STRING) {
server.writeStatusCmd(cmd, RTYPE_ERR, "Setting emission class requires a string.", outputStorage);
return false;
}
v.setEmissionClass(getVehicleEmissionTypeID(inputStorage.readString()));
}
break;
case VAR_WIDTH: {
if (valueDataType != TYPE_DOUBLE) {
server.writeStatusCmd(cmd, RTYPE_ERR, "Setting width requires a double.", outputStorage);
return false;
}
v.setWidth(inputStorage.readDouble());
}
break;
case VAR_MINGAP: {
if (valueDataType != TYPE_DOUBLE) {
server.writeStatusCmd(cmd, RTYPE_ERR, "Setting minimum gap requires a double.", outputStorage);
return false;
}
v.setMinGap(inputStorage.readDouble());
}
break;
case VAR_SHAPECLASS: {
if (valueDataType != TYPE_STRING) {
server.writeStatusCmd(cmd, RTYPE_ERR, "Setting vehicle shape requires a string.", outputStorage);
return false;
}
v.setShape(getVehicleShapeID(inputStorage.readString()));
}
break;
case VAR_ACCEL: {
if (valueDataType != TYPE_DOUBLE) {
server.writeStatusCmd(cmd, RTYPE_ERR, "Setting acceleration requires a double.", outputStorage);
return false;
}
v.getCarFollowModel().setMaxAccel(inputStorage.readDouble());
}
break;
case VAR_DECEL: {
if (valueDataType != TYPE_DOUBLE) {
server.writeStatusCmd(cmd, RTYPE_ERR, "Setting deceleration requires a double.", outputStorage);
return false;
}
v.getCarFollowModel().setMaxDecel(inputStorage.readDouble());
}
break;
case VAR_IMPERFECTION: {
if (valueDataType != TYPE_DOUBLE) {
server.writeStatusCmd(cmd, RTYPE_ERR, "Setting driver imperfection requires a double.", outputStorage);
return false;
}
v.getCarFollowModel().setImperfection(inputStorage.readDouble());
}
break;
case VAR_TAU: {
if (valueDataType != TYPE_DOUBLE) {
server.writeStatusCmd(cmd, RTYPE_ERR, "Setting headway time requires a double.", outputStorage);
return false;
}
v.getCarFollowModel().setHeadwayTime(inputStorage.readDouble());
}
break;
case VAR_COLOR: {
if (valueDataType != TYPE_COLOR) {
server.writeStatusCmd(cmd, RTYPE_ERR, "The color must be given using the according type.", outputStorage);
return false;
}
SUMOReal r = (SUMOReal) inputStorage.readUnsignedByte() / 255.;
SUMOReal g = (SUMOReal) inputStorage.readUnsignedByte() / 255.;
SUMOReal b = (SUMOReal) inputStorage.readUnsignedByte() / 255.;
inputStorage.readUnsignedByte(); // skip alpha level
RGBColor col(r, g, b);
v.setColor(col);
}
break;
default:
break;
}
return true;
}
