SUMO - Simulation of Urban MObility
MSDevice_SSM.cpp
Go to the documentation of this file.
1 /****************************************************************************/
9 // A device which stands as an implementation example and which outputs movereminder calls
10 /****************************************************************************/
11 // SUMO, Simulation of Urban MObility; see http://sumo.dlr.de/
12 // Copyright (C) 2013-2017 DLR (http://www.dlr.de/) and contributors
13 /****************************************************************************/
14 //
15 // This file is part of SUMO.
16 // SUMO is free software: you can redistribute it and/or modify
17 // it under the terms of the GNU General Public License as published by
18 // the Free Software Foundation, either version 3 of the License, or
19 // (at your option) any later version.
20 //
21 /****************************************************************************/
22 
23 // ===========================================================================
24 // included modules
25 // ===========================================================================
26 #ifdef _MSC_VER
27 #include <windows_config.h>
28 #else
29 #include <config.h>
30 #endif
31 
37 #include <microsim/MSNet.h>
38 #include <microsim/MSJunction.h>
39 #include <microsim/MSLane.h>
41 #include <microsim/MSEdge.h>
42 #include <microsim/MSVehicle.h>
43 #include "MSDevice_SSM.h"
44 
45 
46 // Debug constants
47 #define DEBUG_SSM
48 
49 // ===========================================================================
50 // Constants
51 // ===========================================================================
52 // value indicating an invalid double parameter
53 #define INVALID -std::numeric_limits<double>::max()
54 // default value for the maximal episode length for logged encounters
55 #define DEFAULT_FREQUENCY 30.0
56 // default value for the detection range of potential opponents
57 #define DEFAULT_RANGE 50.0
58 
59 // list of implemented SSMs (NOTE: To add more SSMs identifiers are added to AVAILABLE_SSMS
60 // and a default threshold must be defined. A corresponding
61 // case should be added to the switch in buildVehicleDevices)
62 #define AVAILABLE_SSMS "TTC DRAC PET"
63 #define DEFAULT_THRESHOLD_TTC 3. // in [s.], events get logged if below threshold
64 #define DEFAULT_THRESHOLD_DRAC 4. // in [m/s^2], events get logged if above threshold
65 #define DEFAULT_THRESHOLD_PET 2. // in seconds, events get logged if below threshold
66 
67 // ===========================================================================
68 // method definitions
69 // ===========================================================================
70 // ---------------------------------------------------------------------------
71 // static initialisation methods
72 // ---------------------------------------------------------------------------
73 
74 std::set<MSDevice*>* MSDevice_SSM::instances = new std::set<MSDevice*>();
75 
76 const std::set<MSDevice*>&
78  return *instances;
79 }
80 
81 void
83  // Close current encounters and flush conflicts to file for all existing devices
84  if (instances != 0) {
85  for (std::set<MSDevice*>::iterator ii = instances->begin(); ii != instances->end(); ++ii) {
86  static_cast<MSDevice_SSM*>(*ii)->resetEncounters();
87  static_cast<MSDevice_SSM*>(*ii)->flushConflicts(true);
88  }
89  instances->clear();
90  }
91 }
92 
93 void
95  oc.addOptionSubTopic("SSM Device");
96  insertDefaultAssignmentOptions("ssm", "SSM Device", oc);
97 
98  // custom options
99  oc.doRegister("device.ssm.measures", new Option_String(""));
100  oc.addDescription("device.ssm.measures", "SSM Device", "Specifies which measures will be logged (as a space seperated sequence of IDs in ('TTC', 'DRAC', 'PET')).");
101  oc.doRegister("device.ssm.thresholds", new Option_String(""));
102  oc.addDescription("device.ssm.thresholds", "SSM Device", "Specifies thresholds corresponding to the specified measures (see documentation and watch the order!). Only events exceeding the thresholds will be logged.");
103  oc.doRegister("device.ssm.trajectories", new Option_Bool(false));
104  oc.addDescription("device.ssm.trajectories", "SSM Device", "Specifies whether trajectories will be logged (if false, only the extremal values and times are reported).");
105  oc.doRegister("device.ssm.frequency", new Option_Float(DEFAULT_FREQUENCY));
106  oc.addDescription("device.ssm.frequency", "SSM Device", "Specifies the maximal length of stored conflict trajectories in seconds (the frequency of logging, default is " + toString(DEFAULT_FREQUENCY) + "s.).");
107  oc.doRegister("device.ssm.range", new Option_Float(DEFAULT_RANGE));
108  oc.addDescription("device.ssm.range", "SSM Device", "Specifies the detection range in meters (default is " + toString(DEFAULT_RANGE) + "m.). For vehicles below this distance from the equipped vehicle, SSM values are traced.");
109 }
110 
111 void
112 MSDevice_SSM::buildVehicleDevices(SUMOVehicle& v, std::vector<MSDevice*>& into) {
115  WRITE_WARNING("SSM Device for vehicle '" + v.getID() + "' will not be built. (SSMs not supported in MESO)");
116  return;
117  }
118  // ID for the device
119  std::string deviceID = "ssm_" + v.getID();
120 
121  // Load parameters:
122 
123  // Measures and thresholds
124  std::vector<double> thresholds;
125  std::vector<std::string> measures;
126  bool success = getMeasuresAndThresholds(v, deviceID, thresholds, measures);
127  if (!success) {
128  return;
129  }
130 
131  // Trajectories
132  bool trajectories = requestsTrajectories(v);
133 
134  // logging frequency
135  double frequency = getLoggingFrequency(v);
136 
137  // detection range
138  double range = getDetectionRange(v);
139 
140  // File
141  std::string file = getOutputFilename(v, deviceID);
142 
143  // Build the device (XXX: who deletes it?)
144  MSDevice_SSM* device = new MSDevice_SSM(v, deviceID, file, measures, thresholds, trajectories, frequency, range);
145  into.push_back(device);
146  }
147 }
148 
149 
150 MSDevice_SSM::Encounter::Encounter(const MSVehicle* _ego, const MSVehicle* const _foe, double _begin) :
151  ego(_ego),
152  foe(_foe),
153  begin(_begin),
154  end(INVALID),
156 #ifdef DEBUG_SSM
157  std::cout << "\n" << SIMTIME << " Constructing encounter of '"
158  << ego->getID() << "' and '" << foe->getID() << "'" << std::endl;
159 #endif
160 }
161 
163 #ifdef DEBUG_SSM
164  std::cout << "\n" << SIMTIME << " Destroying encounter of '"
165  << ego->getID() << "' and '" << foe->getID() << "' (begin was " << begin << ")" << std::endl;
166 #endif
167 }
168 
169 
170 void
171 MSDevice_SSM::Encounter::add(double time, Position egoX, Position egoV, Position foeX, Position foeV) {
172  this->timespan.push_back(time);
173  this->egoTrajectory.x.push_back(egoX);
174  this->egoTrajectory.v.push_back(egoV);
175  this->foeTrajectory.x.push_back(foeX);
176  this->foeTrajectory.v.push_back(foeV);
177 }
178 
179 void
181  if (myHolder.isOnRoad()) {
182  update();
183  flushConflicts();
184  } else {
185  resetEncounters();
186  flushConflicts(true);
187  }
188 }
189 
190 void
192 #ifdef DEBUG_SSM
193  std::cout << "\n" << SIMTIME << " Device '" << getID() << "' update()\n"
194  << "Size of myActiveEncounters: " << myActiveEncounters.size()
195  << "\nSize of myPastConflicts: " << myPastConflicts.size()
196  << std::endl;
197 #endif
198  // Scan surroundings for other vehicles
199  FoeInfoMap foes;
201 
202  // Update encounters and conflicts -> removes all foes (and deletes corresponding FoeInfos) for which already a corresponding encounter exists
203  processEncounters(foes);
204 
205  // Make new encounters for all foes, which were not removed by processEncounters (and deletes corresponding FoeInfos)
206  createEncounters(foes);
207  foes.clear();
208 
209  // Write out past conflicts
210  flushConflicts();
211 
212 }
213 
214 void
216 #ifdef DEBUG_SSM
217  std::cout << "\n" << SIMTIME << " Device '" << getID() << "' createEncounters()" << std::endl;
218  std::cout << "New foes:\n";
219  for (FoeInfoMap::const_iterator vi = foes.begin(); vi != foes.end(); ++vi) {
220  std::cout << vi->first->getID() << "\n";
221  }
222  std::cout << std::endl;
223 #endif
224 
225  for (FoeInfoMap::const_iterator foe = foes.begin(); foe != foes.end(); ++foe) {
226  std::pair<MSLane*, MSLane*> conflictLanes;
227  Encounter* e = new Encounter(myHolderMS, foe->first, SIMTIME);
228  updateEncounter(e, foe->second); // deletes foe->second
229  myActiveEncounters.push_back(e);
230  }
231 }
232 
233 void
235  // Call processEncounters() with empty vehicle set
236  FoeInfoMap foes;
237  // processEncounters with empty argument closes all encounters
238  processEncounters(foes);
239 }
240 
241 void
243 #ifdef DEBUG_SSM
244  std::cout << "\n" << SIMTIME << " Device '" << getID() << "' processEncounters()" << std::endl;
245  std::cout << "Currently present foes:\n";
246  for (FoeInfoMap::const_iterator vi = foes.begin(); vi != foes.end(); ++vi) {
247  std::cout << vi->first->getID() << "\n";
248  }
249  std::cout << std::endl;
250 #endif
251 
252  // Run through active encounters. If corresponding foe is still present in foes update and
253  // remove foe from foes. If the foe has disappeared close the encounter (check if it qualifies
254  // as a conflict and in case transfer it to myPastConflicts).
255  // Afterwards run through remaining elements in foes and create new encounters for them.
256 
257  EncounterVector::iterator ei = myActiveEncounters.begin();
258  while (ei != myActiveEncounters.end()) {
259  Encounter* e = *ei;
260  if (foes.find(e->foe) != foes.end()) {
261  FoeInfo* foeInfo = foes[e->foe];
262  // Update encounter
263  updateEncounter(e, foeInfo); // deletes foeInfo
264  // Erase foes which were already encountered
265  foes.erase(e->foe);
266  ++ei;
267  } else {
268  // Close encounter
269  closeEncounter(e);
270  if (qualifiesAsConflict(e)) {
271  myPastConflicts.push(e);
272  } else {
273  delete e;
274  }
275  ei = myActiveEncounters.erase(ei);
276  }
277  }
278 
279 }
280 
281 
282 bool
284  // TODO: Check if conflict measure thresholds are exceeded
285 #ifdef DEBUG_SSM
286  std::cout << SIMTIME << " qualifiesAsConflict() for encounter of vehicles '"
287  << e->ego->getID() << "' and '" << e->foe->getID()
288  << "'" << std::endl;
289 #endif
290  return true;
291 }
292 
293 
294 void
296  // TODO: is there anything specific to do here?
297 #ifdef DEBUG_SSM
298  std::cout << SIMTIME << " closeEncounter() of vehicles '"
299  << e->ego->getID() << "' and '" << e->foe->getID()
300  << "'" << std::endl;
301 #endif
302  return;
303 }
304 
305 void
307 #ifdef DEBUG_SSM
308  std::cout << SIMTIME << " updateEncounter() of vehicles '"
309  << e->ego->getID() << "' and '" << e->foe->getID()
310  << "'" << std::endl;
311 #endif
312 
313  // init encounter type with most general identifier
315  UNUSED_PARAMETER(type); // is currently written but never read
316 
317  // Add current states to trajectories
319 
320  // Ego's current Lane
321  const MSLane* egoLane = e->ego->getLane();
322  // Foe's current Lane
323  const MSLane* foeLane = e->foe->getLane();
324 
325  // Ego's conflict lane is memorized in foeInfo
326  const MSLane* egoConflictLane = foeInfo->egoConflictLane;
327  double egoDistToConflictLane = foeInfo->egoDistToConflictLane;
328  // Find conflicting lane and the distance to its entry link for the foe
329  double foeDistToConflictLane;
330  const MSLane* foeConflictLane = findFoeConflictLane(e->foe, foeInfo->egoConflictLane, foeDistToConflictLane);
331 
332 #ifdef DEBUG_SSM
333  std::cout << "egoConflictLane: '" << (egoConflictLane == 0 ? "NULL" : egoConflictLane->getID()) << "'\n"
334  << "foeConflictLane: '" << (foeConflictLane == 0 ? "NULL" : foeConflictLane->getID()) << "'"
335  << "\nEgo's distance to conflict lane: " << egoDistToConflictLane
336  << "\nFoe's distance to conflict lane: " << foeDistToConflictLane
337  << std::endl;
338 #endif
339 
340  // Treat different cases for foeConflictLane and egoConflictLane (internal or non-internal / equal to egoLane or to foeLane),
341  // and thereby determine encounterType and the ego/foeEncounterDistance.
342  // The encounter distance has a different meaning for different types of encounters:
343  // 1) For rear-end conflicts (lead/follow situations) the follower's encounter distance is the distance to the actual back position of the leader. The leaders's distance is undefined.
344  // 2) For merging encounters the encounter distance is the distance until the begin of the common target edge/lane.
345  // 3) For crossing encounters the encounter distance is the distance until crossing point of the conflicting lanes.
346  double egoEncounterDist = INVALID;
347  double foeEncounterDist = INVALID;
348 
349  if (foeConflictLane == 0) {
350  // foe vehicle is not on course towards the ego's route (see findFoeConflictLane)
352 #ifdef DEBUG_SSM
353  std::cout << "-> Encounter type: No conflict." << std::endl;
354 #endif
355  } else if (!egoConflictLane->isInternal()) {
356  // The conflict lane is non-internal, therefore we either have no potential conflict or a lead/follow situation (i.e., no crossing or merging)
357  if (egoConflictLane == egoLane) {
358  // The conflict point is on the ego's current lane.
359  if (foeLane == egoLane) {
360  // Foe is on the same non-internal lane
361  type = ENCOUNTER_TYPE_FOLLOWING; // TODO: specify: check for leader / follower
362 #ifdef DEBUG_SSM
363  std::cout << "-> Encounter type: Lead/follow-situation on non-internal lane '" << egoLane->getID() << "'" << std::endl;
364 #endif
365  } else if (&(foeLane->getEdge()) == &(egoLane->getEdge())) {
366  // Foe is on the same non-internal edge but not on the same lane. Treat this as no conflict for now
367  // XXX: this disregards conflicts for vehicles on adjacent lanes
369 #ifdef DEBUG_SSM
370  std::cout << "-> Encounter type: No conflict (adjacent lanes)." << std::endl;
371 #endif
372  } else {
373  // Foe must be on a route leading into the ego's lane
375  foeEncounterDist = foeDistToConflictLane + e->ego->getBackPositionOnLane();
376 #ifdef DEBUG_SSM
377  std::cout << "-> Encounter type: Ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' leads foe '"
378  << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
379  << " (gap = " << foeEncounterDist << ")"
380  << std::endl;
381 #endif
382  assert(foeConflictLane == egoLane);
383  }
384  } else {
385  // The egoConflictLane is a non-internal lane which is not the ego's current lane. Thus it must lie ahead of the ego vehicle and equal the foe's current lane.
386  // see finSurroundingVehicles() (otherwise the foe would have had to enter the ego's route along a junction and the corresponding
387  // conflict lane would be internal)
389  egoEncounterDist = egoDistToConflictLane + e->foe->getBackPositionOnLane();
390 #ifdef DEBUG_SSM
391  std::cout << "-> Encounter type: Ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' follows foe '"
392  << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
393  << " (gap = " << egoEncounterDist << ")"
394  << std::endl;
395 #endif
396  assert(foeLane == egoConflictLane);
397  assert(foeDistToConflictLane <= 0);
398  }
399  } else {
400  // egoConflictLane is internal, i.e., lies on a junction. Besides the lead/follow situation (which may stretch over different lanes of a connection),
401  // merging or crossing of the conflict lanes is possible.
402  MSLink* egoEntryLink = egoConflictLane->getEntryLink();
403  MSLink* foeEntryLink = foeConflictLane->getEntryLink();
404  if (&(egoEntryLink->getViaLane()->getEdge()) == &(foeEntryLink->getViaLane()->getEdge())) {
405  if (egoEntryLink != foeEntryLink) {
406  // XXX: this disregards conflicts for vehicles on adjacent lanes
408 #ifdef DEBUG_SSM
409  std::cout << "-> Encounter type: No conflict (adjacent lanes)." << std::endl;
410 #endif
411  } else {
412  // Lead / follow situation on connection
413  if (egoLane == egoConflictLane && foeLane != foeConflictLane) {
414  // ego on junction, foe not yet
416  foeEncounterDist = foeDistToConflictLane + e->ego->getBackPositionOnLane();
417 #ifdef DEBUG_SSM
418  std::cout << "-> Encounter type: Ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' leads foe '"
419  << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
420  << " (gap = " << foeEncounterDist << ")"
421  << std::endl;
422 #endif
423  } else if (egoLane != egoConflictLane && foeLane == foeConflictLane) {
424  // foe on junction, ego not yet
426  egoEncounterDist = egoDistToConflictLane + e->foe->getBackPositionOnLane();
427 #ifdef DEBUG_SSM
428  std::cout << "-> Encounter type: Ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' follows foe '"
429  << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
430  << " (gap = " << egoEncounterDist << ")"
431  << std::endl;
432 #endif
433  } else {
434  // Both must be already on the junction in a lead / follow situation on a connection
435  // (since they approach via the same link, findSurroundingVehicles() would have determined a
436  // different conflictLane if both are not on the junction)
437  assert(egoLane == egoConflictLane);
438  assert(foeLane == foeConflictLane);
439  type = ENCOUNTER_TYPE_FOLLOWING; // TODO: specify: check for leader / follower
440 #ifdef DEBUG_SSM
441  std::cout << "-> Encounter type: Lead/follow-situation on connection from '" << egoEntryLink->getLaneBefore()->getID()
442  << "' to '" << egoEntryLink->getLane()->getID() << "'" << std::endl;
443 #endif
444  }
445  }
446  } else {
447  // Entry links to junctions lead to different internal edges.
448  // There are three possibilities, either the edges cross, merge or have no conflict
449  const std::vector<MSLink*>& egoFoeLinks = egoEntryLink->getFoeLinks();
450  const std::vector<MSLink*>& foeFoeLinks = foeEntryLink->getFoeLinks();
451  // Determine whether ego and foe links are foes
452  bool crossOrMerge = (find(egoFoeLinks.begin(), egoFoeLinks.end(), foeEntryLink) != egoFoeLinks.end()
453  || find(foeFoeLinks.begin(), foeFoeLinks.end(), egoEntryLink) != foeFoeLinks.end());
454  if (!crossOrMerge) {
455  if (&(foeEntryLink->getLane()->getEdge()) == &(egoEntryLink->getLane()->getEdge())) {
456  // XXX: the situation of merging into adjacent lanes is disregarded for now
458 #ifdef DEBUG_SSM
459  std::cout << "-> Encounter type: No conflict (adjacent lanes)." << std::endl;
460 #endif
461  } else {
463 #ifdef DEBUG_SSM
464  std::cout << "-> Encounter type: No conflict." << std::endl;
465 #endif
466  }
467  } else if (foeEntryLink->getLane() == egoEntryLink->getLane()) {
468  type = ENCOUNTER_TYPE_MERGING; // TODO: determine projected lead/follow, conflict entries, etc.
469  egoEncounterDist = egoDistToConflictLane + egoEntryLink->getInternalLengthsAfter();
470  foeEncounterDist = foeDistToConflictLane + foeEntryLink->getInternalLengthsAfter();
471 #ifdef DEBUG_SSM
472  std::cout << "-> Encounter type: Merging situation of ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' and foe '"
473  << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
474  << "\nDistances to merge-point: ego: " << egoEncounterDist << ", foe: " << foeEncounterDist
475  << std::endl;
476 #endif
477  } else {
478  type = ENCOUNTER_TYPE_CROSSING; // TODO: determine projected lead/follow, conflict entries, etc.
479  // XXX: GO ON HERE, implement MSLink::getLengthBeforeCrossing()
480  egoEncounterDist = egoDistToConflictLane + egoEntryLink->getLengthBeforeCrossing(foeEntryLink);
481  foeEncounterDist = foeDistToConflictLane + foeEntryLink->getLengthBeforeCrossing(egoEntryLink);
482 #ifdef DEBUG_SSM
483  std::cout << "-> Encounter type: Crossing situation of ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' and foe '"
484  << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
485  << "\nDistances to crossing-point: ego: " << egoEncounterDist << ", foe: " << foeEncounterDist
486  << std::endl;
487 #endif
488  }
489  }
490  }
491  // free foeInfo
492  delete foeInfo;
493 
494  // Compute SSMs
495  computeSSMs(e);
496 
497 }
498 
499 const MSLane*
500 MSDevice_SSM::findFoeConflictLane(const MSVehicle* foe, const MSLane* egoConflictLane, double& distToConflictLane) {
501 
502 #ifdef DEBUG_SSM
503  std::cout << SIMTIME << " findFoeConflictLane() for foe '"
504  << foe->getID() << "' on lane '" << foe->getLane()->getID() << "'"
505  << std::endl;
506 #endif
507  MSLane* foeLane = foe->getLane();
508  std::vector<MSLane*>::const_iterator laneIter = foe->getBestLanesContinuation().begin();
509  std::vector<MSLane*>::const_iterator foeBestLanesEnd = foe->getBestLanesContinuation().end();
510  assert(foeLane->isInternal() || *laneIter == foeLane);
511  distToConflictLane = -foe->getPositionOnLane();
512 
513  // Potential conflict lies on junction if egoConflictLane is internal
514  const MSJunction* conflictJunction = egoConflictLane->isInternal() ? egoConflictLane->getEdge().getToJunction() : 0;
515 #ifdef DEBUG_SSM
516  if (conflictJunction != 0) {
517  std::cout << "Potential conflict on junction '" << conflictJunction->getID()
518  << std::endl;
519  }
520 #endif
521  if (foeLane->isInternal() && foeLane->getEdge().getToJunction() == conflictJunction) {
522  // foe is already on the conflict junction
523  return foeLane;
524  }
525 
526  // Foe is not on the conflict junction
527 
528  // Leading internal lanes in bestlanes are resembled as NULL-pointers skip them
529  while (laneIter != foeBestLanesEnd && *laneIter == 0) {
530  assert(foeLane->isInternal());
531  distToConflictLane += foeLane->getLength();
532  foeLane = foeLane->getLinkCont()[0]->getViaLane();
533  ++laneIter;
534  }
535 
536  // Look for the junction downstream along foeBestLanes
537  while (laneIter != foeBestLanesEnd && distToConflictLane <= myRange) {
538  // Eventual internal lanes were skipped
539  foeLane = *laneIter;
540  assert(!foeLane->isInternal());
541  if (&foeLane->getEdge() == &egoConflictLane->getEdge()) {
542 #ifdef DEBUG_SSM
543  std::cout << "Found conflict lane for foe: '" << foeLane->getID() << "'" << std::endl;
544 #endif
545  // found the potential conflict edge along foeBestLanes
546  return foeLane;
547  }
548  // No conflict on foeLane
549  distToConflictLane += foeLane->getLength();
550 
551  // set laneIter to next non internal lane along foeBestLanes
552  ++laneIter;
553  if (laneIter == foeBestLanesEnd) {
554  return 0;
555  }
556  MSLane* nextNonInternalLane = *laneIter;
557  MSLink* link = foeLane->getLinkTo(nextNonInternalLane);
558  // Set foeLane to first internal lane on the next junction
559  foeLane = link->getViaLane();
560  assert(foeLane->isInternal());
561  if (foeLane->getEdge().getToJunction() == conflictJunction) {
562 #ifdef DEBUG_SSM
563  std::cout << "Found conflict lane for foe: '" << foeLane->getID() << "'" << std::endl;
564 #endif
565  // found egoConflictLane, resp. the conflict junction, along foeBestLanes
566  return foeLane;
567  }
568  // No conflict on junction
569  distToConflictLane += link->getInternalLengthsAfter();
570  foeLane = nextNonInternalLane;
571  }
572  // Didn't find conflicting lane on foeBestLanes within range.
573  return 0;
574 }
575 
576 
577 void
579  // TODO
580 #ifdef DEBUG_SSM
581  std::cout << SIMTIME << " computeSSMs() for vehicles '"
582  << e->ego->getID() << "' and '" << e->foe->getID()
583  << "'" << std::endl;
584 #endif
585 
586  // Determine situation type. Can be:
587  // 1) following
588  // 2) crossing / merging (only one vehicle before conflict point)
589  // 3) crossing / merging (both before conflict point)
590  // 4) non-interacting
591  // 5) collision
592  // For each of 1),3),4), we have two subtypes, one where the ego is leader and one, where it is follower.
593  // In general a situation develops as (3)->(2)->(1/4/5) or as some part starting later.
594 
595 
596 
597 }
598 
599 void
601 #ifdef DEBUG_SSM
602  std::cout << "\n" << SIMTIME << " Device '" << getID() << "' flushConflicts()" << std::endl;
603 #endif
604  double t = SIMTIME;
605  while (!myPastConflicts.empty()) {
606  if (flushAll || myPastConflicts.top()->begin <= t - myFrequency) {
608  delete myPastConflicts.top();
609  myPastConflicts.pop();
610  } else {
611  break;
612  }
613  }
614 }
615 
616 
617 void
619 #ifdef DEBUG_SSM
620  std::cout << SIMTIME << "writeOutConflict() of vehicles '"
621  << e->ego->getID() << "' and '" << e->foe->getID()
622  << "'" << std::endl;
623 #endif
624  myOutputFile->openTag("conflict");
625  myOutputFile->writeAttr("begin", e->begin).writeAttr("end", e->end);
626  myOutputFile->writeAttr("ego", e->ego->getID()).writeAttr("foe", e->foe->getID());
627 
628  myOutputFile->openTag("timespan").writeAttr("values", e->timespan).closeTag();
629 
630  myOutputFile->openTag("egoTrajectory");
634 
635  myOutputFile->openTag("foeTrajectory");
639 
640  // TODO: output SSMs when implemented
641 
643 }
644 
645 
646 
647 
648 
649 // ---------------------------------------------------------------------------
650 // MSDevice_SSM-methods
651 // ---------------------------------------------------------------------------
652 MSDevice_SSM::MSDevice_SSM(SUMOVehicle& holder, const std::string& id, std::string outputFilename, std::vector<std::string> measures, std::vector<double> thresholds,
653  bool trajectories, double frequency, double range) :
654  MSDevice(holder, id),
655  myMeasures(measures),
656  myThresholds(thresholds),
657  mySaveTrajectories(trajectories),
658  myFrequency(frequency),
659  myRange(range) {
660  // Take care! Holder is currently being constructed. Cast occurs before completion.
661  myHolderMS = static_cast<MSVehicle*>(&holder);
662 
663  myComputeTTC = find(myMeasures.begin(), myMeasures.end(), "TTC") != myMeasures.end();
664  myComputeDRAC = find(myMeasures.begin(), myMeasures.end(), "DRAC") != myMeasures.end();
665  myComputePET = find(myMeasures.begin(), myMeasures.end(), "PET") != myMeasures.end();
666 
667  maxTrajectorySize = (int)std::ceil(myFrequency / TS) + 1;
670 
671  // XXX: Who deletes the OutputDevice?
672  myOutputFile = &OutputDevice::getDevice(outputFilename);
673 // myOutputFile.writeXMLHeader("ssm_log", "ssm_log.xsd");
674  myOutputFile->openTag("ssm_log");
675 
676  // register at static instance container
677  instances->insert(this);
678 
679 #ifdef DEBUG_SSM
680  std::cout << "Initialized ssm device '" << id << "' with "
681  << "myMeasures=" << joinToString(myMeasures, " ")
682  << ", myThresholds=" << joinToString(myThresholds, " ")
683  << ", mySaveTrajectories=" << mySaveTrajectories << ", myFrequency=" << myFrequency
684  << ", myRange=" << myRange << ", output file=" << outputFilename << "\n";
685 #endif
686 }
687 
688 
691  // XXX: Who deletes this device?
692  // unregister from static instance container
693  instances->erase((MSDevice*) this);
694  resetEncounters();
695  flushConflicts(true);
696 }
697 
698 
699 bool
701 #ifdef DEBUG_SSM
702  std::cout << "device '" << getID() << "' notifyEnter: reason=" << reason << " currentEdge=" << veh.getLane()->getEdge().getID() << "\n";
703 #endif
704  return true; // keep the device
705 }
706 
707 bool
708 MSDevice_SSM::notifyLeave(SUMOVehicle& veh, double /*lastPos*/,
709  MSMoveReminder::Notification reason, const MSLane* /* enteredLane */) {
710 #ifdef DEBUG_SSM
711  std::cout << "device '" << getID() << "' notifyLeave: reason=" << reason << " currentEdge=" << veh.getLane()->getEdge().getID() << "\n";
712 #endif
713  return true; // keep the device
714 }
715 
716 bool
717 MSDevice_SSM::notifyMove(SUMOVehicle& /* veh */, double /* oldPos */,
718  double /* newPos */, double newSpeed) {
719 #ifdef DEBUG_SSM
720  std::cout << "device '" << getID() << "' notifyMove: newSpeed=" << newSpeed << "\n";
721 #endif
722  return true; // keep the device
723 }
724 
725 
726 void
727 MSDevice_SSM::findSurroundingVehicles(const MSVehicle& veh, double range, FoeInfoMap& foeCollector) {
728 #ifdef DEBUG_SSM
729  std::cout << SIMTIME << " Looking for surrounding vehicles for ego vehicle '" << veh.getID()
730  << "' on edge '" << veh.getLane()->getEdge().getID()
731  << "'."
732  << "\nVehicle's best lanes = " << toString(veh.getBestLanesContinuation())
733  << std::endl;
734 #endif
735 
736  if (!veh.isOnRoad()) {
737  return;
738  }
739 
740  // The requesting vehicle's current route
741  // XXX: Restriction to route scanning may have to be generalized to scanning of possible continuations when
742  // considering situations involving sudden route changes. See also the definition of the EncounterTypes.
743 
744  // If veh is on an internal edge, the edgeIter points towards the last edge before the junction
745  ConstMSEdgeVector::const_iterator edgeIter = veh.getCurrentRouteEdge();
746  assert(*edgeIter != 0);
747 
748  // Best continuation lanes for the ego vehicle
749  const std::vector<MSLane*> egoBestLanes = veh.getBestLanesContinuation();
750  std::vector<MSLane*>::const_iterator laneIter = egoBestLanes.begin();
751 
752  // current lane in loop below
753  const MSLane* lane = veh.getLane();
754  assert(lane->isInternal() || lane == *laneIter);
755  assert(lane != 0);
756  // next non-internal lane on the route
757  const MSLane* nextNonInternalLane = 0;
758 
759  const MSEdge* edge; // current edge in loop below
760 
761  // Init pos with vehicle's current position. Below pos is set to zero to denote
762  // the beginning position of the currently considered edge
763  double pos = veh.getPositionOnLane();
764  // remainingRange is the range minus the distance that is already scanned downstream along the vehicles route
765  double remainingDownstreamRange = range;
766  // distToConflictLane is the distance of the ego vehicle to the start of the currently considered potential conflict lane (can be negative for its current lane)
767  double distToConflictLane = -pos;
768 
769  // if the current edge is internal, collect all vehicles from the junction and below range upstream (except on the vehicles own edge),
770  // this is analogous to the code treating junctions in the loop below. Note that the distance on the junction itself is not included into
771  // range, so vehicles farther away than range can be collected, too.
772  if (lane->isInternal()) {
773  edge = &(lane->getEdge());
774 
775 #ifdef DEBUG_SSM
776  std::cout << SIMTIME << " Vehicle '" << veh.getID() << "' is on internal edge " << edge->getID() << "'.\n"
777  << "Previous edge of its route: '" << (*edgeIter)->getID() << "'" << std::endl;
778 #endif
779 
780  assert(edge->getToJunction() == edge->getFromJunction());
781 
782  const MSJunction* junction = edge->getToJunction();
783  // Collect vehicles on the junction (TODO: Consider the case that this is an internal junction / the vehicles lane is the second part of a two-piece internal lane!!!)
784  getVehiclesOnJunction(junction, distToConflictLane, lane, foeCollector);
785 
786  // Collect vehicles on incoming edges.
787  // Note that this includes the previous edge on the ego vehicle's route.
788  // (The distance on the current internal edge is ignored)
789  const ConstMSEdgeVector& incoming = junction->getIncoming();
790  for (ConstMSEdgeVector::const_iterator ei = incoming.begin(); ei != incoming.end(); ++ei) {
791  if ((*ei)->isInternal()) {
792  continue;
793  }
794  getUpstreamVehicles(*ei, (*ei)->getLength(), range, distToConflictLane, lane, foeCollector);
795  }
796 
797  // Take into account internal distance covered on the current lane
798  remainingDownstreamRange -= lane->getLength() - pos;
799 
800  // Take into account non-internal lengths until next non-internal lane
801  MSLink* link = lane->getLinkCont()[0];
802  remainingDownstreamRange -= link->getInternalLengthsAfter();
803  distToConflictLane += lane->getLength() + link->getInternalLengthsAfter();
804 
805  // The next non-internal lane
806  pos = 0.;
807  lane = *(++laneIter);
808  edge = &lane->getEdge();
809  } else {
810  // Collect all vehicles in range behind ego vehicle
811  edge = &(lane->getEdge());
812  getUpstreamVehicles(edge, pos, range, distToConflictLane, lane, foeCollector);
813  }
814 
815  assert(lane != 0);
816  assert(!lane->isInternal());
817 
818  // Advance downstream the ego vehicle's route for distance 'range'.
819  // Collect all vehicles on the traversed edges and on incoming edges at junctions.
820  while (remainingDownstreamRange > 0.) {
821 #ifdef DEBUG_SSM
822  std::cout << SIMTIME << " Scanning downstream for vehicle '" << veh.getID() << "'.\n"
823  << "Considering edge '" << edge->getID() << "' Remaining downstream range = " << remainingDownstreamRange
824  << "\n"
825  << std::endl;
826 #endif
827  assert(!edge->isInternal());
828  assert(!lane->isInternal());
829  assert(pos == 0 || lane == veh.getLane());
830  if (pos + remainingDownstreamRange < lane->getLength()) {
831  // scan range ends on this lane
832  getUpstreamVehicles(edge, pos + remainingDownstreamRange, remainingDownstreamRange, distToConflictLane, lane, foeCollector);
833  // scanned required downstream range
834  break;
835  } else {
836  // Also need to scan area that reaches beyond the lane
837  // Collecting vehicles on non-internal edge ahead
838  getUpstreamVehicles(edge, edge->getLength(), edge->getLength() - pos, distToConflictLane, lane, foeCollector);
839  // account for scanned distance on lane
840  remainingDownstreamRange -= lane->getLength() - pos;
841  distToConflictLane += lane->getLength();
842  pos = 0.;
843 
844  // proceed to next non-internal lane
845  ++laneIter;
846  assert(laneIter == egoBestLanes.end() || *laneIter != 0);
847 
848  // If the vehicle's best lanes go on, collect vehicles on the upcoming junction
849  if (laneIter != egoBestLanes.end()) {
850  // Upcoming junction
851  const MSJunction* junction = lane->getEdge().getToJunction();
852 
853  // Find connection for ego on the junction
854  nextNonInternalLane = *laneIter;
855  MSLink* link = lane->getLinkTo(nextNonInternalLane);
856  assert(link != 0);
857  // First lane of the connection
858  lane = link->getViaLane();
859  assert(lane != 0); // Collect vehicles on the junction
860 
861  getVehiclesOnJunction(junction, distToConflictLane, lane, foeCollector);
862 
863  // Collect vehicles on incoming edges (except the last edge, where we already collected). Use full range.
864  const ConstMSEdgeVector& incoming = junction->getIncoming();
865  for (ConstMSEdgeVector::const_iterator ei = incoming.begin(); ei != incoming.end(); ++ei) {
866  if (*ei == edge || (*ei)->isInternal()) {
867  continue;
868  }
869  getUpstreamVehicles(*ei, (*ei)->getLength(), range, distToConflictLane, lane, foeCollector);
870  }
871 
872  // account for scanned distance on junction
873  double linkLength = link->getInternalLengthsAfter();
874  remainingDownstreamRange -= linkLength;
875  distToConflictLane += linkLength;
876 
877  // update ego's lane to next non internal edge
878  lane = nextNonInternalLane;
879  edge = &(lane->getEdge());
880  }
881  }
882  }
883  // remove ego vehicle
884  foeCollector.erase(&veh);
885 }
886 
887 void
888 MSDevice_SSM::getUpstreamVehicles(const MSEdge* edge, double pos, double range, double egoDistToConflictLane, const MSLane* const egoConflictLane, FoeInfoMap& foeCollector) {
889 #ifdef DEBUG_SSM
890  std::cout << SIMTIME << " getUpstreamVehicles() for edge '" << edge->getID() << "'"
891  << " pos = " << pos << " range = " << range
892  << "\nFound vehicles:"
893  << std::endl;
894 #endif
895  if (range <= 0) {
896  return;
897  }
898 
899  const std::vector<MSLane*>& lanes = edge->getLanes();
900  // Collect vehicles on the given edge with position in [pos-range,pos]
901  for (std::vector<MSLane*>::const_iterator li = lanes.begin(); li != lanes.end(); ++li) {
902  MSLane* lane = *li;
903  const MSLane::VehCont& vehicles = lane->getVehiclesSecure();
904  for (MSLane::VehCont::const_iterator vi = vehicles.begin(); vi != vehicles.end(); ++vi) {
905  MSVehicle* veh = *vi;
906  if (veh->getPositionOnLane() >= pos - range) {
907 #ifdef DEBUG_SSM
908  std::cout << veh->getID() << "\n";
909 #endif
910  FoeInfo* c = new FoeInfo(); // c is deleted in updateEncounter()
911  c->egoDistToConflictLane = egoDistToConflictLane;
912  c->egoConflictLane = egoConflictLane;
913  foeCollector[veh] = c;
914  }
915  }
916  }
917 
918 #ifdef DEBUG_SSM
919  std::cout << std::endl;
920 #endif
921 
922  // TODO: Gather vehicles from opposite direction. This should happen in any case, where opposite direction overtaking is possible.
923  // If it isn't it might still be nicer to trace oncoming vehicles for the resulting trajectories in the encounters
924  // if (edge->hasOpposite...)
925 
926  if (range <= pos) {
927  return;
928  }
929 
930  // Here we have: range > pos, i.e. we proceed collecting vehicles on preceding edges
931  range -= pos;
932 
933  // Collect vehicles from incoming edges of the junction representing the origin of 'edge'
934  const MSJunction* junction = edge->getFromJunction();
935  if (!edge->isInternal()) {
936  // collect vehicles on preceding junction (for internal edges this is already done in caller,
937  // i.e. findSurroundingVehicles() or the recursive call from getUpstreamVehicles())
938  getVehiclesOnJunction(junction, egoDistToConflictLane, egoConflictLane, foeCollector);
939  }
940  // Collect vehicles from incoming edges from the junction representing the origin of 'edge'
941  const ConstMSEdgeVector& incoming = junction->getIncoming();
942  for (ConstMSEdgeVector::const_iterator ei = incoming.begin(); ei != incoming.end(); ++ei) {
943  if ((*ei)->isInternal()) {
944  continue;
945  }
946  const MSEdge* inEdge = *ei;
947  assert(inEdge != 0);
948  double distOnJunction = edge->isInternal() ? 0. : inEdge->getInternalFollowingLengthTo(edge);
949  if (distOnJunction >= range) {
950  continue;
951  }
952  // account for vehicles on the predecessor edge
953  getUpstreamVehicles(inEdge, inEdge->getLength(), range - distOnJunction, egoDistToConflictLane, egoConflictLane, foeCollector);
954  }
955 }
956 
957 void
958 MSDevice_SSM::getVehiclesOnJunction(const MSJunction* junction, double egoDistToConflictLane, const MSLane* const egoConflictLane, FoeInfoMap& foeCollector) {
959 #ifdef DEBUG_SSM
960  std::cout << SIMTIME << " getVehiclesOnJunction() for junction '" << junction->getID() << "'"
961  << "\nFound vehicles:"
962  << std::endl;
963 #endif
964  // Collect vehicles on internal lanes
965  const std::vector<MSLane*>& lanes = junction->getInternalLanes();
966  for (std::vector<MSLane*>::const_iterator li = lanes.begin(); li != lanes.end(); ++li) {
967  MSLane* lane = *li;
968  const MSLane::VehCont& vehicles = lane->getVehiclesSecure();
969 
970  // Add FoeInfos (XXX: for some situations, a vehicle may be collected twice. Then the later finding overwrites the earlier in foeCollector.
971  // This could lead to neglecting a conflict when determining foeConflictLane later.)
972  for (MSLane::VehCont::const_iterator vi = vehicles.begin(); vi != vehicles.end(); ++vi) {
973  FoeInfo* c = new FoeInfo();
974  c->egoConflictLane = egoConflictLane;
975  c->egoDistToConflictLane = egoDistToConflictLane;
976  foeCollector[*vi] = c;
977 #ifdef DEBUG_SSM
978  for (MSLane::VehCont::const_iterator vi = vehicles.begin(); vi != vehicles.end(); ++vi) {
979  std::cout << (*vi)->getID() << "\n";
980  }
981 #endif
982  }
983 
984  // If there is an internal continuation lane, also collect vehicles on that lane
985  if (lane->getLinkCont().size() > 1 && lane->getLinkCont()[0]->getViaLane() != 0) {
986  // There's a second internal lane of the connection
987  lane = lane->getLinkCont()[0]->getViaLane();
988  // This code must be modified, if more than two-piece internal lanes are allowed. Thus, assert:
989  assert(lane->getLinkCont().size() == 0 || lane->getLinkCont()[0]->getViaLane() == 0);
990 
991  // collect vehicles
992  const MSLane::VehCont& vehicles2 = lane->getVehiclesSecure();
993  // Add FoeInfos. This duplicates the loop for the first internal lane
994  for (MSLane::VehCont::const_iterator vi = vehicles2.begin(); vi != vehicles2.end(); ++vi) {
995  FoeInfo* c = new FoeInfo();
996  c->egoConflictLane = egoConflictLane;
997  c->egoDistToConflictLane = egoDistToConflictLane;
998  foeCollector[*vi] = c;
999 #ifdef DEBUG_SSM
1000  for (MSLane::VehCont::const_iterator vi = vehicles.begin(); vi != vehicles.end(); ++vi) {
1001  std::cout << (*vi)->getID() << "\n";
1002  }
1003 #endif
1004  }
1005  }
1006  }
1007 
1008 #ifdef DEBUG_SSM
1009  std::cout << std::endl;
1010 #endif
1011 }
1012 
1013 
1014 
1015 void
1017  // This is called once at vehicle removal.
1018  // Also: flush myOutputFile? Or is this done automatically?
1020 }
1021 
1022 // ---------------------------------------------------------------------------
1023 // Static parameter load helpers
1024 // ---------------------------------------------------------------------------
1025 std::string
1026 MSDevice_SSM::getOutputFilename(const SUMOVehicle& v, std::string deviceID) {
1027  std::string file = deviceID + ".xml";
1028  if (v.getParameter().knowsParameter("device.ssm.file")) {
1029  try {
1030  file = v.getParameter().getParameter("device.ssm.file", file);
1031  } catch (...) {
1032  WRITE_WARNING("Invalid value '" + v.getParameter().getParameter("device.ssm.file", file) + "'for vehicle parameter 'ssm.measures'");
1033  }
1034  } else if (v.getVehicleType().getParameter().knowsParameter("device.ssm.file")) {
1035  try {
1036  file = v.getVehicleType().getParameter().getParameter("device.ssm.file", file);
1037  } catch (...) {
1038  WRITE_WARNING("Invalid value '" + v.getVehicleType().getParameter().getParameter("device.ssm.file", file) + "'for vType parameter 'ssm.measures'");
1039  }
1040  } else {
1041  std::cout << "vehicle '" << v.getID() << "' does not supply vehicle parameter 'device.ssm.file'. Using default of '" << file << "'\n";
1042  }
1043  return file;
1044 }
1045 
1046 
1047 double
1050  double range = INVALID;
1051  if (v.getParameter().knowsParameter("device.ssm.range")) {
1052  try {
1053  range = TplConvert::_2double(v.getParameter().getParameter("device.ssm.range", "").c_str());
1054  } catch (...) {
1055  WRITE_WARNING("Invalid value '" + v.getParameter().getParameter("device.ssm.range", "") + "'for vehicle parameter 'ssm.range'");
1056  }
1057  } else if (v.getVehicleType().getParameter().knowsParameter("device.ssm.range")) {
1058  try {
1059  range = TplConvert::_2double(v.getVehicleType().getParameter().getParameter("device.ssm.range", "").c_str());
1060  } catch (...) {
1061  WRITE_WARNING("Invalid value '" + v.getVehicleType().getParameter().getParameter("device.ssm.range", "") + "'for vType parameter 'ssm.range'");
1062  }
1063  } else {
1064  range = oc.getFloat("device.ssm.range");
1065 #ifdef DEBUG_SSM
1066  std::cout << "vehicle '" << v.getID() << "' does not supply vehicle parameter 'device.ssm.range'. Using default of '" << range << "'\n";
1067 #endif
1068  }
1069  return range;
1070 }
1071 
1072 double
1075  double frequency = INVALID;
1076  if (v.getParameter().knowsParameter("device.ssm.frequency")) {
1077  try {
1078  frequency = TplConvert::_2double(v.getParameter().getParameter("device.ssm.frequency", "").c_str());
1079  } catch (...) {
1080  WRITE_WARNING("Invalid value '" + v.getParameter().getParameter("device.ssm.frequency", "") + "'for vehicle parameter 'ssm.frequency'");
1081  }
1082  } else if (v.getVehicleType().getParameter().knowsParameter("device.ssm.frequency")) {
1083  try {
1084  frequency = TplConvert::_2double(v.getVehicleType().getParameter().getParameter("device.ssm.frequency", "").c_str());
1085  } catch (...) {
1086  WRITE_WARNING("Invalid value '" + v.getVehicleType().getParameter().getParameter("device.ssm.frequency", "") + "'for vType parameter 'ssm.frequency'");
1087  }
1088  } else {
1089  frequency = oc.getFloat("device.ssm.frequency");
1090 #ifdef DEBUG_SSM
1091  std::cout << "vehicle '" << v.getID() << "' does not supply vehicle parameter 'device.ssm.frequency'. Using default of '" << frequency << "'\n";
1092 #endif
1093  }
1094  return frequency;
1095 }
1096 
1097 bool
1100  bool trajectories = false;
1101  if (v.getParameter().knowsParameter("device.ssm.trajectories")) {
1102  try {
1103  trajectories = TplConvert::_2bool(v.getParameter().getParameter("device.ssm.trajectories", "no").c_str());
1104  } catch (...) {
1105  WRITE_WARNING("Invalid value '" + v.getParameter().getParameter("device.ssm.trajectories", "no") + "'for vehicle parameter 'ssm.trajectories'");
1106  }
1107  } else if (v.getVehicleType().getParameter().knowsParameter("device.ssm.trajectories")) {
1108  try {
1109  trajectories = TplConvert::_2bool(v.getVehicleType().getParameter().getParameter("device.ssm.trajectories", "no").c_str());
1110  } catch (...) {
1111  WRITE_WARNING("Invalid value '" + v.getVehicleType().getParameter().getParameter("device.ssm.trajectories", "no") + "'for vType parameter 'ssm.trajectories'");
1112  }
1113  } else {
1114  trajectories = oc.getBool("device.ssm.trajectories");
1115 #ifdef DEBUG_SSM
1116  std::cout << "vehicle '" << v.getID() << "' does not supply vehicle parameter 'device.ssm.trajectories'. Using default of '" << trajectories << "'\n";
1117 #endif
1118  }
1119  return trajectories;
1120 }
1121 
1122 
1123 bool
1124 MSDevice_SSM::getMeasuresAndThresholds(const SUMOVehicle& v, std::string deviceID, std::vector<double>& thresholds, std::vector<std::string>& measures) {
1126 
1127  // Measures
1128  std::string measures_str = "";
1129  if (v.getParameter().knowsParameter("device.ssm.measures")) {
1130  try {
1131  measures_str = v.getParameter().getParameter("device.ssm.measures", "");
1132  } catch (...) {
1133  WRITE_WARNING("Invalid value '" + v.getParameter().getParameter("device.ssm.measures", "") + "'for vehicle parameter 'ssm.measures'");
1134  }
1135  } else if (v.getVehicleType().getParameter().knowsParameter("device.ssm.measures")) {
1136  try {
1137  measures_str = v.getVehicleType().getParameter().getParameter("device.ssm.measures", "");
1138  } catch (...) {
1139  WRITE_WARNING("Invalid value '" + v.getVehicleType().getParameter().getParameter("device.ssm.measures", "") + "'for vType parameter 'ssm.measures'");
1140  }
1141  } else {
1142  measures_str = oc.getString("device.ssm.measures");
1143  std::cout << "vehicle '" << v.getID() << "' does not supply vehicle parameter 'device.ssm.measures'. Using default of '" << measures_str << "'\n";
1144  }
1145 
1146  // Check retrieved measures
1147  if (measures_str == "") {
1148  WRITE_WARNING("No measures specified for ssm device of vehicle '" + v.getID() + "'. Registering all available SSMs.");
1149  measures_str = AVAILABLE_SSMS;
1150  }
1152  std::vector<std::string> available = st.getVector();
1153  st = StringTokenizer(measures_str);
1154  measures = st.getVector();
1155  for (std::vector<std::string>::const_iterator i = measures.begin(); i != measures.end(); ++i) {
1156  if (std::find(available.begin(), available.end(), *i) == available.end()) {
1157  // Given identifier is unknown
1158  WRITE_ERROR("SSM identifier '" + *i + "' is not supported. Aborting construction of SSM device '" + deviceID + "'.");
1159  return false;
1160  }
1161  }
1162 
1163  // Thresholds
1164  std::string thresholds_str = "";
1165  if (v.getParameter().knowsParameter("device.ssm.thresholds")) {
1166  try {
1167  thresholds_str = v.getParameter().getParameter("device.ssm.thresholds", "");
1168  } catch (...) {
1169  WRITE_WARNING("Invalid value '" + v.getParameter().getParameter("device.ssm.thresholds", "") + "'for vehicle parameter 'ssm.thresholds'");
1170  }
1171  } else if (v.getVehicleType().getParameter().knowsParameter("device.ssm.thresholds")) {
1172  try {
1173  thresholds_str = v.getVehicleType().getParameter().getParameter("device.ssm.thresholds", "");
1174  } catch (...) {
1175  WRITE_WARNING("Invalid value '" + v.getVehicleType().getParameter().getParameter("device.ssm.thresholds", "") + "'for vType parameter 'ssm.thresholds'");
1176  }
1177  } else {
1178  thresholds_str = oc.getString("device.ssm.thresholds");
1179 #ifdef DEBUG_SSM
1180  std::cout << "vehicle '" << v.getID() << "' does not supply vehicle parameter 'device.ssm.thresholds'. Using default of '" << thresholds_str << "'\n";
1181 #endif
1182  }
1183 
1184  // Parse vector of doubles from threshold_str
1185  if (thresholds_str != "") {
1186  st = StringTokenizer(thresholds_str);
1187  while (st.hasNext()) {
1188  thresholds.push_back(TplConvert::_2double(st.next().c_str()));
1189  }
1190  if (thresholds.size() != measures.size()) {
1191  WRITE_ERROR("Given list of thresholds ('" + thresholds_str + "') has not the same length as the assumed list of measures ('" + measures_str + "').");
1192  return false;
1193  }
1194  } else {
1195  // assume default thresholds if none are given
1196  for (std::vector<std::string>::const_iterator i = measures.begin(); i != measures.end(); ++i) {
1197  if (*i == "TTC") {
1198  thresholds.push_back(DEFAULT_THRESHOLD_TTC);
1199  } else if (*i == "DRAC") {
1200  thresholds.push_back(DEFAULT_THRESHOLD_DRAC);
1201  } else if (*i == "PET") {
1202  thresholds.push_back(DEFAULT_THRESHOLD_PET);
1203  } else {
1204  WRITE_ERROR("Unknown SSM identifier '" + (*i) + "'. Aborting construction of ssm device."); // should never occur
1205  return false;
1206  }
1207  }
1208  }
1209  return true;
1210 }
1211 
1212 
1213 
1214 
1215 
1216 /****************************************************************************/
1217 
void doRegister(const std::string &name, Option *v)
Adds an option under the given name.
Definition: OptionsCont.cpp:82
void processEncounters(FoeInfoMap &foes)
Finds encounters for which the foe vehicle has disappeared from range and if an ended encounter is qu...
std::vector< double > timespan
time points corresponding to the trajectories
Definition: MSDevice_SSM.h:144
OutputDevice & writeAttr(const SumoXMLAttr attr, const T &val)
writes a named attribute
Definition: OutputDevice.h:256
#define AVAILABLE_SSMS
static const std::set< MSDevice * > & getInstances()
returns all currently existing SSM devices
MSEdge & getEdge() const
Returns the lane&#39;s edge.
Definition: MSLane.h:582
static void getVehiclesOnJunction(const MSJunction *, double egoDistToConflictLane, const MSLane *const egoConflictLane, FoeInfoMap &foeCollector)
Collects all vehicles on the junction into foeCollector.
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:83
static bool getMeasuresAndThresholds(const SUMOVehicle &v, std::string deviceID, std::vector< double > &thresholds, std::vector< std::string > &measures)
#define DEFAULT_RANGE
void createEncounters(FoeInfoMap &foes)
Makes new encounters for all given vehicles (these should be the ones entering the device&#39;s range in ...
std::string next()
bool notifyLeave(SUMOVehicle &veh, double lastPos, MSMoveReminder::Notification reason, const MSLane *enteredLane=0)
Called whenever the holder leaves a lane.
Position getVelocityVector() const
Returns the vehicle&#39;s direction in radians.
Definition: MSVehicle.h:602
static void cleanup()
Clean up remaining devices instances.
EncounterType
Different types of encounters corresponding to relative positions of the vehicles. The name describes the type from the ego perspective.
Definition: MSDevice_SSM.h:68
MSLane * getLane() const
Returns the lane the vehicle is on.
Definition: MSVehicle.h:488
static bool _2bool(const E *const data)
converts a 0-terminated char-type array into the boolean value described by it
Definition: TplConvert.h:371
static void insertOptions(OptionsCont &oc)
Inserts MSDevice_SSM-options.
MSVehicle * myHolderMS
Definition: MSDevice_SSM.h:400
SUMOVehicle & myHolder
The vehicle that stores the device.
Definition: MSDevice.h:187
A device which collects info on the vehicle trip (mainly on departure and arrival) ...
Definition: MSDevice_SSM.h:60
The base class for an intersection.
Definition: MSJunction.h:64
double getPositionOnLane() const
Get the vehicle&#39;s position along the lane.
Definition: MSVehicle.h:375
EncounterVector myActiveEncounters
Definition: MSDevice_SSM.h:407
#define INVALID
const MSLane * findFoeConflictLane(const MSVehicle *foe, const MSLane *egoConflictLane, double &distToConflictLane)
Computes the conflict lane for the foe.
static double getLoggingFrequency(const SUMOVehicle &v)
Notification
Definition of a vehicle state.
virtual MSLane * getLane() const =0
Returns the lane the vehicle is on.
const std::vector< MSLane * > & getLanes() const
Returns this edge&#39;s lanes.
Definition: MSEdge.h:192
double getLength() const
Returns the lane&#39;s length.
Definition: MSLane.h:484
virtual const VehCont & getVehiclesSecure() const
Returns the vehicles container; locks it for microsimulation.
Definition: MSLane.h:379
void closeEncounter(Encounter *e)
Finalizes the encounter and calculates SSM values.
MSLink * getLinkTo(const MSLane *) const
returns the link to the given lane or 0, if it is not connected
Definition: MSLane.cpp:1604
Position getPosition(const double offset=0) const
Return current position (x/y, cartesian)
Definition: MSVehicle.cpp:777
static double getDetectionRange(const SUMOVehicle &v)
std::vector< const MSEdge * > ConstMSEdgeVector
Definition: MSEdge.h:78
bool getBool(const std::string &name) const
Returns the boolean-value of the named option (only for Option_Bool)
const std::string & getID() const
Returns the id.
Definition: Named.h:66
ENCOUNTER_TYPE_CROSSING.
Definition: MSDevice_SSM.h:89
#define TS
Definition: SUMOTime.h:52
#define DEFAULT_THRESHOLD_PET
double getLength() const
return the length of the edge
Definition: MSEdge.h:586
const MSJunction * getToJunction() const
Definition: MSEdge.h:372
void updateEncounter(Encounter *e, FoeInfo *foeInfo)
Updates the encounter (adds a new trajectory point) and deletes the foeInfo.
void generateOutput() const
Finalizes output. Called on vehicle removal.
#define UNUSED_PARAMETER(x)
Definition: StdDefs.h:38
#define WRITE_WARNING(msg)
Definition: MsgHandler.h:200
static OptionsCont & getOptions()
Retrieves the options.
Definition: OptionsCont.cpp:65
#define SIMTIME
Definition: SUMOTime.h:70
static bool requestsTrajectories(const SUMOVehicle &v)
ENCOUNTER_TYPE_FOLLOWING_LEADER.
Definition: MSDevice_SSM.h:77
bool isInternal() const
Definition: MSLane.cpp:1497
A road/street connecting two junctions.
Definition: MSEdge.h:80
bool isOnRoad() const
Returns the information whether the vehicle is on a road (is simulated)
Definition: MSVehicle.h:510
int maxTrajectorySize
Corresponding maximal trajectory size in points, derived from myFrequency.
Definition: MSDevice_SSM.h:397
void add(double time, Position egoX, Position egoV, Position foeX, Position foeV)
add a new data point
Encounter(const MSVehicle *_ego, const MSVehicle *const _foe, double _begin)
Constructor.
ENCOUNTER_TYPE_MERGING.
Definition: MSDevice_SSM.h:80
void computeSSMs(Encounter *e)
double myFrequency
Maximal timespan duration for a single encounter.
Definition: MSDevice_SSM.h:394
MSLink * getEntryLink() const
Returns the entry link if this is an internal lane, else 0.
Definition: MSLane.cpp:1625
bool knowsParameter(const std::string &key) const
Returns whether the parameter is known.
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:56
static void getUpstreamVehicles(const MSEdge *edge, double pos, double range, double egoDistToConflictLane, const MSLane *const egoConflictLane, FoeInfoMap &foeCollector)
Collects all vehicles within range &#39;range&#39; upstream of the position &#39;pos&#39; on the edge &#39;edge&#39; into foe...
Representation of a vehicle.
Definition: SUMOVehicle.h:67
#define DEFAULT_THRESHOLD_DRAC
A point in 2D or 3D with translation and scaling methods.
Definition: Position.h:46
bool myComputeTTC
Flags for switching on / off comutation of different SSMs, derived from myMeasures.
Definition: MSDevice_SSM.h:399
OutputDevice * myOutputFile
Output device.
Definition: MSDevice_SSM.h:413
void resetEncounters()
Closes all current Encounters and moves conflicts to myPastConflicts,.
const std::vector< MSLane * > & getBestLanesContinuation() const
Returns the best sequence of lanes to continue the route starting at myLane.
Definition: MSVehicle.cpp:3276
double myRange
Definition: MSDevice_SSM.h:395
std::string getString(const std::string &name) const
Returns the string-value of the named option (only for Option_String)
ENCOUNTER_TYPE_FOLLOWING_FOLLOWER.
Definition: MSDevice_SSM.h:75
MSDevice_SSM(SUMOVehicle &holder, const std::string &id, std::string outputFilename, std::vector< std::string > measures, std::vector< double > thresholds, bool trajectories, double frequency, double range)
Constructor.
bool notifyEnter(SUMOVehicle &veh, MSMoveReminder::Notification reason, const MSLane *enteredLane=0)
Called whenever the holder enteres a lane.
const ConstMSEdgeVector & getIncoming() const
Definition: MSJunction.h:105
bool qualifiesAsConflict(Encounter *e)
Tests if the SSM values exceed the threshold for qualification as conflict.
static void insertDefaultAssignmentOptions(const std::string &deviceName, const std::string &optionsTopic, OptionsCont &oc)
Adds common command options that allow to assign devices to vehicles.
Definition: MSDevice.cpp:93
void addOptionSubTopic(const std::string &topic)
Adds an option subtopic.
virtual bool isOnRoad() const =0
Returns the information whether the vehicle is on a road (is simulated)
void updateAndWriteOutput()
This is called once per time step in MSNet::writeOutput() and collects the surrounding vehicles...
bool isInternal() const
return whether this edge is an internal edge
Definition: MSEdge.h:254
ENCOUNTER_TYPE_FOLLOWING.
Definition: MSDevice_SSM.h:73
bool mySaveTrajectories
Definition: MSDevice_SSM.h:391
const SUMOVTypeParameter & getParameter() const
static std::string getOutputFilename(const SUMOVehicle &v, std::string deviceID)
double getFloat(const std::string &name) const
Returns the double-value of the named option (only for Option_Float)
std::vector< MSVehicle * > VehCont
Container for vehicles.
Definition: MSLane.h:91
std::vector< std::string > myMeasures
Definition: MSDevice_SSM.h:389
const MSVehicle * ego
Definition: MSDevice_SSM.h:132
void flushConflicts(bool all=false)
Writes out all past conflicts that have begun earlier than time t-myFrequency (i.e. no active encounter can have an earlier begin)
std::vector< std::string > getVector()
#define WRITE_ERROR(msg)
Definition: MsgHandler.h:206
Abstract in-vehicle device.
Definition: MSDevice.h:71
bool myComputeDRAC
Definition: MSDevice_SSM.h:399
const std::string & getParameter(const std::string &key, const std::string &defaultValue) const
Returns the value for a given key.
std::vector< Encounter * > EncounterVector
Definition: MSDevice_SSM.h:184
std::vector< double > myThresholds
Definition: MSDevice_SSM.h:390
static bool equippedByDefaultAssignmentOptions(const OptionsCont &oc, const std::string &deviceName, SUMOVehicle &v)
Determines whether a vehicle should get a certain device.
Definition: MSDevice.cpp:107
std::map< const MSVehicle *, FoeInfo * > FoeInfoMap
Definition: MSDevice_SSM.h:185
const MSLane * egoConflictLane
Definition: MSDevice_SSM.h:174
Trajectory egoTrajectory
Trajectory of the ego vehicle.
Definition: MSDevice_SSM.h:146
static OutputDevice & getDevice(const std::string &name)
Returns the described OutputDevice.
virtual const SUMOVehicleParameter & getParameter() const =0
Returns the vehicle&#39;s parameter (including departure definition)
static std::set< MSDevice * > * instances
All currently existing SSM devices.
Definition: MSDevice_SSM.h:64
~MSDevice_SSM()
Destructor.
const MSJunction * getFromJunction() const
Definition: MSEdge.h:368
#define DEFAULT_FREQUENCY
A storage for options typed value containers)
Definition: OptionsCont.h:99
static double _2double(const E *const data)
converts a char-type array into the double value described by it
Definition: TplConvert.h:297
double getInternalFollowingLengthTo(const MSEdge *followerAfterInternal) const
returns the length of all internal edges on the junction until reaching the non-internal edge followe...
Definition: MSEdge.cpp:670
ENCOUNTER_TYPE_NOCONFLICT.
Definition: MSDevice_SSM.h:70
double getBackPositionOnLane(const MSLane *lane) const
Get the vehicle&#39;s position relative to the given lane.
Definition: MSVehicle.cpp:2403
An encounter is an episode involving two vehicles, which are closer to each other than some specified...
Definition: MSDevice_SSM.h:103
bool closeTag()
Closes the most recently opened tag.
virtual const std::vector< MSLane * > & getInternalLanes() const
Returns all internal lanes on the junction.
Definition: MSJunction.h:115
#define DEFAULT_THRESHOLD_TTC
void addDescription(const std::string &name, const std::string &subtopic, const std::string &description)
Adds a description for an option.
const MSLinkCont & getLinkCont() const
returns the container with all links !!!
Definition: MSLane.cpp:1597
void writeOutConflict(Encounter *e)
const MSRouteIterator & getCurrentRouteEdge() const
Returns an iterator pointing to the current edge in this vehicles route.
bool notifyMove(SUMOVehicle &veh, double oldPos, double newPos, double newSpeed)
Checks for waiting steps when the vehicle moves.
const std::string & getID() const
Returns the name of the vehicle.
static bool gUseMesoSim
Definition: MSGlobals.h:98
Representation of a lane in the micro simulation.
Definition: MSLane.h:79
const MSVehicle *const foe
Definition: MSDevice_SSM.h:133
virtual const std::string & getID() const =0
Get the vehicle&#39;s ID.
std::string joinToString(const std::vector< T > &v, const T_BETWEEN &between, std::streamsize accuracy=gPrecision)
Definition: ToString.h:228
EncounterQueue myPastConflicts
Past encounters that where qualified as conflicts and are not yet flushed to the output file...
Definition: MSDevice_SSM.h:409
OutputDevice & openTag(const std::string &xmlElement)
Opens an XML tag.
static void buildVehicleDevices(SUMOVehicle &v, std::vector< MSDevice *> &into)
Build devices for the given vehicle, if needed.
Trajectory foeTrajectory
Trajectory of the foe vehicle.
Definition: MSDevice_SSM.h:148
static void findSurroundingVehicles(const MSVehicle &veh, double range, FoeInfoMap &foeCollector)
Returns all vehicles, which are within the given range of the given vehicle.
virtual const MSVehicleType & getVehicleType() const =0
Returns the vehicle&#39;s type.
std::priority_queue< Encounter *, std::vector< Encounter * >, Encounter::compare > EncounterQueue
Definition: MSDevice_SSM.h:183