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 /****************************************************************************/
11 // APIs for getting/setting vehicle values via TraCI
12 /****************************************************************************/
13 // SUMO, Simulation of Urban MObility; see http://sumo.sourceforge.net/
14 // Copyright (C) 2001-2013 DLR (http://www.dlr.de/) and contributors
15 /****************************************************************************/
16 //
17 // This file is part of SUMO.
18 // SUMO is free software: you can redistribute it and/or modify
19 // it under the terms of the GNU General Public License as published by
20 // the Free Software Foundation, either version 3 of the License, or
21 // (at your option) any later version.
22 //
23 /****************************************************************************/
24 
25 
26 // ===========================================================================
27 // included modules
28 // ===========================================================================
29 #ifdef _MSC_VER
30 #include <windows_config.h>
31 #else
32 #include <config.h>
33 #endif
34 
35 #ifndef NO_TRACI
36 
37 #include <microsim/MSNet.h>
38 #include <microsim/MSInsertionControl.h>
39 #include <microsim/MSVehicle.h>
40 #include <microsim/MSLane.h>
41 #include <microsim/MSEdge.h>
42 #include <microsim/MSEdgeWeightsStorage.h>
43 #include <microsim/MSAbstractLaneChangeModel.h>
44 #include <utils/geom/PositionVector.h>
45 #include <utils/common/DijkstraRouterTT.h>
46 #include <utils/common/DijkstraRouterEffort.h>
47 #include <utils/common/HelpersHBEFA.h>
48 #include <utils/common/HelpersHarmonoise.h>
49 #include <utils/common/SUMOVehicleParameter.h>
50 #include "TraCIConstants.h"
51 #include "TraCIServerAPI_Simulation.h"
52 #include "TraCIServerAPI_Vehicle.h"
53 #include "TraCIServerAPI_VehicleType.h"
54 
55 #ifdef CHECK_MEMORY_LEAKS
56 #include <foreign/nvwa/debug_new.h>
57 #endif // CHECK_MEMORY_LEAKS
58 
59 
60 // ===========================================================================
61 // used namespaces
62 // ===========================================================================
63 using namespace traci;
64 
65 
66 
67 std::map<std::string, std::vector<MSLane*> > TraCIServerAPI_Vehicle::gVTDMap;
68 
69 // ===========================================================================
70 // method definitions
71 // ===========================================================================
72 bool
73 TraCIServerAPI_Vehicle::processGet(TraCIServer& server, tcpip::Storage& inputStorage,
74  tcpip::Storage& outputStorage) {
75  // variable & id
76  int variable = inputStorage.readUnsignedByte();
77  std::string id = inputStorage.readString();
78  // check variable
79  if (variable != ID_LIST && variable != VAR_SPEED && variable != VAR_SPEED_WITHOUT_TRACI && variable != VAR_POSITION && variable != VAR_ANGLE
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 && variable != VAR_HCEMISSION && variable != VAR_PMXEMISSION
84  && variable != VAR_NOXEMISSION && variable != VAR_FUELCONSUMPTION && variable != VAR_NOISEEMISSION
85  && variable != VAR_PERSON_NUMBER
86  && variable != VAR_EDGE_TRAVELTIME && variable != VAR_EDGE_EFFORT
87  && variable != VAR_ROUTE_VALID && variable != VAR_EDGES
88  && variable != VAR_SIGNALS
89  && variable != VAR_LENGTH && variable != VAR_MAXSPEED && variable != VAR_VEHICLECLASS
90  && variable != VAR_SPEED_FACTOR && variable != VAR_SPEED_DEVIATION && variable != VAR_EMISSIONCLASS
91  && variable != VAR_WIDTH && variable != VAR_MINGAP && variable != VAR_SHAPECLASS
92  && variable != VAR_ACCEL && variable != VAR_DECEL && variable != VAR_IMPERFECTION
93  && variable != VAR_TAU && variable != VAR_BEST_LANES && variable != DISTANCE_REQUEST
94  && variable != ID_COUNT
95  ) {
96  return server.writeErrorStatusCmd(CMD_GET_VEHICLE_VARIABLE, "Get Vehicle Variable: unsupported variable specified", outputStorage);
97  }
98  // begin response building
99  tcpip::Storage tempMsg;
100  // response-code, variableID, objectID
101  tempMsg.writeUnsignedByte(RESPONSE_GET_VEHICLE_VARIABLE);
102  tempMsg.writeUnsignedByte(variable);
103  tempMsg.writeString(id);
104  // process request
105  if (variable == ID_LIST || variable == ID_COUNT) {
106  std::vector<std::string> ids;
107  MSVehicleControl& c = MSNet::getInstance()->getVehicleControl();
108  for (MSVehicleControl::constVehIt i = c.loadedVehBegin(); i != c.loadedVehEnd(); ++i) {
109  if ((*i).second->isOnRoad()) {
110  ids.push_back((*i).first);
111  }
112  }
113  if (variable == ID_LIST) {
114  tempMsg.writeUnsignedByte(TYPE_STRINGLIST);
115  tempMsg.writeStringList(ids);
116  } else {
117  tempMsg.writeUnsignedByte(TYPE_INTEGER);
118  tempMsg.writeInt((int) ids.size());
119  }
120  } else {
121  SUMOVehicle* sumoVehicle = MSNet::getInstance()->getVehicleControl().getVehicle(id);
122  if (sumoVehicle == 0) {
123  return server.writeErrorStatusCmd(CMD_GET_VEHICLE_VARIABLE, "Vehicle '" + id + "' is not known", outputStorage);
124  }
125  MSVehicle* v = dynamic_cast<MSVehicle*>(sumoVehicle);
126  if (v == 0) {
127  return server.writeErrorStatusCmd(CMD_GET_VEHICLE_VARIABLE, "Vehicle '" + id + "' is not a micro-simulation vehicle", outputStorage);
128  }
129  const bool onRoad = v->isOnRoad();
130  switch (variable) {
131  case VAR_SPEED:
132  tempMsg.writeUnsignedByte(TYPE_DOUBLE);
133  tempMsg.writeDouble(onRoad ? v->getSpeed() : INVALID_DOUBLE_VALUE);
134  break;
135  case VAR_SPEED_WITHOUT_TRACI:
136  tempMsg.writeUnsignedByte(TYPE_DOUBLE);
137  tempMsg.writeDouble(onRoad ? v->getSpeedWithoutTraciInfluence() : INVALID_DOUBLE_VALUE);
138  break;
139  case VAR_POSITION:
140  tempMsg.writeUnsignedByte(POSITION_2D);
141  tempMsg.writeDouble(onRoad ? v->getPosition().x() : INVALID_DOUBLE_VALUE);
142  tempMsg.writeDouble(onRoad ? v->getPosition().y() : INVALID_DOUBLE_VALUE);
143  break;
144  case VAR_ANGLE:
145  tempMsg.writeUnsignedByte(TYPE_DOUBLE);
146  tempMsg.writeDouble(onRoad ? v->getAngle() : INVALID_DOUBLE_VALUE);
147  break;
148  case VAR_ROAD_ID:
149  tempMsg.writeUnsignedByte(TYPE_STRING);
150  tempMsg.writeString(onRoad ? v->getLane()->getEdge().getID() : "");
151  break;
152  case VAR_LANE_ID:
153  tempMsg.writeUnsignedByte(TYPE_STRING);
154  tempMsg.writeString(onRoad ? v->getLane()->getID() : "");
155  break;
156  case VAR_LANE_INDEX:
157  tempMsg.writeUnsignedByte(TYPE_INTEGER);
158  if (onRoad) {
159  const std::vector<MSLane*>& lanes = v->getLane()->getEdge().getLanes();
160  tempMsg.writeInt((int)std::distance(lanes.begin(), std::find(lanes.begin(), lanes.end(), v->getLane())));
161  } else {
162  tempMsg.writeInt(INVALID_INT_VALUE);
163  }
164  break;
165  case VAR_TYPE:
166  tempMsg.writeUnsignedByte(TYPE_STRING);
167  tempMsg.writeString(v->getVehicleType().getID());
168  break;
169  case VAR_ROUTE_ID:
170  tempMsg.writeUnsignedByte(TYPE_STRING);
171  tempMsg.writeString(v->getRoute().getID());
172  break;
173  case VAR_COLOR:
174  tempMsg.writeUnsignedByte(TYPE_COLOR);
175  tempMsg.writeUnsignedByte(v->getParameter().color.red());
176  tempMsg.writeUnsignedByte(v->getParameter().color.green());
177  tempMsg.writeUnsignedByte(v->getParameter().color.blue());
178  tempMsg.writeUnsignedByte(v->getParameter().color.alpha());
179  break;
180  case VAR_LANEPOSITION:
181  tempMsg.writeUnsignedByte(TYPE_DOUBLE);
182  tempMsg.writeDouble(onRoad ? v->getPositionOnLane() : INVALID_DOUBLE_VALUE);
183  break;
184  case VAR_CO2EMISSION:
185  tempMsg.writeUnsignedByte(TYPE_DOUBLE);
186  tempMsg.writeDouble(onRoad ? HelpersHBEFA::computeCO2(v->getVehicleType().getEmissionClass(), v->getSpeed(), v->getAcceleration()) : INVALID_DOUBLE_VALUE);
187  break;
188  case VAR_COEMISSION:
189  tempMsg.writeUnsignedByte(TYPE_DOUBLE);
190  tempMsg.writeDouble(onRoad ? HelpersHBEFA::computeCO(v->getVehicleType().getEmissionClass(), v->getSpeed(), v->getAcceleration()) : INVALID_DOUBLE_VALUE);
191  break;
192  case VAR_HCEMISSION:
193  tempMsg.writeUnsignedByte(TYPE_DOUBLE);
194  tempMsg.writeDouble(onRoad ? HelpersHBEFA::computeHC(v->getVehicleType().getEmissionClass(), v->getSpeed(), v->getAcceleration()) : INVALID_DOUBLE_VALUE);
195  break;
196  case VAR_PMXEMISSION:
197  tempMsg.writeUnsignedByte(TYPE_DOUBLE);
198  tempMsg.writeDouble(onRoad ? HelpersHBEFA::computePMx(v->getVehicleType().getEmissionClass(), v->getSpeed(), v->getAcceleration()) : INVALID_DOUBLE_VALUE);
199  break;
200  case VAR_NOXEMISSION:
201  tempMsg.writeUnsignedByte(TYPE_DOUBLE);
202  tempMsg.writeDouble(onRoad ? HelpersHBEFA::computeNOx(v->getVehicleType().getEmissionClass(), v->getSpeed(), v->getAcceleration()) : INVALID_DOUBLE_VALUE);
203  break;
204  case VAR_FUELCONSUMPTION:
205  tempMsg.writeUnsignedByte(TYPE_DOUBLE);
206  tempMsg.writeDouble(onRoad ? HelpersHBEFA::computeFuel(v->getVehicleType().getEmissionClass(), v->getSpeed(), v->getAcceleration()) : INVALID_DOUBLE_VALUE);
207  break;
208  case VAR_NOISEEMISSION:
209  tempMsg.writeUnsignedByte(TYPE_DOUBLE);
210  tempMsg.writeDouble(onRoad ? HelpersHarmonoise::computeNoise(v->getVehicleType().getEmissionClass(), v->getSpeed(), v->getAcceleration()) : INVALID_DOUBLE_VALUE);
211  break;
212  case VAR_PERSON_NUMBER:
213  tempMsg.writeUnsignedByte(TYPE_INTEGER);
214  tempMsg.writeInt(v->getPersonNumber());
215  break;
216  case VAR_EDGE_TRAVELTIME: {
217  if (inputStorage.readUnsignedByte() != TYPE_COMPOUND) {
218  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Retrieval of travel time requires a compound object.", outputStorage);
219  }
220  if (inputStorage.readInt() != 2) {
221  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Retrieval of travel time requires time, and edge as parameter.", outputStorage);
222  }
223  // time
224  SUMOTime time = 0;
225  if (!server.readTypeCheckingInt(inputStorage, time)) {
226  return server.writeErrorStatusCmd(CMD_GET_VEHICLE_VARIABLE, "Retrieval of travel time requires the referenced time as first parameter.", outputStorage);
227  }
228  // edge
229  std::string edgeID;
230  if (!server.readTypeCheckingString(inputStorage, edgeID)) {
231  return server.writeErrorStatusCmd(CMD_GET_VEHICLE_VARIABLE, "Retrieval of travel time requires the referenced edge as second parameter.", outputStorage);
232  }
233  MSEdge* edge = MSEdge::dictionary(edgeID);
234  if (edge == 0) {
235  return server.writeErrorStatusCmd(CMD_GET_VEHICLE_VARIABLE, "Referenced edge '" + edgeID + "' is not known.", outputStorage);
236  }
237  // retrieve
238  tempMsg.writeUnsignedByte(TYPE_DOUBLE);
239  SUMOReal value;
240  if (!v->getWeightsStorage().retrieveExistingTravelTime(edge, 0, time, value)) {
241  tempMsg.writeDouble(INVALID_DOUBLE_VALUE);
242  } else {
243  tempMsg.writeDouble(value);
244  }
245 
246  }
247  break;
248  case VAR_EDGE_EFFORT: {
249  if (inputStorage.readUnsignedByte() != TYPE_COMPOUND) {
250  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Retrieval of travel time requires a compound object.", outputStorage);
251  }
252  if (inputStorage.readInt() != 2) {
253  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Retrieval of travel time requires time, and edge as parameter.", outputStorage);
254  }
255  // time
256  SUMOTime time = 0;
257  if (!server.readTypeCheckingInt(inputStorage, time)) {
258  return server.writeErrorStatusCmd(CMD_GET_VEHICLE_VARIABLE, "Retrieval of effort requires the referenced time as first parameter.", outputStorage);
259  }
260  // edge
261  std::string edgeID;
262  if (!server.readTypeCheckingString(inputStorage, edgeID)) {
263  return server.writeErrorStatusCmd(CMD_GET_VEHICLE_VARIABLE, "Retrieval of effort requires the referenced edge as second parameter.", outputStorage);
264  }
265  MSEdge* edge = MSEdge::dictionary(edgeID);
266  if (edge == 0) {
267  return server.writeErrorStatusCmd(CMD_GET_VEHICLE_VARIABLE, "Referenced edge '" + edgeID + "' is not known.", outputStorage);
268  }
269  // retrieve
270  tempMsg.writeUnsignedByte(TYPE_DOUBLE);
271  SUMOReal value;
272  if (!v->getWeightsStorage().retrieveExistingEffort(edge, 0, time, value)) {
273  tempMsg.writeDouble(INVALID_DOUBLE_VALUE);
274  } else {
275  tempMsg.writeDouble(value);
276  }
277 
278  }
279  break;
280  case VAR_ROUTE_VALID: {
281  std::string msg;
282  tempMsg.writeUnsignedByte(TYPE_UBYTE);
283  tempMsg.writeUnsignedByte(v->hasValidRoute(msg));
284  }
285  break;
286  case VAR_EDGES: {
287  const MSRoute& r = v->getRoute();
288  tempMsg.writeUnsignedByte(TYPE_STRINGLIST);
289  tempMsg.writeInt(r.size());
290  for (MSRouteIterator i = r.begin(); i != r.end(); ++i) {
291  tempMsg.writeString((*i)->getID());
292  }
293  }
294  break;
295  case VAR_SIGNALS:
296  tempMsg.writeUnsignedByte(TYPE_INTEGER);
297  tempMsg.writeInt(v->getSignals());
298  break;
299  case VAR_BEST_LANES: {
300  tempMsg.writeUnsignedByte(TYPE_COMPOUND);
301  tcpip::Storage tempContent;
302  unsigned int cnt = 0;
303  tempContent.writeUnsignedByte(TYPE_INTEGER);
304  const std::vector<MSVehicle::LaneQ>& bestLanes = onRoad ? v->getBestLanes() : std::vector<MSVehicle::LaneQ>();
305  tempContent.writeInt((int) bestLanes.size());
306  ++cnt;
307  for (std::vector<MSVehicle::LaneQ>::const_iterator i = bestLanes.begin(); i != bestLanes.end(); ++i) {
308  const MSVehicle::LaneQ& lq = *i;
309  tempContent.writeUnsignedByte(TYPE_STRING);
310  tempContent.writeString(lq.lane->getID());
311  ++cnt;
312  tempContent.writeUnsignedByte(TYPE_DOUBLE);
313  tempContent.writeDouble(lq.length);
314  ++cnt;
315  tempContent.writeUnsignedByte(TYPE_DOUBLE);
316  tempContent.writeDouble(lq.nextOccupation);
317  ++cnt;
318  tempContent.writeUnsignedByte(TYPE_BYTE);
319  tempContent.writeByte(lq.bestLaneOffset);
320  ++cnt;
321  tempContent.writeUnsignedByte(TYPE_UBYTE);
322  lq.allowsContinuation ? tempContent.writeUnsignedByte(1) : tempContent.writeUnsignedByte(0);
323  ++cnt;
324  std::vector<std::string> bestContIDs;
325  for (std::vector<MSLane*>::const_iterator j = lq.bestContinuations.begin(); j != lq.bestContinuations.end(); ++j) {
326  bestContIDs.push_back((*j)->getID());
327  }
328  tempContent.writeUnsignedByte(TYPE_STRINGLIST);
329  tempContent.writeStringList(bestContIDs);
330  ++cnt;
331  }
332  tempMsg.writeInt((int) cnt);
333  tempMsg.writeStorage(tempContent);
334  }
335  break;
336  case DISTANCE_REQUEST:
337  if (!commandDistanceRequest(server, inputStorage, tempMsg, v)) {
338  return false;
339  }
340  break;
341  default:
342  TraCIServerAPI_VehicleType::getVariable(variable, v->getVehicleType(), tempMsg);
343  break;
344  }
345  }
346  server.writeStatusCmd(CMD_GET_VEHICLE_VARIABLE, RTYPE_OK, "", outputStorage);
347  server.writeResponseWithLength(outputStorage, tempMsg);
348  return true;
349 }
350 
351 
352 bool
353 TraCIServerAPI_Vehicle::processSet(TraCIServer& server, tcpip::Storage& inputStorage,
354  tcpip::Storage& outputStorage) {
355  std::string warning = ""; // additional description for response
356  // variable
357  int variable = inputStorage.readUnsignedByte();
358  if (variable != CMD_STOP && variable != CMD_CHANGELANE
359  && variable != CMD_SLOWDOWN && variable != CMD_CHANGETARGET
360  && variable != VAR_ROUTE_ID && variable != VAR_ROUTE
361  && variable != VAR_EDGE_TRAVELTIME && variable != VAR_EDGE_EFFORT
362  && variable != CMD_REROUTE_TRAVELTIME && variable != CMD_REROUTE_EFFORT
363  && variable != VAR_SIGNALS && variable != VAR_MOVE_TO
364  && variable != VAR_LENGTH && variable != VAR_MAXSPEED && variable != VAR_VEHICLECLASS
365  && variable != VAR_SPEED_FACTOR && variable != VAR_SPEED_DEVIATION && variable != VAR_EMISSIONCLASS
366  && variable != VAR_WIDTH && variable != VAR_MINGAP && variable != VAR_SHAPECLASS
367  && variable != VAR_ACCEL && variable != VAR_DECEL && variable != VAR_IMPERFECTION
368  && variable != VAR_TAU
369  && variable != VAR_SPEED && variable != VAR_SPEEDSETMODE && variable != VAR_COLOR
370  && variable != ADD && variable != REMOVE
371  && variable != VAR_MOVE_TO_VTD
372  ) {
373  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Change Vehicle State: unsupported variable specified", outputStorage);
374  }
375  // id
376  std::string id = inputStorage.readString();
377  const bool shouldExist = variable != ADD;
378  SUMOVehicle* sumoVehicle = MSNet::getInstance()->getVehicleControl().getVehicle(id);
379  if (sumoVehicle == 0) {
380  if (shouldExist) {
381  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Vehicle '" + id + "' is not known", outputStorage);
382  }
383  }
384  MSVehicle* v = dynamic_cast<MSVehicle*>(sumoVehicle);
385  if (v == 0 && shouldExist) {
386  return server.writeErrorStatusCmd(CMD_GET_VEHICLE_VARIABLE, "Vehicle '" + id + "' is not a micro-simulation vehicle", outputStorage);
387  }
388  switch (variable) {
389  case CMD_STOP: {
390  if (inputStorage.readUnsignedByte() != TYPE_COMPOUND) {
391  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Stop needs a compound object description.", outputStorage);
392  }
393  if (inputStorage.readInt() != 4) {
394  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Stop needs a compound object description of four items.", outputStorage);
395  }
396  // read road map position
397  std::string roadId;
398  if (!server.readTypeCheckingString(inputStorage, roadId)) {
399  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "The first stop parameter must be the edge id given as a string.", outputStorage);
400  }
401  double pos = 0;
402  if (!server.readTypeCheckingDouble(inputStorage, pos)) {
403  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "The second stop parameter must be the position along the edge given as a double.", outputStorage);
404  }
405  int laneIndex = 0;
406  if (!server.readTypeCheckingByte(inputStorage, laneIndex)) {
407  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "The third stop parameter must be the lane index given as a byte.", outputStorage);
408  }
409  // waitTime
410  SUMOTime waitTime = 0;
411  if (!server.readTypeCheckingInt(inputStorage, waitTime)) {
412  return server.writeErrorStatusCmd(CMD_GET_VEHICLE_VARIABLE, "The fourth stop parameter must be the waiting time given as an integer.", outputStorage);
413  }
414  // check
415  if (pos < 0) {
416  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Position on lane must not be negative", outputStorage);
417  }
418  // get the actual lane that is referenced by laneIndex
419  MSEdge* road = MSEdge::dictionary(roadId);
420  if (road == 0) {
421  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Unable to retrieve road with given id", outputStorage);
422  }
423  const std::vector<MSLane*>& allLanes = road->getLanes();
424  if ((laneIndex < 0) || laneIndex >= (int)(allLanes.size())) {
425  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "No lane existing with such id on the given road", outputStorage);
426  }
427  // Forward command to vehicle
428  if (!v->addTraciStop(allLanes[laneIndex], pos, 0, waitTime)) {
429  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Vehicle is too close or behind the stop on " + allLanes[laneIndex]->getID(), outputStorage);
430  }
431  }
432  break;
433  case CMD_CHANGELANE: {
434  if (inputStorage.readUnsignedByte() != TYPE_COMPOUND) {
435  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Lane change needs a compound object description.", outputStorage);
436  }
437  if (inputStorage.readInt() != 2) {
438  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Lane change needs a compound object description of two items.", outputStorage);
439  }
440  // Lane ID
441  int laneIndex = 0;
442  if (!server.readTypeCheckingByte(inputStorage, laneIndex)) {
443  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "The first lane change parameter must be the lane index given as a byte.", outputStorage);
444  }
445  // stickyTime
446  SUMOTime stickyTime = 0;
447  if (!server.readTypeCheckingInt(inputStorage, stickyTime)) {
448  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "The second lane change parameter must be the duration given as an integer.", outputStorage);
449  }
450  if ((laneIndex < 0) || (laneIndex >= (int)(v->getEdge()->getLanes().size()))) {
451  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "No lane existing with given id on the current road", outputStorage);
452  }
453  // Forward command to vehicle
454  std::vector<std::pair<SUMOTime, unsigned int> > laneTimeLine;
455  laneTimeLine.push_back(std::make_pair(MSNet::getInstance()->getCurrentTimeStep(), laneIndex));
456  laneTimeLine.push_back(std::make_pair(MSNet::getInstance()->getCurrentTimeStep() + stickyTime, laneIndex));
457  v->getInfluencer().setLaneTimeLine(laneTimeLine);
458  MSVehicle::ChangeRequest req = v->getInfluencer().checkForLaneChanges(MSNet::getInstance()->getCurrentTimeStep(),
459  *v->getEdge(), v->getLaneIndex());
460  v->getLaneChangeModel().requestLaneChange(req);
461  }
462  break;
463  case CMD_SLOWDOWN: {
464  if (inputStorage.readUnsignedByte() != TYPE_COMPOUND) {
465  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Slow down needs a compound object description.", outputStorage);
466  }
467  if (inputStorage.readInt() != 2) {
468  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Slow down needs a compound object description of two items.", outputStorage);
469  }
470  double newSpeed = 0;
471  if (!server.readTypeCheckingDouble(inputStorage, newSpeed)) {
472  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "The first slow down parameter must be the speed given as a double.", outputStorage);
473  }
474  if (newSpeed < 0) {
475  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Speed must not be negative", outputStorage);
476  }
477  SUMOTime duration = 0;
478  if (!server.readTypeCheckingInt(inputStorage, duration)) {
479  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "The second slow down parameter must be the duration given as an integer.", outputStorage);
480  }
481  if (duration < 0) {
482  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Invalid time interval", outputStorage);
483  }
484  std::vector<std::pair<SUMOTime, SUMOReal> > speedTimeLine;
485  speedTimeLine.push_back(std::make_pair(MSNet::getInstance()->getCurrentTimeStep(), v->getSpeed()));
486  speedTimeLine.push_back(std::make_pair(MSNet::getInstance()->getCurrentTimeStep() + duration, newSpeed));
487  v->getInfluencer().setSpeedTimeLine(speedTimeLine);
488  }
489  break;
490  case CMD_CHANGETARGET: {
491  std::string edgeID;
492  if (!server.readTypeCheckingString(inputStorage, edgeID)) {
493  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Change target requires a string containing the id of the new destination edge as parameter.", outputStorage);
494  }
495  const MSEdge* destEdge = MSEdge::dictionary(edgeID);
496  if (destEdge == 0) {
497  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Can not retrieve road with ID " + edgeID, outputStorage);
498  }
499  // build a new route between the vehicle's current edge and destination edge
500  MSEdgeVector newRoute;
501  const MSEdge* currentEdge = v->getEdge();
502  MSNet::getInstance()->getRouterTT().compute(
503  currentEdge, destEdge, (const MSVehicle * const) v, MSNet::getInstance()->getCurrentTimeStep(), newRoute);
504  // replace the vehicle's route by the new one
505  if (!v->replaceRouteEdges(newRoute)) {
506  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Route replacement failed for " + v->getID(), outputStorage);
507  }
508  }
509  break;
510  case VAR_ROUTE_ID: {
511  std::string rid;
512  if (!server.readTypeCheckingString(inputStorage, rid)) {
513  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "The route id must be given as a string.", outputStorage);
514  }
515  const MSRoute* r = MSRoute::dictionary(rid);
516  if (r == 0) {
517  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "The route '" + rid + "' is not known.", outputStorage);
518  }
519  if (!v->replaceRoute(r)) {
520  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Route replacement failed for " + v->getID(), outputStorage);
521  }
522  }
523  break;
524  case VAR_ROUTE: {
525  std::vector<std::string> edgeIDs;
526  if (!server.readTypeCheckingStringList(inputStorage, edgeIDs)) {
527  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "A route must be defined as a list of edge ids.", outputStorage);
528  }
529  std::vector<const MSEdge*> edges;
530  MSEdge::parseEdgesList(edgeIDs, edges, "<unknown>");
531  if (!v->replaceRouteEdges(edges)) {
532  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Route replacement failed for " + v->getID(), outputStorage);
533  }
534  }
535  break;
536  case VAR_EDGE_TRAVELTIME: {
537  if (inputStorage.readUnsignedByte() != TYPE_COMPOUND) {
538  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting travel time requires a compound object.", outputStorage);
539  }
540  int parameterCount = inputStorage.readInt();
541  if (parameterCount == 4) {
542  // begin time
543  SUMOTime begTime = 0, endTime = 0;
544  if (!server.readTypeCheckingInt(inputStorage, begTime)) {
545  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting travel time using 4 parameters requires the begin time as first parameter.", outputStorage);
546  }
547  // begin time
548  if (!server.readTypeCheckingInt(inputStorage, endTime)) {
549  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting travel time using 4 parameters requires the end time as second parameter.", outputStorage);
550  }
551  // edge
552  std::string edgeID;
553  if (!server.readTypeCheckingString(inputStorage, edgeID)) {
554  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting travel time using 4 parameters requires the referenced edge as third parameter.", outputStorage);
555  }
556  MSEdge* edge = MSEdge::dictionary(edgeID);
557  if (edge == 0) {
558  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Referenced edge '" + edgeID + "' is not known.", outputStorage);
559  }
560  // value
561  double value = 0;
562  if (!server.readTypeCheckingDouble(inputStorage, value)) {
563  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting travel time using 4 parameters requires the travel time as double as fourth parameter.", outputStorage);
564  }
565  // retrieve
566  v->getWeightsStorage().addTravelTime(edge, begTime, endTime, value);
567  } else if (parameterCount == 2) {
568  // edge
569  std::string edgeID;
570  if (!server.readTypeCheckingString(inputStorage, edgeID)) {
571  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting travel time using 2 parameters requires the referenced edge as first parameter.", outputStorage);
572  }
573  MSEdge* edge = MSEdge::dictionary(edgeID);
574  if (edge == 0) {
575  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Referenced edge '" + edgeID + "' is not known.", outputStorage);
576  }
577  // value
578  double value = 0;
579  if (!server.readTypeCheckingDouble(inputStorage, value)) {
580  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting travel time using 2 parameters requires the travel time as second parameter.", outputStorage);
581  }
582  // retrieve
583  while (v->getWeightsStorage().knowsTravelTime(edge)) {
584  v->getWeightsStorage().removeTravelTime(edge);
585  }
586  v->getWeightsStorage().addTravelTime(edge, 0, SUMOTime_MAX, value);
587  } else if (parameterCount == 1) {
588  // edge
589  std::string edgeID;
590  if (!server.readTypeCheckingString(inputStorage, edgeID)) {
591  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting travel time using 1 parameter requires the referenced edge as first parameter.", outputStorage);
592  }
593  MSEdge* edge = MSEdge::dictionary(edgeID);
594  if (edge == 0) {
595  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Referenced edge '" + edgeID + "' is not known.", outputStorage);
596  }
597  // retrieve
598  while (v->getWeightsStorage().knowsTravelTime(edge)) {
599  v->getWeightsStorage().removeTravelTime(edge);
600  }
601  } else {
602  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting travel time requires 1, 2, or 4 parameters.", outputStorage);
603  }
604  }
605  break;
606  case VAR_EDGE_EFFORT: {
607  if (inputStorage.readUnsignedByte() != TYPE_COMPOUND) {
608  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting effort requires a compound object.", outputStorage);
609  }
610  int parameterCount = inputStorage.readInt();
611  if (parameterCount == 4) {
612  // begin time
613  SUMOTime begTime = 0, endTime = 0;
614  if (!server.readTypeCheckingInt(inputStorage, begTime)) {
615  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting effort using 4 parameters requires the begin time as first parameter.", outputStorage);
616  }
617  // begin time
618  if (!server.readTypeCheckingInt(inputStorage, endTime)) {
619  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting effort using 4 parameters requires the end time as second parameter.", outputStorage);
620  }
621  // edge
622  std::string edgeID;
623  if (!server.readTypeCheckingString(inputStorage, edgeID)) {
624  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting effort using 4 parameters requires the referenced edge as third parameter.", outputStorage);
625  }
626  MSEdge* edge = MSEdge::dictionary(edgeID);
627  if (edge == 0) {
628  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Referenced edge '" + edgeID + "' is not known.", outputStorage);
629  }
630  // value
631  double value = 0;
632  if (!server.readTypeCheckingDouble(inputStorage, value)) {
633  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting effort using 4 parameters requires the travel time as fourth parameter.", outputStorage);
634  }
635  // retrieve
636  v->getWeightsStorage().addEffort(edge, begTime, endTime, value);
637  } else if (parameterCount == 2) {
638  // edge
639  std::string edgeID;
640  if (!server.readTypeCheckingString(inputStorage, edgeID)) {
641  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting effort using 2 parameters requires the referenced edge as first parameter.", outputStorage);
642  }
643  MSEdge* edge = MSEdge::dictionary(edgeID);
644  if (edge == 0) {
645  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Referenced edge '" + edgeID + "' is not known.", outputStorage);
646  }
647  // value
648  double value = 0;
649  if (!server.readTypeCheckingDouble(inputStorage, value)) {
650  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting effort using 2 parameters requires the travel time as second parameter.", outputStorage);
651  }
652  // retrieve
653  while (v->getWeightsStorage().knowsEffort(edge)) {
654  v->getWeightsStorage().removeEffort(edge);
655  }
656  v->getWeightsStorage().addEffort(edge, 0, SUMOTime_MAX, value);
657  } else if (parameterCount == 1) {
658  // edge
659  std::string edgeID;
660  if (!server.readTypeCheckingString(inputStorage, edgeID)) {
661  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting effort using 1 parameter requires the referenced edge as first parameter.", outputStorage);
662  }
663  MSEdge* edge = MSEdge::dictionary(edgeID);
664  if (edge == 0) {
665  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Referenced edge '" + edgeID + "' is not known.", outputStorage);
666  }
667  // retrieve
668  while (v->getWeightsStorage().knowsEffort(edge)) {
669  v->getWeightsStorage().removeEffort(edge);
670  }
671  } else {
672  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting effort requires 1, 2, or 4 parameters.", outputStorage);
673  }
674  }
675  break;
676  case CMD_REROUTE_TRAVELTIME: {
677  if (inputStorage.readUnsignedByte() != TYPE_COMPOUND) {
678  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Rerouting requires a compound object.", outputStorage);
679  }
680  if (inputStorage.readInt() != 0) {
681  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Rerouting should obtain an empty compound object.", outputStorage);
682  }
683  v->reroute(MSNet::getInstance()->getCurrentTimeStep(), MSNet::getInstance()->getRouterTT());
684  }
685  break;
686  case CMD_REROUTE_EFFORT: {
687  if (inputStorage.readUnsignedByte() != TYPE_COMPOUND) {
688  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Rerouting requires a compound object.", outputStorage);
689  }
690  if (inputStorage.readInt() != 0) {
691  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Rerouting should obtain an empty compound object.", outputStorage);
692  }
693  v->reroute(MSNet::getInstance()->getCurrentTimeStep(), MSNet::getInstance()->getRouterEffort());
694  }
695  break;
696  case VAR_SIGNALS: {
697  int signals = 0;
698  if (!server.readTypeCheckingInt(inputStorage, signals)) {
699  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting signals requires an integer.", outputStorage);
700  }
701  v->switchOffSignal(0x0fffffff);
702  v->switchOnSignal(signals);
703  }
704  break;
705  case VAR_MOVE_TO: {
706  if (inputStorage.readUnsignedByte() != TYPE_COMPOUND) {
707  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting position requires a compound object.", outputStorage);
708  }
709  if (inputStorage.readInt() != 2) {
710  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting position should obtain the lane id and the position.", outputStorage);
711  }
712  // lane ID
713  std::string laneID;
714  if (!server.readTypeCheckingString(inputStorage, laneID)) {
715  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "The first parameter for setting a position must be the lane ID given as a string.", outputStorage);
716  }
717  // position on lane
718  double position = 0;
719  if (!server.readTypeCheckingDouble(inputStorage, position)) {
720  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "The second parameter for setting a position must be the position given as a double.", outputStorage);
721  }
722  // process
723  MSLane* l = MSLane::dictionary(laneID);
724  if (l == 0) {
725  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Unknown lane '" + laneID + "'.", outputStorage);
726  }
727  MSEdge& destinationEdge = l->getEdge();
728  if (!v->willPass(&destinationEdge)) {
729  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Vehicle '" + laneID + "' may be set onto an edge to pass only.", outputStorage);
730  }
731  v->onRemovalFromNet(MSMoveReminder::NOTIFICATION_TELEPORT);
732  v->getLane()->removeVehicle(v);
733  while (v->getEdge() != &destinationEdge) {
734  const MSEdge* nextEdge = v->succEdge(1);
735  // let the vehicle move to the next edge
736  if (v->enterLaneAtMove(nextEdge->getLanes()[0], true)) {
737  MSNet::getInstance()->getVehicleControl().scheduleVehicleRemoval(v);
738  continue;
739  }
740  }
741  l->forceVehicleInsertion(v, position);
742  }
743  break;
744  case VAR_SPEED: {
745  double speed = 0;
746  if (!server.readTypeCheckingDouble(inputStorage, speed)) {
747  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting speed requires a double.", outputStorage);
748  }
749  std::vector<std::pair<SUMOTime, SUMOReal> > speedTimeLine;
750  if (speed >= 0) {
751  speedTimeLine.push_back(std::make_pair(MSNet::getInstance()->getCurrentTimeStep(), speed));
752  speedTimeLine.push_back(std::make_pair(SUMOTime_MAX, speed));
753  }
754  v->getInfluencer().setSpeedTimeLine(speedTimeLine);
755  }
756  break;
757  case VAR_SPEEDSETMODE: {
758  int speedMode = 0;
759  if (!server.readTypeCheckingInt(inputStorage, speedMode)) {
760  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting speed mode requires an integer.", outputStorage);
761  }
762  v->getInfluencer().setConsiderSafeVelocity((speedMode & 1) != 0);
763  v->getInfluencer().setConsiderMaxAcceleration((speedMode & 2) != 0);
764  v->getInfluencer().setConsiderMaxDeceleration((speedMode & 4) != 0);
765  }
766  break;
767  case VAR_COLOR: {
768  RGBColor col;
769  if (!server.readTypeCheckingColor(inputStorage, col)) {
770  return server.writeErrorStatusCmd(CMD_SET_POLYGON_VARIABLE, "The color must be given using the according type.", outputStorage);
771  }
772  v->getParameter().color.set(col.red(), col.green(), col.blue(), col.alpha());
773  v->getParameter().setParameter |= VEHPARS_COLOR_SET;
774  }
775  break;
776  case ADD: {
777  if (v != 0) {
778  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "The vehicle " + id + " to add already exists.", outputStorage);
779  }
780  if (inputStorage.readUnsignedByte() != TYPE_COMPOUND) {
781  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Adding a vehicle requires a compound object.", outputStorage);
782  }
783  if (inputStorage.readInt() != 6) {
784  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Adding a vehicle needs six parameters.", outputStorage);
785  }
786  SUMOVehicleParameter vehicleParams;
787  vehicleParams.id = id;
788 
789  std::string vTypeID;
790  if (!server.readTypeCheckingString(inputStorage, vTypeID)) {
791  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "First parameter (type) requires a string.", outputStorage);
792  }
793  MSVehicleType* vehicleType = MSNet::getInstance()->getVehicleControl().getVType(vTypeID);
794  if (!vehicleType) {
795  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Invalid type '" + vTypeID + "' for vehicle '" + id + "'", outputStorage);
796  }
797 
798  std::string routeID;
799  if (!server.readTypeCheckingString(inputStorage, routeID)) {
800  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Second parameter (route) requires a string.", outputStorage);
801  }
802  const MSRoute* route = MSRoute::dictionary(routeID);
803  if (!route) {
804  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Invalid route '" + routeID + "' for vehicle: '" + id + "'", outputStorage);
805  }
806 
807  if (!server.readTypeCheckingInt(inputStorage, vehicleParams.depart)) {
808  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Third parameter (depart) requires an integer.", outputStorage);
809  }
810  if (vehicleParams.depart < 0) {
811  const int proc = static_cast<int>(-vehicleParams.depart);
812  if (proc >= static_cast<int>(DEPART_DEF_MAX)) {
813  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Invalid departure time.", outputStorage);
814  }
815  vehicleParams.departProcedure = (DepartDefinition)proc;
816  }
817 
818  double pos;
819  if (!server.readTypeCheckingDouble(inputStorage, pos)) {
820  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Fourth parameter (position) requires a double.", outputStorage);
821  }
822  vehicleParams.departPos = pos;
823  if (vehicleParams.departPos < 0) {
824  const int proc = static_cast<int>(-vehicleParams.departPos);
825  if (proc >= static_cast<int>(DEPART_POS_DEF_MAX)) {
826  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Invalid departure position.", outputStorage);
827  }
828  vehicleParams.departPosProcedure = (DepartPosDefinition)proc;
829  } else {
830  vehicleParams.departPosProcedure = DEPART_POS_GIVEN;
831  }
832 
833  double speed;
834  if (!server.readTypeCheckingDouble(inputStorage, speed)) {
835  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Fifth parameter (speed) requires a double.", outputStorage);
836  }
837  vehicleParams.departSpeed = speed;
838  if (vehicleParams.departSpeed < 0) {
839  const int proc = static_cast<int>(-vehicleParams.departSpeed);
840  if (proc >= static_cast<int>(DEPART_SPEED_DEF_MAX)) {
841  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Invalid departure speed.", outputStorage);
842  }
843  vehicleParams.departSpeedProcedure = (DepartSpeedDefinition)proc;
844  } else {
845  vehicleParams.departSpeedProcedure = DEPART_SPEED_GIVEN;
846  }
847 
848  if (!server.readTypeCheckingByte(inputStorage, vehicleParams.departLane)) {
849  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Sixth parameter (lane) requires a byte.", outputStorage);
850  }
851 
852  if (vehicleParams.departLane < 0) {
853  const int proc = static_cast<int>(-vehicleParams.departLane);
854  if (proc >= static_cast<int>(DEPART_LANE_DEF_MAX)) {
855  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Invalid departure lane.", outputStorage);
856  }
857  vehicleParams.departLaneProcedure = (DepartLaneDefinition)proc;
858  } else {
859  vehicleParams.departLaneProcedure = DEPART_LANE_GIVEN;
860  }
861 
862  SUMOVehicleParameter* params = new SUMOVehicleParameter();
863  *params = vehicleParams;
864  try {
865  SUMOVehicle* vehicle = MSNet::getInstance()->getVehicleControl().buildVehicle(params, route, vehicleType);
866  MSNet::getInstance()->getVehicleControl().addVehicle(vehicleParams.id, vehicle);
867  MSNet::getInstance()->getInsertionControl().add(vehicle);
868  } catch (ProcessError& e) {
869  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, e.what(), outputStorage);
870  }
871  }
872  break;
873  case REMOVE: {
874  int why = 0;
875  if (!server.readTypeCheckingByte(inputStorage, why)) {
876  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Removing a vehicle requires a byte.", outputStorage);
877  }
878  MSMoveReminder::Notification n = MSMoveReminder::NOTIFICATION_ARRIVED;
879  switch (why) {
880  case REMOVE_TELEPORT:
881  n = MSMoveReminder::NOTIFICATION_TELEPORT;
882  break;
883  case REMOVE_PARKING:
884  n = MSMoveReminder::NOTIFICATION_PARKING;
885  break;
886  case REMOVE_ARRIVED:
887  n = MSMoveReminder::NOTIFICATION_ARRIVED;
888  break;
889  case REMOVE_VAPORIZED:
890  n = MSMoveReminder::NOTIFICATION_VAPORIZED;
891  break;
892  case REMOVE_TELEPORT_ARRIVED:
893  n = MSMoveReminder::NOTIFICATION_TELEPORT_ARRIVED;
894  break;
895  default:
896  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Unknown removal status.", outputStorage);
897  }
898  if(v->hasDeparted()) {
899  v->onRemovalFromNet(n);
900  if(v->getLane()!=0) {
901  v->getLane()->removeVehicle(v);
902  }
903  MSNet::getInstance()->getVehicleControl().scheduleVehicleRemoval(v);
904  }
905  }
906  break;
907  case VAR_MOVE_TO_VTD: {
908  if (inputStorage.readUnsignedByte() != TYPE_COMPOUND) {
909  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting VTD vehicle requires a compound object.", outputStorage);
910  }
911  if (inputStorage.readInt() != 4) {
912  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Setting VTD vehicle should obtain: edgeID, lane, x, y.", outputStorage);
913  }
914  // edge ID
915  std::string edgeID;
916  if (!server.readTypeCheckingString(inputStorage, edgeID)) {
917  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);
918  }
919  // lane index
920  int laneNum = 0;
921  if (!server.readTypeCheckingInt(inputStorage, laneNum)) {
922  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "The second parameter for setting a VTD vehicle must be lane given as an int.", outputStorage);
923  }
924  // x
925  double x = 0, y = 0;
926  if (!server.readTypeCheckingDouble(inputStorage, x)) {
927  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);
928  }
929  // y
930  if (!server.readTypeCheckingDouble(inputStorage, y)) {
931  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);
932  }
933  // process
934  if (!v->isOnRoad()) {
935  break;
936  }
937  std::string origID = edgeID + " " + toString(laneNum);
938  if (laneNum < 0) {
939  edgeID = '-' + edgeID;
940  laneNum = -laneNum;
941  }
942  Position pos(x, y);
943 
944  Position vehPos = v->getPosition();
945  v->getBestLanes();
946  bool report = server.vtdDebug();
947  if(report) std::cout << std::endl << "begin vehicle " << v->getID() << " vehPos:" << vehPos << " lane:" << v->getLane()->getID() << std::endl;
948  if(report) std::cout << " want pos:" << pos << " edge:" << edgeID << " laneNum:" << laneNum << std::endl;
949 
950  MSEdgeVector edgesA, edgesB, edgesC;
951  MSLane *laneA, *laneB, *laneC;
952  laneA = laneB = laneC = 0;
953  SUMOReal lanePosA, lanePosB, lanePosC;
954  SUMOReal bestDistanceA, bestDistanceB, bestDistanceC;
955  bestDistanceA = bestDistanceB = bestDistanceC = 1000.;//pos.distanceSquaredTo2D(vehPos);
956  int routeOffsetA, routeOffsetB, routeOffsetC;
957  routeOffsetA = routeOffsetB = routeOffsetC = 0;
958  // case a): edge/lane is known and matches route
959  bool aFound = vtdMap_matchingEdgeLane(pos, origID, *v, server.vtdDebug(), bestDistanceA, &laneA, lanePosA, routeOffsetA, edgesA);
960  // case b): position is at route, should be somewhere near to it
961  bool bFound = vtdMap_matchingRoutePosition(pos, origID, *v, server.vtdDebug(), bestDistanceB, &laneB, lanePosB, routeOffsetB, edgesB);
962  // case c) nearest matching lane
963  bool cFound = vtdMap_matchingNearest(pos, origID, *v, server, server.vtdDebug(), bestDistanceC, &laneC, lanePosC, routeOffsetC, edgesC);
964  //
965  SUMOReal maxRouteDistance = 50;
966  if(cFound && (bestDistanceA>maxRouteDistance&&bestDistanceC>maxRouteDistance)) {
967  // both route-based approach yield in a position too far away from the submitted --> new route!?
968  server.setVTDControlled(v, laneC, lanePosC, routeOffsetC, edgesC);
969  } else {
970  // use the best we have
971  if(bFound) {
972  server.setVTDControlled(v, laneB, lanePosB, routeOffsetB, edgesB);
973  } else if(aFound) {
974  server.setVTDControlled(v, laneA, lanePosA, routeOffsetA, edgesA);
975  } else if (cFound) {
976  server.setVTDControlled(v, laneC, lanePosC, routeOffsetC, edgesC);
977  } else {
978  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, "Could not map vehicle.", outputStorage);
979  }
980  }
981  }
982  break;
983  default:
984  try {
985  if (!TraCIServerAPI_VehicleType::setVariable(CMD_SET_VEHICLE_VARIABLE, variable, getSingularType(v), server, inputStorage, outputStorage)) {
986  return false;
987  }
988  } catch (ProcessError& e) {
989  return server.writeErrorStatusCmd(CMD_SET_VEHICLE_VARIABLE, e.what(), outputStorage);
990  }
991  break;
992  }
993  server.writeStatusCmd(CMD_SET_VEHICLE_VARIABLE, RTYPE_OK, warning, outputStorage);
994  return true;
995 }
996 
997 
998 bool
999 TraCIServerAPI_Vehicle::vtdMap_matchingEdgeLane(const Position &pos, const std::string &origID, MSVehicle &v, bool report,
1000  SUMOReal &bestDistance, MSLane **lane, SUMOReal &lanePos, int &routeOffset, MSEdgeVector &edges) {
1001  const std::map<std::string, std::vector<MSLane*> > &vtdMap = getOrBuildVTDMap();
1002  if(vtdMap.find(origID)==vtdMap.end()) {
1003  if(report) std::cout << " a failed - lane not in map" << std::endl;
1004  return false;
1005  }
1006  const std::vector<MSLane*> &lanes = vtdMap.find(origID)->second;
1007  for(std::vector<MSLane*>::const_iterator i=lanes.begin(); i!=lanes.end()&&bestDistance>POSITION_EPS; ++i) {
1008  MSLane* l = *i;
1009  SUMOReal dist = l->getShape().distance(pos);
1010  if(report) std::cout << " a at lane " << l->getID() << " dist:" << dist << " best:" << bestDistance << std::endl;
1011  if (dist < bestDistance) {
1012  bestDistance = dist;
1013  *lane = l;
1014  }
1015  }
1016  MSLane *pni = *lane;
1017  while(pni!=0&&pni->getEdge().getPurpose()==MSEdge::EDGEFUNCTION_INTERNAL&&pni->getIncomingLanes().size()!=0) {
1018  pni = pni->getIncomingLanes()[0].lane;
1019  }
1020  if(pni==0||pni->getEdge().getPurpose()==MSEdge::EDGEFUNCTION_INTERNAL) {
1021  // not found
1022  if(report) std::cout << " a failed - no incoming lane" << std::endl;
1023  return false;
1024  }
1025  const MSEdgeVector &tedges = v.getRoute().getEdges();
1026  MSEdgeVector::const_iterator p = std::find(tedges.begin() + v.getRoutePosition(), tedges.end(), &pni->getEdge());
1027  if(p!=tedges.end()) {
1028  lanePos = MAX2(SUMOReal(0), MIN2((*lane)->getLength()-POSITION_EPS, (*lane)->getShape().nearest_position_on_line_to_point2D(pos, false)));
1029  routeOffset = std::distance(tedges.begin(), p) - v.getRoutePosition();
1030  if(report) std::cout << " a ok lane:" << (*lane)->getID() << " lanePos:" << lanePos << " routeOffset:" << routeOffset << std::endl;
1031  return true;
1032  }
1033  if(report) std::cout << " a failed - route position beyond route length" << std::endl;
1034  return false;
1035 }
1036 
1037 
1038 bool
1039 TraCIServerAPI_Vehicle::vtdMap_matchingRoutePosition(const Position &pos, const std::string &origID, MSVehicle &v, bool report,
1040  SUMOReal &bestDistance, MSLane **lane, SUMOReal &lanePos, int &routeOffset, MSEdgeVector &edges) {
1041 
1042  int lastBestRouteEdge = 0;
1043  int lastRouteEdge = 0;
1044  MSLane *bestRouteLane = 0;
1045  const std::vector<MSLane*>& bestLaneConts = v.getBestLanesContinuation(v.getLane());
1046  for(std::vector<MSLane*>::const_iterator i=bestLaneConts.begin(); i!=bestLaneConts.end()&&bestDistance>POSITION_EPS; ++i) {
1047  MSEdge &e = (*i)->getEdge();
1048  if(i!=bestLaneConts.begin()&&e.getPurpose()!=MSEdge::EDGEFUNCTION_INTERNAL) {
1049  ++lastRouteEdge;
1050  }
1051  const std::vector<MSLane*>& lanes = e.getLanes();
1052  for (std::vector<MSLane*>::const_iterator k = lanes.begin(); k != lanes.end()&&bestDistance>POSITION_EPS; ++k) {
1053  MSLane* cl = *k;
1054  SUMOReal dist = cl->getShape().distance(pos);
1055  if(report) std::cout << " b at lane " << cl->getID() << " dist:" << dist << " best:" << bestDistance << std::endl;
1056  if (dist < bestDistance) {
1057  bestDistance = dist;
1058  *lane = cl;
1059  lastBestRouteEdge = lastRouteEdge;
1060  if(e.getPurpose()==MSEdge::EDGEFUNCTION_INTERNAL) {
1061  bestRouteLane = *i;
1062  } else {
1063  bestRouteLane = *lane;
1064  }
1065  }
1066  }
1067  }
1068  if(bestRouteLane==0) {
1069  if(report) std::cout << " b failed - no best route lane" << std::endl;
1070  return false;
1071  }
1072  lanePos = MAX2(SUMOReal(0), MIN2(bestRouteLane->getLength()-POSITION_EPS, bestRouteLane->getShape().nearest_position_on_line_to_point2D(pos, false)));
1073  routeOffset = lastBestRouteEdge;
1074  if(report) std::cout << " b ok lane " << bestRouteLane->getID() << " lanePos:" << lanePos << " best:" << lastBestRouteEdge << std::endl;
1075  return true;
1076 }
1077 
1078 
1079 bool
1080 TraCIServerAPI_Vehicle::vtdMap_matchingNearest(const Position &pos, const std::string &origID, MSVehicle &v, traci::TraCIServer& server, bool report,
1081  SUMOReal &bestDistance, MSLane **lane, SUMOReal &lanePos, int &routeOffset, MSEdgeVector &edges) {
1082  unsigned int r = 0;
1083  SUMOReal minDist = 1 << (11);
1084  MSLane* minDistLane = 0;
1085  MSLane* nameMatchingLane = 0;
1086  SUMOReal minDistNameMatchingLane = 1 << (11);
1087  for (; minDistLane == 0 && r < 10 && nameMatchingLane == 0; ++r) {
1088  std::set<std::string> into;
1089  PositionVector shape;
1090  shape.push_back(pos);
1091  server.collectObjectsInRange(CMD_GET_EDGE_VARIABLE, shape, 1 << r, into);
1092  for (std::set<std::string>::const_iterator j = into.begin(); j != into.end(); ++j) {
1093  MSEdge* e = MSEdge::dictionary(*j);
1094  const std::vector<MSLane*>& lanes = e->getLanes();
1095  for (std::vector<MSLane*>::const_iterator k = lanes.begin(); k != lanes.end(); ++k) {
1096  MSLane* lane = *k;
1097  SUMOReal dist = lane->getShape().distance(pos);
1098  if (lane->knowsParameter("origId")) {
1099  if (lane->getParameter("origId", "") == origID) {
1100  if (dist < minDistNameMatchingLane) {
1101  minDistNameMatchingLane = dist;
1102  nameMatchingLane = lane;
1103  }
1104  }
1105  }
1106  if (dist < minDist) {
1107  minDist = dist;
1108  minDistLane = lane;
1109  }
1110  }
1111  }
1112  }
1113  *lane = nameMatchingLane != 0 ? nameMatchingLane : minDistLane;
1114  if(lane==0) {
1115  if(report) std::cout << " c failed - no matching lane" << std::endl;
1116  return false;
1117  }
1118  lanePos = (*lane)->interpolateGeometryPosToLanePos((*lane)->getShape().nearest_position_on_line_to_point2D(pos, false));
1119  if (*lane == v.getLane()) {
1120  routeOffset = 0;
1121  if(report) std::cout << " c ok, on same lane" << std::endl;
1122  return true;
1123  }
1124  MSEdge& destinationEdge = (*lane)->getEdge();
1125  MSEdge* routePos = &destinationEdge;
1126  while (routePos->getPurpose() == MSEdge::EDGEFUNCTION_INTERNAL) {
1127  routePos = &routePos->getLanes()[0]->getLogicalPredecessorLane()->getEdge();
1128  }
1129  r = 0;
1130  const MSRoute& route = v.getRoute();
1131  unsigned int c = v.getRoutePosition();
1132  unsigned int l = (int)route.getEdges().size();
1133  unsigned int rindex = 0;
1134  bool found = false;
1135  while (!found && ((int)(c - r) >= 0 || c + r < l)) {
1136  if ((int)(c - r) >= 0 && route[c - r] == routePos) {
1137  rindex = c - r;
1138  found = true;
1139  }
1140  if (c + r < l && route[c + r] == routePos) {
1141  rindex = c + r;
1142  found = true;
1143  }
1144  ++r;
1145  }
1146  if(found) {
1147  // the matching lane is part of the route
1148  routeOffset = rindex - v.getRoutePosition();
1149  if(report) std::cout << " c ok, on a different edge of same route" << std::endl;
1150  return true;
1151  }
1152  // build new route
1153  MSLane* firstLane = *lane;
1154  if (destinationEdge.getPurpose() != MSEdge::EDGEFUNCTION_INTERNAL) {
1155  edges.push_back(&destinationEdge);
1156  } else {
1157  firstLane = (*lane)->getLogicalPredecessorLane();
1158  edges.push_back(&firstLane->getEdge());
1159  }
1160  const MSLinkCont& lc = firstLane->getLinkCont();
1161  if (lc.size() != 0 && lc[0]->getLane() != 0) {
1162  edges.push_back(&lc[0]->getLane()->getEdge());
1163  }
1164  if(report) std::cout << " c ok, on a different route" << std::endl;
1165  return true;
1166 }
1167 
1168 
1169 bool
1170 TraCIServerAPI_Vehicle::commandDistanceRequest(traci::TraCIServer& server, tcpip::Storage& inputStorage,
1171  tcpip::Storage& outputStorage, const MSVehicle* v) {
1172  if (inputStorage.readUnsignedByte() != TYPE_COMPOUND) {
1173  return server.writeErrorStatusCmd(CMD_GET_VEHICLE_VARIABLE, "Retrieval of distance requires a compound object.", outputStorage);
1174  }
1175  if (inputStorage.readInt() != 2) {
1176  return server.writeErrorStatusCmd(CMD_GET_VEHICLE_VARIABLE, "Retrieval of distance requires position and distance type as parameter.", outputStorage);
1177  }
1178 
1179  Position pos;
1180  std::pair<const MSLane*, SUMOReal> roadPos;
1181 
1182  // read position
1183  int posType = inputStorage.readUnsignedByte();
1184  switch (posType) {
1185  case POSITION_ROADMAP:
1186  try {
1187  std::string roadID = inputStorage.readString();
1188  roadPos.second = inputStorage.readDouble();
1189  roadPos.first = TraCIServerAPI_Simulation::getLaneChecking(roadID, inputStorage.readUnsignedByte(), roadPos.second);
1190  pos = roadPos.first->getShape().positionAtLengthPosition(roadPos.second);
1191  } catch (TraCIException& e) {
1192  return server.writeErrorStatusCmd(CMD_GET_VEHICLE_VARIABLE, e.what(), outputStorage);
1193  }
1194  break;
1195  case POSITION_2D:
1196  case POSITION_3D: {
1197  const double p1x = inputStorage.readDouble();
1198  const double p1y = inputStorage.readDouble();
1199  pos.set(p1x, p1y);
1200  }
1201  if (posType == POSITION_3D) {
1202  inputStorage.readDouble(); // z value is ignored
1203  }
1204  roadPos = TraCIServerAPI_Simulation::convertCartesianToRoadMap(pos);
1205  break;
1206  default:
1207  return server.writeErrorStatusCmd(CMD_GET_VEHICLE_VARIABLE, "Unknown position format used for distance request", outputStorage);
1208  }
1209 
1210  // read distance type
1211  int distType = inputStorage.readUnsignedByte();
1212 
1213  SUMOReal distance = INVALID_DOUBLE_VALUE;
1214  if (v->isOnRoad()) {
1215  if (distType == REQUEST_DRIVINGDIST) {
1216  distance = v->getRoute().getDistanceBetween(v->getPositionOnLane(), roadPos.second,
1217  v->getEdge(), &roadPos.first->getEdge());
1218  if (distance == std::numeric_limits<SUMOReal>::max()) {
1219  distance = INVALID_DOUBLE_VALUE;
1220  }
1221  } else {
1222  // compute air distance (default)
1223  distance = v->getPosition().distanceTo(pos);
1224  }
1225  }
1226  // write response command
1227  outputStorage.writeUnsignedByte(TYPE_DOUBLE);
1228  outputStorage.writeDouble(distance);
1229  return true;
1230 }
1231 
1232 
1233 // ------ helper functions ------
1234 bool
1235 TraCIServerAPI_Vehicle::getPosition(const std::string& id, Position& p) {
1236  MSVehicle* v = dynamic_cast<MSVehicle*>(MSNet::getInstance()->getVehicleControl().getVehicle(id));
1237  if (v == 0) {
1238  return false;
1239  }
1240  p = v->getPosition();
1241  return true;
1242 }
1243 
1244 
1245 MSVehicleType&
1246 TraCIServerAPI_Vehicle::getSingularType(SUMOVehicle* const veh) {
1247  const MSVehicleType& oType = veh->getVehicleType();
1248  std::string newID = oType.getID().find('@') == std::string::npos ? oType.getID() + "@" + veh->getID() : oType.getID();
1249  MSVehicleType* type = MSVehicleType::build(newID, &oType);
1250  static_cast<MSVehicle*>(veh)->replaceVehicleType(type);
1251  return *type;
1252 }
1253 
1254 
1255 #include <microsim/MSEdgeControl.h>
1256 
1257 const std::map<std::string, std::vector<MSLane*> > &
1258 TraCIServerAPI_Vehicle::getOrBuildVTDMap() {
1259  if(gVTDMap.size()==0) {
1260  const std::vector<MSEdge*> &edges = MSNet::getInstance()->getEdgeControl().getEdges();
1261  for(std::vector<MSEdge*>::const_iterator i=edges.begin(); i!=edges.end(); ++i) {
1262  const std::vector<MSLane*> &lanes = (*i)->getLanes();
1263  for(std::vector<MSLane*>::const_iterator j=lanes.begin(); j!=lanes.end(); ++j) {
1264  if ((*j)->knowsParameter("origId")) {
1265  std::string origID = (*j)->getParameter("origId", "");
1266  if(gVTDMap.find(origID)==gVTDMap.end()) {
1267  gVTDMap[origID] = std::vector<MSLane*>();
1268  }
1269  gVTDMap[origID].push_back(*j);
1270  }
1271  }
1272  }
1273  if(gVTDMap.size()==0) {
1274  gVTDMap["unknown"] = std::vector<MSLane*>();
1275  }
1276  }
1277  return gVTDMap;
1278 }
1279 
1280 
1281 #endif
1282 
1283 
1284 /****************************************************************************/
1285