SUMO - Simulation of Urban MObility
TraCIServer.cpp
Go to the documentation of this file.
00001 /****************************************************************************/
00016 /****************************************************************************/
00017 // SUMO, Simulation of Urban MObility; see http://sumo.sourceforge.net/
00018 // Copyright (C) 2001-2012 DLR (http://www.dlr.de/) and contributors
00019 /****************************************************************************/
00020 //
00021 //   This file is part of SUMO.
00022 //   SUMO is free software: you can redistribute it and/or modify
00023 //   it under the terms of the GNU General Public License as published by
00024 //   the Free Software Foundation, either version 3 of the License, or
00025 //   (at your option) any later version.
00026 //
00027 /****************************************************************************/
00028 
00029 // ===========================================================================
00030 // included modules
00031 // ===========================================================================
00032 #ifdef _MSC_VER
00033 #include <windows_config.h>
00034 #else
00035 #include <config.h>
00036 #endif
00037 
00038 #ifdef HAVE_VERSION_H
00039 #include <version.h>
00040 #endif
00041 
00042 #ifndef NO_TRACI
00043 
00044 #ifdef HAVE_PYTHON
00045 #include <Python.h>
00046 #endif
00047 
00048 #include <string>
00049 #include <map>
00050 #include <iostream>
00051 #include <foreign/tcpip/socket.h>
00052 #include <foreign/tcpip/storage.h>
00053 #include <utils/common/SUMOTime.h>
00054 #include <utils/common/DijkstraRouterTT.h>
00055 #include <utils/common/NamedObjectCont.h>
00056 #include <utils/common/RandHelper.h>
00057 #include <utils/common/MsgHandler.h>
00058 #include <utils/common/HelpersHBEFA.h>
00059 #include <utils/common/HelpersHarmonoise.h>
00060 #include <utils/common/SUMOVehicleParameter.h>
00061 #include <utils/shapes/PointOfInterest.h>
00062 #include <utils/shapes/ShapeContainer.h>
00063 #include <utils/shapes/Polygon.h>
00064 #include <utils/xml/XMLSubSys.h>
00065 #include <microsim/MSNet.h>
00066 #include <microsim/MSVehicleControl.h>
00067 #include <microsim/MSVehicle.h>
00068 #include <microsim/MSEdge.h>
00069 #include <microsim/MSJunctionControl.h>
00070 #include <microsim/MSJunction.h>
00071 #include <microsim/MSEdgeControl.h>
00072 #include <microsim/MSLane.h>
00073 #include <microsim/MSGlobals.h>
00074 #include <microsim/traffic_lights/MSTLLogicControl.h>
00075 #include "TraCIConstants.h"
00076 #include "TraCIServer.h"
00077 #include "TraCIServerAPI_InductionLoop.h"
00078 #include "TraCIServerAPI_Junction.h"
00079 #include "TraCIServerAPI_Lane.h"
00080 #include "TraCIServerAPI_MeMeDetector.h"
00081 #include "TraCIServerAPI_TLS.h"
00082 #include "TraCIServerAPI_Vehicle.h"
00083 #include "TraCIServerAPI_VehicleType.h"
00084 #include "TraCIServerAPI_Route.h"
00085 #include "TraCIServerAPI_POI.h"
00086 #include "TraCIServerAPI_Polygon.h"
00087 #include "TraCIServerAPI_Edge.h"
00088 #include "TraCIServerAPI_Simulation.h"
00089 
00090 #ifdef CHECK_MEMORY_LEAKS
00091 #include <foreign/nvwa/debug_new.h>
00092 #endif // CHECK_MEMORY_LEAKS
00093 
00094 
00095 // ===========================================================================
00096 // used namespaces
00097 // ===========================================================================
00098 namespace traci {
00099 
00100 // ===========================================================================
00101 // static member definitions
00102 // ===========================================================================
00103 TraCIServer* TraCIServer::myInstance = 0;
00104 bool TraCIServer::myDoCloseConnection = false;
00105 
00106 
00107 // ===========================================================================
00108 // method definitions
00109 // ===========================================================================
00110 
00111 void
00112 TraCIServer::openSocket(const std::map<int, CmdExecutor> &execs) {
00113     if (myInstance == 0) {
00114         if (!myDoCloseConnection && OptionsCont::getOptions().getInt("remote-port") != 0) {
00115             myInstance = new traci::TraCIServer(OptionsCont::getOptions().getInt("remote-port"));
00116             for (std::map<int, CmdExecutor>::const_iterator i = execs.begin(); i != execs.end(); ++i) {
00117                 myInstance->myExecutors[i->first] = i->second;
00118             }
00119         }
00120     }
00121 }
00122 
00123 /*****************************************************************************/
00124 
00125 TraCIServer::TraCIServer(int port)
00126     : mySocket(0), myTargetTime(0), myDoingSimStep(false), myHaveWarnedDeprecation(false), myAmEmbedded(port == 0) {
00127     myVehicleStateChanges[MSNet::VEHICLE_STATE_BUILT] = std::vector<std::string>();
00128     myVehicleStateChanges[MSNet::VEHICLE_STATE_DEPARTED] = std::vector<std::string>();
00129     myVehicleStateChanges[MSNet::VEHICLE_STATE_STARTING_TELEPORT] = std::vector<std::string>();
00130     myVehicleStateChanges[MSNet::VEHICLE_STATE_ENDING_TELEPORT] = std::vector<std::string>();
00131     myVehicleStateChanges[MSNet::VEHICLE_STATE_ARRIVED] = std::vector<std::string>();
00132     myVehicleStateChanges[MSNet::VEHICLE_STATE_NEWROUTE] = std::vector<std::string>();
00133     MSNet::getInstance()->addVehicleStateListener(this);
00134 
00135     myExecutors[CMD_GET_INDUCTIONLOOP_VARIABLE] = &TraCIServerAPI_InductionLoop::processGet;
00136     myExecutors[CMD_GET_MULTI_ENTRY_EXIT_DETECTOR_VARIABLE] = &TraCIServerAPI_MeMeDetector::processGet;
00137     myExecutors[CMD_GET_TL_VARIABLE] = &TraCIServerAPI_TLS::processGet;
00138     myExecutors[CMD_SET_TL_VARIABLE] = &TraCIServerAPI_TLS::processSet;
00139     myExecutors[CMD_GET_LANE_VARIABLE] = &TraCIServerAPI_Lane::processGet;
00140     myExecutors[CMD_SET_LANE_VARIABLE] = &TraCIServerAPI_Lane::processSet;
00141     myExecutors[CMD_GET_VEHICLE_VARIABLE] = &TraCIServerAPI_Vehicle::processGet;
00142     myExecutors[CMD_SET_VEHICLE_VARIABLE] = &TraCIServerAPI_Vehicle::processSet;
00143     myExecutors[CMD_GET_VEHICLETYPE_VARIABLE] = &TraCIServerAPI_VehicleType::processGet;
00144     myExecutors[CMD_SET_VEHICLETYPE_VARIABLE] = &TraCIServerAPI_VehicleType::processSet;
00145     myExecutors[CMD_GET_ROUTE_VARIABLE] = &TraCIServerAPI_Route::processGet;
00146     myExecutors[CMD_SET_ROUTE_VARIABLE] = &TraCIServerAPI_Route::processSet;
00147     myExecutors[CMD_GET_POI_VARIABLE] = &TraCIServerAPI_POI::processGet;
00148     myExecutors[CMD_SET_POI_VARIABLE] = &TraCIServerAPI_POI::processSet;
00149     myExecutors[CMD_GET_POLYGON_VARIABLE] = &TraCIServerAPI_Polygon::processGet;
00150     myExecutors[CMD_SET_POLYGON_VARIABLE] = &TraCIServerAPI_Polygon::processSet;
00151     myExecutors[CMD_GET_JUNCTION_VARIABLE] = &TraCIServerAPI_Junction::processGet;
00152     myExecutors[CMD_GET_EDGE_VARIABLE] = &TraCIServerAPI_Edge::processGet;
00153     myExecutors[CMD_SET_EDGE_VARIABLE] = &TraCIServerAPI_Edge::processSet;
00154     myExecutors[CMD_GET_SIM_VARIABLE] = &TraCIServerAPI_Simulation::processGet;
00155 
00156     myDoCloseConnection = false;
00157 
00158     // display warning if internal lanes are not used
00159     if (!MSGlobals::gUsingInternalLanes) {
00160         WRITE_WARNING("Starting TraCI without using internal lanes!");
00161         MsgHandler::getWarningInstance()->inform("Vehicles will jump over junctions.", false);
00162         MsgHandler::getWarningInstance()->inform("Use without option --no-internal-links to avoid unexpected behavior", false);
00163     }
00164 
00165     if (!myAmEmbedded) {
00166         try {
00167             WRITE_MESSAGE("***Starting server on port " + toString(port) + " ***");
00168             mySocket = new tcpip::Socket(port);
00169             mySocket->accept();
00170             // When got here, a client has connected
00171         } catch (tcpip::SocketException& e) {
00172             throw ProcessError(e.what());
00173         }
00174     }
00175 }
00176 
00177 /*****************************************************************************/
00178 
00179 TraCIServer::~TraCIServer() {
00180     MSNet::getInstance()->removeVehicleStateListener(this);
00181     if (mySocket != NULL) {
00182         mySocket->close();
00183         delete mySocket;
00184     }
00185 }
00186 
00187 /*****************************************************************************/
00188 
00189 void
00190 TraCIServer::vehicleStateChanged(const SUMOVehicle* const vehicle, MSNet::VehicleState to) {
00191     if (myDoCloseConnection || OptionsCont::getOptions().getInt("remote-port") == 0) {
00192         return;
00193     }
00194     myVehicleStateChanges[to].push_back(vehicle->getID());
00195 }
00196 
00197 /*****************************************************************************/
00198 void
00199 TraCIServer::processCommandsUntilSimStep(SUMOTime step) {
00200     try {
00201         if (myInstance == 0) {
00202             if (!myDoCloseConnection && OptionsCont::getOptions().getInt("remote-port") != 0) {
00203                 myInstance = new traci::TraCIServer(OptionsCont::getOptions().getInt("remote-port"));
00204             } else {
00205                 return;
00206             }
00207         }
00208         if (myInstance->myAmEmbedded || step < myInstance->myTargetTime) {
00209             return;
00210         }
00211         // Simulation should run until
00212         // 1. end time reached or
00213         // 2. got CMD_CLOSE or
00214         // 3. Client closes socket connection
00215         if (myInstance->myDoingSimStep) {
00216             myInstance->postProcessSimulationStep2();
00217             myInstance->myDoingSimStep = false;
00218         }
00219         while (!myDoCloseConnection) {
00220             if (!myInstance->myInputStorage.valid_pos()) {
00221                 if (myInstance->myOutputStorage.size() > 0) {
00222                     // send out all answers as one storage
00223                     myInstance->mySocket->sendExact(myInstance->myOutputStorage);
00224                 }
00225                 myInstance->myInputStorage.reset();
00226                 myInstance->myOutputStorage.reset();
00227                 // Read a message
00228                 myInstance->mySocket->receiveExact(myInstance->myInputStorage);
00229             }
00230             while (myInstance->myInputStorage.valid_pos() && !myDoCloseConnection) {
00231                 // dispatch each command
00232                 int cmd = myInstance->dispatchCommand();
00233                 if (cmd == CMD_SIMSTEP2) {
00234                     myInstance->myDoingSimStep = true;
00235                     for (std::map<MSNet::VehicleState, std::vector<std::string> >::iterator i = myInstance->myVehicleStateChanges.begin(); i != myInstance->myVehicleStateChanges.end(); ++i) {
00236                         (*i).second.clear();
00237                     }
00238                     return;
00239                 }
00240             }
00241         }
00242         if (myDoCloseConnection && myInstance->myOutputStorage.size() > 0) {
00243             // send out all answers as one storage
00244             myInstance->mySocket->sendExact(myInstance->myOutputStorage);
00245         }
00246         for (std::map<MSNet::VehicleState, std::vector<std::string> >::iterator i = myInstance->myVehicleStateChanges.begin(); i != myInstance->myVehicleStateChanges.end(); ++i) {
00247             (*i).second.clear();
00248         }
00249     } catch (std::invalid_argument& e) {
00250         throw ProcessError(e.what());
00251     } catch (TraCIException& e) {
00252         throw ProcessError(e.what());
00253     } catch (tcpip::SocketException& e) {
00254         throw ProcessError(e.what());
00255     }
00256     if (myInstance != NULL) {
00257         delete myInstance;
00258         myInstance = 0;
00259         myDoCloseConnection = true;
00260     }
00261 }
00262 
00263 /*****************************************************************************/
00264 
00265 bool
00266 TraCIServer::wasClosed() {
00267     return myDoCloseConnection;
00268 }
00269 
00270 
00271 void
00272 TraCIServer::close() {
00273     if (myInstance != 0) {
00274         delete myInstance;
00275         myInstance = 0;
00276         myDoCloseConnection = true;
00277     }
00278 }
00279 
00280 /*****************************************************************************/
00281 
00282 #ifdef HAVE_PYTHON
00283 // ===========================================================================
00284 // python functions (traciemb module)
00285 // ===========================================================================
00286 static PyObject*
00287 traciemb_execute(PyObject* self, PyObject* args) {
00288     const char* msg;
00289     int size;
00290     if (!PyArg_ParseTuple(args, "s#", &msg, &size)) {
00291         return NULL;
00292     }
00293     std::string result = traci::TraCIServer::execute(std::string(msg, size));
00294     return Py_BuildValue("s#", result.c_str(), result.size());
00295 }
00296 
00297 static PyMethodDef EmbMethods[] = {
00298     {
00299         "execute", traciemb_execute, METH_VARARGS,
00300         "Execute the given TraCI command and return the result."
00301     },
00302     {NULL, NULL, 0, NULL}
00303 };
00304 
00305 
00306 std::string
00307 TraCIServer::execute(std::string cmd) {
00308     try {
00309         if (myInstance == 0) {
00310             if (!myDoCloseConnection) {
00311                 myInstance = new traci::TraCIServer();
00312             } else {
00313                 return "";
00314             }
00315         }
00316         myInstance->myInputStorage.reset();
00317         myInstance->myOutputStorage.reset();
00318         for (std::string::iterator i = cmd.begin(); i != cmd.end(); ++i) {
00319             myInstance->myInputStorage.writeChar(*i);
00320         }
00321         myInstance->dispatchCommand();
00322         return std::string(myInstance->myOutputStorage.begin(), myInstance->myOutputStorage.end());
00323     } catch (std::invalid_argument& e) {
00324         throw ProcessError(e.what());
00325     } catch (TraCIException& e) {
00326         throw ProcessError(e.what());
00327     } catch (tcpip::SocketException& e) {
00328         throw ProcessError(e.what());
00329     }
00330 }
00331 
00332 
00333 void
00334 TraCIServer::runEmbedded(std::string pyFile) {
00335     PyObject* pName, *pModule;
00336     Py_Initialize();
00337     Py_InitModule("traciemb", EmbMethods);
00338     if (pyFile.length() > 3 && !pyFile.compare(pyFile.length() - 3, 3, ".py")) {
00339         FILE* pFile = fopen(pyFile.c_str(), "r");
00340         PyRun_SimpleFile(pFile, pyFile.c_str());
00341         fclose(pFile);
00342     } else {
00343         pName = PyString_FromString(pyFile.c_str());
00344         /* Error checking of pName left out */
00345         pModule = PyImport_Import(pName);
00346         Py_DECREF(pName);
00347         if (pModule == NULL) {
00348             PyErr_Print();
00349             throw ProcessError("Failed to load \"" + pyFile + "\"!");
00350         }
00351     }
00352     Py_Finalize();
00353 }
00354 #endif
00355 
00356 /*****************************************************************************/
00357 
00358 int
00359 TraCIServer::dispatchCommand() {
00360     unsigned int commandStart = myInputStorage.position();
00361     unsigned int commandLength = myInputStorage.readUnsignedByte();
00362     if (commandLength == 0) {
00363         commandLength = myInputStorage.readInt();
00364     }
00365 
00366     int commandId = myInputStorage.readUnsignedByte();
00367     bool success = false;
00368     // dispatch commands
00369     if (myExecutors.find(commandId) != myExecutors.end()) {
00370         success = myExecutors[commandId](*this, myInputStorage, myOutputStorage);
00371     } else {
00372         switch (commandId) {
00373             case CMD_GETVERSION:
00374                 success = commandGetVersion();
00375                 break;
00376             case CMD_SIMSTEP2: {
00377                 SUMOTime nextT = myInputStorage.readInt();
00378                 success = true;
00379                 if (nextT != 0) {
00380                     myTargetTime = nextT;
00381                 } else {
00382                     myTargetTime += DELTA_T;
00383                 }
00384                 if (myAmEmbedded) {
00385                     MSNet::getInstance()->simulationStep();
00386                     postProcessSimulationStep2();
00387                 }
00388                 return commandId;
00389             }
00390             case CMD_CLOSE:
00391                 success = commandCloseConnection();
00392                 break;
00393             case CMD_POSITIONCONVERSION: {
00394                 if (!myHaveWarnedDeprecation) {
00395                     WRITE_WARNING("Using old TraCI API, please update your client!");
00396                     myHaveWarnedDeprecation = true;
00397                 }
00398                 tcpip::Storage tempMsg;
00399                 success = TraCIServerAPI_Simulation::commandPositionConversion(*this, myInputStorage, tempMsg, CMD_POSITIONCONVERSION);
00400                 if (success) {
00401                     writeStatusCmd(CMD_POSITIONCONVERSION, RTYPE_OK, "");
00402                     myOutputStorage.writeStorage(tempMsg);
00403                 }
00404             }
00405             break;
00406             case CMD_ADDVEHICLE:
00407                 if (!myHaveWarnedDeprecation) {
00408                     WRITE_WARNING("Using old TraCI API, please update your client!");
00409                     myHaveWarnedDeprecation = true;
00410                 }
00411                 success = commandAddVehicle();
00412                 break;
00413             case CMD_DISTANCEREQUEST: {
00414                 if (!myHaveWarnedDeprecation) {
00415                     WRITE_WARNING("Using old TraCI API, please update your client!");
00416                     myHaveWarnedDeprecation = true;
00417                 }
00418                 tcpip::Storage tempMsg;
00419                 success = TraCIServerAPI_Simulation::commandDistanceRequest(*this, myInputStorage, tempMsg, CMD_DISTANCEREQUEST);
00420                 if (success) {
00421                     writeStatusCmd(CMD_DISTANCEREQUEST, RTYPE_OK, "");
00422                     myOutputStorage.writeStorage(tempMsg);
00423                 }
00424             }
00425             break;
00426             case CMD_SUBSCRIBE_INDUCTIONLOOP_VARIABLE:
00427             case CMD_SUBSCRIBE_MULTI_ENTRY_EXIT_DETECTOR_VARIABLE:
00428             case CMD_SUBSCRIBE_TL_VARIABLE:
00429             case CMD_SUBSCRIBE_LANE_VARIABLE:
00430             case CMD_SUBSCRIBE_VEHICLE_VARIABLE:
00431             case CMD_SUBSCRIBE_VEHICLETYPE_VARIABLE:
00432             case CMD_SUBSCRIBE_ROUTE_VARIABLE:
00433             case CMD_SUBSCRIBE_POI_VARIABLE:
00434             case CMD_SUBSCRIBE_POLYGON_VARIABLE:
00435             case CMD_SUBSCRIBE_JUNCTION_VARIABLE:
00436             case CMD_SUBSCRIBE_EDGE_VARIABLE:
00437             case CMD_SUBSCRIBE_SIM_VARIABLE:
00438             case CMD_SUBSCRIBE_GUI_VARIABLE:
00439                 success = addSubscription(commandId);
00440                 break;
00441             default:
00442                 writeStatusCmd(commandId, RTYPE_NOTIMPLEMENTED, "Command not implemented in sumo");
00443         }
00444     }
00445     if (!success) {
00446         while (myInputStorage.valid_pos() && myInputStorage.position() < commandStart + commandLength) {
00447             myInputStorage.readChar();
00448         }
00449     }
00450     if (myInputStorage.position() != commandStart + commandLength) {
00451         std::ostringstream msg;
00452         msg << "Wrong position in requestMessage after dispatching command.";
00453         msg << " Expected command length was " << commandLength;
00454         msg << " but " << myInputStorage.position() - commandStart << " Bytes were read.";
00455         writeStatusCmd(commandId, RTYPE_ERR, msg.str());
00456         myDoCloseConnection = true;
00457     }
00458     return commandId;
00459 }
00460 
00461 /*****************************************************************************/
00462 
00463 void
00464 TraCIServer::postProcessSimulationStep2() {
00465     SUMOTime t = MSNet::getInstance()->getCurrentTimeStep();
00466     writeStatusCmd(CMD_SIMSTEP2, RTYPE_OK, "");
00467     int noActive = 0;
00468     for (std::vector<Subscription>::iterator i = mySubscriptions.begin(); i != mySubscriptions.end();) {
00469         const Subscription& s = *i;
00470         bool isArrivedVehicle = (s.commandId == CMD_SUBSCRIBE_VEHICLE_VARIABLE) && (find(myVehicleStateChanges[MSNet::VEHICLE_STATE_ARRIVED].begin(), myVehicleStateChanges[MSNet::VEHICLE_STATE_ARRIVED].end(), s.id) != myVehicleStateChanges[MSNet::VEHICLE_STATE_ARRIVED].end());
00471         if ((s.endTime < t) || isArrivedVehicle) {
00472             i = mySubscriptions.erase(i);
00473             continue;
00474         }
00475         ++i;
00476         if (s.beginTime > t) {
00477             continue;
00478         }
00479         ++noActive;
00480     }
00481     myOutputStorage.writeInt(noActive);
00482     for (std::vector<Subscription>::iterator i = mySubscriptions.begin(); i != mySubscriptions.end(); ++i) {
00483         const Subscription& s = *i;
00484         if (s.beginTime > t) {
00485             continue;
00486         }
00487         tcpip::Storage into;
00488         std::string errors;
00489         processSingleSubscription(s, into, errors);
00490         myOutputStorage.writeStorage(into);
00491     }
00492 }
00493 
00494 /*****************************************************************************/
00495 
00496 bool
00497 TraCIServer::commandGetVersion() {
00498 
00499     std::string sumoVersion = VERSION_STRING;
00500 
00501     // Prepare response
00502     tcpip::Storage answerTmp;
00503 
00504     answerTmp.writeInt(TRACI_VERSION);
00505     answerTmp.writeString(std::string("SUMO ") + sumoVersion);
00506 
00507     // When we get here, the response is stored in answerTmp -> put into myOutputStorage
00508     writeStatusCmd(CMD_GETVERSION, RTYPE_OK, "");
00509 
00510     // command length
00511     myOutputStorage.writeUnsignedByte(1 + 1 + static_cast<int>(answerTmp.size()));
00512     // command type
00513     myOutputStorage.writeUnsignedByte(CMD_GETVERSION);
00514     // and the parameter dependant part
00515     myOutputStorage.writeStorage(answerTmp);
00516     return true;
00517 }
00518 
00519 /*****************************************************************************/
00520 
00521 bool
00522 TraCIServer::commandCloseConnection() {
00523     myDoCloseConnection = true;
00524     // write answer
00525     writeStatusCmd(CMD_CLOSE, RTYPE_OK, "Goodbye");
00526     return true;
00527 }
00528 
00529 /*****************************************************************************/
00530 
00531 bool
00532 TraCIServer::commandAddVehicle() {
00533 
00534     // read parameters
00535     std::string vehicleId = myInputStorage.readString();
00536     std::string vehicleTypeId = myInputStorage.readString();
00537     std::string routeId = myInputStorage.readString();
00538     std::string laneId = myInputStorage.readString();
00539     SUMOReal insertionPosition = myInputStorage.readFloat();
00540     SUMOReal insertionSpeed = myInputStorage.readFloat();
00541 
00542     // find vehicleType
00543     MSVehicleType* vehicleType = MSNet::getInstance()->getVehicleControl().getVType(vehicleTypeId);
00544     if (!vehicleType) {
00545         writeStatusCmd(CMD_ADDVEHICLE, RTYPE_ERR, "Invalid vehicleTypeId: '" + vehicleTypeId + "'");
00546         return false;
00547     }
00548 
00549     // find route
00550     const MSRoute* route = MSRoute::dictionary(routeId);
00551     if (!route) {
00552         writeStatusCmd(CMD_ADDVEHICLE, RTYPE_ERR, "Invalid routeId: '" + routeId + "'");
00553         return false;
00554     }
00555 
00556     // find lane
00557     MSLane* lane;
00558     if (laneId != "") {
00559         lane = MSLane::dictionary(laneId);
00560         if (!lane) {
00561             writeStatusCmd(CMD_ADDVEHICLE, RTYPE_ERR, "Invalid laneId: '" + laneId + "'");
00562             return false;
00563         }
00564     } else {
00565         lane = route->getEdges()[0]->getLanes()[0];
00566         if (!lane) {
00567             writeStatusCmd(CMD_STOP, RTYPE_ERR, "Could not find first lane of first edge in routeId '" + routeId + "'");
00568             return false;
00569         }
00570     }
00571 
00572     if (&lane->getEdge() != *route->begin()) {
00573         writeStatusCmd(CMD_STOP, RTYPE_ERR, "The route must start at the edge the lane starts at.");
00574         return false;
00575     }
00576 
00577     // build vehicle
00578     SUMOVehicleParameter* vehicleParams = new SUMOVehicleParameter();
00579     vehicleParams->id = vehicleId;
00580     vehicleParams->depart = MSNet::getInstance()->getCurrentTimeStep() + 1;
00581     MSVehicle* vehicle = static_cast<MSVehicle*>(MSNet::getInstance()->getVehicleControl().buildVehicle(vehicleParams, route, vehicleType));
00582     if (vehicle == NULL) {
00583         writeStatusCmd(CMD_STOP, RTYPE_ERR, "Could not build vehicle");
00584         return false;
00585     }
00586 
00587     // calculate speed
00588     float clippedInsertionSpeed;
00589     if (insertionSpeed < 0) {
00590         clippedInsertionSpeed = (float) MIN2(lane->getMaxSpeed(), vehicle->getMaxSpeed());
00591     } else {
00592         clippedInsertionSpeed = (float) MIN3(lane->getMaxSpeed(), vehicle->getMaxSpeed(), insertionSpeed);
00593     }
00594 
00595     // insert vehicle into the dictionary
00596     if (!MSNet::getInstance()->getVehicleControl().addVehicle(vehicle->getID(), vehicle)) {
00597         writeStatusCmd(CMD_ADDVEHICLE, RTYPE_ERR, "Could not add vehicle to VehicleControl");
00598         return false;
00599     }
00600 
00601     // try to emit
00602     if (!lane->isInsertionSuccess(vehicle, clippedInsertionSpeed, insertionPosition, true)) {
00603         MSNet::getInstance()->getVehicleControl().deleteVehicle(vehicle);
00604         writeStatusCmd(CMD_ADDVEHICLE, RTYPE_ERR, "Could not insert vehicle");
00605         return false;
00606     }
00607 
00608     // exec callback
00609     vehicle->onDepart();
00610 
00611     // create a reply message
00612     writeStatusCmd(CMD_ADDVEHICLE, RTYPE_OK, "");
00613 
00614     return true;
00615 }
00616 
00617 
00618 /*****************************************************************************/
00619 
00620 void
00621 TraCIServer::writeStatusCmd(int commandId, int status, const std::string& description) {
00622     writeStatusCmd(commandId, status, description, myOutputStorage);
00623 }
00624 
00625 
00626 void
00627 TraCIServer::writeStatusCmd(int commandId, int status, const std::string& description, tcpip::Storage& outputStorage) {
00628     if (status == RTYPE_ERR) {
00629         WRITE_ERROR("Answered with error to command " + toString(commandId) + ": " + description);
00630     } else if (status == RTYPE_NOTIMPLEMENTED) {
00631         WRITE_ERROR("Requested command not implemented (" + toString(commandId) + "): " + description);
00632     }
00633     outputStorage.writeUnsignedByte(1 + 1 + 1 + 4 + static_cast<int>(description.length())); // command length
00634     outputStorage.writeUnsignedByte(commandId); // command type
00635     outputStorage.writeUnsignedByte(status); // status
00636     outputStorage.writeString(description); // description
00637 }
00638 
00639 /*****************************************************************************/
00640 
00641 bool
00642 TraCIServer::addSubscription(int commandId) {
00643     SUMOTime beginTime = myInputStorage.readInt();
00644     SUMOTime endTime = myInputStorage.readInt();
00645     std::string id = myInputStorage.readString();
00646     int no = myInputStorage.readUnsignedByte();
00647     std::vector<int> variables;
00648     for (int i = 0; i < no; ++i) {
00649         variables.push_back(myInputStorage.readUnsignedByte());
00650     }
00651     // check subscribe/unsubscribe
00652     bool ok = true;
00653     if (variables.size() == 0) {
00654         // try unsubscribe
00655         bool found = false;
00656         for (std::vector<Subscription>::iterator j = mySubscriptions.begin(); j != mySubscriptions.end();) {
00657             if ((*j).id == id && (*j).commandId == commandId) {
00658                 j = mySubscriptions.erase(j);
00659                 found = true;
00660                 continue;
00661             }
00662             ++j;
00663         }
00664         if (found) {
00665             writeStatusCmd(commandId, RTYPE_OK, "");
00666         } else {
00667             writeStatusCmd(commandId, RTYPE_OK, "The subscription to remove was not found.");
00668         }
00669     } else {
00670         // process subscription
00671         Subscription s(commandId, id, variables, beginTime, endTime);
00672         tcpip::Storage writeInto;
00673         std::string errors;
00674         if (s.endTime < MSNet::getInstance()->getCurrentTimeStep()) {
00675             processSingleSubscription(s, writeInto, errors);
00676             writeStatusCmd(s.commandId, RTYPE_ERR, "Subscription has ended.");
00677         } else {
00678             if (processSingleSubscription(s, writeInto, errors)) {
00679                 mySubscriptions.push_back(s);
00680                 writeStatusCmd(s.commandId, RTYPE_OK, "");
00681             } else {
00682                 writeStatusCmd(s.commandId, RTYPE_ERR, "Could not add subscription (" + errors + ").");
00683             }
00684         }
00685         myOutputStorage.writeStorage(writeInto);
00686     }
00687     return ok;
00688 }
00689 
00690 
00691 bool
00692 TraCIServer::processSingleSubscription(const Subscription& s, tcpip::Storage& writeInto,
00693                                        std::string& errors) {
00694     bool ok = true;
00695     tcpip::Storage outputStorage;
00696     for (std::vector<int>::const_iterator i = s.variables.begin(); i != s.variables.end(); ++i) {
00697         tcpip::Storage message;
00698         message.writeUnsignedByte(*i);
00699         message.writeString(s.id);
00700         tcpip::Storage tmpOutput;
00701         int getId = s.commandId - 0x30;
00702         if (myExecutors.find(getId) != myExecutors.end()) {
00703             ok &= myExecutors[getId](*this, message, tmpOutput);
00704         } else {
00705             writeStatusCmd(s.commandId, RTYPE_NOTIMPLEMENTED, "Unsupported command specified", tmpOutput);
00706             ok = false;
00707         }
00708         // copy response part
00709         if (ok) {
00710             int length = tmpOutput.readUnsignedByte();
00711             while (--length > 0) {
00712                 tmpOutput.readUnsignedByte();
00713             }
00714             int lengthLength = 1;
00715             length = tmpOutput.readUnsignedByte();
00716             if (length == 0) {
00717                 lengthLength = 5;
00718                 length = tmpOutput.readInt();
00719             }
00720             //read responseType
00721             tmpOutput.readUnsignedByte();
00722             int variable = tmpOutput.readUnsignedByte();
00723             std::string id = tmpOutput.readString();
00724             outputStorage.writeUnsignedByte(variable);
00725             outputStorage.writeUnsignedByte(RTYPE_OK);
00726             length -= (lengthLength + 1 + 4 + (int)id.length());
00727             while (--length > 0) {
00728                 outputStorage.writeUnsignedByte(tmpOutput.readUnsignedByte());
00729             }
00730         } else {
00731             //read length
00732             tmpOutput.readUnsignedByte();
00733             //read cmd
00734             tmpOutput.readUnsignedByte();
00735             //read status
00736             tmpOutput.readUnsignedByte();
00737             std::string msg = tmpOutput.readString();
00738             outputStorage.writeUnsignedByte(*i);
00739             outputStorage.writeUnsignedByte(RTYPE_ERR);
00740             outputStorage.writeUnsignedByte(TYPE_STRING);
00741             outputStorage.writeString(msg);
00742             errors = errors + msg;
00743         }
00744     }
00745     writeInto.writeUnsignedByte(0); // command length -> extended
00746     writeInto.writeInt((1 + 4) + 1 + (4 + (int)(s.id.length())) + 1 + (int)outputStorage.size());
00747     writeInto.writeUnsignedByte(s.commandId + 0x10);
00748     writeInto.writeString(s.id);
00749     writeInto.writeUnsignedByte((int)(s.variables.size()));
00750     writeInto.writeStorage(outputStorage);
00751     return ok;
00752 }
00753 
00754 void
00755 TraCIServer::writeResponseWithLength(tcpip::Storage& outputStorage, tcpip::Storage& tempMsg) {
00756     if (tempMsg.size() < 254) {
00757         outputStorage.writeUnsignedByte(1 + (int)tempMsg.size()); // command length -> short
00758     } else {
00759         outputStorage.writeUnsignedByte(0); // command length -> extended
00760         outputStorage.writeInt(1 + 4 + (int)tempMsg.size());
00761     }
00762     outputStorage.writeStorage(tempMsg);
00763 }
00764 
00765 }
00766 
00767 #endif
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Friends Defines