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