bool
TraCIServerAPI_Simulation::commandDistanceRequest(traci::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().positionAtLengthPosition(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().positionAtLengthPosition(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_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;
}
bool
TraCIServerAPI_Simulation::commandPositionConversion(traci::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_LAT_LON:
case POSITION_LAT_LON_ALT: {
SUMOReal x = inputStorage.readDouble();
SUMOReal y = inputStorage.readDouble();
if (srcPosType != POSITION_2D && srcPosType != POSITION_LAT_LON) {
z = inputStorage.readDouble();
}
geoPos.set(x, y);
cartesianPos.set(x, y);
if (srcPosType == POSITION_LAT_LON || srcPosType == POSITION_LAT_LON_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 {
cartesianPos = geoPos = getLaneChecking(roadID, laneIdx, pos)->getShape().positionAtLengthPosition(pos);
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 type = inputStorage.readUnsignedByte();
if (type != TYPE_UBYTE) {
server.writeStatusCmd(commandId, RTYPE_ERR, "Destination position type must be of type ubyte.");
return false;
}
int destPosType = inputStorage.readUnsignedByte();
switch (destPosType) {
case POSITION_ROADMAP: {
// convert road map to 3D position
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_LAT_LON:
case POSITION_LAT_LON_ALT:
outputStorage.writeUnsignedByte(destPosType);
if (destPosType == POSITION_LAT_LON || destPosType == POSITION_LAT_LON_ALT) {
outputStorage.writeDouble(geoPos.x());
outputStorage.writeDouble(geoPos.y());
} else {
outputStorage.writeDouble(cartesianPos.x());
outputStorage.writeDouble(cartesianPos.y());
}
if (destPosType != POSITION_2D && destPosType != POSITION_LAT_LON) {
outputStorage.writeDouble(z);
}
break;
default:
server.writeStatusCmd(commandId, RTYPE_ERR, "Destination position type not supported");
return false;
}
return true;
}