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