SUMO - Simulation of Urban MObility
|
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