00001
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #include "TraCIConstants.h"
00026 #include "TraCIServer.h"
00027 #include "TraCIHandler.h"
00028 #include "TraCIDijkstraRouter.h"
00029
00030 #ifndef NO_TRACI
00031
00032 #include <foreign/tcpip/socket.h>
00033 #include <foreign/tcpip/storage.h>
00034 #include <utils/common/SUMOTime.h>
00035 #include <utils/common/DijkstraRouterTT.h>
00036 #include <utils/common/NamedObjectCont.h>
00037 #include <utils/common/RandHelper.h>
00038 #include <utils/common/MsgHandler.h>
00039 #include <utils/shapes/PointOfInterest.h>
00040 #include <utils/shapes/ShapeContainer.h>
00041 #include <utils/shapes/Polygon2D.h>
00042 #include <utils/xml/XMLSubSys.h>
00043 #include <microsim/MSNet.h>
00044 #include <microsim/MSVehicleControl.h>
00045 #include <microsim/MSVehicle.h>
00046 #include <microsim/MSEdge.h>
00047 #include <microsim/MSRouteHandler.h>
00048 #include <microsim/MSRouteLoaderControl.h>
00049 #include <microsim/MSRouteLoader.h>
00050 #include <microsim/MSJunctionControl.h>
00051 #include <microsim/MSJunction.h>
00052 #include <microsim/traffic_lights/MSTLLogicControl.h>
00053 #include "TraCIServerAPI_InductionLoop.h"
00054 #include "TraCIServerAPI_Junction.h"
00055 #include "TraCIServerAPI_Lane.h"
00056 #include "TraCIServerAPI_MeMeDetector.h"
00057 #include "TraCIServerAPI_TLS.h"
00058 #include "TraCIServerAPI_Vehicle.h"
00059 #include "TraCIServerAPI_VehicleType.h"
00060 #include "TraCIServerAPI_Route.h"
00061 #include "TraCIServerAPI_POI.h"
00062 #include "TraCIServerAPI_Polygon.h"
00063 #include "TraCIServerAPI_Edge.h"
00064 #include "TraCIServerAPI_Simulation.h"
00065 #include "TraCIServerAPIHelper.h"
00066
00067
00068
00069 #include <microsim/MSEdgeControl.h>
00070 #include <microsim/MSLane.h>
00071 #include <microsim/trigger/MSCalibrator.h>
00072 #include <microsim/MSGlobals.h>
00073
00074 #include <xercesc/sax2/SAX2XMLReader.hpp>
00075
00076 #include <string>
00077 #include <map>
00078 #include <iostream>
00079 #include <utils/common/HelpersHBEFA.h>
00080 #include <utils/common/HelpersHarmonoise.h>
00081
00082
00083 #ifdef CHECK_MEMORY_LEAKS
00084 #include <foreign/nvwa/debug_new.h>
00085 #endif // CHECK_MEMORY_LEAKS
00086
00087
00088
00089
00090
00091 using namespace std;
00092 using namespace tcpip;
00093
00094
00095 namespace traci {
00096
00097
00098
00099
00100 TraCIServer* TraCIServer::instance_ = 0;
00101 bool TraCIServer::closeConnection_ = false;
00102
00103
00104
00105
00106
00107
00108
00109
00110 TraCIServer::TraCIServer() {
00111 myVehicleStateChanges[MSNet::VEHICLE_STATE_BUILT] = std::vector<std::string>();
00112 myVehicleStateChanges[MSNet::VEHICLE_STATE_DEPARTED] = std::vector<std::string>();
00113 myVehicleStateChanges[MSNet::VEHICLE_STATE_STARTING_TELEPORT] = std::vector<std::string>();
00114 myVehicleStateChanges[MSNet::VEHICLE_STATE_ENDING_TELEPORT] = std::vector<std::string>();
00115 myVehicleStateChanges[MSNet::VEHICLE_STATE_ARRIVED] = std::vector<std::string>();
00116 MSNet::getInstance()->addVehicleStateListener(this);
00117
00118 OptionsCont &oc = OptionsCont::getOptions();
00119 targetTime_ = 0;
00120 penetration_ = oc.getFloat("penetration");
00121 routeFile_ = oc.getString("route-files");
00122 isMapChanged_ = true;
00123 numEquippedVehicles_ = 0;
00124 totalNumVehicles_ = 0;
00125 closeConnection_ = false;
00126 netBoundary_ = NULL;
00127 myDoingSimStep = false;
00128
00129
00130 if (!MSGlobals::gUsingInternalLanes) {
00131 MsgHandler::getWarningInstance()->inform("Starting TraCI without using internal lanes!");
00132 MsgHandler::getWarningInstance()->inform("Vehicles will jump over junctions.", false);
00133 MsgHandler::getWarningInstance()->inform("Use without option --no-internal-links to avoid unexpected behavior", false);
00134 }
00135
00136
00137 trafficLightsInt2ExtId.clear();
00138 trafficLightsExt2IntId.clear();
00139 poiExt2IntId.clear();
00140 poiInt2ExtId.clear();
00141 polygonExt2IntId.clear();
00142 polygonInt2ExtId.clear();
00143 int extId = 0;
00144 std::vector<std::string> tllIds = MSNet::getInstance()->getTLSControl().getAllTLIds();
00145 for (std::vector<std::string>::iterator it=tllIds.begin(); it != tllIds.end(); it++) {
00146 trafficLightsInt2ExtId[(*it)] = extId;
00147 trafficLightsExt2IntId[extId] = (*it);
00148
00149 extId++;
00150 }
00151 int poiId = 0;
00152 int polyId = 0;
00153 ShapeContainer& shapeCont = MSNet::getInstance()->getShapeContainer();
00154 for (int i = shapeCont.getMinLayer(); i <= shapeCont.getMaxLayer(); i++) {
00155 std::vector<PointOfInterest*> poiList = shapeCont.getPOICont(i).getTempVector();
00156 for (std::vector<PointOfInterest*>::iterator it=poiList.begin(); it != poiList.end(); it++) {
00157 poiInt2ExtId[(*it)->getID()] = poiId;
00158 poiExt2IntId[poiId] = (*it)->getID();
00159 poiId++;
00160 }
00161 std::vector<Polygon2D*> polyList = shapeCont.getPolygonCont(i).getTempVector();
00162 for (std::vector<Polygon2D*>::iterator it=polyList.begin(); it != polyList.end(); it++) {
00163 polygonInt2ExtId[(*it)->getID()] = polyId;
00164 polygonExt2IntId[polyId] = (*it)->getID();
00165 polyId++;
00166 }
00167 }
00168
00169
00170 TraCIHandler xmlHandler;
00171 SAX2XMLReader* xmlParser = XMLSubSys::getSAXReader(xmlHandler);
00172
00173
00174 if (oc.isSet("route-files")) {
00175 std::vector<std::string> fileList = oc.getStringVector("route-files");
00176 for (std::vector<std::string>::iterator file=fileList.begin(); file != fileList.end(); file++) {
00177 if (oc.isUsableFileList("route-files")) {
00178 xmlHandler.setFileName((*file));
00179 xmlHandler.resetTotalVehicleCount();
00180 xmlParser->parse(file->c_str());
00181
00182 if (!MsgHandler::getErrorInstance()->wasInformed()) {
00183 totalNumVehicles_ += xmlHandler.getTotalVehicleCount();
00184 }
00185 }
00186 }
00187 }
00188
00189
00190 if (oc.isSet("additional-files")) {
00191 std::vector<std::string> fileList = oc.getStringVector("additional-files");
00192 for (std::vector<std::string>::iterator file = fileList.begin(); file != fileList.end(); file++) {
00193 if (oc.isUsableFileList("additional-files")) {
00194 xmlHandler.setFileName((*file));
00195 xmlHandler.resetTotalVehicleCount();
00196 xmlParser->parse(file->c_str());
00197
00198 if (!MsgHandler::getErrorInstance()->wasInformed()) {
00199 totalNumVehicles_ += xmlHandler.getTotalVehicleCount();
00200 }
00201 }
00202 }
00203 }
00204
00205 delete xmlParser;
00206
00207 try {
00208 int port = oc.getInt("remote-port");
00209
00210 MsgHandler::getMessageInstance()->inform("***Starting server on port " + toString(port) + " ***");
00211 socket_ = new Socket(port);
00212 socket_->accept();
00213
00214 } catch (SocketException &e) {
00215 throw ProcessError(e.what());
00216 }
00217
00218 }
00219
00220
00221
00222 TraCIServer::~TraCIServer() throw() {
00223 MSNet::getInstance()->removeVehicleStateListener(this);
00224 if (socket_ != NULL) {
00225 socket_->close();
00226 delete socket_;
00227 }
00228 if (netBoundary_ != NULL) delete netBoundary_;
00229 }
00230
00231
00232
00233 void
00234 TraCIServer::close() {
00235 if (instance_!=0) {
00236 delete instance_;
00237 instance_ = 0;
00238 closeConnection_ = true;
00239 }
00240 }
00241
00242
00243
00244 void
00245 TraCIServer::vehicleStateChanged(const MSVehicle * const vehicle, MSNet::VehicleState to) throw() {
00246 if (closeConnection_ || OptionsCont::getOptions().getInt("remote-port") == 0) {
00247 return;
00248 }
00249
00250
00251
00252
00253
00254 myVehicleStateChanges[to].push_back(vehicle->getID());
00255 }
00256
00257
00258 void
00259 TraCIServer::processCommandsUntilSimStep(SUMOTime step) throw(ProcessError) {
00260 try {
00261 if (instance_ == 0) {
00262 if (!closeConnection_ && OptionsCont::getOptions().getInt("remote-port") != 0) {
00263 instance_ = new traci::TraCIServer();
00264 } else {
00265 return;
00266 }
00267 }
00268 if (step < instance_->targetTime_) {
00269 return;
00270 }
00271
00272
00273
00274
00275 if (instance_->myDoingSimStep) {
00276 switch (instance_->simStepCommand) {
00277 case CMD_SIMSTEP:
00278 instance_->postProcessSimulationStep();
00279 break;
00280 case CMD_SIMSTEP2:
00281 instance_->postProcessSimulationStep2();
00282 break;
00283 }
00284 instance_->myDoingSimStep = false;
00285 }
00286 while (!closeConnection_) {
00287 if (!instance_->myInputStorage.valid_pos()) {
00288 if (instance_->myOutputStorage.size() > 0) {
00289
00290 instance_->socket_->sendExact(instance_->myOutputStorage);
00291 }
00292 instance_->myInputStorage.reset();
00293 instance_->myOutputStorage.reset();
00294
00295 instance_->socket_->receiveExact(instance_->myInputStorage);
00296 }
00297 while (instance_->myInputStorage.valid_pos() && !closeConnection_) {
00298
00299 int cmd = instance_->dispatchCommand();
00300 if (cmd == CMD_SIMSTEP || cmd==CMD_SIMSTEP2) {
00301 instance_->simStepCommand = cmd;
00302 instance_->myDoingSimStep = true;
00303 for (std::map<MSNet::VehicleState, std::vector<std::string> >::iterator i=instance_->myVehicleStateChanges.begin(); i!=instance_->myVehicleStateChanges.end(); ++i) {
00304 (*i).second.clear();
00305 }
00306 return;
00307 }
00308 }
00309 }
00310 if (closeConnection_ && instance_->myOutputStorage.size() > 0) {
00311
00312 instance_->socket_->sendExact(instance_->myOutputStorage);
00313 }
00314 for (std::map<MSNet::VehicleState, std::vector<std::string> >::iterator i=instance_->myVehicleStateChanges.begin(); i!=instance_->myVehicleStateChanges.end(); ++i) {
00315 (*i).second.clear();
00316 }
00317 } catch (std::invalid_argument &e) {
00318 throw ProcessError(e.what());
00319 } catch (TraCIException &e) {
00320 throw ProcessError(e.what());
00321 } catch (SocketException &e) {
00322 throw ProcessError(e.what());
00323 }
00324 if (instance_ != NULL) {
00325 delete instance_;
00326 instance_ = 0;
00327 closeConnection_ = true;
00328 }
00329 }
00330
00331 bool
00332 TraCIServer::wasClosed() {
00333 return closeConnection_;
00334 }
00335
00336
00337
00338 int
00339 TraCIServer::dispatchCommand()
00340 throw(TraCIException, std::invalid_argument) {
00341 int commandStart = myInputStorage.position();
00342 int commandLength = myInputStorage.readUnsignedByte();
00343 if (commandLength==0) {
00344 commandLength = myInputStorage.readInt();
00345 }
00346
00347 int commandId = myInputStorage.readUnsignedByte();
00348 bool success = false;
00349
00350 switch (commandId) {
00351 case CMD_SETMAXSPEED:
00352 success = commandSetMaximumSpeed();
00353 break;
00354 case CMD_SIMSTEP:
00355 success = targetTime_ = static_cast<SUMOTime>(myInputStorage.readInt());
00356 return commandId;
00357 case CMD_SIMSTEP2: {
00358 SUMOTime nextT = myInputStorage.readInt();
00359 success = true;
00360 if (nextT!=0) {
00361 targetTime_ = (SUMOReal) nextT;
00362 } else {
00363 targetTime_ += DELTA_T;
00364 }
00365 return commandId;
00366 }
00367 case CMD_STOP:
00368 success = commandStopNode();
00369 break;
00370 case CMD_CHANGELANE:
00371 success = commandChangeLane();
00372 break;
00373 case CMD_CHANGEROUTE:
00374 success = commandChangeRoute();
00375 break;
00376 case CMD_CHANGETARGET:
00377 success = commandChangeTarget();
00378 break;
00379 case CMD_GETALLTLIDS:
00380 success = commandGetAllTLIds();
00381 break;
00382 case CMD_GETTLSTATUS:
00383 success = commandGetTLStatus();
00384 break;
00385 case CMD_CLOSE:
00386 success = commandCloseConnection();
00387 break;
00388 case CMD_UPDATECALIBRATOR:
00389 success = commandUpdateCalibrator();
00390 break;
00391 case CMD_POSITIONCONVERSION:
00392 success = commandPositionConversion();
00393 break;
00394 case CMD_SLOWDOWN:
00395 success = commandSlowDown();
00396 break;
00397 case CMD_SCENARIO:
00398 success = commandScenario();
00399 break;
00400 case CMD_ADDVEHICLE:
00401 success = commandAddVehicle();
00402 break;
00403 case CMD_DISTANCEREQUEST:
00404 success = commandDistanceRequest();
00405 break;
00406 case CMD_SUBSCRIBELIFECYCLES:
00407 success = commandSubscribeLifecycles();
00408 break;
00409 case CMD_UNSUBSCRIBELIFECYCLES:
00410 success = commandUnsubscribeLifecycles();
00411 break;
00412 case CMD_SUBSCRIBEDOMAIN:
00413 success = commandSubscribeDomain();
00414 break;
00415 case CMD_UNSUBSCRIBEDOMAIN:
00416 success = commandUnsubscribeDomain();
00417 break;
00418 case CMD_GET_INDUCTIONLOOP_VARIABLE:
00419 success = TraCIServerAPI_InductionLoop::processGet(myInputStorage, myOutputStorage);
00420 break;
00421 case CMD_GET_MULTI_ENTRY_EXIT_DETECTOR_VARIABLE:
00422 success = TraCIServerAPI_MeMeDetector::processGet(myInputStorage, myOutputStorage);
00423 break;
00424 case CMD_GET_TL_VARIABLE:
00425 success = TraCIServerAPI_TLS::processGet(myInputStorage, myOutputStorage);
00426 break;
00427 case CMD_SET_TL_VARIABLE:
00428 success = TraCIServerAPI_TLS::processSet(myInputStorage, myOutputStorage);
00429 break;
00430 case CMD_GET_LANE_VARIABLE:
00431 success = TraCIServerAPI_Lane::processGet(myInputStorage, myOutputStorage);
00432 break;
00433 case CMD_SET_LANE_VARIABLE:
00434 success = TraCIServerAPI_Lane::processSet(myInputStorage, myOutputStorage);
00435 break;
00436 case CMD_GET_VEHICLE_VARIABLE:
00437 success = TraCIServerAPI_Vehicle::processGet(myInputStorage, myOutputStorage);
00438 break;
00439 case CMD_SET_VEHICLE_VARIABLE:
00440 success = TraCIServerAPI_Vehicle::processSet(myInputStorage, myOutputStorage);
00441 break;
00442 case CMD_GET_VEHICLETYPE_VARIABLE:
00443 success = TraCIServerAPI_VehicleType::processGet(myInputStorage, myOutputStorage);
00444 break;
00445 case CMD_GET_ROUTE_VARIABLE:
00446 success = TraCIServerAPI_Route::processGet(myInputStorage, myOutputStorage);
00447 break;
00448 case CMD_GET_POI_VARIABLE:
00449 success = TraCIServerAPI_POI::processGet(myInputStorage, myOutputStorage);
00450 break;
00451 case CMD_SET_POI_VARIABLE:
00452 success = TraCIServerAPI_POI::processSet(myInputStorage, myOutputStorage);
00453 break;
00454 case CMD_GET_POLYGON_VARIABLE:
00455 success = TraCIServerAPI_Polygon::processGet(myInputStorage, myOutputStorage);
00456 break;
00457 case CMD_SET_POLYGON_VARIABLE:
00458 success = TraCIServerAPI_Polygon::processSet(myInputStorage, myOutputStorage);
00459 break;
00460 case CMD_GET_JUNCTION_VARIABLE:
00461 success = TraCIServerAPI_Junction::processGet(myInputStorage, myOutputStorage);
00462 break;
00463 case CMD_GET_EDGE_VARIABLE:
00464 success = TraCIServerAPI_Edge::processGet(myInputStorage, myOutputStorage);
00465 break;
00466 case CMD_SET_EDGE_VARIABLE:
00467 success = TraCIServerAPI_Edge::processSet(myInputStorage, myOutputStorage);
00468 break;
00469 case CMD_GET_SIM_VARIABLE:
00470 success = TraCIServerAPI_Simulation::processGet(myInputStorage, myOutputStorage, myVehicleStateChanges);
00471 break;
00472 case CMD_SUBSCRIBE_INDUCTIONLOOP_VARIABLE:
00473 case CMD_SUBSCRIBE_MULTI_ENTRY_EXIT_DETECTOR_VARIABLE:
00474 case CMD_SUBSCRIBE_TL_VARIABLE:
00475 case CMD_SUBSCRIBE_LANE_VARIABLE:
00476 case CMD_SUBSCRIBE_VEHICLE_VARIABLE:
00477 case CMD_SUBSCRIBE_VEHICLETYPE_VARIABLE:
00478 case CMD_SUBSCRIBE_ROUTE_VARIABLE:
00479 case CMD_SUBSCRIBE_POI_VARIABLE:
00480 case CMD_SUBSCRIBE_POLYGON_VARIABLE:
00481 case CMD_SUBSCRIBE_JUNCTION_VARIABLE:
00482 case CMD_SUBSCRIBE_EDGE_VARIABLE:
00483 case CMD_SUBSCRIBE_SIM_VARIABLE:
00484 success = addSubscription(commandId);
00485 break;
00486 default:
00487 writeStatusCmd(commandId, RTYPE_NOTIMPLEMENTED, "Command not implemented in sumo");
00488 }
00489 if (!success) {
00490 while (myInputStorage.valid_pos() && myInputStorage.position() < commandStart + commandLength) {
00491 myInputStorage.readChar();
00492 }
00493 }
00494 if (myInputStorage.position() != commandStart + commandLength) {
00495 ostringstream msg;
00496 msg << "Wrong position in requestMessage after dispatching command.";
00497 msg << " Expected command length was " << commandLength;
00498 msg << " but " << myInputStorage.position() - commandStart << " Bytes were read.";
00499 writeStatusCmd(commandId, RTYPE_ERR, msg.str());
00500 closeConnection_ = true;
00501 }
00502 return commandId;
00503 }
00504
00505
00506
00507 bool
00508 TraCIServer::commandSetMaximumSpeed() throw(TraCIException, std::invalid_argument) {
00509 MSVehicle* veh = getVehicleByExtId(myInputStorage.readInt());
00510 float maxspeed = myInputStorage.readFloat();
00511
00512 if (veh == NULL) {
00513 writeStatusCmd(CMD_SETMAXSPEED, RTYPE_ERR, "Can not retrieve node with given ID");
00514 return false;
00515 }
00516
00517 if (maxspeed>=0.0) {
00518 veh->setIndividualMaxSpeed(maxspeed);
00519 } else {
00520 veh->unsetIndividualMaxSpeed();
00521 }
00522
00523
00524 writeStatusCmd(CMD_SETMAXSPEED, RTYPE_OK, "");
00525
00526 return true;
00527 }
00528
00529
00530
00531 void
00532 TraCIServer::postProcessSimulationStep() throw(TraCIException, std::invalid_argument) {
00533
00534 int resType = myInputStorage.readUnsignedByte();
00535 if (resType != POSITION_NONE && resType != POSITION_2D && resType != POSITION_ROADMAP
00536 && resType != POSITION_2_5D && resType != POSITION_3D) {
00537 writeStatusCmd(CMD_SIMSTEP, RTYPE_ERR, "Error: unsupported return format requested.");
00538 return;
00539 }
00540 isMapChanged_ = true;
00541
00542 writeStatusCmd(CMD_SIMSTEP, RTYPE_OK, "");
00543
00544
00545 try {
00546 MSNet *net = MSNet::getInstance();
00547
00548 map<int, const MSVehicle*> activeEquippedVehicles;
00549
00550 MSVehicleControl &vehControl = net->getVehicleControl();
00551
00552 for (map<string, MSVehicle*>::const_iterator iter = vehControl.loadedVehBegin();
00553 iter != vehControl.loadedVehEnd(); ++iter) {
00554
00555 const std::string vehicleId = (*iter).first;
00556 const MSVehicle *vehicle = (*iter).second;
00557
00558 std::map<std::string, int>::const_iterator equippedVeh = equippedVehicles_.find(vehicleId);
00559 if (equippedVeh == equippedVehicles_.end()) {
00560
00561 if (penetration_ >= 1. || RandHelper::rand() <= penetration_) {
00562
00563 equippedVehicles_[vehicleId] = numEquippedVehicles_;
00564
00565 if (vehicle->isOnRoad()) {
00566 if ((myLifecycleSubscriptions.count(DOM_VEHICLE) != 0) &&
00567 (myLivingVehicles.count(numEquippedVehicles_) == 0)) {
00568 myCreatedVehicles.insert(numEquippedVehicles_);
00569 myLivingVehicles.insert(numEquippedVehicles_);
00570 }
00571 activeEquippedVehicles[numEquippedVehicles_] = vehicle;
00572 }
00573 numEquippedVehicles_++;
00574 } else {
00575
00576 equippedVehicles_[vehicleId] = -1;
00577 }
00578 } else if (equippedVeh->second >= 0 && vehicle->isOnRoad()) {
00579 int extId = equippedVeh->second;
00580 if ((myLifecycleSubscriptions.count(DOM_VEHICLE) != 0) &&
00581 (myLivingVehicles.count(extId) == 0)) {
00582 myCreatedVehicles.insert(extId);
00583 myLivingVehicles.insert(extId);
00584 }
00585 activeEquippedVehicles[extId] = vehicle;
00586
00587 }
00588 }
00589 if (myLifecycleSubscriptions.count(DOM_VEHICLE) != 0) {
00590
00591 for (std::set<int>::iterator i = myLivingVehicles.begin();
00592 i != myLivingVehicles.end();) {
00593 int extId = *i;
00594 if (activeEquippedVehicles.find(extId) == activeEquippedVehicles.end()) {
00595 myDestroyedVehicles.insert(extId);
00596 myLivingVehicles.erase(i++);
00597 } else {
00598 i++;
00599 }
00600 }
00601 }
00602
00603 handleLifecycleSubscriptions();
00604
00605
00606 for (std::map<std::string, int>::iterator iter = equippedVehicles_.begin();
00607 iter != equippedVehicles_.end(); ++iter) {
00608 if ((*iter).second != -1) {
00609 MSVehicle* veh = net->getVehicleControl().getVehicle((*iter).first);
00610 if (myVehiclesToReroute.find(veh)!=myVehiclesToReroute.end()) {
00611
00612 if (!veh->hasStops() && veh->isOnRoad() && veh->getLane().getEdge().getPurpose()!=MSEdge::EDGEFUNCTION_INTERNAL) {
00613 MSNet::EdgeWeightsProxi proxi(veh->getWeightsStorage(), MSNet::getInstance()->getWeightsStorage());
00614 DijkstraRouterTT_ByProxi<MSEdge, SUMOVehicle, prohibited_withRestrictions<MSEdge, SUMOVehicle>, MSNet::EdgeWeightsProxi> router(MSEdge::dictSize(), true, &proxi, &MSNet::EdgeWeightsProxi::getTravelTime);
00615 veh->reroute(MSNet::getInstance()->getCurrentTimeStep(), router);
00616 }
00617 myVehiclesToReroute.erase(veh);
00618 }
00619 if (veh != NULL) {
00620 veh->processTraCICommands(targetTime_);
00621 }
00622 }
00623 }
00624
00625 handleDomainSubscriptions(targetTime_, activeEquippedVehicles);
00626
00627
00628
00629
00630
00631 if (resType != POSITION_NONE) {
00632 for (map<int, const MSVehicle*>::iterator iter = activeEquippedVehicles.begin();
00633 iter != activeEquippedVehicles.end(); ++iter) {
00634 int extId = (*iter).first;
00635 const MSVehicle* vehicle = (*iter).second;
00636 Storage tempMsg;
00637
00638
00639 tempMsg.writeUnsignedByte(CMD_MOVENODE);
00640
00641 tempMsg.writeInt(extId);
00642
00643 tempMsg.writeInt(targetTime_);
00644
00645 if (resType == POSITION_ROADMAP) {
00646
00647 tempMsg.writeUnsignedByte(POSITION_ROADMAP);
00648
00649 tempMsg.writeString(vehicle->getEdge()->getID());
00650 tempMsg.writeFloat(vehicle->getPositionOnLane());
00651
00652
00653 int laneId = 0;
00654 const MSLane* lane = &vehicle->getLane();
00655 while ((lane = lane->getRightLane()) != NULL) {
00656 laneId++;
00657 }
00658 tempMsg.writeUnsignedByte(laneId);
00659 } else if (resType == POSITION_2D || resType == POSITION_3D || resType == POSITION_2_5D) {
00660 tempMsg.writeUnsignedByte(resType);
00661 Position2D pos = vehicle->getPosition();
00662 tempMsg.writeFloat(pos.x());
00663 tempMsg.writeFloat(pos.y());
00664 if (resType != POSITION_2D) {
00665
00666 tempMsg.writeFloat(0);
00667 }
00668 }
00669
00670
00671 myOutputStorage.writeUnsignedByte(tempMsg.size()+1);
00672
00673 myOutputStorage.writeStorage(tempMsg);
00674 }
00675 }
00676 } catch (...) {
00677 writeStatusCmd(CMD_SIMSTEP, RTYPE_ERR, "some error happen in command: simulation step. Sumo shuts down.");
00678 closeConnection_ = true;
00679 }
00680 }
00681
00682
00683 void
00684 TraCIServer::postProcessSimulationStep2() throw(TraCIException, std::invalid_argument) {
00685 SUMOTime t = MSNet::getInstance()->getCurrentTimeStep();
00686 writeStatusCmd(CMD_SIMSTEP2, RTYPE_OK, "");
00687 int noActive = 0;
00688 for (std::vector<Subscription>::iterator i=mySubscriptions.begin(); i!=mySubscriptions.end();) {
00689 const Subscription &s = *i;
00690 if (s.endTime<t) {
00691 i = mySubscriptions.erase(i);
00692 continue;
00693 }
00694 ++i;
00695 if (s.beginTime>t) {
00696 continue;
00697 }
00698 ++noActive;
00699 }
00700 myOutputStorage.writeInt(noActive);
00701 for (std::vector<Subscription>::iterator i=mySubscriptions.begin(); i!=mySubscriptions.end(); ++i) {
00702 const Subscription &s = *i;
00703 if (s.beginTime>t) {
00704 continue;
00705 }
00706 tcpip::Storage into;
00707 std::string errors;
00708 processSingleSubscription(s, into, errors);
00709 myOutputStorage.writeStorage(into);
00710 }
00711 }
00712
00713
00714
00715 bool
00716 TraCIServer::commandStopNode() throw(TraCIException, std::invalid_argument) {
00717
00718
00719
00720 RoadMapPos roadPos;
00721 MSLane* actLane;
00722
00723
00724 int nodeId = myInputStorage.readInt();
00725 MSVehicle* veh = getVehicleByExtId(nodeId);
00726
00727 if (veh == NULL) {
00728 writeStatusCmd(CMD_STOP, RTYPE_ERR, "Can not retrieve node with given ID");
00729 return false;
00730 }
00731
00732
00733 unsigned char posType = myInputStorage.readUnsignedByte();
00734 switch (posType) {
00735 case POSITION_ROADMAP:
00736
00737 roadPos.roadId = myInputStorage.readString();
00738 roadPos.pos = myInputStorage.readFloat();
00739 roadPos.laneId = myInputStorage.readUnsignedByte();
00740 break;
00741 case POSITION_2D:
00742 case POSITION_3D:
00743
00744 {
00745 float x = myInputStorage.readFloat();
00746 float y = myInputStorage.readFloat();
00747 roadPos = convertCartesianToRoadMap(Position2D(x,y));
00748 }
00749 if (posType == POSITION_3D) {
00750 myInputStorage.readFloat();
00751 }
00752 break;
00753 default:
00754 writeStatusCmd(CMD_STOP, RTYPE_ERR, "Not supported or unknown Position Format");
00755 return false;
00756 }
00757
00758
00759 float radius = myInputStorage.readFloat();
00760
00761 SUMOTime waitTime = myInputStorage.readInt();
00762
00763 if (roadPos.pos < 0) {
00764 writeStatusCmd(CMD_STOP, RTYPE_ERR, "Position on lane must not be negative");
00765 return false;
00766 }
00767
00768
00769 MSEdge* road = MSEdge::dictionary(roadPos.roadId);
00770 if (road == NULL) {
00771 writeStatusCmd(CMD_STOP, RTYPE_ERR, "Unable to retrieve road with given id");
00772 return false;
00773 }
00774
00775 const std::vector<MSLane*> &allLanes = road->getLanes();
00776 if (roadPos.laneId >= allLanes.size()) {
00777 writeStatusCmd(CMD_STOP, RTYPE_ERR, "No lane existing with such id on the given road");
00778 return false;
00779 }
00780
00781 actLane = allLanes[0];
00782 int index = 0;
00783 while (road->rightLane(actLane) != NULL) {
00784 actLane = road->rightLane(actLane);
00785 index++;
00786 }
00787 actLane = allLanes[0];
00788 if (index < roadPos.laneId) {
00789 for (int i=0; i < (roadPos.laneId - index); i++) {
00790 actLane = road->leftLane(actLane);
00791 }
00792 } else {
00793 for (int i=0; i < (index - roadPos.laneId); i++) {
00794 actLane = road->rightLane(actLane);
00795 }
00796 }
00797
00798
00799 if (!veh->addTraciStop(actLane, roadPos.pos, radius, waitTime)) {
00800 writeStatusCmd(CMD_STOP, RTYPE_ERR, "Vehicle is too close or behind the stop on " + actLane->getID());
00801 return false;
00802 }
00803
00804
00805 writeStatusCmd(CMD_STOP, RTYPE_OK, "");
00806
00807 int length = 1 + 1 + 4 + 1 + (4+roadPos.roadId.length()) + 4 + 1 + 4 + 4;
00808 myOutputStorage.writeUnsignedByte(length);
00809 myOutputStorage.writeUnsignedByte(CMD_STOP);
00810 myOutputStorage.writeInt(nodeId);
00811 myOutputStorage.writeUnsignedByte(POSITION_ROADMAP);
00812 myOutputStorage.writeString(roadPos.roadId);
00813 myOutputStorage.writeFloat(roadPos.pos);
00814 myOutputStorage.writeUnsignedByte(roadPos.laneId);
00815 myOutputStorage.writeFloat(radius);
00816 myOutputStorage.writeInt(waitTime);
00817
00818 return true;
00819 }
00820
00821
00822 bool
00823 TraCIServer::commandChangeLane() throw(TraCIException, std::invalid_argument) {
00824
00825 MSVehicle* veh = getVehicleByExtId(myInputStorage.readInt());
00826
00827 char laneIndex = myInputStorage.readByte();
00828
00829 SUMOTime stickyTime = myInputStorage.readInt();
00830
00831 if (veh == NULL) {
00832 writeStatusCmd(CMD_CHANGELANE, RTYPE_ERR, "Can not retrieve node with given ID");
00833 return false;
00834 }
00835
00836
00837
00838 if ((laneIndex < 0) || (laneIndex >= veh->getEdge()->getLanes().size())) {
00839 writeStatusCmd(CMD_CHANGELANE, RTYPE_ERR, "No lane existing with given id on the current road");
00840 return false;
00841 }
00842
00843
00844 veh->startLaneChange(static_cast<unsigned>(laneIndex), static_cast<SUMOTime>(stickyTime));
00845
00846
00847 writeStatusCmd(CMD_CHANGELANE, RTYPE_OK, "");
00848
00849 return true;
00850 }
00851
00852
00853 bool
00854 TraCIServer::commandChangeRoute() throw(TraCIException, std::invalid_argument) {
00855
00856 int vehId = myInputStorage.readInt();
00857 MSVehicle* veh = getVehicleByExtId(vehId);
00858
00859 std::string edgeId = myInputStorage.readString();
00860 MSEdge* edge = MSEdge::dictionary(edgeId);
00861
00862 double travelTime = myInputStorage.readDouble();
00863 if (veh == NULL) {
00864 writeStatusCmd(CMD_CHANGEROUTE, RTYPE_ERR, "Can not retrieve node with ID " + toString(vehId));
00865 return false;
00866 }
00867 if (edge == NULL) {
00868 writeStatusCmd(CMD_CHANGEROUTE, RTYPE_ERR, "Can not retrieve edge with ID " + edgeId);
00869 return false;
00870 }
00871
00872 SUMOTime currentTime = MSNet::getInstance()->getCurrentTimeStep() / 1000.;
00873 MSNet::EdgeWeightsProxi proxi(veh->getWeightsStorage(), MSNet::getInstance()->getWeightsStorage());
00874 SUMOReal effortBefore = proxi.getTravelTime(edge, veh, currentTime);
00875 bool hadError = false;
00876 if (travelTime < 0) {
00877 if (!veh->getWeightsStorage().knowsTravelTime(edge)) {
00878 hadError = true;
00879 }
00880 veh->getWeightsStorage().removeTravelTime(edge);
00881 } else {
00882 SUMOTime end = string2time(OptionsCont::getOptions().getString("end"));
00883 if(end<0) {
00884 end = SUMOTime_MAX;
00885 }
00886 veh->getWeightsStorage().addTravelTime(edge, currentTime, (SUMOReal) end / 1000., travelTime);
00887 }
00888 SUMOReal effortAfter = proxi.getTravelTime(edge, veh, currentTime);
00889 if (myVehiclesToReroute.find(veh)==myVehiclesToReroute.end() && (effortBefore != effortAfter)) {
00890
00891 if ((effortBefore > effortAfter) ^ veh->willPass(edge)) {
00892 myVehiclesToReroute.insert(veh);
00893 }
00894 }
00895 if (!hadError) {
00896 writeStatusCmd(CMD_CHANGEROUTE, RTYPE_OK, "");
00897 return true;
00898 } else {
00899 writeStatusCmd(CMD_CHANGEROUTE, RTYPE_ERR, "Could not (re-)set travel time properly");
00900 return false;
00901 }
00902 }
00903
00904
00905 bool
00906 TraCIServer::commandChangeTarget() throw(TraCIException, std::invalid_argument) {
00907
00908 MSVehicle* veh = getVehicleByExtId(myInputStorage.readInt());
00909
00910 std::string edgeID = myInputStorage.readString();
00911
00912
00913 const MSEdge* destEdge = MSEdge::dictionary(edgeID);
00914
00915 if (veh == NULL) {
00916 writeStatusCmd(CMD_CHANGETARGET, RTYPE_ERR, "Can not retrieve node with given ID");
00917 return false;
00918 }
00919
00920 if (destEdge == NULL) {
00921 writeStatusCmd(CMD_CHANGETARGET, RTYPE_ERR, "Can not retrieve road with ID " + edgeID);
00922 return false;
00923 }
00924
00925
00926 MSEdgeVector newRoute;
00927 const MSEdge* currentEdge = veh->getEdge();
00928 MSNet::EdgeWeightsProxi proxi(veh->getWeightsStorage(), MSNet::getInstance()->getWeightsStorage());
00929 DijkstraRouterTT_ByProxi<MSEdge, SUMOVehicle, prohibited_withRestrictions<MSEdge, SUMOVehicle>, MSNet::EdgeWeightsProxi> router(MSEdge::dictSize(), true, &proxi, &MSNet::EdgeWeightsProxi::getTravelTime);
00930 router.compute(currentEdge, destEdge, (const MSVehicle* const) veh,
00931 MSNet::getInstance()->getCurrentTimeStep(), newRoute);
00932
00933 if (veh->replaceRoute(newRoute, MSNet::getInstance()->getCurrentTimeStep())) {
00934 writeStatusCmd(CMD_CHANGETARGET, RTYPE_OK, "");
00935 return true;
00936 } else {
00937 writeStatusCmd(CMD_CHANGETARGET, RTYPE_ERR, "Route replacement failed for " + veh->getID());
00938 return false;
00939 }
00940 }
00941
00942
00943 bool
00944 TraCIServer::commandGetAllTLIds() throw(TraCIException) {
00945 tcpip::Storage tempMsg;
00946
00947
00948 MSTLLogicControl &tlsControl = MSNet::getInstance()->getTLSControl();
00949
00950 std::vector<std::string> idList = tlsControl.getAllTLIds();
00951
00952 if (idList.size() == 0) {
00953
00954 writeStatusCmd(CMD_GETALLTLIDS, RTYPE_ERR, "Could not retrieve any traffic light id");
00955 return false;
00956 }
00957
00958
00959 writeStatusCmd(CMD_GETALLTLIDS, RTYPE_OK, "");
00960
00961
00962 for (std::vector<std::string>::iterator iter = idList.begin(); iter != idList.end(); iter++) {
00963
00964 myOutputStorage.writeByte(2 + (4 + (*iter).size()));
00965
00966 myOutputStorage.writeByte(CMD_TLIDLIST);
00967
00968 myOutputStorage.writeString((*iter));
00969 }
00970 return true;
00971 }
00972
00973
00974 bool
00975 TraCIServer::commandGetTLStatus() throw(TraCIException) {
00976 SUMOTime lookback = 60*1000.;
00977
00978 tcpip::Storage tempMsg;
00979
00980
00981 int extId = myInputStorage.readInt();
00982
00983 SUMOTime timeFrom = myInputStorage.readInt();
00984
00985 SUMOTime timeTo = myInputStorage.readInt();
00986
00987
00988 MSTrafficLightLogic* const tlLogic = getTLLogicByExtId(extId);
00989
00990
00991 if (tlLogic == NULL) {
00992 writeStatusCmd(CMD_GETTLSTATUS, RTYPE_ERR, "Could not retrieve traffic light with given id");
00993 return false;
00994 }
00995 if ((timeTo < timeFrom) || (timeTo < 0) || (timeFrom < 0)) {
00996 writeStatusCmd(CMD_GETTLSTATUS, RTYPE_ERR, "The given time interval is not valid");
00997 return false;
00998 }
00999
01000
01001 writeStatusCmd(CMD_GETTLSTATUS, RTYPE_OK, "");
01002 std::vector<MSLink::LinkState> linkStates;
01003 std::vector<double> yellowTimes;
01004 size_t lastStep = tlLogic->getCurrentPhaseIndex();
01005 MSPhaseDefinition phase = tlLogic->getCurrentPhaseDef();
01006 MSTrafficLightLogic::LinkVectorVector affectedLinks = tlLogic->getLinks();
01007
01008
01009 for (int i = 0; i < affectedLinks.size(); i++) {
01010 linkStates.push_back(phase.getSignalState(i));
01011 yellowTimes.push_back(-1);
01012 }
01013
01014
01015 for (SUMOTime time = timeFrom - lookback; time <= timeTo; time+=DELTA_T) {
01016 if (time < 0) time = 0;
01017 size_t position = tlLogic->getPhaseIndexAtTime(time);
01018 size_t currentStep = tlLogic->getIndexFromOffset(position);
01019
01020 if (currentStep != lastStep) {
01021 lastStep = currentStep;
01022 phase = tlLogic->getPhase(currentStep);
01023
01024
01025
01026 std::map<const MSEdge*, pair<const MSEdge*, int> > writtenEdgePairs;
01027 for (int i = 0; i < linkStates.size(); i++) {
01028 MSLink::LinkState nextLinkState = phase.getSignalState(i);
01029
01030 if (nextLinkState == MSLink::LINKSTATE_TL_YELLOW_MAJOR || nextLinkState == MSLink::LINKSTATE_TL_YELLOW_MINOR) {
01031 if (yellowTimes[i] < 0) yellowTimes[i] = time;
01032 } else {
01033 if (nextLinkState != linkStates[i] && time >= timeFrom) {
01034 linkStates[i] = nextLinkState;
01035
01036
01037 MSTrafficLightLogic::LinkVector linkGroup = affectedLinks[i];
01038
01039 MSTrafficLightLogic::LaneVector laneGroup = tlLogic->getLanesAt(i);
01040
01041 for (int j = 0; j < linkGroup.size(); j++) {
01042 MSEdge &precEdge = laneGroup[j]->getEdge();
01043 MSEdge &succEdge = linkGroup[j]->getLane()->getEdge();
01044
01045
01046 std::map<const MSEdge*, pair<const MSEdge*, int> >::iterator itPair = writtenEdgePairs.find(&precEdge);
01047 if (itPair != writtenEdgePairs.end()) {
01048 if (itPair->second.first == &succEdge && itPair->second.second == nextLinkState) {
01049 continue;
01050 }
01051 }
01052
01053 writtenEdgePairs[&precEdge] = std::make_pair(&succEdge, nextLinkState);
01054
01055
01056 tempMsg.writeInt(time);
01057
01058 tempMsg.writeString(precEdge.getID());
01059
01060 tempMsg.writeFloat(laneGroup[j]->getShape().length());
01061
01062 tempMsg.writeString(succEdge.getID());
01063
01064 switch (nextLinkState) {
01065 case MSLink::LINKSTATE_TL_GREEN_MAJOR:
01066 case MSLink::LINKSTATE_TL_GREEN_MINOR:
01067 tempMsg.writeUnsignedByte(TLPHASE_GREEN);
01068 break;
01069 case MSLink::LINKSTATE_TL_RED:
01070 tempMsg.writeUnsignedByte(TLPHASE_RED);
01071 break;
01072 case MSLink::LINKSTATE_TL_OFF_BLINKING:
01073 tempMsg.writeUnsignedByte(TLPHASE_BLINKING);
01074 break;
01075 case MSLink::LINKSTATE_TL_OFF_NOSIGNAL:
01076 tempMsg.writeUnsignedByte(TLPHASE_NOSIGNAL);
01077 break;
01078 default:
01079 tempMsg.writeUnsignedByte(TLPHASE_NOSIGNAL);
01080 }
01081
01082 tempMsg.writeInt(yellowTimes[i]<0 ? 0 : time - yellowTimes[i]);
01083
01084 if (tempMsg.size() <= 253) {
01085
01086 myOutputStorage.writeUnsignedByte(1 + 1 + tempMsg.size());
01087 } else {
01088
01089 myOutputStorage.writeUnsignedByte(0);
01090 myOutputStorage.writeInt(1 + 4 + 1 + tempMsg.size());
01091 }
01092
01093 myOutputStorage.writeUnsignedByte(CMD_TLSWITCH);
01094
01095 myOutputStorage.writeStorage(tempMsg);
01096 tempMsg.reset();
01097 }
01098 }
01099 yellowTimes[i] = -1;
01100 }
01101 }
01102 }
01103 }
01104 return true;
01105 }
01106
01107
01108
01109 bool
01110 TraCIServer::commandSlowDown() throw(TraCIException) {
01111
01112 MSVehicle* veh = getVehicleByExtId(myInputStorage.readInt());
01113
01114 float newSpeed = MAX2(myInputStorage.readFloat(), 0.0f);
01115
01116 SUMOTime duration = myInputStorage.readInt();
01117
01118 if (veh == NULL) {
01119 writeStatusCmd(CMD_SLOWDOWN, RTYPE_ERR, "Can not retrieve node with given ID");
01120 return false;
01121 }
01122
01123
01124
01125
01126 if (duration <= 0) {
01127 writeStatusCmd(CMD_SLOWDOWN, RTYPE_ERR, "Invalid time interval");
01128 return false;
01129 }
01130
01131 if (!veh->startSpeedAdaption(newSpeed, duration, MSNet::getInstance()->getCurrentTimeStep())) {
01132 writeStatusCmd(CMD_SLOWDOWN, RTYPE_ERR, "Could not slow down");
01133 return false;
01134 }
01135
01136
01137 writeStatusCmd(CMD_SLOWDOWN, RTYPE_OK, "");
01138 return true;
01139 }
01140
01141
01142
01143 bool
01144 TraCIServer::commandCloseConnection() throw(TraCIException) {
01145 closeConnection_ = true;
01146
01147 writeStatusCmd(CMD_CLOSE, RTYPE_OK, "Goodbye");
01148 return true;
01149 }
01150
01151
01152
01153 bool
01154 TraCIServer::commandSimulationParameter() throw(TraCIException) {
01155 bool setParameter = (myInputStorage.readByte() != 0);
01156 std::string parameter = myInputStorage.readString();
01157
01158
01159 tcpip::Storage answerTmp;
01160
01161 if (parameter.compare("maxX")) {
01162 if (setParameter) {
01163 writeStatusCmd(CMD_SIMPARAMETER, RTYPE_ERR, "maxX is a read only parameter");
01164 return false;
01165 } else {
01166 answerTmp.writeFloat(getNetBoundary().getWidth());
01167 }
01168 } else if (parameter.compare("maxY")) {
01169 if (setParameter) {
01170 writeStatusCmd(CMD_SIMPARAMETER, RTYPE_ERR, "maxY is a read only parameter");
01171 return false;
01172 } else {
01173 answerTmp.writeFloat(getNetBoundary().getHeight());
01174 }
01175 } else if (parameter.compare("numberOfNodes")) {
01176 if (setParameter) {
01177 writeStatusCmd(CMD_SIMPARAMETER, RTYPE_ERR, "numberOfNodes is a read only parameter");
01178 return false;
01179 } else {
01180 writeStatusCmd(CMD_SIMPARAMETER, RTYPE_NOTIMPLEMENTED, "numberOfNodes not implemented yet");
01181 return false;
01182
01183 }
01184 } else if (parameter.compare("airDistance")) {
01185 MSVehicle* veh1 = getVehicleByExtId(myInputStorage.readInt());
01186 MSVehicle* veh2 = getVehicleByExtId(myInputStorage.readInt());
01187
01188 if (veh1 != NULL && veh2 != NULL) {
01189 if (setParameter) {
01190 writeStatusCmd(CMD_SIMPARAMETER, RTYPE_ERR, "airDistance is a read only parameter");
01191 return false;
01192 } else {
01193 float dx = veh1->getPosition().x() - veh2->getPosition().x();
01194 float dy = veh1->getPosition().y() - veh2->getPosition().y();
01195 answerTmp.writeFloat(sqrt(dx * dx + dy * dy));
01196 }
01197 } else {
01198 writeStatusCmd(CMD_SIMPARAMETER, RTYPE_ERR, "Can not retrieve node with given ID");
01199 return false;
01200 }
01201 } else if (parameter.compare("drivingDistance")) {
01202 MSVehicle* veh1 = getVehicleByExtId(myInputStorage.readInt());
01203 MSVehicle* veh2 = getVehicleByExtId(myInputStorage.readInt());
01204
01205 if (veh1 != NULL && veh2 != NULL) {
01206 if (setParameter) {
01207 writeStatusCmd(CMD_SIMPARAMETER, RTYPE_ERR, "airDistance is a read only parameter");
01208 return false;
01209 } else {
01210 writeStatusCmd(CMD_SIMPARAMETER, RTYPE_NOTIMPLEMENTED, "drivingDistance not implemented yet");
01211 return false;
01212
01213
01214
01215
01216 }
01217 } else {
01218 writeStatusCmd(CMD_SIMPARAMETER, RTYPE_ERR, "Can not retrieve node with given ID");
01219 return false;
01220 }
01221 }
01222
01223
01224 writeStatusCmd(CMD_SIMPARAMETER, RTYPE_OK, "");
01225
01226
01227 myOutputStorage.writeUnsignedByte(1 + 1 + 1 + 4 + static_cast<int>(parameter.length()) + answerTmp.size());
01228
01229 myOutputStorage.writeUnsignedByte(CMD_SIMPARAMETER);
01230
01231 myOutputStorage.writeUnsignedByte(1);
01232
01233 myOutputStorage.writeString(parameter);
01234
01235 myOutputStorage.writeStorage(answerTmp);
01236 return true;
01237 }
01238
01239
01240
01241 bool
01242 TraCIServer::commandUpdateCalibrator() throw(TraCIException) {
01243 myOutputStorage.reset();
01244
01245 int countTime = myInputStorage.readInt();
01246 int vehicleCount = myInputStorage.readInt();
01247 std::string calibratorId = myInputStorage.readString();
01248
01249 MSCalibrator::updateCalibrator(calibratorId, countTime, vehicleCount);
01250
01251
01252
01253 return true;
01254 }
01255
01256
01257
01258 bool
01259 TraCIServer::commandPositionConversion() throw(TraCIException) {
01260 tcpip::Storage tmpResult;
01261 RoadMapPos roadPos;
01262 Position2D cartesianPos;
01263 float x = 0;
01264 float y = 0;
01265 float z = 0;
01266 unsigned char destPosType;
01267
01268
01269 unsigned char srcPosType = myInputStorage.readUnsignedByte();
01270
01271 switch (srcPosType) {
01272 case POSITION_2D:
01273 case POSITION_2_5D:
01274 case POSITION_3D:
01275 x = myInputStorage.readFloat();
01276 y = myInputStorage.readFloat();
01277 if (srcPosType != POSITION_2D) {
01278 z = myInputStorage.readFloat();
01279 }
01280
01281 destPosType = myInputStorage.readUnsignedByte();
01282
01283 switch (destPosType) {
01284 case POSITION_ROADMAP:
01285
01286 roadPos = convertCartesianToRoadMap(Position2D(x, y));
01287
01288
01289 tmpResult.writeUnsignedByte(POSITION_ROADMAP);
01290 tmpResult.writeString(roadPos.roadId);
01291 tmpResult.writeFloat(roadPos.pos);
01292 tmpResult.writeUnsignedByte(roadPos.laneId);
01293 break;
01294 case POSITION_3D:
01295 writeStatusCmd(CMD_POSITIONCONVERSION, RTYPE_ERR,
01296 "Destination position type is same as source position type");
01297 return false;
01298 default:
01299 writeStatusCmd(CMD_POSITIONCONVERSION, RTYPE_ERR,
01300 "Destination position type not supported");
01301 return false;
01302 }
01303 break;
01304 case POSITION_ROADMAP:
01305 roadPos.roadId = myInputStorage.readString();
01306 roadPos.pos = myInputStorage.readFloat();
01307 roadPos.laneId = myInputStorage.readUnsignedByte();
01308
01309
01310 destPosType = myInputStorage.readUnsignedByte();
01311
01312 switch (destPosType) {
01313 case POSITION_2D:
01314 case POSITION_2_5D:
01315 case POSITION_3D:
01316
01317 try {
01318 Position2D result = convertRoadMapToCartesian(roadPos);
01319 x = result.x();
01320 y = result.y();
01321 } catch (TraCIException &e) {
01322 writeStatusCmd(CMD_POSITIONCONVERSION, RTYPE_ERR, e.what());
01323 return false;
01324 }
01325
01326
01327 tmpResult.writeUnsignedByte(destPosType);
01328 tmpResult.writeFloat(x);
01329 tmpResult.writeFloat(y);
01330 if (destPosType != POSITION_2D) {
01331 tmpResult.writeFloat(z);
01332 }
01333 break;
01334 case POSITION_ROADMAP:
01335 writeStatusCmd(CMD_POSITIONCONVERSION, RTYPE_ERR,
01336 "Destination position type is same as source position type");
01337 return false;
01338 default:
01339 writeStatusCmd(CMD_POSITIONCONVERSION, RTYPE_ERR,
01340 "Destination position type not supported");
01341 return false;
01342 }
01343 break;
01344 default:
01345 writeStatusCmd(CMD_POSITIONCONVERSION, RTYPE_ERR,
01346 "Source position type not supported");
01347 return false;
01348 }
01349
01350
01351 writeStatusCmd(CMD_POSITIONCONVERSION, RTYPE_OK, "");
01352
01353 myOutputStorage.writeUnsignedByte(1 + 1 + tmpResult.size() + 1);
01354 myOutputStorage.writeUnsignedByte(CMD_POSITIONCONVERSION);
01355 myOutputStorage.writeStorage(tmpResult);
01356 myOutputStorage.writeUnsignedByte(destPosType);
01357 return true;
01358 }
01359
01360
01361
01362 bool
01363 TraCIServer::commandScenario() throw(TraCIException) {
01364 Storage tmpResult;
01365 std::string warning = "";
01366
01367
01368 bool isWriteCommand = myInputStorage.readUnsignedByte();
01369
01370
01371 int domain = myInputStorage.readUnsignedByte();
01372
01373 try {
01374 switch (domain) {
01375 case DOM_ROADMAP:
01376 warning = handleRoadMapDomain(isWriteCommand, tmpResult);
01377 break;
01378 case DOM_VEHICLE:
01379 warning = handleVehicleDomain(isWriteCommand, tmpResult);
01380 break;
01381 case DOM_TRAFFICLIGHTS:
01382 warning = handleTrafficLightDomain(isWriteCommand, tmpResult);
01383 break;
01384 case DOM_POI:
01385 warning = handlePoiDomain(isWriteCommand, tmpResult);
01386 break;
01387 case DOM_POLYGON:
01388 warning = handlePolygonDomain(isWriteCommand, tmpResult);
01389 break;
01390 default:
01391 writeStatusCmd(CMD_SCENARIO, RTYPE_ERR, "Unknown domain specified");
01392 return false;
01393 }
01394 } catch (TraCIException &e) {
01395 writeStatusCmd(CMD_SCENARIO, RTYPE_ERR, e.what());
01396 return false;
01397 }
01398
01399
01400 writeStatusCmd(CMD_SCENARIO, RTYPE_OK, warning);
01401
01402 if (!isWriteCommand) {
01403 if (tmpResult.size() <= 253) {
01404 myOutputStorage.writeUnsignedByte(1 + 1 + tmpResult.size());
01405 myOutputStorage.writeUnsignedByte(CMD_SCENARIO);
01406 myOutputStorage.writeStorage(tmpResult);
01407 } else {
01408 myOutputStorage.writeUnsignedByte(0);
01409 myOutputStorage.writeInt(1 + 4 + 1 + tmpResult.size());
01410 myOutputStorage.writeUnsignedByte(CMD_SCENARIO);
01411 myOutputStorage.writeStorage(tmpResult);
01412 }
01413 }
01414 return true;
01415 }
01416
01417
01418
01419 bool
01420 TraCIServer::commandAddVehicle() throw(TraCIException) {
01421
01422
01423 std::string vehicleId = myInputStorage.readString();
01424 std::string vehicleTypeId = myInputStorage.readString();
01425 std::string routeId = myInputStorage.readString();
01426 std::string laneId = myInputStorage.readString();
01427 SUMOReal emitPosition = myInputStorage.readFloat();
01428 SUMOReal emitSpeed = myInputStorage.readFloat();
01429
01430
01431 MSVehicleType *vehicleType = MSNet::getInstance()->getVehicleControl().getVType(vehicleTypeId);
01432 if (!vehicleType) {
01433 writeStatusCmd(CMD_ADDVEHICLE, RTYPE_ERR, "Invalid vehicleTypeId: '"+vehicleTypeId+"'");
01434 return false;
01435 }
01436
01437
01438 const MSRoute *route = MSRoute::dictionary(routeId);
01439 if (!route) {
01440 writeStatusCmd(CMD_ADDVEHICLE, RTYPE_ERR, "Invalid routeId: '"+routeId+"'");
01441 return false;
01442 }
01443
01444
01445 MSLane *lane;
01446 if (laneId != "") {
01447 lane = MSLane::dictionary(laneId);
01448 if (!lane) {
01449 writeStatusCmd(CMD_ADDVEHICLE, RTYPE_ERR, "Invalid laneId: '"+laneId+"'");
01450 return false;
01451 }
01452 } else {
01453 lane = route->getEdges()[0]->getLanes()[0];
01454 if (!lane) {
01455 writeStatusCmd(CMD_STOP, RTYPE_ERR, "Could not find first lane of first edge in routeId '"+routeId+"'");
01456 return false;
01457 }
01458 }
01459
01460
01461 SUMOVehicleParameter* vehicleParams = new SUMOVehicleParameter();
01462 vehicleParams->id = vehicleId;
01463 vehicleParams->depart = MSNet::getInstance()->getCurrentTimeStep()+1;
01464 MSVehicle *vehicle = MSNet::getInstance()->getVehicleControl().buildVehicle(vehicleParams, route, vehicleType);
01465 if (vehicle == NULL) {
01466 writeStatusCmd(CMD_STOP, RTYPE_ERR, "Could not build vehicle");
01467 return false;
01468 }
01469
01470
01471 float clippedEmitSpeed;
01472 if (emitSpeed<0) {
01473 clippedEmitSpeed = MIN2(lane->getMaxSpeed(), vehicle->getMaxSpeed());
01474 } else {
01475 clippedEmitSpeed = MIN3(lane->getMaxSpeed(), vehicle->getMaxSpeed(), emitSpeed);
01476 }
01477
01478
01479 if (!MSNet::getInstance()->getVehicleControl().addVehicle(vehicle->getID(), vehicle)) {
01480 writeStatusCmd(CMD_ADDVEHICLE, RTYPE_ERR, "Could not add vehicle to VehicleControl");
01481 return false;
01482 }
01483
01484
01485 if (!lane->isEmissionSuccess(vehicle, clippedEmitSpeed, emitPosition, true)) {
01486 MSNet::getInstance()->getVehicleControl().deleteVehicle(vehicle);
01487 writeStatusCmd(CMD_ADDVEHICLE, RTYPE_ERR, "Could not emit vehicle");
01488 return false;
01489 }
01490
01491
01492 vehicle->onDepart();
01493
01494
01495 writeStatusCmd(CMD_ADDVEHICLE, RTYPE_OK, "");
01496
01497 return true;
01498 }
01499
01500
01501
01502 bool
01503 TraCIServer::commandDistanceRequest() throw(TraCIException) {
01504 Position2D pos1;
01505 Position2D pos2;
01506 RoadMapPos roadPos1;
01507 RoadMapPos roadPos2;
01508 const std::vector<MSLane*>* lanes;
01509
01510
01511 int posType = myInputStorage.readUnsignedByte();
01512 switch (posType) {
01513 case POSITION_ROADMAP:
01514 roadPos1.roadId = myInputStorage.readString();
01515 roadPos1.pos = myInputStorage.readFloat();
01516 roadPos1.laneId = myInputStorage.readUnsignedByte();
01517 try {
01518 pos1 = convertRoadMapToCartesian(roadPos1);
01519 } catch (TraCIException &e) {
01520 writeStatusCmd(CMD_DISTANCEREQUEST, RTYPE_ERR, e.what());
01521 return false;
01522 }
01523 break;
01524 case POSITION_2D:
01525 case POSITION_2_5D:
01526 case POSITION_3D: {
01527 float p1x = myInputStorage.readFloat();
01528 float p1y = myInputStorage.readFloat();
01529 pos1.set(p1x, p1y);
01530 }
01531 if ((posType == POSITION_2_5D) || (posType == POSITION_3D)) {
01532 myInputStorage.readFloat();
01533 }
01534 roadPos1 = convertCartesianToRoadMap(pos1);
01535 break;
01536 default:
01537 writeStatusCmd(CMD_DISTANCEREQUEST, RTYPE_ERR, "Unknown position format used for distance request");
01538 return false;
01539 }
01540
01541
01542 posType = myInputStorage.readUnsignedByte();
01543 switch (posType) {
01544 case POSITION_ROADMAP:
01545 roadPos2.roadId = myInputStorage.readString();
01546 roadPos2.pos = myInputStorage.readFloat();
01547 roadPos2.laneId = myInputStorage.readUnsignedByte();
01548 try {
01549 pos2 = convertRoadMapToCartesian(roadPos2);
01550 } catch (TraCIException &e) {
01551 writeStatusCmd(CMD_DISTANCEREQUEST, RTYPE_ERR, e.what());
01552 return false;
01553 }
01554 break;
01555 case POSITION_2D:
01556 case POSITION_2_5D:
01557 case POSITION_3D: {
01558 float p2x = myInputStorage.readFloat();
01559 float p2y = myInputStorage.readFloat();
01560 pos2.set(p2x, p2y);
01561 }
01562 if ((posType == POSITION_2_5D) || (posType == POSITION_3D)) {
01563 myInputStorage.readFloat();
01564 }
01565 roadPos2 = convertCartesianToRoadMap(pos2);
01566 break;
01567 default:
01568 writeStatusCmd(CMD_DISTANCEREQUEST, RTYPE_ERR, "Unknown position format used for distance request");
01569 return false;
01570 }
01571
01572
01573 int distType = myInputStorage.readUnsignedByte();
01574
01575 float distance = 0.0;
01576 if (distType == REQUEST_DRIVINGDIST) {
01577
01578 std::vector<const MSEdge*> edges;
01579 TraCIDijkstraRouter<MSEdge> router(MSEdge::dictSize());
01580
01581 if ((roadPos1.roadId.compare(roadPos2.roadId) == 0)
01582 && (roadPos1.pos <= roadPos2.pos)) {
01583 distance = roadPos2.pos - roadPos1.pos;
01584 } else {
01585 router.compute(MSEdge::dictionary(roadPos1.roadId), MSEdge::dictionary(roadPos2.roadId), NULL,
01586 MSNet::getInstance()->getCurrentTimeStep(), edges);
01587 MSRoute route("", edges, false, RGBColor::DEFAULT_COLOR, std::vector<SUMOVehicleParameter::Stop>());
01588 distance = static_cast<float>(route.getDistanceBetween(roadPos1.pos, roadPos2.pos,
01589 MSEdge::dictionary(roadPos1.roadId), MSEdge::dictionary(roadPos2.roadId)));
01590 }
01591 } else {
01592
01593
01594 distType = REQUEST_AIRDIST;
01595 distance = static_cast<float>(pos1.distanceTo(pos2));
01596 }
01597
01598
01599 writeStatusCmd(CMD_DISTANCEREQUEST, RTYPE_OK, "");
01600
01601 myOutputStorage.writeUnsignedByte(1 + 1 + 1 + 4);
01602 myOutputStorage.writeUnsignedByte(CMD_DISTANCEREQUEST);
01603 myOutputStorage.writeUnsignedByte(distType);
01604 myOutputStorage.writeFloat(distance);
01605 return true;
01606 }
01607
01608
01609
01610 bool
01611 TraCIServer::commandSubscribeLifecycles() throw(TraCIException) {
01612
01613 int domain = myInputStorage.readUnsignedByte();
01614
01615 if (domain != DOM_VEHICLE) {
01616
01617 writeStatusCmd(CMD_SUBSCRIBELIFECYCLES, RTYPE_NOTIMPLEMENTED, "Can only subscribe to lifecycle of DOM_VEHICLE at this time");
01618 return false;
01619 }
01620
01621 myLifecycleSubscriptions.insert(domain);
01622
01623 if (domain == DOM_VEHICLE) {
01624 myCreatedVehicles.clear();
01625 myDestroyedVehicles.clear();
01626 }
01627
01628
01629 writeStatusCmd(CMD_SUBSCRIBELIFECYCLES, RTYPE_OK, "");
01630 return true;
01631 }
01632
01633
01634
01635 bool
01636 TraCIServer::commandUnsubscribeLifecycles() throw(TraCIException) {
01637
01638 int domain = myInputStorage.readUnsignedByte();
01639
01640 if (domain != DOM_VEHICLE) {
01641
01642 writeStatusCmd(CMD_UNSUBSCRIBELIFECYCLES, RTYPE_NOTIMPLEMENTED, "Can only subscribe to lifecycle of DOM_VEHICLE at this time");
01643 return false;
01644 }
01645
01646 myLifecycleSubscriptions.erase(domain);
01647
01648 if (domain == DOM_VEHICLE) {
01649 myCreatedVehicles.clear();
01650 myDestroyedVehicles.clear();
01651 }
01652
01653
01654 writeStatusCmd(CMD_UNSUBSCRIBELIFECYCLES, RTYPE_OK, "");
01655 return true;
01656 }
01657
01658
01659
01660 bool
01661 TraCIServer::commandSubscribeDomain() throw(TraCIException) {
01662
01663 int domainId = myInputStorage.readUnsignedByte();
01664
01665 if (domainId != DOM_VEHICLE) {
01666
01667 writeStatusCmd(CMD_SUBSCRIBEDOMAIN, RTYPE_NOTIMPLEMENTED, "Can only subscribe to DOM_VEHICLE at this time");
01668 return false;
01669 }
01670
01671
01672 int variableCount = myInputStorage.readUnsignedByte();
01673
01674
01675 std::list<std::pair<int, int> > subscribedVariables;
01676 for (int i = 0; i < variableCount; i++) {
01677
01678 int variableId = myInputStorage.readUnsignedByte();
01679
01680
01681 int dataType = myInputStorage.readUnsignedByte();
01682
01683 if (!(
01684 ((domainId == DOM_VEHICLE) && (variableId == DOMVAR_SIMTIME) && (dataType == TYPE_DOUBLE))
01685 ||
01686 ((domainId == DOM_VEHICLE) && (variableId == DOMVAR_SPEED) && (dataType == TYPE_FLOAT))
01687 ||
01688 ((domainId == DOM_VEHICLE) && (variableId == DOMVAR_ALLOWED_SPEED) && (dataType == TYPE_FLOAT))
01689 ||
01690 ((domainId == DOM_VEHICLE) && (variableId == DOMVAR_POSITION) && (dataType == POSITION_2D))
01691 ||
01692 ((domainId == DOM_VEHICLE) && (variableId == DOMVAR_POSITION) && (dataType == POSITION_ROADMAP))
01693 ||
01694 ((domainId == DOM_VEHICLE) && (variableId == DOMVAR_ANGLE) && (dataType == TYPE_FLOAT))
01695 ||
01696 ((domainId == DOM_VEHICLE) && (variableId == DOMVAR_CO2EMISSION) && (dataType == TYPE_FLOAT))
01697 ||
01698 ((domainId == DOM_VEHICLE) && (variableId == DOMVAR_COEMISSION) && (dataType == TYPE_FLOAT))
01699 ||
01700 ((domainId == DOM_VEHICLE) && (variableId == DOMVAR_HCEMISSION) && (dataType == TYPE_FLOAT))
01701 ||
01702 ((domainId == DOM_VEHICLE) && (variableId == DOMVAR_PMXEMISSION) && (dataType == TYPE_FLOAT))
01703 ||
01704 ((domainId == DOM_VEHICLE) && (variableId == DOMVAR_NOXEMISSION) && (dataType == TYPE_FLOAT))
01705 ||
01706 ((domainId == DOM_VEHICLE) && (variableId == DOMVAR_FUELCONSUMPTION) && (dataType == TYPE_FLOAT))
01707 )) {
01708
01709 writeStatusCmd(CMD_SUBSCRIBEDOMAIN, RTYPE_NOTIMPLEMENTED, "Can not subscribe to this domain/variable/type combination");
01710 return false;
01711 }
01712
01713 subscribedVariables.push_back(std::pair<int, int>(variableId, dataType));
01714 }
01715
01716
01717 myDomainSubscriptions[domainId] = subscribedVariables;
01718
01719
01720 writeStatusCmd(CMD_SUBSCRIBEDOMAIN, RTYPE_OK, "");
01721 return true;
01722 }
01723
01724
01725
01726 bool
01727 TraCIServer::commandUnsubscribeDomain() throw(TraCIException) {
01728
01729 int domain = myInputStorage.readUnsignedByte();
01730
01731 if (domain != DOM_VEHICLE) {
01732
01733 writeStatusCmd(CMD_UNSUBSCRIBEDOMAIN, RTYPE_NOTIMPLEMENTED, "Can only subscribe to lifecycle of DOM_VEHICLE at this time");
01734 return false;
01735 }
01736
01737 myDomainSubscriptions.erase(domain);
01738
01739
01740 writeStatusCmd(CMD_UNSUBSCRIBEDOMAIN, RTYPE_OK, "");
01741 return true;
01742 }
01743
01744
01745
01746 void
01747 TraCIServer::writeStatusCmd(int commandId, int status, std::string description) {
01748 if (status == RTYPE_ERR) {
01749 MsgHandler::getErrorInstance()->inform("Answered with error to command " + toString(commandId) +
01750 ": " + description);
01751 } else if (status == RTYPE_NOTIMPLEMENTED) {
01752 MsgHandler::getErrorInstance()->inform("Requested command not implemented (" + toString(commandId) +
01753 "): " + description);
01754 }
01755
01756
01757 myOutputStorage.writeUnsignedByte(1 + 1 + 1 + 4 + static_cast<int>(description.length()));
01758
01759 myOutputStorage.writeUnsignedByte(commandId);
01760
01761 myOutputStorage.writeUnsignedByte(status);
01762
01763 myOutputStorage.writeString(description);
01764
01765 return;
01766 }
01767
01768
01769
01770 void
01771 TraCIServer::convertExt2IntId(int extId, std::string& intId) {
01772 if (isMapChanged_) {
01773 isMapChanged_ = false;
01774 ext2intId.clear();
01775 for (map<std::string, int>::const_iterator iter = equippedVehicles_.begin(); iter != equippedVehicles_.end(); ++iter) {
01776 if (iter->second > -1) {
01777 ext2intId[iter->second] = iter->first;
01778 }
01779 }
01780 }
01781
01782
01783 map<int, std::string>::const_iterator it = ext2intId.find(extId);
01784 if (it != ext2intId.end()) intId = it->second;
01785 else intId = "";
01786 }
01787
01788
01789
01790 MSVehicle*
01791 TraCIServer::getVehicleByExtId(int extId) {
01792 std::string intId;
01793 convertExt2IntId(extId, intId);
01794 return MSNet::getInstance()->getVehicleControl().getVehicle(intId);
01795 }
01796
01797
01798
01799 MSTrafficLightLogic*
01800 TraCIServer::getTLLogicByExtId(int extId) {
01801 std::string intId = "";
01802 std::map<int, std::string>::iterator iter = trafficLightsExt2IntId.find(extId);
01803 if (iter != trafficLightsExt2IntId.end()) {
01804 intId = iter->second;
01805 }
01806
01807 return MSNet::getInstance()->getTLSControl().getActive(intId);
01808 }
01809
01810
01811
01812 PointOfInterest*
01813 TraCIServer::getPoiByExtId(int extId) {
01814 std::string intId = "";
01815 std::map<int, std::string>::iterator iter = poiExt2IntId.find(extId);
01816 if (iter != poiExt2IntId.end()) {
01817 intId = iter->second;
01818 }
01819
01820 ShapeContainer& shapeCont = MSNet::getInstance()->getShapeContainer();
01821 PointOfInterest* poi = 0;
01822 for (int i = shapeCont.getMinLayer(); i <= shapeCont.getMaxLayer(); i++) {
01823 if (poi == 0) {
01824 poi = shapeCont.getPOICont(i).get(intId);
01825 }
01826 }
01827
01828 return poi;
01829 }
01830
01831
01832
01833 Polygon2D*
01834 TraCIServer::getPolygonByExtId(int extId) {
01835 std::string intId = "";
01836 map<int, std::string>::const_iterator it = ext2intId.find(extId);
01837 if (it != ext2intId.end()) {
01838 intId = it->second;
01839 }
01840
01841 ShapeContainer& shapeCont = MSNet::getInstance()->getShapeContainer();
01842 Polygon2D* polygon = NULL;
01843 for (int i = shapeCont.getMinLayer(); i <= shapeCont.getMaxLayer(); i++) {
01844 if (polygon == NULL) {
01845 polygon = shapeCont.getPolygonCont(i).get(intId);
01846 }
01847 }
01848
01849 return polygon;
01850 }
01851
01852
01853
01854 const Boundary&
01855 TraCIServer::getNetBoundary() {
01856
01857 if (netBoundary_ != NULL) return *netBoundary_;
01858
01859
01860 netBoundary_ = new Boundary();
01861
01862
01863
01864
01865
01866
01867
01868
01869
01870
01871
01872
01873
01874 const std::vector<MSEdge*> &edges = MSNet::getInstance()->getEdgeControl().getEdges();
01875
01876
01877 for (std::vector<MSEdge*>::const_iterator e = edges.begin(); e != edges.end(); ++e) {
01878 const std::vector<MSLane*> &lanes = (*e)->getLanes();
01879 for (std::vector<MSLane*>::const_iterator laneIt = lanes.begin(); laneIt != lanes.end(); ++laneIt) {
01880 netBoundary_->add((*laneIt)->getShape().getBoxBoundary());
01881 }
01882 }
01883 return *netBoundary_;
01884 }
01885
01886
01887
01888 TraCIServer::RoadMapPos
01889 TraCIServer::convertCartesianToRoadMap(Position2D pos) {
01890 RoadMapPos result;
01891 std::vector<std::string> allEdgeIds;
01892 MSEdge* edge;
01893 Position2D lineStart;
01894 Position2D lineEnd;
01895 double minDistance = HUGE_VAL;
01896 SUMOReal newDistance;
01897 Position2D intersection;
01898 MSLane* tmpLane;
01899
01900
01901 allEdgeIds = MSNet::getInstance()->getEdgeControl().getEdgeNames();
01902
01903
01904 for (std::vector<std::string>::iterator itId = allEdgeIds.begin(); itId != allEdgeIds.end(); itId++) {
01905 edge = MSEdge::dictionary((*itId));
01906 const std::vector<MSLane*> &allLanes = edge->getLanes();
01907
01908
01909
01910
01911 for (std::vector<MSLane*>::const_iterator itLane = allLanes.begin(); itLane != allLanes.end(); itLane++) {
01912 Position2DVector shape = (*itLane)->getShape();
01913
01914
01915
01916
01917 for (int i = 0; i < shape.size()-1; i++) {
01918 lineStart = shape[i];
01919 lineEnd = shape[i+1];
01920
01921
01922
01923
01924
01925 if ((lineStart.y() > (pos.y()+minDistance) && lineEnd.y() > (pos.y()+minDistance))
01926 || (lineStart.y() < (pos.y()-minDistance) && lineEnd.y() < (pos.y()-minDistance))
01927 || (lineStart.x() > (pos.x()+minDistance) && lineEnd.x() > (pos.x()+minDistance))
01928 || (lineStart.x() < (pos.x()-minDistance) && lineEnd.x() < (pos.x()-minDistance))) {
01929
01930
01931
01932 continue;
01933 } else {
01934
01935 newDistance = GeomHelper::closestDistancePointLine(pos, lineStart, lineEnd, intersection);
01936
01937
01938
01939 if (newDistance < minDistance && newDistance != -1.0) {
01940
01941 minDistance = newDistance;
01942 result.roadId = (*itId);
01943 result.laneId = 0;
01944 tmpLane = (*itLane);
01945 while ((tmpLane =tmpLane->getRightLane()) != NULL) {
01946 result.laneId++;
01947 }
01948 result.pos = lineStart.distanceTo(intersection);
01949 for (int j = 0; j < i; j++) {
01950 result.pos += shape[j].distanceTo(shape[j+1]);
01951 }
01952
01953
01954
01955 }
01956
01957
01958 }
01959 }
01960 }
01961 }
01962
01963 return result;
01964 }
01965
01966
01967
01968 Position2D
01969 TraCIServer::convertRoadMapToCartesian(traci::TraCIServer::RoadMapPos roadPos)
01970 throw(TraCIException) {
01971 if (roadPos.pos < 0) {
01972 throw TraCIException("Position on lane must not be negative");
01973 }
01974
01975
01976 MSEdge* road = MSEdge::dictionary(roadPos.roadId);
01977 if (road == NULL) {
01978 throw TraCIException("Unable to retrieve road with given id");
01979 }
01980
01981 const std::vector<MSLane*> &allLanes = road->getLanes();
01982 if ((roadPos.laneId >= allLanes.size()) || (allLanes.size() == 0)) {
01983 throw TraCIException("No lane existing with such id on the given road");
01984 }
01985
01986
01987 Position2DVector shape = allLanes[roadPos.laneId]->getShape();
01988 return shape.positionAtLengthPosition(roadPos.pos);
01989 }
01990
01991
01992
01993 std::string
01994 TraCIServer::handleRoadMapDomain(bool isWriteCommand, tcpip::Storage& response)
01995 throw(TraCIException) {
01996 std::string name = "";
01997 std::string warning = "";
01998 DataTypeContainer dataCont;
01999
02000
02001 int objectId = myInputStorage.readInt();
02002
02003 if (objectId < 0 || objectId >= MSEdge::dictSize()) {
02004 throw TraCIException("Invalid object id specified");
02005 }
02006 MSEdge* edge = MSEdge::dictionary(objectId);
02007
02008
02009 int variableId = myInputStorage.readUnsignedByte();
02010
02011
02012 int dataType = myInputStorage.readUnsignedByte();
02013
02014
02015 if (myInputStorage.valid_pos()) {
02016 dataCont.readValue(dataType, myInputStorage);
02017 }
02018
02019 if (isWriteCommand) {
02020 throw TraCIException("Road map domain does not contain writable variables");
02021 }
02022
02023
02024 response.writeUnsignedByte((isWriteCommand ? 0x01 : 0x00));
02025 response.writeUnsignedByte(DOM_ROADMAP);
02026 response.writeInt(objectId);
02027 response.writeUnsignedByte(variableId);
02028
02029 switch (variableId) {
02030
02031 case DOMVAR_NAME:
02032 if (edge != NULL) {
02033 name = edge->getID();
02034 response.writeUnsignedByte(TYPE_STRING);
02035 response.writeString(name);
02036 if (dataType != TYPE_STRING) {
02037 warning = "Warning: requested data type could not be used; using string instead!";
02038 }
02039 } else {
02040 throw TraCIException("Unable to retrieve road with given id");
02041 }
02042 break;
02043
02044
02045 case DOMVAR_EXTID:
02046
02047 if (dataCont.getLastValueRead() != TYPE_STRING) {
02048 throw TraCIException("Internal id must be given as string value");
02049 }
02050
02051 name = dataCont.getString();
02052 edge = MSEdge::dictionary(name);
02053 if (edge != NULL) {
02054 response.writeUnsignedByte(TYPE_INTEGER);
02055 response.writeInt(edge->getNumericalID());
02056 } else {
02057 std::stringstream msg;
02058 msg << "Edge with internal id " << name << " not existing";
02059 throw TraCIException(msg.str());
02060 }
02061 break;
02062
02063
02064 case DOMVAR_BOUNDINGBOX:
02065 response.writeUnsignedByte(TYPE_BOUNDINGBOX);
02066 response.writeFloat(getNetBoundary().xmin());
02067 response.writeFloat(getNetBoundary().ymin());
02068 response.writeFloat(getNetBoundary().xmax());
02069 response.writeFloat(getNetBoundary().ymax());
02070
02071 if (dataType != TYPE_BOUNDINGBOX) {
02072 warning = "Warning: requested data type could not be used; using boundary box type instead!";
02073 }
02074 break;
02075
02076
02077 case DOMVAR_COUNT:
02078 response.writeUnsignedByte(TYPE_INTEGER);
02079 response.writeInt(MSNet::getInstance()->getEdgeControl().getEdgeNames().size());
02080 if (dataType != TYPE_INTEGER) {
02081 warning = "Warning: requested data type could not be used; using integer instead!";
02082 }
02083 break;
02084
02085
02086 case DOMVAR_SHAPE:
02087 if (edge != NULL) {
02088 const std::vector<MSLane*> &lanes = edge->getLanes();
02089 const Position2DVector shape = lanes[lanes.size()/2]->getShape();
02090 response.writeUnsignedByte(TYPE_POLYGON);
02091 response.writeUnsignedByte(MIN2(static_cast<size_t>(255),shape.size()));
02092 for (int iPoint=0; iPoint < MIN2(static_cast<size_t>(255),shape.size()); iPoint++) {
02093 response.writeFloat(shape[iPoint].x());
02094 response.writeFloat(shape[iPoint].y());
02095 }
02096 if (dataType != TYPE_POLYGON) {
02097 warning = "Warning: requested data type could not be used; using polygon type instead!";
02098 }
02099 } else {
02100 throw TraCIException("Unable to retrieve road with given id");
02101 }
02102 break;
02103
02104
02105 default:
02106 throw TraCIException("Unknown domain variable specified");
02107 }
02108
02109 return warning;
02110 }
02111
02112
02113
02114 std::string
02115 TraCIServer::handleVehicleDomain(bool isWriteCommand, tcpip::Storage& response)
02116 throw(TraCIException) {
02117 std::string name;
02118 int count = 0;
02119 MSVehicleControl* vehControl = NULL;
02120 std::string warning = "";
02121 DataTypeContainer dataCont;
02122
02123
02124 int objectId = myInputStorage.readInt();
02125 MSVehicle* veh = getVehicleByExtId(objectId);
02126
02127
02128 int variableId = myInputStorage.readUnsignedByte();
02129
02130
02131 int dataType = myInputStorage.readUnsignedByte();
02132
02133
02134 if (myInputStorage.valid_pos()) {
02135 dataCont.readValue(dataType, myInputStorage);
02136 }
02137
02138 if (isWriteCommand) {
02139 throw TraCIException("Vehicle domain does not contain writable variables");
02140 }
02141
02142
02143 response.writeUnsignedByte((isWriteCommand ? 0x01 : 0x00));
02144 response.writeUnsignedByte(DOM_VEHICLE);
02145 response.writeInt(objectId);
02146 response.writeUnsignedByte(variableId);
02147
02148 switch (variableId) {
02149
02150 case DOMVAR_NAME:
02151 if (veh != NULL) {
02152 name = veh->getID();
02153 response.writeUnsignedByte(TYPE_STRING);
02154 response.writeString(name);
02155
02156 if (dataType != TYPE_STRING) {
02157 warning = "Warning: requested data type could not be used; using string instead!";
02158 }
02159 } else {
02160 throw TraCIException("Unable to retrieve node with given ID");
02161 }
02162 break;
02163
02164
02165 case DOMVAR_EXTID:
02166
02167 if (dataCont.getLastValueRead() != TYPE_STRING) {
02168 throw TraCIException("Internal id must be given as string value");
02169 }
02170
02171 name = dataCont.getString();
02172 if (equippedVehicles_.find(name) != equippedVehicles_.end()) {
02173 response.writeUnsignedByte(TYPE_INTEGER);
02174 response.writeInt(equippedVehicles_[name]);
02175 } else {
02176 std::stringstream msg;
02177 msg << "Vehicle with internal id " << name << " not existing or not accessible via TraCI";
02178 throw TraCIException(msg.str());
02179 }
02180 break;
02181
02182
02183 case DOMVAR_COUNT:
02184 vehControl = &MSNet::getInstance()->getVehicleControl();
02185 for (MSVehicleControl::constVehIt vehIt = vehControl->loadedVehBegin(); vehIt != vehControl->loadedVehEnd(); vehIt++) {
02186 if (vehIt->second->isOnRoad()) {
02187 count++;
02188 }
02189 }
02190 response.writeUnsignedByte(TYPE_INTEGER);
02191 response.writeInt(count);
02192 if (dataType != TYPE_INTEGER) {
02193 warning = "Warning: requested data type could not be used; using integer instead!";
02194 }
02195 break;
02196
02197
02198 case DOMVAR_MAXCOUNT:
02199 response.writeUnsignedByte(TYPE_INTEGER);
02200 response.writeInt(totalNumVehicles_);
02201 if (dataType != TYPE_INTEGER) {
02202 warning = "Warning: requested data type could not be used; using integer instead!";
02203 }
02204 break;
02205
02206
02207 case DOMVAR_EQUIPPEDCOUNT:
02208 for (std::map<std::string, int>::iterator it=equippedVehicles_.begin(); it != equippedVehicles_.end(); it++) {
02209 MSVehicle* veh = MSNet::getInstance()->getVehicleControl().getVehicle(it->first);
02210 if (veh->isOnRoad()) {
02211 count++;
02212 }
02213 }
02214 response.writeUnsignedByte(TYPE_INTEGER);
02215 response.writeInt(count);
02216 if (dataType != TYPE_INTEGER) {
02217 warning = "Warning: requested data type could not be used; using integer instead!";
02218 }
02219 break;
02220
02221
02222 case DOMVAR_EQUIPPEDCOUNTMAX:
02223 response.writeUnsignedByte(TYPE_INTEGER);
02224 response.writeInt(static_cast<int>(0.5 + totalNumVehicles_ * penetration_));
02225 if (dataType != TYPE_INTEGER) {
02226 warning = "Warning: requested data type could not be used; using integer instead!";
02227 }
02228 break;
02229
02230
02231 case DOMVAR_POSITION:
02232 if (veh != NULL) {
02233 switch (dataType) {
02234 case POSITION_3D:
02235 response.writeUnsignedByte(POSITION_3D);
02236 response.writeFloat(veh->getPosition().x());
02237 response.writeFloat(veh->getPosition().y());
02238 response.writeFloat(0);
02239 break;
02240 default:
02241 response.writeByte(POSITION_ROADMAP);
02242 response.writeString(veh->getEdge()->getID());
02243 response.writeFloat(veh->getPositionOnLane());
02244 int laneId = 0;
02245 MSLane* lane = veh->getLane().getRightLane();
02246 while (lane != NULL) {
02247 laneId++;
02248 lane =lane->getRightLane();
02249 }
02250 response.writeUnsignedByte(laneId);
02251
02252 if (dataType != POSITION_ROADMAP) {
02253 warning = "Warning: requested data type could not be used; using road map position instead!";
02254 }
02255 }
02256 } else {
02257 throw TraCIException("Unable to retrieve node with given ID");
02258 }
02259 break;
02260
02261
02262 case DOMVAR_SPEED:
02263 if (veh != NULL) {
02264 response.writeUnsignedByte(TYPE_FLOAT);
02265 response.writeFloat(veh->getSpeed());
02266
02267 if (dataType != TYPE_FLOAT) {
02268 warning = "Warning: requested data type could not be used; using float instead!";
02269 }
02270 } else {
02271 throw TraCIException("Unable to retrieve node with given ID");
02272 }
02273 break;
02274
02275
02276 case DOMVAR_ALLOWED_SPEED:
02277 if (veh != NULL) {
02278 response.writeUnsignedByte(TYPE_FLOAT);
02279 response.writeFloat(veh->getLane().getMaxSpeed());
02280
02281 if (dataType != TYPE_FLOAT) {
02282 warning = "Warning: requested data type could not be used; using float instead!";
02283 }
02284 } else {
02285 throw TraCIException("Unable to retrieve node with given ID");
02286 }
02287 break;
02288
02289
02290 case DOMVAR_ROUTE:
02291 if (veh != NULL) {
02292 response.writeUnsignedByte(TYPE_STRING);
02293 MSRoute r = veh->getRoute();
02294 std::string strRoute = "";
02295 for (MSRouteIterator it = r.begin(); it != r.end(); ++it) {
02296 if (strRoute.length()) strRoute.append(" ");
02297 strRoute.append((*it)->getID());
02298 }
02299 response.writeString(strRoute);
02300
02301 if (dataType != TYPE_STRING) {
02302 warning = "Warning: requested data type could not be used; using string instead!";
02303 }
02304 } else {
02305 throw TraCIException("Unable to retrieve node with given ID");
02306 }
02307 break;
02308
02309
02310 case DOMVAR_AIRDISTANCE:
02311
02312 case DOMVAR_DRIVINGDISTANCE:
02313 if (veh != NULL) {
02314 Position2D destPos;
02315 RoadMapPos destRoadPos;
02316 float distance = 0.0;
02317
02318
02319 float x,y;
02320 switch (dataCont.getLastValueRead()) {
02321 case POSITION_ROADMAP:
02322 destRoadPos = dataCont.getRoadMapPosition();
02323 destPos = convertRoadMapToCartesian(destRoadPos);
02324 if (MSEdge::dictionary(destRoadPos.roadId) == NULL) throw TraCIException("Unable to retrieve edge with given id");
02325 break;
02326 case POSITION_3D:
02327 case POSITION_2_5D:
02328 case POSITION_2D:
02329 destPos = dataCont.getAnyPosition();
02330 destRoadPos = convertCartesianToRoadMap(destPos);
02331 break;
02332 default:
02333 throw TraCIException("Distance request to unknown destination");
02334 }
02335
02336
02337
02338
02339
02340
02341
02342
02344
02345
02346
02347
02348
02349
02350
02351
02352
02353
02354
02355
02356
02357
02358
02359
02360
02361
02362
02363
02364
02365
02366
02367
02368
02369
02370
02371
02372
02373
02374
02375 if (variableId == DOMVAR_AIRDISTANCE) {
02376 distance = static_cast<float>(veh->getPosition().distanceTo(destPos));
02377 } else {
02378 distance = static_cast<float>(veh->getDistanceToPosition(destRoadPos.pos, MSEdge::dictionary(destRoadPos.roadId)));
02379 }
02380
02381 response.writeUnsignedByte(TYPE_FLOAT);
02382 response.writeFloat(distance);
02383 } else {
02384 throw TraCIException("Unable to retrieve node with given ID");
02385 }
02386 break;
02387
02388
02389 case DOMVAR_CO2EMISSION:
02390 if (veh != NULL) {
02391 response.writeUnsignedByte(TYPE_FLOAT);
02392 response.writeFloat(HelpersHBEFA::computeCO2(veh->getVehicleType().getEmissionClass(), veh->getSpeed(), veh->getPreDawdleAcceleration()));
02393
02394 if (dataType != TYPE_FLOAT) {
02395 warning = "Warning: requested data type could not be used; using float instead!";
02396 }
02397 } else {
02398 throw TraCIException("Unable to retrieve node with given ID");
02399 }
02400 break;
02401
02402 case DOMVAR_COEMISSION:
02403 if (veh != NULL) {
02404 response.writeUnsignedByte(TYPE_FLOAT);
02405 response.writeFloat(HelpersHBEFA::computeCO(veh->getVehicleType().getEmissionClass(), veh->getSpeed(), veh->getPreDawdleAcceleration()));
02406
02407 if (dataType != TYPE_FLOAT) {
02408 warning = "Warning: requested data type could not be used; using float instead!";
02409 }
02410 } else {
02411 throw TraCIException("Unable to retrieve node with given ID");
02412 }
02413 break;
02414
02415 case DOMVAR_HCEMISSION:
02416 if (veh != NULL) {
02417 response.writeUnsignedByte(TYPE_FLOAT);
02418 response.writeFloat(HelpersHBEFA::computeHC(veh->getVehicleType().getEmissionClass(), veh->getSpeed(), veh->getPreDawdleAcceleration()));
02419
02420 if (dataType != TYPE_FLOAT) {
02421 warning = "Warning: requested data type could not be used; using float instead!";
02422 }
02423 } else {
02424 throw TraCIException("Unable to retrieve node with given ID");
02425 }
02426 break;
02427
02428 case DOMVAR_PMXEMISSION:
02429 if (veh != NULL) {
02430 response.writeUnsignedByte(TYPE_FLOAT);
02431 response.writeFloat(HelpersHBEFA::computePMx(veh->getVehicleType().getEmissionClass(), veh->getSpeed(), veh->getPreDawdleAcceleration()));
02432
02433 if (dataType != TYPE_FLOAT) {
02434 warning = "Warning: requested data type could not be used; using float instead!";
02435 }
02436 } else {
02437 throw TraCIException("Unable to retrieve node with given ID");
02438 }
02439 break;
02440
02441 case DOMVAR_NOXEMISSION:
02442 if (veh != NULL) {
02443 response.writeUnsignedByte(TYPE_FLOAT);
02444 response.writeFloat(HelpersHBEFA::computeNOx(veh->getVehicleType().getEmissionClass(), veh->getSpeed(), veh->getPreDawdleAcceleration()));
02445
02446 if (dataType != TYPE_FLOAT) {
02447 warning = "Warning: requested data type could not be used; using float instead!";
02448 }
02449 } else {
02450 throw TraCIException("Unable to retrieve node with given ID");
02451 }
02452 break;
02453
02454 case DOMVAR_FUELCONSUMPTION:
02455 if (veh != NULL) {
02456 response.writeUnsignedByte(TYPE_FLOAT);
02457 response.writeFloat(HelpersHBEFA::computeFuel(veh->getVehicleType().getEmissionClass(), veh->getSpeed(), veh->getPreDawdleAcceleration()));
02458
02459 if (dataType != TYPE_FLOAT) {
02460 warning = "Warning: requested data type could not be used; using float instead!";
02461 }
02462 } else {
02463 throw TraCIException("Unable to retrieve node with given ID");
02464 }
02465 break;
02466
02467 case DOMVAR_NOISEEMISSION:
02468 if (veh != NULL) {
02469 response.writeUnsignedByte(TYPE_FLOAT);
02470 response.writeFloat(HelpersHarmonoise::computeNoise(veh->getVehicleType().getEmissionClass(), veh->getSpeed(), veh->getPreDawdleAcceleration()));
02471
02472 if (dataType != TYPE_FLOAT) {
02473 warning = "Warning: requested data type could not be used; using float instead!";
02474 }
02475 } else {
02476 throw TraCIException("Unable to retrieve node with given ID");
02477 }
02478 break;
02479
02480
02481 default:
02482 throw TraCIException("Unknown domain variable specified");
02483 }
02484
02485 return warning;
02486 }
02487
02488
02489
02490 std::string
02491 TraCIServer::handleTrafficLightDomain(bool isWriteCommand, tcpip::Storage& response)
02492 throw(TraCIException) {
02493 int count = 0;
02494 std::string name;
02495 std::string warning = "";
02496 DataTypeContainer dataCont;
02497
02498
02499 int objectId = myInputStorage.readInt();
02500 MSTrafficLightLogic* tlLogic = getTLLogicByExtId(objectId);
02501
02502
02503 int variableId = myInputStorage.readUnsignedByte();
02504
02505
02506 int dataType = myInputStorage.readUnsignedByte();
02507
02508
02509 if (myInputStorage.valid_pos()) {
02510 dataCont.readValue(dataType, myInputStorage);
02511 }
02512
02513 if (isWriteCommand) {
02514 throw TraCIException("Traffic Light domain does not contain writable variables");
02515 }
02516
02517
02518 response.writeUnsignedByte((isWriteCommand ? 0x01 : 0x00));
02519 response.writeUnsignedByte(DOM_TRAFFICLIGHTS);
02520 response.writeInt(objectId);
02521 response.writeUnsignedByte(variableId);
02522
02523 switch (variableId) {
02524
02525 case DOMVAR_NAME:
02526 if (tlLogic != NULL) {
02527 name = tlLogic->getID();
02528 response.writeUnsignedByte(TYPE_STRING);
02529 response.writeString(name);
02530
02531 if (dataType != TYPE_STRING) {
02532 warning = "Warning: requested data type could not be used; using string instead!";
02533 }
02534 } else {
02535 throw TraCIException("Unable to retrieve traffic light with given id");
02536 }
02537 break;
02538
02539
02540 case DOMVAR_EXTID:
02541
02542 if (dataCont.getLastValueRead() != TYPE_STRING) {
02543 throw TraCIException("Internal id must be given as string value");
02544 }
02545
02546 name = dataCont.getString();
02547 if (trafficLightsInt2ExtId.find(name) != trafficLightsInt2ExtId.end()) {
02548 response.writeUnsignedByte(TYPE_INTEGER);
02549 response.writeInt(trafficLightsInt2ExtId[name]);
02550 } else {
02551 std::stringstream msg;
02552 msg << "Traffic Light with internal id " << name << " not existing";
02553 throw TraCIException(msg.str());
02554 }
02555 break;
02556
02557
02558 case DOMVAR_COUNT:
02559 count = MSNet::getInstance()->getTLSControl().getAllTLIds().size();
02560 response.writeUnsignedByte(TYPE_INTEGER);
02561 response.writeInt(count);
02562
02563 if (dataType != TYPE_INTEGER) {
02564 warning = "Warning: requested data type could not be used; using integer instead!";
02565 }
02566 break;
02567
02568
02569 case DOMVAR_POSITION:
02570 if (tlLogic != NULL) {
02571 MSJunction* junc = MSNet::getInstance()->getJunctionControl().get(tlLogic->getID());
02572 response.writeUnsignedByte(POSITION_3D);
02573 response.writeFloat(junc->getPosition().x());
02574 response.writeFloat(junc->getPosition().y());
02575 response.writeFloat(0);
02576
02577 if (dataType != POSITION_3D) {
02578 warning = "Warning: requested data type could not be used; using position 3d instead!";
02579 }
02580 } else {
02581 throw TraCIException("Unable to retrieve traffic light with given id");
02582 }
02583 break;
02584
02585
02586 case DOMVAR_CURTLPHASE:
02587 case DOMVAR_NEXTTLPHASE:
02588 if (tlLogic != NULL) {
02589
02590 size_t step = tlLogic->getCurrentPhaseIndex();
02591 if (variableId == DOMVAR_NEXTTLPHASE) {
02592 size_t curStep = tlLogic->getCurrentPhaseIndex();
02593 size_t pos = tlLogic->getPhaseIndexAtTime(MSNet::getInstance()->getCurrentTimeStep());
02594 do {
02595 pos++;
02596 } while ((step=tlLogic->getIndexFromOffset(pos)) == curStep);
02597 }
02598 MSPhaseDefinition phase = tlLogic->getPhase(step);
02599
02600
02601 MSTrafficLightLogic::LinkVectorVector affectedLinks = tlLogic->getLinks();
02602
02603
02604
02605 Storage phaseList;
02606 int listLength = 0;
02607
02608 std::map<const MSEdge*, pair<const MSEdge*, int> > writtenEdgePairs;
02609 for (int i=0; i<affectedLinks.size(); i++) {
02610
02611 MSTrafficLightLogic::LinkVector linkGroup = affectedLinks[i];
02612
02613 MSTrafficLightLogic::LaneVector laneGroup = tlLogic->getLanesAt(i);
02614
02615 MSLink::LinkState tlState = phase.getSignalState(i);
02616
02617
02618
02619 for (int linkNo=0; linkNo<linkGroup.size(); linkNo++) {
02620
02621
02622
02623
02624
02625
02626
02627
02628
02629
02630
02631
02632
02633
02634
02635
02636
02637
02638
02639
02640
02641
02642
02643 MSEdge &precEdge = laneGroup[linkNo]->getEdge();
02644 MSEdge &succEdge = linkGroup[linkNo]->getLane()->getEdge();
02645
02646
02647 std::map<const MSEdge*, pair<const MSEdge*, int> >::iterator itPair = writtenEdgePairs.find(&precEdge);
02648 if (itPair != writtenEdgePairs.end()) {
02649 if (itPair->second.first == &succEdge && itPair->second.second == tlState) {
02650 continue;
02651 }
02652 }
02653
02654 writtenEdgePairs[&precEdge] = std::make_pair(&succEdge, tlState);
02655
02656
02657 phaseList.writeString(precEdge.getID());
02658
02659 phaseList.writeString(succEdge.getID());
02660
02661 switch (tlState) {
02662 case MSLink::LINKSTATE_TL_GREEN_MAJOR:
02663 case MSLink::LINKSTATE_TL_GREEN_MINOR:
02664 phaseList.writeUnsignedByte(TLPHASE_GREEN);
02665 break;
02666 case MSLink::LINKSTATE_TL_YELLOW_MAJOR:
02667 case MSLink::LINKSTATE_TL_YELLOW_MINOR:
02668 phaseList.writeUnsignedByte(TLPHASE_YELLOW);
02669 break;
02670 case MSLink::LINKSTATE_TL_RED:
02671 phaseList.writeUnsignedByte(TLPHASE_RED);
02672 break;
02673 case MSLink::LINKSTATE_TL_OFF_BLINKING:
02674 phaseList.writeUnsignedByte(TLPHASE_BLINKING);
02675 break;
02676 case MSLink::LINKSTATE_TL_OFF_NOSIGNAL:
02677 phaseList.writeUnsignedByte(TLPHASE_NOSIGNAL);
02678 break;
02679 default:
02680 phaseList.writeUnsignedByte(TLPHASE_NOSIGNAL);
02681 }
02682
02683 listLength++;
02684 }
02685 }
02686
02687 response.writeUnsignedByte(TYPE_TLPHASELIST);
02688
02689 response.writeUnsignedByte(listLength);
02690
02691 response.writeStorage(phaseList);
02692
02693
02694 if (dataType != TYPE_TLPHASELIST) {
02695 warning = "Warning: requested data type could not be used; using type traffic light phase list instead!";
02696 }
02697 } else {
02698 throw TraCIException("Unable to retrieve traffic light with given id");
02699 }
02700 break;
02701
02702
02703 default:
02704 throw TraCIException("Unknown domain variable specified");
02705 }
02706
02707 return warning;
02708 }
02709
02710
02711
02712 std::string
02713 TraCIServer::handlePoiDomain(bool isWriteCommand, tcpip::Storage& response)
02714 throw(TraCIException) {
02715 std::string name;
02716 std::string warning = "";
02717 DataTypeContainer dataCont;
02718
02719
02720 int objectId = myInputStorage.readInt();
02721 PointOfInterest* poi = getPoiByExtId(objectId);
02722
02723
02724 int variableId = myInputStorage.readUnsignedByte();
02725
02726
02727 int dataType = myInputStorage.readUnsignedByte();
02728
02729
02730 if (myInputStorage.valid_pos()) {
02731 dataCont.readValue(dataType, myInputStorage);
02732 }
02733
02734 if (isWriteCommand) {
02735 throw TraCIException("Point of interest domain does not contain writable variables");
02736 }
02737
02738
02739 response.writeUnsignedByte((isWriteCommand ? 0x01 : 0x00));
02740 response.writeUnsignedByte(DOM_POI);
02741 response.writeInt(objectId);
02742 response.writeUnsignedByte(variableId);
02743
02744
02745 int count = 0;
02746 ShapeContainer& shapeCont = MSNet::getInstance()->getShapeContainer();
02747 for (int i = shapeCont.getMinLayer(); i <= shapeCont.getMaxLayer(); i++) {
02748 count += shapeCont.getPOICont(i).size();
02749 }
02750
02751 switch (variableId) {
02752
02753 case DOMVAR_NAME:
02754 if (poi != NULL) {
02755 name = poi->getID();
02756 response.writeUnsignedByte(TYPE_STRING);
02757 response.writeString(name);
02758
02759 if (dataType != TYPE_STRING) {
02760 warning = "Warning: requested data type could not be used; using string instead!";
02761 }
02762 } else {
02763 throw TraCIException("Unable to retrieve point of interest with given id");
02764 }
02765 break;
02766
02767
02768 case DOMVAR_EXTID:
02769
02770 if (dataCont.getLastValueRead() != TYPE_STRING) {
02771 throw TraCIException("Internal id must be given as string value");
02772 }
02773
02774 name = dataCont.getString();
02775 if (poiInt2ExtId.find(name) != poiInt2ExtId.end()) {
02776 response.writeUnsignedByte(TYPE_INTEGER);
02777 response.writeInt(poiInt2ExtId[name]);
02778 } else {
02779 std::stringstream msg;
02780 msg << "Point of interest with internal id " << name << " not existing";
02781 throw TraCIException(msg.str());
02782 }
02783 break;
02784
02785
02786 case DOMVAR_COUNT:
02787 response.writeUnsignedByte(TYPE_INTEGER);
02788 response.writeInt(count);
02789
02790 if (dataType != TYPE_INTEGER) {
02791 warning = "Warning: requested data type could not be used; using integer instead!";
02792 }
02793 break;
02794
02795
02796 case DOMVAR_POSITION:
02797 if (poi == NULL) {
02798 throw TraCIException("Unable to retrieve point of interest with given id");
02799 } else {
02800 response.writeUnsignedByte(POSITION_3D);
02801 response.writeFloat(poi->x());
02802 response.writeFloat(poi->y());
02803 response.writeFloat(0);
02804 }
02805
02806 if (dataType != POSITION_3D) {
02807 warning = "Warning: requested data type could not be used; using 3D position instead!";
02808 }
02809 break;
02810
02811
02812 case DOMVAR_TYPE:
02813 if (poi == NULL) {
02814 throw TraCIException("Unable to retrieve point of interest with given id");
02815 } else {
02816 response.writeUnsignedByte(TYPE_STRING);
02817 response.writeString(poi->getType());
02818 }
02819
02820 if (dataType != TYPE_STRING) {
02821 warning = "Warning: requested data type could not be used; using string instead!";
02822 }
02823 break;
02824
02825
02826 case DOMVAR_LAYER:
02827
02828 default:
02829 throw TraCIException("Unknown domain variable specified");
02830 }
02831
02832 return warning;
02833 }
02834
02835
02836
02837 std::string
02838 TraCIServer::handlePolygonDomain(bool isWriteCommand, tcpip::Storage& response)
02839 throw(TraCIException) {
02840 std::string name;
02841 std::string warning = "";
02842 DataTypeContainer dataCont;
02843
02844
02845 int objectId = myInputStorage.readInt();
02846 Polygon2D* poly = getPolygonByExtId(objectId);
02847
02848
02849 int variableId = myInputStorage.readUnsignedByte();
02850
02851
02852 int dataType = myInputStorage.readUnsignedByte();
02853
02854
02855 if (myInputStorage.valid_pos()) {
02856 dataCont.readValue(dataType, myInputStorage);
02857 }
02858
02859 if (isWriteCommand) {
02860 throw TraCIException("Polygon domain does not contain writable variables");
02861 }
02862
02863
02864 response.writeUnsignedByte((isWriteCommand ? 0x01 : 0x00));
02865 response.writeUnsignedByte(DOM_POLYGON);
02866 response.writeInt(objectId);
02867 response.writeUnsignedByte(variableId);
02868
02869
02870 int count = 0;
02871 ShapeContainer& shapeCont = MSNet::getInstance()->getShapeContainer();
02872 for (int i = shapeCont.getMinLayer(); i <= shapeCont.getMaxLayer(); i++) {
02873 count += shapeCont.getPolygonCont(i).size();
02874 }
02875
02876 switch (variableId) {
02877
02878 case DOMVAR_NAME:
02879 if (poly != NULL) {
02880 name = poly->getID();
02881 response.writeUnsignedByte(TYPE_STRING);
02882 response.writeString(name);
02883 if (dataType != TYPE_STRING) {
02884 warning = "Warning: requested data type could not be used; using string instead!";
02885 }
02886 } else {
02887 throw TraCIException("Unable to retrieve polygon with given id");
02888 }
02889 break;
02890
02891
02892 case DOMVAR_EXTID:
02893
02894 if (dataCont.getLastValueRead() != TYPE_STRING) {
02895 throw TraCIException("Internal id must be given as string value");
02896 }
02897
02898 name = dataCont.getString();
02899 if (polygonInt2ExtId.find(name) != polygonInt2ExtId.end()) {
02900 response.writeUnsignedByte(TYPE_INTEGER);
02901 response.writeInt(polygonInt2ExtId[name]);
02902 } else {
02903 std::stringstream msg;
02904 msg << "Polygon with internal id " << name << " not existing";
02905 throw TraCIException(msg.str());
02906 }
02907 break;
02908
02909
02910 case DOMVAR_COUNT:
02911 response.writeUnsignedByte(TYPE_INTEGER);
02912 response.writeInt(count);
02913
02914 if (dataType != TYPE_INTEGER) {
02915 warning = "Warning: requested data type could not be used; using integer instead!";
02916 }
02917 break;
02918
02919
02920 case DOMVAR_POSITION:
02921 if (poly == NULL) {
02922 throw TraCIException("Unable to retrieve polygon with given id");
02923 } else {
02924 response.writeUnsignedByte(POSITION_3D);
02925 response.writeFloat(poly->getShape().getPolygonCenter().x());
02926 response.writeFloat(poly->getShape().getPolygonCenter().y());
02927 response.writeFloat(0);
02928 }
02929
02930 if (dataType != POSITION_3D) {
02931 warning = "Warning: requested data type could not be used; using 3D position instead!";
02932 }
02933 break;
02934
02935
02936 case DOMVAR_TYPE:
02937 if (poly == NULL) {
02938 throw TraCIException("Unable to retrieve polygon with given id");
02939 } else {
02940 response.writeUnsignedByte(TYPE_STRING);
02941 response.writeString(poly->getType());
02942 }
02943
02944 if (dataType != TYPE_STRING) {
02945 warning = "Warning: requested data type could not be used; using string instead!";
02946 }
02947 break;
02948
02949
02950 case DOMVAR_SHAPE:
02951 if (poly == NULL) {
02952 throw TraCIException("Unable to retrieve polygon with given id");
02953 } else {
02954 response.writeUnsignedByte(TYPE_POLYGON);
02955 response.writeUnsignedByte(MIN2(static_cast<size_t>(255),poly->getShape().size()));
02956 for (int i=0; i < MIN2(static_cast<size_t>(255),poly->getShape().size()); i++) {
02957 response.writeFloat(poly->getShape()[i].x());
02958 response.writeFloat(poly->getShape()[i].y());
02959 }
02960 }
02961
02962 if (dataType != TYPE_POLYGON) {
02963 warning = "Warning: requested data type could not be used; using polygon type instead!";
02964 }
02965 break;
02966
02967
02968 case DOMVAR_LAYER:
02969
02970 default:
02971 throw TraCIException("Unknown domain variable specified");
02972 }
02973
02974 return warning;
02975 }
02976
02977
02978
02979 void
02980 TraCIServer::handleLifecycleSubscriptions()
02981 throw(TraCIException) {
02982
02983 if (myLifecycleSubscriptions.count(DOM_VEHICLE) != 0) {
02984
02985 for (std::set<int>::const_iterator i = myDestroyedVehicles.begin(); i != myDestroyedVehicles.end(); i++) {
02986 int extId = *i;
02987
02988 myOutputStorage.writeUnsignedByte(1+1+1+4);
02989 myOutputStorage.writeUnsignedByte(CMD_OBJECTDESTRUCTION);
02990 myOutputStorage.writeUnsignedByte(DOM_VEHICLE);
02991 myOutputStorage.writeInt(extId);
02992 }
02993 myDestroyedVehicles.clear();
02994
02995 for (std::set<int>::const_iterator i = myCreatedVehicles.begin(); i != myCreatedVehicles.end(); i++) {
02996 int extId = *i;
02997
02998 myOutputStorage.writeUnsignedByte(1+1+1+4);
02999 myOutputStorage.writeUnsignedByte(CMD_OBJECTCREATION);
03000 myOutputStorage.writeUnsignedByte(DOM_VEHICLE);
03001 myOutputStorage.writeInt(extId);
03002 }
03003 myCreatedVehicles.clear();
03004
03005 }
03006 }
03007
03008
03009
03010 void
03011 TraCIServer::handleDomainSubscriptions(const SUMOTime& currentTime, const map<int, const MSVehicle*>& activeEquippedVehicles)
03012 throw(TraCIException) {
03013
03014 if (myDomainSubscriptions.count(DOM_VEHICLE) != 0) {
03015
03016 std::list<std::pair<int, int> > subscribedVariables = myDomainSubscriptions[DOM_VEHICLE];
03017
03018
03019 for (map<int, const MSVehicle*>::const_iterator iter = activeEquippedVehicles.begin(); iter != activeEquippedVehicles.end(); ++iter) {
03020 int extId = (*iter).first;
03021 const MSVehicle* vehicle = (*iter).second;
03022 Storage tempMsg;
03023
03024
03025 tempMsg.writeUnsignedByte(CMD_UPDATEOBJECT);
03026
03027
03028 tempMsg.writeByte(DOM_VEHICLE);
03029
03030
03031 tempMsg.writeInt(extId);
03032
03033
03034 for (std::list<std::pair<int, int> >::const_iterator i = subscribedVariables.begin(); i != subscribedVariables.end(); i++) {
03035 int variableId = i->first;
03036 int dataType = i->second;
03037
03038 if ((variableId == DOMVAR_SIMTIME) && (dataType == TYPE_DOUBLE)) {
03039 tempMsg.writeDouble(currentTime);
03040 }
03041 if ((variableId == DOMVAR_SPEED) && (dataType == TYPE_FLOAT)) {
03042 tempMsg.writeFloat(vehicle->getSpeed());
03043 }
03044 if ((variableId == DOMVAR_ALLOWED_SPEED) && (dataType == TYPE_FLOAT)) {
03045 tempMsg.writeFloat(vehicle->getLane().getMaxSpeed());
03046 }
03047 if ((variableId == DOMVAR_POSITION) && (dataType == POSITION_2D)) {
03048 Position2D pos = vehicle->getPosition();
03049 tempMsg.writeFloat(pos.x());
03050 tempMsg.writeFloat(pos.y());
03051 }
03052 if ((variableId == DOMVAR_POSITION) && (dataType == POSITION_ROADMAP)) {
03053 tempMsg.writeString(vehicle->getEdge()->getID());
03054 }
03055 if ((variableId == DOMVAR_ANGLE) && (dataType == TYPE_FLOAT)) {
03056 tempMsg.writeFloat(vehicle->getLane().getShape().rotationDegreeAtLengthPosition(vehicle->getPositionOnLane()));
03057 }
03058 }
03059
03060 myOutputStorage.writeUnsignedByte(tempMsg.size()+1);
03061
03062 myOutputStorage.writeStorage(tempMsg);
03063 }
03064 }
03065 }
03066
03067
03068 bool
03069 TraCIServer::addSubscription(int commandId) throw(TraCIException) {
03070 SUMOTime beginTime = myInputStorage.readInt();
03071 SUMOTime endTime = myInputStorage.readInt();
03072 std::string id = myInputStorage.readString();
03073 int no = myInputStorage.readUnsignedByte();
03074 std::vector<int> variables;
03075 for (int i=0; i<no; ++i) {
03076 variables.push_back(myInputStorage.readUnsignedByte());
03077 }
03078
03079 bool ok = true;
03080 if (variables.size()==0) {
03081
03082 bool found = false;
03083 for (std::vector<Subscription>::iterator j=mySubscriptions.begin(); j!=mySubscriptions.end();) {
03084 if ((*j).id==id&&(*j).commandId==commandId) {
03085 j = mySubscriptions.erase(j);
03086 found = true;
03087 continue;
03088 }
03089 ++j;
03090 }
03091 if (found) {
03092 writeStatusCmd(commandId, RTYPE_OK, "");
03093 } else {
03094 writeStatusCmd(commandId, RTYPE_OK, "The subscription to remove was not found.");
03095 }
03096 } else {
03097
03098 Subscription s(commandId, id, variables, beginTime, endTime);
03099 tcpip::Storage writeInto;
03100 std::string errors;
03101 if (s.endTime<MSNet::getInstance()->getCurrentTimeStep()) {
03102 processSingleSubscription(s, writeInto, errors);
03103 writeStatusCmd(s.commandId, RTYPE_ERR, "Subscription has ended.");
03104 } else {
03105 if (processSingleSubscription(s, writeInto, errors)) {
03106 mySubscriptions.push_back(s);
03107 writeStatusCmd(s.commandId, RTYPE_OK, "");
03108 } else {
03109 writeStatusCmd(s.commandId, RTYPE_ERR, "Could not add subscription (" + errors + ").");
03110 }
03111 }
03112 myOutputStorage.writeStorage(writeInto);
03113 }
03114 return ok;
03115 }
03116
03117
03118 bool
03119 TraCIServer::processSingleSubscription(const Subscription &s, tcpip::Storage &writeInto,
03120 std::string &errors) throw(TraCIException) {
03121 bool ok = true;
03122 tcpip::Storage outputStorage;
03123 size_t wholeSize = 0;
03124 for (std::vector<int>::const_iterator i=s.variables.begin(); i!=s.variables.end(); ++i) {
03125 tcpip::Storage message;
03126 message.writeUnsignedByte(*i);
03127 message.writeString(s.id);
03128 tcpip::Storage tmpOutput;
03129 switch (s.commandId) {
03130
03131 case CMD_SUBSCRIBE_INDUCTIONLOOP_VARIABLE:
03132 ok &= TraCIServerAPI_InductionLoop::processGet(message, tmpOutput, false);
03133 break;
03134 case CMD_SUBSCRIBE_MULTI_ENTRY_EXIT_DETECTOR_VARIABLE:
03135 ok &= TraCIServerAPI_MeMeDetector::processGet(message, tmpOutput, false);
03136 break;
03137 case CMD_SUBSCRIBE_TL_VARIABLE:
03138 ok &= TraCIServerAPI_TLS::processGet(message, tmpOutput, false);
03139 break;
03140 case CMD_SUBSCRIBE_LANE_VARIABLE:
03141 ok &= TraCIServerAPI_Lane::processGet(message, tmpOutput, false);
03142 break;
03143 case CMD_SUBSCRIBE_VEHICLE_VARIABLE:
03144 ok &= TraCIServerAPI_Vehicle::processGet(message, tmpOutput, false);
03145 break;
03146 case CMD_SUBSCRIBE_VEHICLETYPE_VARIABLE:
03147 ok &= TraCIServerAPI_VehicleType::processGet(message, tmpOutput, false);
03148 break;
03149 case CMD_SUBSCRIBE_ROUTE_VARIABLE:
03150 ok &= TraCIServerAPI_Route::processGet(message, tmpOutput, false);
03151 break;
03152 case CMD_SUBSCRIBE_POI_VARIABLE:
03153 ok &= TraCIServerAPI_POI::processGet(message, tmpOutput, false);
03154 break;
03155 case CMD_SUBSCRIBE_POLYGON_VARIABLE:
03156 ok &= TraCIServerAPI_Polygon::processGet(message, tmpOutput, false);
03157 break;
03158 case CMD_SUBSCRIBE_JUNCTION_VARIABLE:
03159 ok &= TraCIServerAPI_Junction::processGet(message, tmpOutput, false);
03160 break;
03161 case CMD_SUBSCRIBE_EDGE_VARIABLE:
03162 ok &= TraCIServerAPI_Edge::processGet(message, tmpOutput, false);
03163 break;
03164 case CMD_SUBSCRIBE_SIM_VARIABLE:
03165 ok &= TraCIServerAPI_Simulation::processGet(message, tmpOutput, myVehicleStateChanges, false);
03166 break;
03167 default:
03168 TraCIServerAPIHelper::writeStatusCmd(s.commandId, RTYPE_NOTIMPLEMENTED, "Unsupported command specified", tmpOutput);
03169 ok = false;
03170 break;
03171 }
03172
03173 if (ok) {
03174 int length = tmpOutput.readUnsignedByte();
03175 length = tmpOutput.readInt();
03176 int responseType = tmpOutput.readUnsignedByte();
03177 int variable = tmpOutput.readUnsignedByte();
03178 std::string id = tmpOutput.readString();
03179 size_t p = 1 + 4 + 1 + 1 + 4 + id.length();
03180 wholeSize += (length - p);
03181 outputStorage.writeUnsignedByte(variable);
03182 outputStorage.writeUnsignedByte(RTYPE_OK);
03183 while (p++<tmpOutput.size()) {
03184 outputStorage.writeUnsignedByte(tmpOutput.readUnsignedByte());
03185 }
03186 } else {
03187 int length = tmpOutput.readUnsignedByte();
03188 int cmd = tmpOutput.readUnsignedByte();
03189 int status = tmpOutput.readUnsignedByte();
03190 std::string msg = tmpOutput.readString();
03191 outputStorage.writeUnsignedByte(*i);
03192 outputStorage.writeUnsignedByte(RTYPE_ERR);
03193 outputStorage.writeUnsignedByte(TYPE_STRING);
03194 outputStorage.writeString(msg);
03195 errors = errors + msg;
03196 }
03197 }
03198
03199 writeInto.writeUnsignedByte(0);
03200 writeInto.writeInt(1 + 4 + s.id.length() + 1 + outputStorage.size());
03201 writeInto.writeUnsignedByte(s.commandId + 0x10);
03202 writeInto.writeString(s.id);
03203 writeInto.writeUnsignedByte(s.variables.size());
03204 writeInto.writeStorage(outputStorage);
03205 return ok;
03206 }
03207
03208 }
03209
03210 #endif
