SUMO - Simulation of Urban MObility
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
TraCIServerAPI_Vehicle.cpp
Go to the documentation of this file.
1 /****************************************************************************/
13 // APIs for getting/setting vehicle values via TraCI
14 /****************************************************************************/
15 // SUMO, Simulation of Urban MObility; see http://sumo-sim.org/
16 // Copyright (C) 2009-2014 DLR (http://www.dlr.de/) and contributors
17 /****************************************************************************/
18 //
19 // This file is part of SUMO.
20 // SUMO is free software: you can redistribute it and/or modify
21 // it under the terms of the GNU General Public License as published by
22 // the Free Software Foundation, either version 3 of the License, or
23 // (at your option) any later version.
24 //
25 /****************************************************************************/
26 
27 
28 // ===========================================================================
29 // included modules
30 // ===========================================================================
31 #ifdef _MSC_VER
32 #include <windows_config.h>
33 #else
34 #include <config.h>
35 #endif
36 
37 #ifndef NO_TRACI
38 
39 #include <microsim/MSNet.h>
41 #include <microsim/MSVehicle.h>
42 #include <microsim/MSLane.h>
43 #include <microsim/MSEdge.h>
52 #include "TraCIConstants.h"
54 #include "TraCIServerAPI_Vehicle.h"
56 
57 #ifdef CHECK_MEMORY_LEAKS
58 #include <foreign/nvwa/debug_new.h>
59 #endif // CHECK_MEMORY_LEAKS
60 
61 
62 // ===========================================================================
63 // static member variables
64 // ===========================================================================
65 std::map<std::string, std::vector<MSLane*> > TraCIServerAPI_Vehicle::gVTDMap;
66 
67 
68 // ===========================================================================
69 // method definitions
70 // ===========================================================================
71 bool
73  tcpip::Storage& outputStorage) {
74  // variable & id
75  int variable = inputStorage.readUnsignedByte();
76  std::string id = inputStorage.readString();
77  // check variable
78  if (variable != ID_LIST && variable != VAR_SPEED && variable != VAR_SPEED_WITHOUT_TRACI
79  && variable != VAR_POSITION && variable != VAR_ANGLE && variable != VAR_POSITION3D
80  && variable != VAR_ROAD_ID && variable != VAR_LANE_ID && variable != VAR_LANE_INDEX
81  && variable != VAR_TYPE && variable != VAR_ROUTE_ID && variable != VAR_COLOR
82  && variable != VAR_LANEPOSITION
83  && variable != VAR_CO2EMISSION && variable != VAR_COEMISSION
84  && variable != VAR_HCEMISSION && variable != VAR_PMXEMISSION
85  && variable != VAR_NOXEMISSION && variable != VAR_FUELCONSUMPTION && variable != VAR_NOISEEMISSION
86  && variable != VAR_PERSON_NUMBER && variable != VAR_LEADER
87  && variable != VAR_EDGE_TRAVELTIME && variable != VAR_EDGE_EFFORT
88  && variable != VAR_ROUTE_VALID && variable != VAR_EDGES
89  && variable != VAR_SIGNALS && variable != VAR_DISTANCE
90  && variable != VAR_LENGTH && variable != VAR_MAXSPEED && variable != VAR_VEHICLECLASS
91  && variable != VAR_SPEED_FACTOR && variable != VAR_SPEED_DEVIATION
92  && variable != VAR_ALLOWED_SPEED && variable != VAR_EMISSIONCLASS
93  && variable != VAR_WIDTH && variable != VAR_MINGAP && variable != VAR_SHAPECLASS
94  && variable != VAR_ACCEL && variable != VAR_DECEL && variable != VAR_IMPERFECTION
95  && variable != VAR_TAU && variable != VAR_BEST_LANES && variable != DISTANCE_REQUEST
96  && variable != ID_COUNT && variable != VAR_STOPSTATE && variable != VAR_WAITING_TIME
97  ) {
98  return server.writeErrorStatusCmd(CMD_GET_VEHICLE_VARIABLE, "Get Vehicle Variable: unsupported variable specified", outputStorage);
99  }
100  // begin response building
101  tcpip::Storage tempMsg;
102  // response-code, variableID, objectID
104  tempMsg.writeUnsignedByte(variable);
105  tempMsg.writeString(id);
106  // process request
107  if (variable == ID_LIST || variable == ID_COUNT) {
108  std::vector<std::string> ids;
110  for (MSVehicleControl::constVehIt i = c.loadedVehBegin(); i != c.loadedVehEnd(); ++i) {
111  if ((*i).second->isOnRoad()) {
112  ids.push_back((*i).first);
113  }
114  }
115  if (variable == ID_LIST) {
117  tempMsg.writeStringList(ids);
118  } else {
120  tempMsg.writeInt((int) ids.size());
121  }
122  } else {
124  if (sumoVehicle == 0) {
125  return server.writeErrorStatusCmd(CMD_GET_VEHICLE_VARIABLE, "Vehicle '" + id + "' is not known", outputStorage);
126  }
127  MSVehicle* v = dynamic_cast<MSVehicle*>(sumoVehicle);
128  if (v == 0) {
129  return server.writeErrorStatusCmd(CMD_GET_VEHICLE_VARIABLE, "Vehicle '" + id + "' is not a micro-simulation vehicle", outputStorage);
130  }
131  const bool onRoad = v->isOnRoad();
132  switch (variable) {
133  case VAR_SPEED:
135  tempMsg.writeDouble(onRoad ? v->getSpeed() : INVALID_DOUBLE_VALUE);
136  break;
140  break;
141  case VAR_POSITION:
143  tempMsg.writeDouble(onRoad ? v->getPosition().x() : INVALID_DOUBLE_VALUE);
144  tempMsg.writeDouble(onRoad ? v->getPosition().y() : INVALID_DOUBLE_VALUE);
145  break;
146  case VAR_POSITION3D:
148  tempMsg.writeDouble(onRoad ? v->getPosition().x() : INVALID_DOUBLE_VALUE);
149  tempMsg.writeDouble(onRoad ? v->getPosition().y() : INVALID_DOUBLE_VALUE);
150  tempMsg.writeDouble(onRoad ? v->getPosition().z() : INVALID_DOUBLE_VALUE);
151  break;
152  case VAR_ANGLE:
154  tempMsg.writeDouble(onRoad ? v->getAngle() : INVALID_DOUBLE_VALUE);
155  break;
156  case VAR_ROAD_ID:
158  tempMsg.writeString(onRoad ? v->getLane()->getEdge().getID() : "");
159  break;
160  case VAR_LANE_ID:
162  tempMsg.writeString(onRoad ? v->getLane()->getID() : "");
163  break;
164  case VAR_LANE_INDEX:
166  if (onRoad) {
167  const std::vector<MSLane*>& lanes = v->getLane()->getEdge().getLanes();
168  tempMsg.writeInt((int)std::distance(lanes.begin(), std::find(lanes.begin(), lanes.end(), v->getLane())));
169  } else {
170  tempMsg.writeInt(INVALID_INT_VALUE);
171  }
172  break;
173  case VAR_TYPE:
175  tempMsg.writeString(v->getVehicleType().getID());
176  break;
177  case VAR_ROUTE_ID:
179  tempMsg.writeString(v->getRoute().getID());
180  break;
181  case VAR_COLOR:
182  tempMsg.writeUnsignedByte(TYPE_COLOR);
183  tempMsg.writeUnsignedByte(v->getParameter().color.red());
184  tempMsg.writeUnsignedByte(v->getParameter().color.green());
185  tempMsg.writeUnsignedByte(v->getParameter().color.blue());
186  tempMsg.writeUnsignedByte(v->getParameter().color.alpha());
187  break;
188  case VAR_LANEPOSITION:
190  tempMsg.writeDouble(onRoad ? v->getPositionOnLane() : INVALID_DOUBLE_VALUE);
191  break;
192  case VAR_CO2EMISSION:
194  tempMsg.writeDouble(onRoad ? v->getCO2Emissions() : INVALID_DOUBLE_VALUE);
195  break;
196  case VAR_COEMISSION:
198  tempMsg.writeDouble(onRoad ? v->getCOEmissions() : INVALID_DOUBLE_VALUE);
199  break;
200  case VAR_HCEMISSION:
202  tempMsg.writeDouble(onRoad ? v->getHCEmissions() : INVALID_DOUBLE_VALUE);
203  break;
204  case VAR_PMXEMISSION:
206  tempMsg.writeDouble(onRoad ? v->getPMxEmissions() : INVALID_DOUBLE_VALUE);
207  break;
208  case VAR_NOXEMISSION:
210  tempMsg.writeDouble(onRoad ? v->getNOxEmissions() : INVALID_DOUBLE_VALUE);
211  break;
212  case VAR_FUELCONSUMPTION:
214  tempMsg.writeDouble(onRoad ? v->getFuelConsumption() : INVALID_DOUBLE_VALUE);
215  break;
216  case VAR_NOISEEMISSION:
219  break;
220  case VAR_PERSON_NUMBER:
222  tempMsg.writeInt(v->getPersonNumber());
223  break;
224  case VAR_LEADER: {
225  double dist = 0;
226  if (!server.readTypeCheckingDouble(inputStorage, dist)) {
227  return server.writeErrorStatusCmd(CMD_GET_VEHICLE_VARIABLE, "Leader retrieval requires a double.", outputStorage);
228  }
229  std::pair<const MSVehicle* const, SUMOReal> leaderInfo = v->getLeader(dist);
231  tempMsg.writeInt(2);
233  tempMsg.writeString(leaderInfo.first != 0 ? leaderInfo.first->getID() : "");
235  tempMsg.writeDouble(leaderInfo.second);
236  }
237  break;
238  case VAR_WAITING_TIME:
240  tempMsg.writeDouble(v->getWaitingSeconds());
241  break;
242  case VAR_EDGE_TRAVELTIME: {
243  if (inputStorage.readUnsignedByte() != TYPE_COMPOUND) {
244  return server.writeErrorStatusCmd(CMD_GET_VEHICLE_VARIABLE, "Retrieval of travel time requires a compound object.", outputStorage);
245  }
246  if (inputStorage.readInt() != 2) {
247  return server.writeErrorStatusCmd(CMD_GET_VEHICLE_VARIABLE, "Retrieval of travel time requires time, and edge as parameter.", outputStorage);
248  }
249  // time
250  SUMOTime time = 0;
251  if (!server.readTypeCheckingInt(inputStorage, time)) {
252  return server.writeErrorStatusCmd(CMD_GET_VEHICLE_VARIABLE, "Retrieval of travel time requires the referenced time as first parameter.", outputStorage);
253  }
254  // edge
255  std::string edgeID;
256  if (!server.readTypeCheckingString(inputStorage, edgeID)) {
257  return server.writeErrorStatusCmd(CMD_GET_VEHICLE_VARIABLE, "Retrieval of travel time requires the referenced edge as second parameter.", outputStorage);
258  }
259  MSEdge* edge = MSEdge::dictionary(edgeID);
260  if (edge == 0) {
261  return server.writeErrorStatusCmd(CMD_GET_VEHICLE_VARIABLE, "Referenced edge '" + edgeID + "' is not known.", outputStorage);
262  }
263  // retrieve
265  SUMOReal value;
266  if (!v->getWeightsStorage().retrieveExistingTravelTime(edge, time, value)) {
268  } else {
269  tempMsg.writeDouble(value);
270  }
271 
272  }
273  break;
274  case VAR_EDGE_EFFORT: {
275  if (inputStorage.readUnsignedByte() != TYPE_COMPOUND) {
276  return server.writeErrorStatusCmd(CMD_GET_VEHICLE_VARIABLE, "Retrieval of travel time requires a compound object.", outputStorage);
277  }
278  if (inputStorage.readInt() != 2) {
279  return server.writeErrorStatusCmd(CMD_GET_VEHICLE_VARIABLE, "Retrieval of travel time requires time, and edge as parameter.", outputStorage);
280  }
281  // time
282  SUMOTime time = 0;
283  if (!server.readTypeCheckingInt(inputStorage, time)) {
284  return server.writeErrorStatusCmd(CMD_GET_VEHICLE_VARIABLE, "Retrieval of effort requires the referenced time as first parameter.", outputStorage);
285  }
286  // edge
287  std::string edgeID;
288  if (!server.readTypeCheckingString(inputStorage, edgeID)) {
289  return server.writeErrorStatusCmd(CMD_GET_VEHICLE_VARIABLE, "Retrieval of effort requires the referenced edge as second parameter.", outputStorage);
290  }
291  MSEdge* edge = MSEdge::dictionary(edgeID);
292  if (edge == 0) {
293  return server.writeErrorStatusCmd(CMD_GET_VEHICLE_VARIABLE, "Referenced edge '" + edgeID + "' is not known.", outputStorage);
294  }
295  // retrieve
297  SUMOReal value;
298  if (!v->getWeightsStorage().retrieveExistingEffort(edge, time, value)) {
300  } else {
301  tempMsg.writeDouble(value);
302  }
303 
304  }
305  break;
306  case VAR_ROUTE_VALID: {
307  std::string msg;
308  tempMsg.writeUnsignedByte(TYPE_UBYTE);
309  tempMsg.writeUnsignedByte(v->hasValidRoute(msg));
310  }
311  break;
312  case VAR_EDGES: {
313  const MSRoute& r = v->getRoute();
315  tempMsg.writeInt(r.size());
316  for (MSRouteIterator i = r.begin(); i != r.end(); ++i) {
317  tempMsg.writeString((*i)->getID());
318  }
319  }
320  break;
321  case VAR_SIGNALS:
323  tempMsg.writeInt(v->getSignals());
324  break;
325  case VAR_BEST_LANES: {
327  tcpip::Storage tempContent;
328  unsigned int cnt = 0;
329  tempContent.writeUnsignedByte(TYPE_INTEGER);
330  const std::vector<MSVehicle::LaneQ>& bestLanes = onRoad ? v->getBestLanes() : std::vector<MSVehicle::LaneQ>();
331  tempContent.writeInt((int) bestLanes.size());
332  ++cnt;
333  for (std::vector<MSVehicle::LaneQ>::const_iterator i = bestLanes.begin(); i != bestLanes.end(); ++i) {
334  const MSVehicle::LaneQ& lq = *i;
335  tempContent.writeUnsignedByte(TYPE_STRING);
336  tempContent.writeString(lq.lane->getID());
337  ++cnt;
338  tempContent.writeUnsignedByte(TYPE_DOUBLE);
339  tempContent.writeDouble(lq.length);
340  ++cnt;
341  tempContent.writeUnsignedByte(TYPE_DOUBLE);
342  tempContent.writeDouble(lq.nextOccupation);
343  ++cnt;
344  tempContent.writeUnsignedByte(TYPE_BYTE);
345  tempContent.writeByte(lq.bestLaneOffset);
346  ++cnt;
347  tempContent.writeUnsignedByte(TYPE_UBYTE);
348  lq.allowsContinuation ? tempContent.writeUnsignedByte(1) : tempContent.writeUnsignedByte(0);
349  ++cnt;
350  std::vector<std::string> bestContIDs;
351  for (std::vector<MSLane*>::const_iterator j = lq.bestContinuations.begin(); j != lq.bestContinuations.end(); ++j) {
352  bestContIDs.push_back((*j)->getID());
353  }
354  tempContent.writeUnsignedByte(TYPE_STRINGLIST);
355  tempContent.writeStringList(bestContIDs);
356  ++cnt;
357  }
358  tempMsg.writeInt((int) cnt);
359  tempMsg.writeStorage(tempContent);
360  }
361  break;
362  case VAR_STOPSTATE: {
363  char b = (
364  1 * (v->isStopped() ? 1 : 0) +
365  2 * (v->isParking() ? 1 : 0) +
366  4 * (v->isStoppedTriggered() ? 1 : 0));
367  tempMsg.writeUnsignedByte(TYPE_UBYTE);
368  tempMsg.writeUnsignedByte(b);
369  }
370  break;
371  case VAR_DISTANCE: {
373  SUMOReal distance = onRoad ? v->getRoute().getDistanceBetween(0, v->getPositionOnLane(), v->getRoute().getEdges()[0], &v->getLane()->getEdge()) : INVALID_DOUBLE_VALUE;
374  if (distance == std::numeric_limits<SUMOReal>::max()) {
375  distance = INVALID_DOUBLE_VALUE;
376  }
377  tempMsg.writeDouble(distance);
378  }
379  break;
380  case DISTANCE_REQUEST:
381  if (!commandDistanceRequest(server, inputStorage, tempMsg, v)) {
382  return false;
383  }
384  break;
385  case VAR_ALLOWED_SPEED:
387  tempMsg.writeDouble(onRoad ? v->getLane()->getVehicleMaxSpeed(v) : INVALID_DOUBLE_VALUE);
388  break;
389  case VAR_SPEED_FACTOR:
391  tempMsg.writeDouble(v->getChosenSpeedFactor());
392  break;
393  default:
395  break;
396  }
397  }
398  server.writeStatusCmd(CMD_GET_VEHICLE_VARIABLE, RTYPE_OK, "", outputStorage);
399  server.writeResponseWithLength(outputStorage, tempMsg);
400  return true;
401 }
402 
403 
404 bool
406  tcpip::Storage& outputStorage) {
407  std::string warning = ""; // additional description for response
408  // variable
409  int variable = inputStorage.readUnsignedByte();
410  if (variable != CMD_STOP && variable != CMD_CHANGELANE
411  && variable != CMD_SLOWDOWN && variable != CMD_CHANGETARGET && variable != CMD_RESUME
412  && variable != VAR_TYPE && variable != VAR_ROUTE_ID && variable != VAR_ROUTE
413  && variable != VAR_EDGE_TRAVELTIME && variable != VAR_EDGE_EFFORT
414  && variable != CMD_REROUTE_TRAVELTIME && variable != CMD_REROUTE_EFFORT
415  && variable != VAR_SIGNALS && variable != VAR_MOVE_TO
416  && variable != VAR_LENGTH && variable != VAR_MAXSPEED && variable != VAR_VEHICLECLASS
417  && variable != VAR_SPEED_FACTOR && variable != VAR_EMISSIONCLASS
418  && variable != VAR_WIDTH && variable != VAR_MINGAP && variable != VAR_SHAPECLASS
419  && variable != VAR_ACCEL && variable != VAR_DECEL && variable != VAR_IMPERFECTION
420  && variable != VAR_TAU && variable != VAR_LANECHANGE_MODE
421  && variable != VAR_SPEED && variable != VAR_SPEEDSETMODE && variable != VAR_COLOR
422  && variable != ADD && variable != ADD_FULL && variable != REMOVE
423  && variable != VAR_MOVE_TO_VTD
424  ) {
425  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Change Vehicle State: unsupported variable specified", outputStorage);
426  }
427  // id
428  std::string id = inputStorage.readString();
429  const bool shouldExist = variable != ADD && variable != ADD_FULL;
431  if (sumoVehicle == 0) {
432  if (shouldExist) {
433  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Vehicle '" + id + "' is not known", outputStorage);
434  }
435  }
436  MSVehicle* v = dynamic_cast<MSVehicle*>(sumoVehicle);
437  if (v == 0 && shouldExist) {
438  return server.writeErrorStatusCmd(CMD_GET_VEHICLE_VARIABLE, "Vehicle '" + id + "' is not a micro-simulation vehicle", outputStorage);
439  }
440  switch (variable) {
441  case CMD_STOP: {
442  if (inputStorage.readUnsignedByte() != TYPE_COMPOUND) {
443  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Stop needs a compound object description.", outputStorage);
444  }
445  int compoundSize = inputStorage.readInt();
446  if (compoundSize != 4 && compoundSize != 5) {
447  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Stop needs a compound object description of four of five items.", outputStorage);
448  }
449  // read road map position
450  std::string roadId;
451  if (!server.readTypeCheckingString(inputStorage, roadId)) {
452  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "The first stop parameter must be the edge id given as a string.", outputStorage);
453  }
454  double pos = 0;
455  if (!server.readTypeCheckingDouble(inputStorage, pos)) {
456  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "The second stop parameter must be the position along the edge given as a double.", outputStorage);
457  }
458  int laneIndex = 0;
459  if (!server.readTypeCheckingByte(inputStorage, laneIndex)) {
460  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "The third stop parameter must be the lane index given as a byte.", outputStorage);
461  }
462  // waitTime
463  SUMOTime waitTime = 0;
464  if (!server.readTypeCheckingInt(inputStorage, waitTime)) {
465  return server.writeErrorStatusCmd(CMD_GET_VEHICLE_VARIABLE, "The fourth stop parameter must be the waiting time given as an integer.", outputStorage);
466  }
467  // optional stop flags
468  bool parking = false;
469  bool triggered = false;
470  if (compoundSize == 5) {
471  int stopFlags;
472  if (!server.readTypeCheckingByte(inputStorage, stopFlags)) {
473  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "The fifth stop parameter must be a byte indicating its parking/triggered status.", outputStorage);
474  }
475  parking = ((stopFlags & 1) != 0);
476  triggered = ((stopFlags & 2) != 0);
477  }
478  // check
479  if (pos < 0) {
480  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Position on lane must not be negative.", outputStorage);
481  }
482  // get the actual lane that is referenced by laneIndex
483  MSEdge* road = MSEdge::dictionary(roadId);
484  if (road == 0) {
485  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Unable to retrieve road with given id.", outputStorage);
486  }
487  const std::vector<MSLane*>& allLanes = road->getLanes();
488  if ((laneIndex < 0) || laneIndex >= (int)(allLanes.size())) {
489  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "No lane with index '" + toString(laneIndex) + "' on road '" + roadId + "'.", outputStorage);
490  }
491  // Forward command to vehicle
492  if (!v->addTraciStop(allLanes[laneIndex], pos, 0, waitTime, parking, triggered)) {
493  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Vehicle is too close or behind the stop on '" + allLanes[laneIndex]->getID() + "'.", outputStorage);
494  }
495  }
496  break;
497  case CMD_RESUME: {
498  if (inputStorage.readUnsignedByte() != TYPE_COMPOUND) {
499  server.writeStatusCmd(CMD_SET_VEHICLE_VARIABLE, RTYPE_ERR, "Resuming requires a compound object.", outputStorage);
500  return false;
501  }
502  if (inputStorage.readInt() != 0) {
503  server.writeStatusCmd(CMD_SET_VEHICLE_VARIABLE, RTYPE_ERR, "Resuming should obtain an empty compound object.", outputStorage);
504  return false;
505  }
506  if (!static_cast<MSVehicle*>(v)->hasStops()) {
507  server.writeStatusCmd(CMD_SET_VEHICLE_VARIABLE, RTYPE_ERR, "Failed to resume vehicle '" + v->getID() + "', it has no stops.", outputStorage);
508  return false;
509  }
510  if (!static_cast<MSVehicle*>(v)->resumeFromStopping()) {
511  MSVehicle::Stop& sto = (static_cast<MSVehicle*>(v))->getNextStop();
512  std::ostringstream strs;
513  strs << "reached: " << sto.reached;
514  strs << ", duration:" << sto.duration;
515  strs << ", edge:" << (*sto.edge)->getID();
516  strs << ", startPos: " << sto.startPos;
517  std::string posStr = strs.str();
518  server.writeStatusCmd(CMD_SET_VEHICLE_VARIABLE, RTYPE_ERR, "Failed to resume a non parking vehicle '" + v->getID() + "', " + posStr, outputStorage);
519  return false;
520  }
521  }
522  break;
523  case CMD_CHANGELANE: {
524  if (inputStorage.readUnsignedByte() != TYPE_COMPOUND) {
525  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Lane change needs a compound object description.", outputStorage);
526  }
527  if (inputStorage.readInt() != 2) {
528  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Lane change needs a compound object description of two items.", outputStorage);
529  }
530  // Lane ID
531  int laneIndex = 0;
532  if (!server.readTypeCheckingByte(inputStorage, laneIndex)) {
533  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "The first lane change parameter must be the lane index given as a byte.", outputStorage);
534  }
535  // stickyTime
536  SUMOTime stickyTime = 0;
537  if (!server.readTypeCheckingInt(inputStorage, stickyTime)) {
538  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "The second lane change parameter must be the duration given as an integer.", outputStorage);
539  }
540  if ((laneIndex < 0) || (laneIndex >= (int)(v->getEdge()->getLanes().size()))) {
541  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "No lane with index '" + toString(laneIndex) + "' on road '" + v->getEdge()->getID() + "'.", outputStorage);
542  }
543  // Forward command to vehicle
544  std::vector<std::pair<SUMOTime, unsigned int> > laneTimeLine;
545  laneTimeLine.push_back(std::make_pair(MSNet::getInstance()->getCurrentTimeStep(), laneIndex));
546  laneTimeLine.push_back(std::make_pair(MSNet::getInstance()->getCurrentTimeStep() + stickyTime, laneIndex));
547  v->getInfluencer().setLaneTimeLine(laneTimeLine);
548  }
549  break;
550  case CMD_SLOWDOWN: {
551  if (inputStorage.readUnsignedByte() != TYPE_COMPOUND) {
552  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Slow down needs a compound object description.", outputStorage);
553  }
554  if (inputStorage.readInt() != 2) {
555  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Slow down needs a compound object description of two items.", outputStorage);
556  }
557  double newSpeed = 0;
558  if (!server.readTypeCheckingDouble(inputStorage, newSpeed)) {
559  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "The first slow down parameter must be the speed given as a double.", outputStorage);
560  }
561  if (newSpeed < 0) {
562  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Speed must not be negative", outputStorage);
563  }
564  SUMOTime duration = 0;
565  if (!server.readTypeCheckingInt(inputStorage, duration)) {
566  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "The second slow down parameter must be the duration given as an integer.", outputStorage);
567  }
568  if (duration < 0 || STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep()) + STEPS2TIME(duration) > STEPS2TIME(SUMOTime_MAX - DELTA_T)) {
569  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Invalid time interval", outputStorage);
570  }
571  std::vector<std::pair<SUMOTime, SUMOReal> > speedTimeLine;
572  speedTimeLine.push_back(std::make_pair(MSNet::getInstance()->getCurrentTimeStep(), v->getSpeed()));
573  speedTimeLine.push_back(std::make_pair(MSNet::getInstance()->getCurrentTimeStep() + duration, newSpeed));
574  v->getInfluencer().setSpeedTimeLine(speedTimeLine);
575  }
576  break;
577  case CMD_CHANGETARGET: {
578  std::string edgeID;
579  if (!server.readTypeCheckingString(inputStorage, edgeID)) {
580  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Change target requires a string containing the id of the new destination edge as parameter.", outputStorage);
581  }
582  const MSEdge* destEdge = MSEdge::dictionary(edgeID);
583  if (destEdge == 0) {
584  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Can not retrieve road with ID " + edgeID, outputStorage);
585  }
586  // build a new route between the vehicle's current edge and destination edge
587  MSEdgeVector newRoute;
588  const MSEdge* currentEdge = v->getRerouteOrigin();
590  currentEdge, destEdge, (const MSVehicle * const) v, MSNet::getInstance()->getCurrentTimeStep(), newRoute);
591  // replace the vehicle's route by the new one
592  if (!v->replaceRouteEdges(newRoute, v->getLane() == 0)) {
593  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Route replacement failed for " + v->getID(), outputStorage);
594  }
595  }
596  break;
597  case VAR_TYPE: {
598  std::string vTypeID;
599  if (!server.readTypeCheckingString(inputStorage, vTypeID)) {
600  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "The vehicle type id must be given as a string.", outputStorage);
601  }
602  MSVehicleType* vehicleType = MSNet::getInstance()->getVehicleControl().getVType(vTypeID);
603  if (vehicleType == 0) {
604  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "The vehicle type '" + vTypeID + "' is not known.", outputStorage);
605  }
606  v->replaceVehicleType(vehicleType);
607  }
608  break;
609  case VAR_ROUTE_ID: {
610  std::string rid;
611  if (!server.readTypeCheckingString(inputStorage, rid)) {
612  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "The route id must be given as a string.", outputStorage);
613  }
614  const MSRoute* r = MSRoute::dictionary(rid);
615  if (r == 0) {
616  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "The route '" + rid + "' is not known.", outputStorage);
617  }
618  if (!v->replaceRoute(r, v->getLane() == 0)) {
619  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Route replacement failed for " + v->getID(), outputStorage);
620  }
621  }
622  break;
623  case VAR_ROUTE: {
624  std::vector<std::string> edgeIDs;
625  if (!server.readTypeCheckingStringList(inputStorage, edgeIDs)) {
626  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "A route must be defined as a list of edge ids.", outputStorage);
627  }
628  std::vector<const MSEdge*> edges;
629  MSEdge::parseEdgesList(edgeIDs, edges, "<unknown>");
630  if (!v->replaceRouteEdges(edges, v->getLane() == 0)) {
631  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Route replacement failed for " + v->getID(), outputStorage);
632  }
633  }
634  break;
635  case VAR_EDGE_TRAVELTIME: {
636  if (inputStorage.readUnsignedByte() != TYPE_COMPOUND) {
637  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting travel time requires a compound object.", outputStorage);
638  }
639  int parameterCount = inputStorage.readInt();
640  if (parameterCount == 4) {
641  // begin time
642  SUMOTime begTime = 0, endTime = 0;
643  if (!server.readTypeCheckingInt(inputStorage, begTime)) {
644  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting travel time using 4 parameters requires the begin time as first parameter.", outputStorage);
645  }
646  // begin time
647  if (!server.readTypeCheckingInt(inputStorage, endTime)) {
648  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting travel time using 4 parameters requires the end time as second parameter.", outputStorage);
649  }
650  // edge
651  std::string edgeID;
652  if (!server.readTypeCheckingString(inputStorage, edgeID)) {
653  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting travel time using 4 parameters requires the referenced edge as third parameter.", outputStorage);
654  }
655  MSEdge* edge = MSEdge::dictionary(edgeID);
656  if (edge == 0) {
657  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Referenced edge '" + edgeID + "' is not known.", outputStorage);
658  }
659  // value
660  double value = 0;
661  if (!server.readTypeCheckingDouble(inputStorage, value)) {
662  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting travel time using 4 parameters requires the travel time as double as fourth parameter.", outputStorage);
663  }
664  // retrieve
665  v->getWeightsStorage().addTravelTime(edge, begTime, endTime, value);
666  } else if (parameterCount == 2) {
667  // edge
668  std::string edgeID;
669  if (!server.readTypeCheckingString(inputStorage, edgeID)) {
670  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting travel time using 2 parameters requires the referenced edge as first parameter.", outputStorage);
671  }
672  MSEdge* edge = MSEdge::dictionary(edgeID);
673  if (edge == 0) {
674  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Referenced edge '" + edgeID + "' is not known.", outputStorage);
675  }
676  // value
677  double value = 0;
678  if (!server.readTypeCheckingDouble(inputStorage, value)) {
679  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting travel time using 2 parameters requires the travel time as second parameter.", outputStorage);
680  }
681  // retrieve
682  while (v->getWeightsStorage().knowsTravelTime(edge)) {
684  }
685  v->getWeightsStorage().addTravelTime(edge, 0, SUMOTime_MAX, value);
686  } else if (parameterCount == 1) {
687  // edge
688  std::string edgeID;
689  if (!server.readTypeCheckingString(inputStorage, edgeID)) {
690  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting travel time using 1 parameter requires the referenced edge as first parameter.", outputStorage);
691  }
692  MSEdge* edge = MSEdge::dictionary(edgeID);
693  if (edge == 0) {
694  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Referenced edge '" + edgeID + "' is not known.", outputStorage);
695  }
696  // retrieve
697  while (v->getWeightsStorage().knowsTravelTime(edge)) {
699  }
700  } else {
701  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting travel time requires 1, 2, or 4 parameters.", outputStorage);
702  }
703  }
704  break;
705  case VAR_EDGE_EFFORT: {
706  if (inputStorage.readUnsignedByte() != TYPE_COMPOUND) {
707  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting effort requires a compound object.", outputStorage);
708  }
709  int parameterCount = inputStorage.readInt();
710  if (parameterCount == 4) {
711  // begin time
712  SUMOTime begTime = 0, endTime = 0;
713  if (!server.readTypeCheckingInt(inputStorage, begTime)) {
714  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting effort using 4 parameters requires the begin time as first parameter.", outputStorage);
715  }
716  // begin time
717  if (!server.readTypeCheckingInt(inputStorage, endTime)) {
718  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting effort using 4 parameters requires the end time as second parameter.", outputStorage);
719  }
720  // edge
721  std::string edgeID;
722  if (!server.readTypeCheckingString(inputStorage, edgeID)) {
723  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting effort using 4 parameters requires the referenced edge as third parameter.", outputStorage);
724  }
725  MSEdge* edge = MSEdge::dictionary(edgeID);
726  if (edge == 0) {
727  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Referenced edge '" + edgeID + "' is not known.", outputStorage);
728  }
729  // value
730  double value = 0;
731  if (!server.readTypeCheckingDouble(inputStorage, value)) {
732  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting effort using 4 parameters requires the travel time as fourth parameter.", outputStorage);
733  }
734  // retrieve
735  v->getWeightsStorage().addEffort(edge, begTime, endTime, value);
736  } else if (parameterCount == 2) {
737  // edge
738  std::string edgeID;
739  if (!server.readTypeCheckingString(inputStorage, edgeID)) {
740  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting effort using 2 parameters requires the referenced edge as first parameter.", outputStorage);
741  }
742  MSEdge* edge = MSEdge::dictionary(edgeID);
743  if (edge == 0) {
744  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Referenced edge '" + edgeID + "' is not known.", outputStorage);
745  }
746  // value
747  double value = 0;
748  if (!server.readTypeCheckingDouble(inputStorage, value)) {
749  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting effort using 2 parameters requires the travel time as second parameter.", outputStorage);
750  }
751  // retrieve
752  while (v->getWeightsStorage().knowsEffort(edge)) {
753  v->getWeightsStorage().removeEffort(edge);
754  }
755  v->getWeightsStorage().addEffort(edge, 0, SUMOTime_MAX, value);
756  } else if (parameterCount == 1) {
757  // edge
758  std::string edgeID;
759  if (!server.readTypeCheckingString(inputStorage, edgeID)) {
760  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting effort using 1 parameter requires the referenced edge as first parameter.", outputStorage);
761  }
762  MSEdge* edge = MSEdge::dictionary(edgeID);
763  if (edge == 0) {
764  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Referenced edge '" + edgeID + "' is not known.", outputStorage);
765  }
766  // retrieve
767  while (v->getWeightsStorage().knowsEffort(edge)) {
768  v->getWeightsStorage().removeEffort(edge);
769  }
770  } else {
771  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting effort requires 1, 2, or 4 parameters.", outputStorage);
772  }
773  }
774  break;
775  case CMD_REROUTE_TRAVELTIME: {
776  if (inputStorage.readUnsignedByte() != TYPE_COMPOUND) {
777  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Rerouting requires a compound object.", outputStorage);
778  }
779  if (inputStorage.readInt() != 0) {
780  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Rerouting should obtain an empty compound object.", outputStorage);
781  }
782  v->reroute(MSNet::getInstance()->getCurrentTimeStep(), MSNet::getInstance()->getRouterTT());
783  }
784  break;
785  case CMD_REROUTE_EFFORT: {
786  if (inputStorage.readUnsignedByte() != TYPE_COMPOUND) {
787  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Rerouting requires a compound object.", outputStorage);
788  }
789  if (inputStorage.readInt() != 0) {
790  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Rerouting should obtain an empty compound object.", outputStorage);
791  }
792  v->reroute(MSNet::getInstance()->getCurrentTimeStep(), MSNet::getInstance()->getRouterEffort());
793  }
794  break;
795  case VAR_SIGNALS: {
796  int signals = 0;
797  if (!server.readTypeCheckingInt(inputStorage, signals)) {
798  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting signals requires an integer.", outputStorage);
799  }
800  v->switchOffSignal(0x0fffffff);
801  v->switchOnSignal(signals);
802  }
803  break;
804  case VAR_MOVE_TO: {
805  if (inputStorage.readUnsignedByte() != TYPE_COMPOUND) {
806  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting position requires a compound object.", outputStorage);
807  }
808  if (inputStorage.readInt() != 2) {
809  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting position should obtain the lane id and the position.", outputStorage);
810  }
811  // lane ID
812  std::string laneID;
813  if (!server.readTypeCheckingString(inputStorage, laneID)) {
814  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "The first parameter for setting a position must be the lane ID given as a string.", outputStorage);
815  }
816  // position on lane
817  double position = 0;
818  if (!server.readTypeCheckingDouble(inputStorage, position)) {
819  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "The second parameter for setting a position must be the position given as a double.", outputStorage);
820  }
821  // process
822  MSLane* l = MSLane::dictionary(laneID);
823  if (l == 0) {
824  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Unknown lane '" + laneID + "'.", outputStorage);
825  }
826  MSEdge& destinationEdge = l->getEdge();
827  if (!v->willPass(&destinationEdge)) {
828  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Vehicle '" + laneID + "' may be set onto an edge to pass only.", outputStorage);
829  }
832  while (v->getEdge() != &destinationEdge) {
833  const MSEdge* nextEdge = v->succEdge(1);
834  // let the vehicle move to the next edge
835  if (v->enterLaneAtMove(nextEdge->getLanes()[0], true)) {
837  continue;
838  }
839  }
840  l->forceVehicleInsertion(v, position);
841  }
842  break;
843  case VAR_SPEED: {
844  double speed = 0;
845  if (!server.readTypeCheckingDouble(inputStorage, speed)) {
846  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting speed requires a double.", outputStorage);
847  }
848  std::vector<std::pair<SUMOTime, SUMOReal> > speedTimeLine;
849  if (speed >= 0) {
850  speedTimeLine.push_back(std::make_pair(MSNet::getInstance()->getCurrentTimeStep(), speed));
851  speedTimeLine.push_back(std::make_pair(SUMOTime_MAX - DELTA_T, speed));
852  }
853  v->getInfluencer().setSpeedTimeLine(speedTimeLine);
854  }
855  break;
856  case VAR_SPEEDSETMODE: {
857  int speedMode = 0;
858  if (!server.readTypeCheckingInt(inputStorage, speedMode)) {
859  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting speed mode requires an integer.", outputStorage);
860  }
861  v->getInfluencer().setConsiderSafeVelocity((speedMode & 1) != 0);
862  v->getInfluencer().setConsiderMaxAcceleration((speedMode & 2) != 0);
863  v->getInfluencer().setConsiderMaxDeceleration((speedMode & 4) != 0);
864  v->getInfluencer().setRespectJunctionPriority((speedMode & 8) != 0);
865  v->getInfluencer().setEmergencyBrakeRedLight((speedMode & 16) != 0);
866  }
867  break;
868  case VAR_LANECHANGE_MODE: {
869  int laneChangeMode = 0;
870  if (!server.readTypeCheckingInt(inputStorage, laneChangeMode)) {
871  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting lane change mode requires an integer.", outputStorage);
872  }
873  v->getInfluencer().setLaneChangeMode(laneChangeMode);
874  }
875  break;
876  case VAR_COLOR: {
877  RGBColor col;
878  if (!server.readTypeCheckingColor(inputStorage, col)) {
879  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "The color must be given using the according type.", outputStorage);
880  }
881  v->getParameter().color.set(col.red(), col.green(), col.blue(), col.alpha());
883  }
884  break;
885  case ADD: {
886  if (v != 0) {
887  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "The vehicle " + id + " to add already exists.", outputStorage);
888  }
889  if (inputStorage.readUnsignedByte() != TYPE_COMPOUND) {
890  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Adding a vehicle requires a compound object.", outputStorage);
891  }
892  if (inputStorage.readInt() != 6) {
893  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Adding a vehicle needs six parameters.", outputStorage);
894  }
895  SUMOVehicleParameter vehicleParams;
896  vehicleParams.id = id;
897 
898  std::string vTypeID;
899  if (!server.readTypeCheckingString(inputStorage, vTypeID)) {
900  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "First parameter (type) requires a string.", outputStorage);
901  }
902  MSVehicleType* vehicleType = MSNet::getInstance()->getVehicleControl().getVType(vTypeID);
903  if (!vehicleType) {
904  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Invalid type '" + vTypeID + "' for vehicle '" + id + "'", outputStorage);
905  }
906 
907  std::string routeID;
908  if (!server.readTypeCheckingString(inputStorage, routeID)) {
909  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Second parameter (route) requires a string.", outputStorage);
910  }
911  const MSRoute* route = MSRoute::dictionary(routeID);
912  if (!route) {
913  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Invalid route '" + routeID + "' for vehicle: '" + id + "'", outputStorage);
914  }
915 
916  if (!server.readTypeCheckingInt(inputStorage, vehicleParams.depart)) {
917  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Third parameter (depart) requires an integer.", outputStorage);
918  }
919  if (vehicleParams.depart < 0) {
920  const int proc = static_cast<int>(-vehicleParams.depart);
921  if (proc >= static_cast<int>(DEPART_DEF_MAX)) {
922  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Invalid departure time.", outputStorage);
923  }
924  vehicleParams.departProcedure = (DepartDefinition)proc;
925  } else if (vehicleParams.depart < MSNet::getInstance()->getCurrentTimeStep()) {
926  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Departure time in the past.", outputStorage);
927  }
928 
929  double pos;
930  if (!server.readTypeCheckingDouble(inputStorage, pos)) {
931  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Fourth parameter (position) requires a double.", outputStorage);
932  }
933  vehicleParams.departPos = pos;
934  if (vehicleParams.departPos < 0) {
935  const int proc = static_cast<int>(-vehicleParams.departPos);
936  if (fabs(proc + vehicleParams.departPos) > NUMERICAL_EPS || proc >= static_cast<int>(DEPART_POS_DEF_MAX) || proc == static_cast<int>(DEPART_POS_GIVEN)) {
937  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Invalid departure position.", outputStorage);
938  }
939  vehicleParams.departPosProcedure = (DepartPosDefinition)proc;
940  } else {
941  vehicleParams.departPosProcedure = DEPART_POS_GIVEN;
942  }
943 
944  double speed;
945  if (!server.readTypeCheckingDouble(inputStorage, speed)) {
946  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Fifth parameter (speed) requires a double.", outputStorage);
947  }
948  vehicleParams.departSpeed = speed;
949  if (vehicleParams.departSpeed < 0) {
950  const int proc = static_cast<int>(-vehicleParams.departSpeed);
951  if (proc >= static_cast<int>(DEPART_SPEED_DEF_MAX)) {
952  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Invalid departure speed.", outputStorage);
953  }
954  vehicleParams.departSpeedProcedure = (DepartSpeedDefinition)proc;
955  } else {
957  }
958 
959  if (!server.readTypeCheckingByte(inputStorage, vehicleParams.departLane)) {
960  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Sixth parameter (lane) requires a byte.", outputStorage);
961  }
962 
963  if (vehicleParams.departLane < 0) {
964  const int proc = static_cast<int>(-vehicleParams.departLane);
965  if (proc >= static_cast<int>(DEPART_LANE_DEF_MAX)) {
966  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Invalid departure lane.", outputStorage);
967  }
968  vehicleParams.departLaneProcedure = (DepartLaneDefinition)proc;
969  } else {
970  vehicleParams.departLaneProcedure = DEPART_LANE_GIVEN;
971  }
972 
973  SUMOVehicleParameter* params = new SUMOVehicleParameter(vehicleParams);
974  try {
975  SUMOVehicle* vehicle = MSNet::getInstance()->getVehicleControl().buildVehicle(params, route, vehicleType);
976  MSNet::getInstance()->getVehicleControl().addVehicle(vehicleParams.id, vehicle);
978  } catch (ProcessError& e) {
979  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, e.what(), outputStorage);
980  }
981  }
982  break;
983  case ADD_FULL: {
984  if (v != 0) {
985  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "The vehicle " + id + " to add already exists.", outputStorage);
986  }
987  if (inputStorage.readUnsignedByte() != TYPE_COMPOUND) {
988  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Adding a vehicle requires a compound object.", outputStorage);
989  }
990  if (inputStorage.readInt() != 14) {
991  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Adding a fully specified vehicle needs fourteen parameters.", outputStorage);
992  }
993  SUMOVehicleParameter vehicleParams;
994  vehicleParams.id = id;
995 
996  std::string routeID;
997  if (!server.readTypeCheckingString(inputStorage, routeID)) {
998  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Second parameter (route) requires a string.", outputStorage);
999  }
1000  const MSRoute* route = MSRoute::dictionary(routeID);
1001  if (!route) {
1002  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Invalid route '" + routeID + "' for vehicle: '" + id + "'", outputStorage);
1003  }
1004 
1005  std::string vTypeID;
1006  if (!server.readTypeCheckingString(inputStorage, vTypeID)) {
1007  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "First parameter (type) requires a string.", outputStorage);
1008  }
1009  MSVehicleType* vehicleType = MSNet::getInstance()->getVehicleControl().getVType(vTypeID);
1010  if (!vehicleType) {
1011  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Invalid type '" + vTypeID + "' for vehicle '" + id + "'", outputStorage);
1012  }
1013 
1014  std::string helper;
1015  std::string error;
1016  if (!server.readTypeCheckingString(inputStorage, helper)) {
1017  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Third parameter (depart) requires an string.", outputStorage);
1018  }
1019  if (!SUMOVehicleParameter::parseDepart(helper, "vehicle", id, vehicleParams.depart, vehicleParams.departProcedure, error)) {
1020  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, error, outputStorage);
1021  }
1022  if (vehicleParams.departProcedure == DEPART_GIVEN && vehicleParams.depart < MSNet::getInstance()->getCurrentTimeStep()) {
1023  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Departure time in the past.", outputStorage);
1024  }
1025 
1026  if (!server.readTypeCheckingString(inputStorage, helper)) {
1027  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Fourth parameter (depart lane) requires a string.", outputStorage);
1028  }
1029  if (!SUMOVehicleParameter::parseDepartLane(helper, "vehicle", id, vehicleParams.departLane, vehicleParams.departLaneProcedure, error)) {
1030  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, error, outputStorage);
1031  }
1032  if (!server.readTypeCheckingString(inputStorage, helper)) {
1033  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Fifth parameter (depart position) requires a string.", outputStorage);
1034  }
1035  if (!SUMOVehicleParameter::parseDepartPos(helper, "vehicle", id, vehicleParams.departPos, vehicleParams.departPosProcedure, error)) {
1036  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, error, outputStorage);
1037  }
1038  if (!server.readTypeCheckingString(inputStorage, helper)) {
1039  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Sixth parameter (depart speed) requires a string.", outputStorage);
1040  }
1041  if (!SUMOVehicleParameter::parseDepartSpeed(helper, "vehicle", id, vehicleParams.departSpeed, vehicleParams.departSpeedProcedure, error)) {
1042  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, error, outputStorage);
1043  }
1044 
1045  if (!server.readTypeCheckingString(inputStorage, helper)) {
1046  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Seventh parameter (arrival lane) requires a string.", outputStorage);
1047  }
1048  if (!SUMOVehicleParameter::parseArrivalLane(helper, "vehicle", id, vehicleParams.arrivalLane, vehicleParams.arrivalLaneProcedure, error)) {
1049  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, error, outputStorage);
1050  }
1051  if (!server.readTypeCheckingString(inputStorage, helper)) {
1052  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Eighth parameter (arrival position) requires a string.", outputStorage);
1053  }
1054  if (!SUMOVehicleParameter::parseArrivalPos(helper, "vehicle", id, vehicleParams.arrivalPos, vehicleParams.arrivalPosProcedure, error)) {
1055  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, error, outputStorage);
1056  }
1057  if (!server.readTypeCheckingString(inputStorage, helper)) {
1058  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Ninth parameter (arrival speed) requires a string.", outputStorage);
1059  }
1060  if (!SUMOVehicleParameter::parseArrivalSpeed(helper, "vehicle", id, vehicleParams.arrivalSpeed, vehicleParams.arrivalSpeedProcedure, error)) {
1061  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, error, outputStorage);
1062  }
1063 
1064  if (!server.readTypeCheckingString(inputStorage, vehicleParams.fromTaz)) {
1065  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Tenth parameter (from taz) requires a string.", outputStorage);
1066  }
1067  if (!server.readTypeCheckingString(inputStorage, vehicleParams.toTaz)) {
1068  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Eleventh parameter (to taz) requires a string.", outputStorage);
1069  }
1070  if (!server.readTypeCheckingString(inputStorage, vehicleParams.line)) {
1071  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Twelth parameter (line) requires a string.", outputStorage);
1072  }
1073 
1074  int num;
1075  if (!server.readTypeCheckingInt(inputStorage, num)) {
1076  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "13th parameter (person capacity) requires an int.", outputStorage);
1077  }
1078  vehicleParams.personCapacity = num;
1079  if (!server.readTypeCheckingInt(inputStorage, num)) {
1080  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "14th parameter (person number) requires an int.", outputStorage);
1081  }
1082  vehicleParams.personNumber = num;
1083 
1084  SUMOVehicleParameter* params = new SUMOVehicleParameter(vehicleParams);
1085  try {
1086  SUMOVehicle* vehicle = MSNet::getInstance()->getVehicleControl().buildVehicle(params, route, vehicleType);
1087  MSNet::getInstance()->getVehicleControl().addVehicle(vehicleParams.id, vehicle);
1089  } catch (ProcessError& e) {
1090  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, e.what(), outputStorage);
1091  }
1092  }
1093  break;
1094  case REMOVE: {
1095  int why = 0;
1096  if (!server.readTypeCheckingByte(inputStorage, why)) {
1097  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Removing a vehicle requires a byte.", outputStorage);
1098  }
1100  switch (why) {
1101  case REMOVE_TELEPORT:
1102  // XXX semantics unclear
1103  // n = MSMoveReminder::NOTIFICATION_TELEPORT;
1105  break;
1106  case REMOVE_PARKING:
1107  // XXX semantics unclear
1108  // n = MSMoveReminder::NOTIFICATION_PARKING;
1110  break;
1111  case REMOVE_ARRIVED:
1113  break;
1114  case REMOVE_VAPORIZED:
1116  break;
1119  break;
1120  default:
1121  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Unknown removal status.", outputStorage);
1122  }
1123  if (v->hasDeparted()) {
1124  v->onRemovalFromNet(n);
1125  if (v->getLane() != 0) {
1126  v->getLane()->removeVehicle(v, n);
1127  }
1129  }
1130  }
1131  break;
1132  case VAR_MOVE_TO_VTD: {
1133  if (inputStorage.readUnsignedByte() != TYPE_COMPOUND) {
1134  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting VTD vehicle requires a compound object.", outputStorage);
1135  }
1136  if (inputStorage.readInt() != 4) {
1137  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting VTD vehicle should obtain: edgeID, lane, x, y.", outputStorage);
1138  }
1139  // edge ID
1140  std::string edgeID;
1141  if (!server.readTypeCheckingString(inputStorage, edgeID)) {
1142  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "The first parameter for setting a VTD vehicle must be the edge ID given as a string.", outputStorage);
1143  }
1144  // lane index
1145  int laneNum = 0;
1146  if (!server.readTypeCheckingInt(inputStorage, laneNum)) {
1147  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "The second parameter for setting a VTD vehicle must be lane given as an int.", outputStorage);
1148  }
1149  // x
1150  double x = 0, y = 0;
1151  if (!server.readTypeCheckingDouble(inputStorage, x)) {
1152  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "The third parameter for setting a VTD vehicle must be the x-position given as a double.", outputStorage);
1153  }
1154  // y
1155  if (!server.readTypeCheckingDouble(inputStorage, y)) {
1156  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "The fourth parameter for setting a VTD vehicle must be the y-position given as a double.", outputStorage);
1157  }
1158  // process
1159  if (!v->isOnRoad()) {
1160  break;
1161  }
1162  std::string origID = edgeID + " " + toString(laneNum);
1163  if (laneNum < 0) {
1164  edgeID = '-' + edgeID;
1165  laneNum = -laneNum;
1166  }
1167  Position pos(x, y);
1168 
1169  Position vehPos = v->getPosition();
1170  v->updateBestLanes();
1171  bool report = server.vtdDebug();
1172  if (report) {
1173  std::cout << std::endl << "begin vehicle " << v->getID() << " vehPos:" << vehPos << " lane:" << v->getLane()->getID() << std::endl;
1174  }
1175  if (report) {
1176  std::cout << " want pos:" << pos << " edge:" << edgeID << " laneNum:" << laneNum << std::endl;
1177  }
1178 
1179  MSEdgeVector edgesA, edgesB, edgesC;
1180  MSLane* laneA, *laneB, *laneC;
1181  laneA = laneB = laneC = 0;
1182  SUMOReal lanePosA, lanePosB, lanePosC;
1183  SUMOReal bestDistanceA, bestDistanceB, bestDistanceC;
1184  bestDistanceA = bestDistanceB = bestDistanceC = 1000.;//pos.distanceSquaredTo2D(vehPos);
1185  int routeOffsetA, routeOffsetB, routeOffsetC;
1186  routeOffsetA = routeOffsetB = routeOffsetC = 0;
1187  // case a): edge/lane is known and matches route
1188  bool aFound = vtdMap_matchingEdgeLane(pos, origID, *v, server.vtdDebug(), bestDistanceA, &laneA, lanePosA, routeOffsetA, edgesA);
1189  // case b): position is at route, should be somewhere near to it
1190  bool bFound = vtdMap_matchingRoutePosition(pos, origID, *v, server.vtdDebug(), bestDistanceB, &laneB, lanePosB, routeOffsetB, edgesB);
1191  // case c) nearest matching lane
1192  bool cFound = vtdMap_matchingNearest(pos, origID, *v, server, server.vtdDebug(), bestDistanceC, &laneC, lanePosC, routeOffsetC, edgesC);
1193  //
1194  SUMOReal maxRouteDistance = 50;
1195  if (cFound && (bestDistanceA > maxRouteDistance && bestDistanceC > maxRouteDistance)) {
1196  // both route-based approach yield in a position too far away from the submitted --> new route!?
1197  server.setVTDControlled(v, laneC, lanePosC, routeOffsetC, edgesC);
1198  } else {
1199  // use the best we have
1200  if (bFound) {
1201  server.setVTDControlled(v, laneB, lanePosB, routeOffsetB, edgesB);
1202  } else if (aFound) {
1203  server.setVTDControlled(v, laneA, lanePosA, routeOffsetA, edgesA);
1204  } else if (cFound) {
1205  server.setVTDControlled(v, laneC, lanePosC, routeOffsetC, edgesC);
1206  } else {
1207  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Could not map vehicle.", outputStorage);
1208  }
1209  }
1210  }
1211  break;
1212  case VAR_SPEED_FACTOR: {
1213  double factor = 0;
1214  if (!server.readTypeCheckingDouble(inputStorage, factor)) {
1215  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting speed factor requires a double.", outputStorage);
1216  }
1217  v->setChosenSpeedFactor(factor);
1218  }
1219  break;
1220  default:
1221  try {
1222  if (!TraCIServerAPI_VehicleType::setVariable(CMD_SET_VEHICLE_VARIABLE, variable, getSingularType(v), server, inputStorage, outputStorage)) {
1223  return false;
1224  }
1225  } catch (ProcessError& e) {
1226  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, e.what(), outputStorage);
1227  }
1228  break;
1229  }
1230  server.writeStatusCmd(CMD_SET_VEHICLE_VARIABLE, RTYPE_OK, warning, outputStorage);
1231  return true;
1232 }
1233 
1234 
1235 bool
1236 TraCIServerAPI_Vehicle::vtdMap_matchingEdgeLane(const Position& pos, const std::string& origID, MSVehicle& v, bool report,
1237  SUMOReal& bestDistance, MSLane** lane, SUMOReal& lanePos, int& routeOffset, MSEdgeVector& edges) {
1238  UNUSED_PARAMETER(edges);
1239  const std::map<std::string, std::vector<MSLane*> >& vtdMap = getOrBuildVTDMap();
1240  if (vtdMap.find(origID) == vtdMap.end()) {
1241  if (report) {
1242  std::cout << " a failed - lane not in map" << std::endl;
1243  }
1244  return false;
1245  }
1246  const std::vector<MSLane*>& lanes = vtdMap.find(origID)->second;
1247  for (std::vector<MSLane*>::const_iterator i = lanes.begin(); i != lanes.end() && bestDistance > POSITION_EPS; ++i) {
1248  MSLane* l = *i;
1249  SUMOReal dist = l->getShape().distance(pos);
1250  if (report) {
1251  std::cout << " a at lane " << l->getID() << " dist:" << dist << " best:" << bestDistance << std::endl;
1252  }
1253  if (dist < bestDistance) {
1254  bestDistance = dist;
1255  *lane = l;
1256  }
1257  }
1258  MSLane* pni = *lane;
1259  while (pni != 0 && pni->getEdge().getPurpose() == MSEdge::EDGEFUNCTION_INTERNAL && pni->getIncomingLanes().size() != 0) {
1260  pni = pni->getIncomingLanes()[0].lane;
1261  }
1262  if (pni == 0 || pni->getEdge().getPurpose() == MSEdge::EDGEFUNCTION_INTERNAL) {
1263  // not found
1264  if (report) {
1265  std::cout << " a failed - no incoming lane" << std::endl;
1266  }
1267  return false;
1268  }
1269  const MSEdgeVector& tedges = v.getRoute().getEdges();
1270  MSEdgeVector::const_iterator p = std::find(tedges.begin() + v.getRoutePosition(), tedges.end(), &pni->getEdge());
1271  if (p != tedges.end()) {
1272  lanePos = MAX2(SUMOReal(0), MIN2(SUMOReal((*lane)->getLength() - POSITION_EPS), (*lane)->getShape().nearest_offset_to_point2D(pos, false)));
1273  routeOffset = (int)(std::distance(tedges.begin(), p) - v.getRoutePosition());
1274  if (report) {
1275  std::cout << " a ok lane:" << (*lane)->getID() << " lanePos:" << lanePos << " routeOffset:" << routeOffset << std::endl;
1276  }
1277  return true;
1278  }
1279  if (report) {
1280  std::cout << " a failed - route position beyond route length" << std::endl;
1281  }
1282  return false;
1283 }
1284 
1285 
1286 bool
1287 TraCIServerAPI_Vehicle::vtdMap_matchingRoutePosition(const Position& pos, const std::string& origID, MSVehicle& v, bool report,
1288  SUMOReal& bestDistance, MSLane** lane, SUMOReal& lanePos, int& routeOffset, MSEdgeVector& edges) {
1289  UNUSED_PARAMETER(edges);
1290  UNUSED_PARAMETER(origID);
1291  int lastBestRouteEdge = 0;
1292  int lastRouteEdge = 0;
1293  MSLane* bestRouteLane = 0;
1294  const std::vector<MSLane*>& bestLaneConts = v.getBestLanesContinuation(v.getLane());
1295  for (std::vector<MSLane*>::const_iterator i = bestLaneConts.begin(); i != bestLaneConts.end() && bestDistance > POSITION_EPS; ++i) {
1296  MSEdge& e = (*i)->getEdge();
1297  if (i != bestLaneConts.begin() && e.getPurpose() != MSEdge::EDGEFUNCTION_INTERNAL) {
1298  ++lastRouteEdge;
1299  }
1300  const std::vector<MSLane*>& lanes = e.getLanes();
1301  for (std::vector<MSLane*>::const_iterator k = lanes.begin(); k != lanes.end() && bestDistance > POSITION_EPS; ++k) {
1302  MSLane* cl = *k;
1303  SUMOReal dist = cl->getShape().distance(pos);
1304  if (report) {
1305  std::cout << " b at lane " << cl->getID() << " dist:" << dist << " best:" << bestDistance << std::endl;
1306  }
1307  if (dist < bestDistance) {
1308  bestDistance = dist;
1309  *lane = cl;
1310  lastBestRouteEdge = lastRouteEdge;
1312  bestRouteLane = *i;
1313  } else {
1314  bestRouteLane = *lane;
1315  }
1316  }
1317  }
1318  }
1319  if (bestRouteLane == 0) {
1320  if (report) {
1321  std::cout << " b failed - no best route lane" << std::endl;
1322  }
1323  return false;
1324  }
1325  lanePos = MAX2(SUMOReal(0), MIN2(SUMOReal(bestRouteLane->getLength() - POSITION_EPS), bestRouteLane->getShape().nearest_offset_to_point2D(pos, false)));
1326  routeOffset = lastBestRouteEdge;
1327  if (report) {
1328  std::cout << " b ok lane " << bestRouteLane->getID() << " lanePos:" << lanePos << " best:" << lastBestRouteEdge << std::endl;
1329  }
1330  return true;
1331 }
1332 
1333 
1334 bool
1335 TraCIServerAPI_Vehicle::vtdMap_matchingNearest(const Position& pos, const std::string& origID, MSVehicle& v, TraCIServer& server, bool report,
1336  SUMOReal& bestDistance, MSLane** lane, SUMOReal& lanePos, int& routeOffset, MSEdgeVector& edges) {
1337  UNUSED_PARAMETER(bestDistance);
1338  unsigned int r = 0;
1339  SUMOReal minDist = 1 << (11);
1340  MSLane* minDistLane = 0;
1341  MSLane* nameMatchingLane = 0;
1342  SUMOReal minDistNameMatchingLane = 1 << (11);
1343  for (; minDistLane == 0 && r < 10 && nameMatchingLane == 0; ++r) {
1344  std::set<std::string> into;
1345  PositionVector shape;
1346  shape.push_back(pos);
1347  server.collectObjectsInRange(CMD_GET_EDGE_VARIABLE, shape, 1 << r, into);
1348  for (std::set<std::string>::const_iterator j = into.begin(); j != into.end(); ++j) {
1349  MSEdge* e = MSEdge::dictionary(*j);
1350  const std::vector<MSLane*>& lanes = e->getLanes();
1351  for (std::vector<MSLane*>::const_iterator k = lanes.begin(); k != lanes.end(); ++k) {
1352  MSLane* lane = *k;
1353  SUMOReal dist = lane->getShape().distance(pos);
1354  if (lane->knowsParameter("origId")) {
1355  if (lane->getParameter("origId", "") == origID) {
1356  if (dist < minDistNameMatchingLane) {
1357  minDistNameMatchingLane = dist;
1358  nameMatchingLane = lane;
1359  }
1360  }
1361  }
1362  if (dist < minDist) {
1363  minDist = dist;
1364  minDistLane = lane;
1365  }
1366  }
1367  }
1368  }
1369  *lane = nameMatchingLane != 0 ? nameMatchingLane : minDistLane;
1370  if (lane == 0) {
1371  if (report) {
1372  std::cout << " c failed - no matching lane" << std::endl;
1373  }
1374  return false;
1375  }
1376  lanePos = (*lane)->interpolateGeometryPosToLanePos((*lane)->getShape().nearest_offset_to_point2D(pos, false));
1377  if (*lane == v.getLane()) {
1378  routeOffset = 0;
1379  if (report) {
1380  std::cout << " c ok, on same lane" << std::endl;
1381  }
1382  return true;
1383  }
1384  MSEdge& destinationEdge = (*lane)->getEdge();
1385  MSEdge* routePos = &destinationEdge;
1386  while (routePos->getPurpose() == MSEdge::EDGEFUNCTION_INTERNAL) {
1387  routePos = &routePos->getLanes()[0]->getLogicalPredecessorLane()->getEdge();
1388  }
1389  r = 0;
1390  const MSRoute& route = v.getRoute();
1391  unsigned int c = v.getRoutePosition();
1392  unsigned int l = (int)route.getEdges().size();
1393  unsigned int rindex = 0;
1394  bool found = false;
1395  while (!found && ((int)(c - r) >= 0 || c + r < l)) {
1396  if ((int)(c - r) >= 0 && route[c - r] == routePos) {
1397  rindex = c - r;
1398  found = true;
1399  }
1400  if (c + r < l && route[c + r] == routePos) {
1401  rindex = c + r;
1402  found = true;
1403  }
1404  ++r;
1405  }
1406  if (found) {
1407  // the matching lane is part of the route
1408  routeOffset = rindex - v.getRoutePosition();
1409  if (report) {
1410  std::cout << " c ok, on a different edge of same route" << std::endl;
1411  }
1412  return true;
1413  }
1414  // build new route
1415  MSLane* firstLane = *lane;
1416  if (destinationEdge.getPurpose() != MSEdge::EDGEFUNCTION_INTERNAL) {
1417  edges.push_back(&destinationEdge);
1418  } else {
1419  firstLane = (*lane)->getLogicalPredecessorLane();
1420  edges.push_back(&firstLane->getEdge());
1421  }
1422  const MSLinkCont& lc = firstLane->getLinkCont();
1423  if (lc.size() != 0 && lc[0]->getLane() != 0) {
1424  edges.push_back(&lc[0]->getLane()->getEdge());
1425  }
1426  if (report) {
1427  std::cout << " c ok, on a different route" << std::endl;
1428  }
1429  return true;
1430 }
1431 
1432 
1433 bool
1435  tcpip::Storage& outputStorage, const MSVehicle* v) {
1436  if (inputStorage.readUnsignedByte() != TYPE_COMPOUND) {
1437  return server.writeErrorStatusCmd(CMD_GET_VEHICLE_VARIABLE, "Retrieval of distance requires a compound object.", outputStorage);
1438  }
1439  if (inputStorage.readInt() != 2) {
1440  return server.writeErrorStatusCmd(CMD_GET_VEHICLE_VARIABLE, "Retrieval of distance requires position and distance type as parameter.", outputStorage);
1441  }
1442 
1443  Position pos;
1444  std::pair<const MSLane*, SUMOReal> roadPos;
1445 
1446  // read position
1447  int posType = inputStorage.readUnsignedByte();
1448  switch (posType) {
1449  case POSITION_ROADMAP:
1450  try {
1451  std::string roadID = inputStorage.readString();
1452  roadPos.second = inputStorage.readDouble();
1453  roadPos.first = TraCIServerAPI_Simulation::getLaneChecking(roadID, inputStorage.readUnsignedByte(), roadPos.second);
1454  pos = roadPos.first->getShape().positionAtOffset(roadPos.second);
1455  } catch (TraCIException& e) {
1456  return server.writeErrorStatusCmd(CMD_GET_VEHICLE_VARIABLE, e.what(), outputStorage);
1457  }
1458  break;
1459  case POSITION_2D:
1460  case POSITION_3D: {
1461  const double p1x = inputStorage.readDouble();
1462  const double p1y = inputStorage.readDouble();
1463  pos.set(p1x, p1y);
1464  }
1465  if (posType == POSITION_3D) {
1466  inputStorage.readDouble(); // z value is ignored
1467  }
1469  break;
1470  default:
1471  return server.writeErrorStatusCmd(CMD_GET_VEHICLE_VARIABLE, "Unknown position format used for distance request", outputStorage);
1472  }
1473 
1474  // read distance type
1475  int distType = inputStorage.readUnsignedByte();
1476 
1477  SUMOReal distance = INVALID_DOUBLE_VALUE;
1478  if (v->isOnRoad()) {
1479  if (distType == REQUEST_DRIVINGDIST) {
1480  distance = v->getRoute().getDistanceBetween(v->getPositionOnLane(), roadPos.second,
1481  v->getEdge(), &roadPos.first->getEdge());
1482  if (distance == std::numeric_limits<SUMOReal>::max()) {
1483  distance = INVALID_DOUBLE_VALUE;
1484  }
1485  } else {
1486  // compute air distance (default)
1487  distance = v->getPosition().distanceTo(pos);
1488  }
1489  }
1490  // write response command
1491  outputStorage.writeUnsignedByte(TYPE_DOUBLE);
1492  outputStorage.writeDouble(distance);
1493  return true;
1494 }
1495 
1496 
1497 // ------ helper functions ------
1498 bool
1499 TraCIServerAPI_Vehicle::getPosition(const std::string& id, Position& p) {
1500  MSVehicle* v = dynamic_cast<MSVehicle*>(MSNet::getInstance()->getVehicleControl().getVehicle(id));
1501  if (v == 0) {
1502  return false;
1503  }
1504  p = v->getPosition();
1505  return true;
1506 }
1507 
1508 
1511  const MSVehicleType& oType = veh->getVehicleType();
1512  std::string newID = oType.getID().find('@') == std::string::npos ? oType.getID() + "@" + veh->getID() : oType.getID();
1513  MSVehicleType* type = MSVehicleType::build(newID, &oType);
1514  static_cast<MSVehicle*>(veh)->replaceVehicleType(type);
1515  return *type;
1516 }
1517 
1518 
1519 #include <microsim/MSEdgeControl.h>
1520 
1521 const std::map<std::string, std::vector<MSLane*> >&
1523  if (gVTDMap.size() == 0) {
1524  const std::vector<MSEdge*>& edges = MSNet::getInstance()->getEdgeControl().getEdges();
1525  for (std::vector<MSEdge*>::const_iterator i = edges.begin(); i != edges.end(); ++i) {
1526  const std::vector<MSLane*>& lanes = (*i)->getLanes();
1527  for (std::vector<MSLane*>::const_iterator j = lanes.begin(); j != lanes.end(); ++j) {
1528  if ((*j)->knowsParameter("origId")) {
1529  std::string origID = (*j)->getParameter("origId", "");
1530  if (gVTDMap.find(origID) == gVTDMap.end()) {
1531  gVTDMap[origID] = std::vector<MSLane*>();
1532  }
1533  gVTDMap[origID].push_back(*j);
1534  }
1535  }
1536  }
1537  if (gVTDMap.size() == 0) {
1538  gVTDMap["unknown"] = std::vector<MSLane*>();
1539  }
1540  }
1541  return gVTDMap;
1542 }
1543 
1544 
1545 #endif
1546 
1547 
1548 /****************************************************************************/
1549