SUMO - Simulation of Urban MObility

TraCIServerAPI_Simulation.cpp

Go to the documentation of this file.
00001 /****************************************************************************/
00010 // APIs for getting/setting edge values via TraCI
00011 /****************************************************************************/
00012 // SUMO, Simulation of Urban MObility; see http://sumo.sourceforge.net/
00013 // Copyright (C) 2001-2012 DLR (http://www.dlr.de/) and contributors
00014 /****************************************************************************/
00015 //
00016 //   This file is part of SUMO.
00017 //   SUMO is free software: you can redistribute it and/or modify
00018 //   it under the terms of the GNU General Public License as published by
00019 //   the Free Software Foundation, either version 3 of the License, or
00020 //   (at your option) any later version.
00021 //
00022 /****************************************************************************/
00023 
00024 
00025 // ===========================================================================
00026 // included modules
00027 // ===========================================================================
00028 #ifdef _MSC_VER
00029 #include <windows_config.h>
00030 #else
00031 #include <config.h>
00032 #endif
00033 
00034 #include <utils/common/StdDefs.h>
00035 #include <utils/geom/GeoConvHelper.h>
00036 #include <microsim/MSNet.h>
00037 #include <microsim/MSEdgeControl.h>
00038 #include <microsim/MSInsertionControl.h>
00039 #include <microsim/MSLane.h>
00040 #include <microsim/MSVehicle.h>
00041 #include "TraCIConstants.h"
00042 #include "TraCIDijkstraRouter.h"
00043 #include "TraCIServerAPI_Simulation.h"
00044 
00045 #ifdef CHECK_MEMORY_LEAKS
00046 #include <foreign/nvwa/debug_new.h>
00047 #endif // CHECK_MEMORY_LEAKS
00048 
00049 
00050 // ===========================================================================
00051 // used namespaces
00052 // ===========================================================================
00053 using namespace traci;
00054 
00055 
00056 // ===========================================================================
00057 // method definitions
00058 // ===========================================================================
00059 bool
00060 TraCIServerAPI_Simulation::processGet(TraCIServer& server, tcpip::Storage& inputStorage,
00061                                       tcpip::Storage& outputStorage) {
00062     std::string warning = ""; // additional description for response
00063     // variable & id
00064     int variable = inputStorage.readUnsignedByte();
00065     std::string id = inputStorage.readString();
00066     // check variable
00067     if (variable != VAR_TIME_STEP
00068             && variable != VAR_LOADED_VEHICLES_NUMBER && variable != VAR_LOADED_VEHICLES_IDS
00069             && variable != VAR_DEPARTED_VEHICLES_NUMBER && variable != VAR_DEPARTED_VEHICLES_IDS
00070             && variable != VAR_TELEPORT_STARTING_VEHICLES_NUMBER && variable != VAR_TELEPORT_STARTING_VEHICLES_IDS
00071             && variable != VAR_TELEPORT_ENDING_VEHICLES_NUMBER && variable != VAR_TELEPORT_ENDING_VEHICLES_IDS
00072             && variable != VAR_ARRIVED_VEHICLES_NUMBER && variable != VAR_ARRIVED_VEHICLES_IDS
00073             && variable != VAR_DELTA_T && variable != VAR_NET_BOUNDING_BOX
00074             && variable != VAR_MIN_EXPECTED_VEHICLES
00075             && variable != POSITION_CONVERSION && variable != DISTANCE_REQUEST
00076        ) {
00077         server.writeStatusCmd(CMD_GET_SIM_VARIABLE, RTYPE_ERR, "Get Simulation Variable: unsupported variable specified", outputStorage);
00078         return false;
00079     }
00080     // begin response building
00081     tcpip::Storage tempMsg;
00082     //  response-code, variableID, objectID
00083     tempMsg.writeUnsignedByte(RESPONSE_GET_SIM_VARIABLE);
00084     tempMsg.writeUnsignedByte(variable);
00085     tempMsg.writeString(id);
00086     // process request
00087     switch (variable) {
00088         case VAR_TIME_STEP:
00089             tempMsg.writeUnsignedByte(TYPE_INTEGER);
00090             tempMsg.writeInt(MSNet::getInstance()->getCurrentTimeStep());
00091             break;
00092         case VAR_LOADED_VEHICLES_NUMBER: {
00093             const std::vector<std::string> &ids = server.getVehicleStateChanges().find(MSNet::VEHICLE_STATE_BUILT)->second;
00094             tempMsg.writeUnsignedByte(TYPE_INTEGER);
00095             tempMsg.writeInt((int) ids.size());
00096         }
00097         break;
00098         case VAR_LOADED_VEHICLES_IDS: {
00099             const std::vector<std::string> &ids = server.getVehicleStateChanges().find(MSNet::VEHICLE_STATE_BUILT)->second;
00100             tempMsg.writeUnsignedByte(TYPE_STRINGLIST);
00101             tempMsg.writeStringList(ids);
00102         }
00103         break;
00104         case VAR_DEPARTED_VEHICLES_NUMBER: {
00105             const std::vector<std::string> &ids = server.getVehicleStateChanges().find(MSNet::VEHICLE_STATE_DEPARTED)->second;
00106             tempMsg.writeUnsignedByte(TYPE_INTEGER);
00107             tempMsg.writeInt((int) ids.size());
00108         }
00109         break;
00110         case VAR_DEPARTED_VEHICLES_IDS: {
00111             const std::vector<std::string> &ids = server.getVehicleStateChanges().find(MSNet::VEHICLE_STATE_DEPARTED)->second;
00112             tempMsg.writeUnsignedByte(TYPE_STRINGLIST);
00113             tempMsg.writeStringList(ids);
00114         }
00115         break;
00116         case VAR_TELEPORT_STARTING_VEHICLES_NUMBER: {
00117             const std::vector<std::string> &ids = server.getVehicleStateChanges().find(MSNet::VEHICLE_STATE_STARTING_TELEPORT)->second;
00118             tempMsg.writeUnsignedByte(TYPE_INTEGER);
00119             tempMsg.writeInt((int) ids.size());
00120         }
00121         break;
00122         case VAR_TELEPORT_STARTING_VEHICLES_IDS: {
00123             const std::vector<std::string> &ids = server.getVehicleStateChanges().find(MSNet::VEHICLE_STATE_STARTING_TELEPORT)->second;
00124             tempMsg.writeUnsignedByte(TYPE_STRINGLIST);
00125             tempMsg.writeStringList(ids);
00126         }
00127         break;
00128         case VAR_TELEPORT_ENDING_VEHICLES_NUMBER: {
00129             const std::vector<std::string> &ids = server.getVehicleStateChanges().find(MSNet::VEHICLE_STATE_ENDING_TELEPORT)->second;
00130             tempMsg.writeUnsignedByte(TYPE_INTEGER);
00131             tempMsg.writeInt((int) ids.size());
00132         }
00133         break;
00134         case VAR_TELEPORT_ENDING_VEHICLES_IDS: {
00135             const std::vector<std::string> &ids = server.getVehicleStateChanges().find(MSNet::VEHICLE_STATE_ENDING_TELEPORT)->second;
00136             tempMsg.writeUnsignedByte(TYPE_STRINGLIST);
00137             tempMsg.writeStringList(ids);
00138         }
00139         break;
00140         case VAR_ARRIVED_VEHICLES_NUMBER: {
00141             const std::vector<std::string> &ids = server.getVehicleStateChanges().find(MSNet::VEHICLE_STATE_ARRIVED)->second;
00142             tempMsg.writeUnsignedByte(TYPE_INTEGER);
00143             tempMsg.writeInt((int) ids.size());
00144         }
00145         break;
00146         case VAR_ARRIVED_VEHICLES_IDS: {
00147             const std::vector<std::string> &ids = server.getVehicleStateChanges().find(MSNet::VEHICLE_STATE_ARRIVED)->second;
00148             tempMsg.writeUnsignedByte(TYPE_STRINGLIST);
00149             tempMsg.writeStringList(ids);
00150         }
00151         break;
00152         case VAR_DELTA_T:
00153             tempMsg.writeUnsignedByte(TYPE_INTEGER);
00154             tempMsg.writeInt(DELTA_T);
00155             break;
00156         case VAR_NET_BOUNDING_BOX: {
00157             tempMsg.writeUnsignedByte(TYPE_BOUNDINGBOX);
00158             Boundary b = GeoConvHelper::getFinal().getConvBoundary();
00159             tempMsg.writeDouble(b.xmin());
00160             tempMsg.writeDouble(b.ymin());
00161             tempMsg.writeDouble(b.xmax());
00162             tempMsg.writeDouble(b.ymax());
00163             break;
00164         }
00165         break;
00166         case VAR_MIN_EXPECTED_VEHICLES: {
00167             const std::vector<std::string> &ids = server.getVehicleStateChanges().find(MSNet::VEHICLE_STATE_ARRIVED)->second;
00168             tempMsg.writeUnsignedByte(TYPE_INTEGER);
00169             tempMsg.writeInt(MSNet::getInstance()->getVehicleControl().getActiveVehicleCount() + MSNet::getInstance()->getInsertionControl().getPendingFlowCount());
00170         }
00171         break;
00172         case POSITION_CONVERSION:
00173             if (inputStorage.readUnsignedByte() != TYPE_COMPOUND) {
00174                 server.writeStatusCmd(CMD_GET_SIM_VARIABLE, RTYPE_ERR, "Position conversion requires a compound object.", outputStorage);
00175                 return false;
00176             }
00177             if (inputStorage.readInt() != 2) {
00178                 server.writeStatusCmd(CMD_GET_SIM_VARIABLE, RTYPE_ERR, "Position conversion requires a source position and a position type as parameter.", outputStorage);
00179                 return false;
00180             }
00181             if (!commandPositionConversion(server, inputStorage, tempMsg, CMD_GET_SIM_VARIABLE)) {
00182                 return false;
00183             }
00184             break;
00185         case DISTANCE_REQUEST:
00186             if (inputStorage.readUnsignedByte() != TYPE_COMPOUND) {
00187                 server.writeStatusCmd(CMD_GET_SIM_VARIABLE, RTYPE_ERR, "Retrieval of distance requires a compound object.", outputStorage);
00188                 return false;
00189             }
00190             if (inputStorage.readInt() != 3) {
00191                 server.writeStatusCmd(CMD_GET_SIM_VARIABLE, RTYPE_ERR, "Retrieval of distance requires two positions and a distance type as parameter.", outputStorage);
00192                 return false;
00193             }
00194             if (!commandDistanceRequest(server, inputStorage, tempMsg, CMD_GET_SIM_VARIABLE)) {
00195                 return false;
00196             }
00197             break;
00198         default:
00199             break;
00200     }
00201     server.writeStatusCmd(CMD_GET_SIM_VARIABLE, RTYPE_OK, warning, outputStorage);
00202     server.writeResponseWithLength(outputStorage, tempMsg);
00203     return true;
00204 }
00205 
00206 
00207 std::pair<MSLane*, SUMOReal>
00208 TraCIServerAPI_Simulation::convertCartesianToRoadMap(Position pos) {
00209     std::pair<MSLane*, SUMOReal> result;
00210     std::vector<std::string> allEdgeIds;
00211     SUMOReal minDistance = std::numeric_limits<SUMOReal>::max();
00212 
00213     allEdgeIds = MSNet::getInstance()->getEdgeControl().getEdgeNames();
00214     for (std::vector<std::string>::iterator itId = allEdgeIds.begin(); itId != allEdgeIds.end(); itId++) {
00215         const std::vector<MSLane*> &allLanes = MSEdge::dictionary((*itId))->getLanes();
00216         for (std::vector<MSLane*>::const_iterator itLane = allLanes.begin(); itLane != allLanes.end(); itLane++) {
00217             const SUMOReal newDistance = (*itLane)->getShape().distance(pos);
00218             if (newDistance < minDistance) {
00219                 minDistance = newDistance;
00220                 result.first = (*itLane);
00221             }
00222         }
00223     }
00224     result.second = result.first->getShape().nearest_position_on_line_to_point(pos, false);
00225     return result;
00226 }
00227 
00228 
00229 const MSLane*
00230 TraCIServerAPI_Simulation::getLaneChecking(std::string roadID, int laneIndex, SUMOReal pos) {
00231     const MSEdge* edge = MSEdge::dictionary(roadID);
00232     if (edge == 0) {
00233         throw TraCIException("Unknown edge " + roadID);
00234     }
00235     if (laneIndex < 0 || laneIndex >= (int)edge->getLanes().size()) {
00236         throw TraCIException("Invalid lane index for " + roadID);
00237     }
00238     const MSLane* lane = edge->getLanes()[laneIndex];
00239     if (pos < 0 || pos > lane->getLength()) {
00240         throw TraCIException("Position on lane invalid");
00241     }
00242     return lane;
00243 }
00244 
00245 
00246 bool
00247 TraCIServerAPI_Simulation::commandPositionConversion(traci::TraCIServer& server, tcpip::Storage& inputStorage,
00248         tcpip::Storage& outputStorage, int commandId) {
00249     tcpip::Storage tmpResult;
00250     std::pair<MSLane*, SUMOReal> roadPos;
00251     Position cartesianPos;
00252     Position geoPos;
00253     SUMOReal z = 0;
00254 
00255     // actual position type that will be converted
00256     int srcPosType = inputStorage.readUnsignedByte();
00257 
00258     switch (srcPosType) {
00259         case POSITION_2D:
00260         case POSITION_3D:
00261         case POSITION_LAT_LON:
00262         case POSITION_LAT_LON_ALT: {
00263             SUMOReal x = inputStorage.readDouble();
00264             SUMOReal y = inputStorage.readDouble();
00265             if (srcPosType != POSITION_2D && srcPosType != POSITION_LAT_LON) {
00266                 z = inputStorage.readDouble();
00267             }
00268             geoPos.set(x, y);
00269             cartesianPos.set(x, y);
00270             if (srcPosType == POSITION_LAT_LON || srcPosType == POSITION_LAT_LON_ALT) {
00271                 GeoConvHelper::getFinal().x2cartesian_const(cartesianPos);
00272             } else {
00273                 GeoConvHelper::getFinal().cartesian2geo(geoPos);
00274             }
00275         }
00276         break;
00277         case POSITION_ROADMAP: {
00278             std::string roadID = inputStorage.readString();
00279             SUMOReal pos = inputStorage.readDouble();
00280             int laneIdx = inputStorage.readUnsignedByte();
00281             try {
00282                 cartesianPos = geoPos = getLaneChecking(roadID, laneIdx, pos)->getShape().positionAtLengthPosition(pos);
00283                 GeoConvHelper::getFinal().cartesian2geo(geoPos);
00284             } catch (TraCIException& e) {
00285                 server.writeStatusCmd(commandId, RTYPE_ERR, e.what());
00286                 return false;
00287             }
00288         }
00289         break;
00290         default:
00291             server.writeStatusCmd(commandId, RTYPE_ERR,
00292                                   "Source position type not supported");
00293             return false;
00294     }
00295 
00296     int destPosType = inputStorage.readUnsignedByte();
00297 
00298     switch (destPosType) {
00299         case POSITION_ROADMAP: {
00300             if (commandId != CMD_POSITIONCONVERSION) {
00301                 // skip empty values
00302                 inputStorage.readString();
00303                 inputStorage.readDouble();
00304                 inputStorage.readUnsignedByte();
00305             }
00306             // convert road map to 3D position
00307             roadPos = convertCartesianToRoadMap(cartesianPos);
00308 
00309             // write result that is added to response msg
00310             tmpResult.writeUnsignedByte(POSITION_ROADMAP);
00311             tmpResult.writeString(roadPos.first->getEdge().getID());
00312             tmpResult.writeDouble(roadPos.second);
00313             const std::vector<MSLane*> lanes = roadPos.first->getEdge().getLanes();
00314             tmpResult.writeUnsignedByte((int)distance(lanes.begin(), find(lanes.begin(), lanes.end(), roadPos.first)));
00315         }
00316         break;
00317         case POSITION_2D:
00318         case POSITION_3D:
00319         case POSITION_LAT_LON:
00320         case POSITION_LAT_LON_ALT:
00321             if (commandId != CMD_POSITIONCONVERSION) {
00322                 // skip empty values
00323                 inputStorage.readDouble();
00324                 inputStorage.readDouble();
00325                 if (destPosType != POSITION_2D && destPosType != POSITION_LAT_LON) {
00326                     inputStorage.readDouble();
00327                 }
00328             }
00329             tmpResult.writeUnsignedByte(destPosType);
00330             if (srcPosType == POSITION_LAT_LON || srcPosType == POSITION_LAT_LON_ALT) {
00331                 tmpResult.writeDouble(geoPos.x());
00332                 tmpResult.writeDouble(geoPos.y());
00333             } else {
00334                 tmpResult.writeDouble(cartesianPos.x());
00335                 tmpResult.writeDouble(cartesianPos.y());
00336             }
00337             if (destPosType != POSITION_2D && destPosType != POSITION_LAT_LON) {
00338                 tmpResult.writeDouble(z);
00339             }
00340             break;
00341         default:
00342             server.writeStatusCmd(commandId, RTYPE_ERR,
00343                                   "Destination position type not supported");
00344             return false;
00345     }
00346     if (commandId == CMD_POSITIONCONVERSION) {
00347         // add converted Position to response
00348         outputStorage.writeUnsignedByte(1 + 1 + (int)tmpResult.size() + 1); // length
00349         outputStorage.writeUnsignedByte(commandId); // command id
00350         outputStorage.writeStorage(tmpResult);  // position dependant part
00351         outputStorage.writeUnsignedByte(destPosType);   // destination type
00352     } else {
00353         outputStorage.writeStorage(tmpResult);  // position dependant part
00354     }
00355     return true;
00356 }
00357 
00358 /****************************************************************************/
00359 
00360 bool
00361 TraCIServerAPI_Simulation::commandDistanceRequest(traci::TraCIServer& server, tcpip::Storage& inputStorage,
00362         tcpip::Storage& outputStorage, int commandId) {
00363     Position pos1;
00364     Position pos2;
00365     std::pair<const MSLane*, SUMOReal> roadPos1;
00366     std::pair<const MSLane*, SUMOReal> roadPos2;
00367 
00368     // read position 1
00369     int posType = inputStorage.readUnsignedByte();
00370     switch (posType) {
00371         case POSITION_ROADMAP:
00372             try {
00373                 std::string roadID = inputStorage.readString();
00374                 roadPos1.second = inputStorage.readDouble();
00375                 roadPos1.first = getLaneChecking(roadID, inputStorage.readUnsignedByte(), roadPos1.second);
00376                 pos1 = roadPos1.first->getShape().positionAtLengthPosition(roadPos1.second);
00377             } catch (TraCIException& e) {
00378                 server.writeStatusCmd(commandId, RTYPE_ERR, e.what());
00379                 return false;
00380             }
00381             break;
00382         case POSITION_2D:
00383         case POSITION_3D: {
00384             SUMOReal p1x = inputStorage.readDouble();
00385             SUMOReal p1y = inputStorage.readDouble();
00386             pos1.set(p1x, p1y);
00387         }
00388         if (posType == POSITION_3D) {
00389             inputStorage.readDouble();      // z value is ignored
00390         }
00391         roadPos1 = convertCartesianToRoadMap(pos1);
00392         break;
00393         default:
00394             server.writeStatusCmd(commandId, RTYPE_ERR, "Unknown position format used for distance request");
00395             return false;
00396     }
00397 
00398     // read position 2
00399     posType = inputStorage.readUnsignedByte();
00400     switch (posType) {
00401         case POSITION_ROADMAP:
00402             try {
00403                 std::string roadID = inputStorage.readString();
00404                 roadPos2.second = inputStorage.readDouble();
00405                 roadPos2.first = getLaneChecking(roadID, inputStorage.readUnsignedByte(), roadPos2.second);
00406                 pos2 = roadPos2.first->getShape().positionAtLengthPosition(roadPos2.second);
00407             } catch (TraCIException& e) {
00408                 server.writeStatusCmd(commandId, RTYPE_ERR, e.what());
00409                 return false;
00410             }
00411             break;
00412         case POSITION_2D:
00413         case POSITION_3D: {
00414             SUMOReal p2x = inputStorage.readDouble();
00415             SUMOReal p2y = inputStorage.readDouble();
00416             pos2.set(p2x, p2y);
00417         }
00418         if (posType == POSITION_3D) {
00419             inputStorage.readDouble();      // z value is ignored
00420         }
00421         roadPos2 = convertCartesianToRoadMap(pos2);
00422         break;
00423         default:
00424             server.writeStatusCmd(commandId, RTYPE_ERR, "Unknown position format used for distance request");
00425             return false;
00426     }
00427 
00428     // read distance type
00429     int distType = inputStorage.readUnsignedByte();
00430 
00431     SUMOReal distance = 0.0;
00432     if (distType == REQUEST_DRIVINGDIST) {
00433         // compute driving distance
00434         std::vector<const MSEdge*> edges;
00435         TraCIDijkstraRouter<MSEdge> router(MSEdge::dictSize());
00436 
00437         if ((roadPos1.first == roadPos2.first)
00438                 && (roadPos1.second <= roadPos2.second)) {
00439             distance = roadPos2.second - roadPos1.second;
00440         } else {
00441             router.compute(&roadPos1.first->getEdge(), &roadPos2.first->getEdge(), NULL,
00442                            MSNet::getInstance()->getCurrentTimeStep(), edges);
00443             MSRoute route("", edges, false, RGBColor::DEFAULT_COLOR, std::vector<SUMOVehicleParameter::Stop>());
00444             distance = route.getDistanceBetween(roadPos1.second, roadPos2.second,
00445                                                 &roadPos1.first->getEdge(), &roadPos2.first->getEdge());
00446         }
00447     } else {
00448         // compute air distance (default)
00449         // correct the distance type in case it was not valid
00450         distType = REQUEST_AIRDIST;
00451         distance = pos1.distanceTo(pos2);
00452     }
00453     // write response command
00454     if (commandId == CMD_DISTANCEREQUEST) {
00455         outputStorage.writeUnsignedByte(1 + 1 + 1 + 8); // length
00456         outputStorage.writeUnsignedByte(commandId);
00457         outputStorage.writeUnsignedByte(distType);
00458     } else {
00459         outputStorage.writeUnsignedByte(TYPE_DOUBLE);
00460     }
00461     outputStorage.writeDouble(distance);
00462     return true;
00463 }
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Friends Defines