SUMO - Simulation of Urban MObility
MSDevice_SSM.cpp
Go to the documentation of this file.
1 /****************************************************************************/
2 // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3 // Copyright (C) 2013-2017 German Aerospace Center (DLR) and others.
4 /****************************************************************************/
5 //
6 // This program and the accompanying materials
7 // are made available under the terms of the Eclipse Public License v2.0
8 // which accompanies this distribution, and is available at
9 // http://www.eclipse.org/legal/epl-v20.html
10 //
11 /****************************************************************************/
20 // An SSM-device logs encounters / conflicts of the carrying vehicle with other surrounding vehicles
21 // XXX: Preliminary implementation. Use with care. Especially rerouting vehicles could be problematic.
22 // TODO: implement SSM time-gap (estimated conflict entry and exit times are already calculated for PET calculation)
23 /****************************************************************************/
24 
25 // ===========================================================================
26 // included modules
27 // ===========================================================================
28 #ifdef _MSC_VER
29 #include <windows_config.h>
30 #else
31 #include <config.h>
32 #endif
33 
34 #include <iostream>
36 #include <utils/geom/GeomHelper.h>
41 #include <microsim/MSNet.h>
42 #include <microsim/MSJunction.h>
43 #include <microsim/MSLane.h>
44 #include <microsim/MSEdge.h>
45 #include <microsim/MSVehicle.h>
47 #include <utils/geom/Position.h>
49 #include "MSDevice_SSM.h"
50 
51 // ===========================================================================
52 // Debug constants
53 // ===========================================================================
54 //#define DEBUG_SSM
55 //#define DEBUG_SSM_DRAC
56 //#define DEBUG_SSM_SURROUNDING
57 // #define DEBUG_SSM_NOTIFICATIONS
58 
59 // ===========================================================================
60 // Constants
61 // ===========================================================================
62 // value indicating an invalid double parameter
63 #define INVALID std::numeric_limits<double>::max()
64 
65 // default value for the detection range of potential opponents
66 #define DEFAULT_RANGE 50.0
67 
68 // list of implemented SSMs (NOTE: To add more SSMs, identifiers are added to AVAILABLE_SSMS
69 // and a default threshold must be defined. A corresponding
70 // case should be added to the switch in buildVehicleDevices,
71 // and in computeSSMs(), the SSM-value should be computed.)
72 #define AVAILABLE_SSMS "TTC DRAC PET"
73 #define DEFAULT_THRESHOLD_TTC 3. // in [s.], events get logged if below threshold (1.5s. is an appropriate criticality threshold according to Van der Horst, A. R. A. (1991). Time-to-collision as a Cue for Decision-making in Braking [also see Guido et al. 2011])
74 #define DEFAULT_THRESHOLD_DRAC 3. // in [m/s^2], events get logged if above threshold (3.4s. is an appropriate criticality threshold according to American Association of State Highway and Transportation Officials (2004). A Policy on Geometric Design of Highways and Streets [also see Guido et al. 2011])
75 #define DEFAULT_THRESHOLD_PET 2. // in seconds, events get logged if below threshold
76 #define DEFAULT_EXTRA_TIME 5. // in seconds, events get logged for extra time even if encounter is over
77 
78 // ===========================================================================
79 // method definitions
80 // ===========================================================================
81 
82 
83 
85 std::ostream& operator<<(std::ostream& out, MSDevice_SSM::EncounterType type) {
86  switch (type) {
88  out << "NOCONFLICT_AHEAD";
89  break;
91  out << "FOLLOWING";
92  break;
94  out << "FOLLOWING_FOLLOWER";
95  break;
97  out << "FOLLOWING_LEADER";
98  break;
100  out << "ON_ADJACENT_LANES";
101  break;
103  out << "MERGING";
104  break;
106  out << "MERGING_LEADER";
107  break;
109  out << "MERGING_FOLLOWER";
110  break;
112  out << "MERGING_ADJACENT";
113  break;
115  out << "CROSSING";
116  break;
118  out << "CROSSING_LEADER";
119  break;
121  out << "CROSSING_FOLLOWER";
122  break;
124  out << "EGO_ENTERED_CONFLICT_AREA";
125  break;
127  out << "FOE_ENTERED_CONFLICT_AREA";
128  break;
130  out << "BOTH_ENTERED_CONFLICT_AREA";
131  break;
133  out << "EGO_LEFT_CONFLICT_AREA";
134  break;
136  out << "FOE_LEFT_CONFLICT_AREA";
137  break;
139  out << "BOTH_LEFT_CONFLICT_AREA";
140  break;
142  out << "FOLLOWING_PASSED";
143  break;
145  out << "MERGING_PASSED";
146  break;
147  // Collision (currently unused, might be differentiated further)
149  out << "COLLISION";
150  break;
151  default:
152  out << "unknown type (" << int(type) << ")";
153  break;
154  }
155  return out;
156 }
157 
158 
159 // ---------------------------------------------------------------------------
160 // static initialisation methods
161 // ---------------------------------------------------------------------------
162 
163 std::set<MSDevice*>* MSDevice_SSM::instances = new std::set<MSDevice*>();
164 
165 std::set<std::string> MSDevice_SSM::createdOutputFiles;
166 
167 const std::set<MSDevice*>&
169  return *instances;
170 }
171 
172 void
174  // Close current encounters and flush conflicts to file for all existing devices
175  if (instances != 0) {
176  for (std::set<MSDevice*>::iterator ii = instances->begin(); ii != instances->end(); ++ii) {
177  static_cast<MSDevice_SSM*>(*ii)->resetEncounters();
178  static_cast<MSDevice_SSM*>(*ii)->flushConflicts(true);
179  }
180  instances->clear();
181  }
182  for (auto& fn : createdOutputFiles) {
184  file->closeTag();
185  }
186 }
187 
188 void
190  oc.addOptionSubTopic("SSM Device");
191  insertDefaultAssignmentOptions("ssm", "SSM Device", oc);
192 
193  // custom options
194  oc.doRegister("device.ssm.measures", new Option_String(""));
195  oc.addDescription("device.ssm.measures", "SSM Device", "Specifies which measures will be logged (as a space seperated sequence of IDs in ('TTC', 'DRAC', 'PET')).");
196  oc.doRegister("device.ssm.thresholds", new Option_String(""));
197  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.");
198  oc.doRegister("device.ssm.trajectories", new Option_Bool(false));
199  oc.addDescription("device.ssm.trajectories", "SSM Device", "Specifies whether trajectories will be logged (if false, only the extremal values and times are reported, this is the default).");
200  oc.doRegister("device.ssm.range", new Option_Float(DEFAULT_RANGE));
201  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.");
202  oc.doRegister("device.ssm.extratime", new Option_Float(DEFAULT_EXTRA_TIME));
203  oc.addDescription("device.ssm.extratime", "SSM Device", "Specifies the time in seconds to be logged after a conflict is over (default is " + toString(DEFAULT_EXTRA_TIME) + "secs.). Required >0 if PET is to be calculated for crossing conflicts.");
204  oc.doRegister("device.ssm.geo", new Option_Bool(false));
205  oc.addDescription("device.ssm.geo", "SSM Device", "Whether to use coordinates of the original reference system in output (default is false).");
206 }
207 
208 void
209 MSDevice_SSM::buildVehicleDevices(SUMOVehicle& v, std::vector<MSDevice*>& into) {
212  WRITE_WARNING("SSM Device for vehicle '" + v.getID() + "' will not be built. (SSMs not supported in MESO)");
213  return;
214  }
215  // ID for the device
216  std::string deviceID = "ssm_" + v.getID();
217 
218  // Load parameters:
219 
220  // Measures and thresholds
221  std::map<std::string, double> thresholds;
222  bool success = getMeasuresAndThresholds(v, deviceID, thresholds);
223  if (!success) {
224  return;
225  }
226 
227  // TODO: modify trajectory option: "all", "conflictPoints", ("position" && "speed" == "vehState"), "SSMs"!
228  // Trajectories
229  bool trajectories = requestsTrajectories(v);
230 
231  // detection range
232  double range = getDetectionRange(v);
233 
234  // extra time
235  double extraTime = getExtraTime(v);
236 
237  // File
238  std::string file = getOutputFilename(v, deviceID);
239 
240  const bool useGeo = useGeoCoords(v);
241 
242  // Build the device (XXX: who deletes it?)
243  MSDevice_SSM* device = new MSDevice_SSM(v, deviceID, file, thresholds, trajectories, range, extraTime, useGeo);
244  into.push_back(device);
245  }
246 }
247 
248 
249 MSDevice_SSM::Encounter::Encounter(const MSVehicle* _ego, const MSVehicle* const _foe, double _begin, double extraTime) :
250  ego(_ego),
251  foe(_foe),
252  egoID(_ego->getID()),
253  foeID(_foe->getID()),
254  begin(_begin),
255  end(-INVALID),
256  currentType(ENCOUNTER_TYPE_NOCONFLICT_AHEAD),
257  remainingExtraTime(extraTime),
258  egoConflictEntryTime(INVALID),
259  egoConflictExitTime(INVALID),
260  foeConflictEntryTime(INVALID),
261  foeConflictExitTime(INVALID),
262  minTTC(INVALID, Position::invalidPosition(), ENCOUNTER_TYPE_NOCONFLICT_AHEAD, INVALID),
263  maxDRAC(INVALID, Position::invalidPosition(), ENCOUNTER_TYPE_NOCONFLICT_AHEAD, INVALID),
264  PET(INVALID, Position::invalidPosition(), ENCOUNTER_TYPE_NOCONFLICT_AHEAD, INVALID),
265  closingRequested(false) {
266 #ifdef DEBUG_SSM
267  std::cout << "\n" << SIMTIME << " Constructing encounter of '"
268  << ego->getID() << "' and '" << foe->getID() << "'" << std::endl;
269 #endif
270 }
271 
273 #ifdef DEBUG_SSM
274  std::cout << "\n" << SIMTIME << " Destroying encounter of '"
275  << egoID << "' and '" << foeID << "' (begin was " << begin << ")" << std::endl;
276 #endif
277 }
278 
279 
280 void
281 MSDevice_SSM::Encounter::add(double time, const EncounterType type, Position egoX, Position egoV, Position foeX, Position foeV,
282  Position conflictPoint, double egoDistToConflict, double foeDistToConflict, double ttc, double drac, std::pair<double, double> pet) {
283 #ifdef DEBUG_SSM
284  std::cout << time << " Adding data point for encounter of '" << egoID << "' and '" << foeID << "':\n"
285  << "type=" << type << ", egoDistToConflict=" << (egoDistToConflict == INVALID ? "NA" : toString(egoDistToConflict))
286  << ", foeDistToConflict=" << (foeDistToConflict == INVALID ? "NA" : toString(foeDistToConflict))
287  << ",\nttc=" << (ttc == INVALID ? "NA" : toString(ttc))
288  << ", drac=" << (drac == INVALID ? "NA" : toString(drac))
289  << ", pet=" << (pet.second == INVALID ? "NA" : toString(pet.second))
290  << std::endl;
291 #endif
292  currentType = type;
293 
294  timeSpan.push_back(time);
295  typeSpan.push_back(type);
296  egoTrajectory.x.push_back(egoX);
297  egoTrajectory.v.push_back(egoV);
298  foeTrajectory.x.push_back(foeX);
299  foeTrajectory.v.push_back(foeV);
300  conflictPointSpan.push_back(conflictPoint);
301  egoDistsToConflict.push_back(egoDistToConflict);
302  foeDistsToConflict.push_back(foeDistToConflict);
303 
304  TTCspan.push_back(ttc);
305  if (ttc != INVALID && (ttc < minTTC.value || minTTC.value == INVALID)) {
306  minTTC.value = ttc;
307  minTTC.time = time;
308  minTTC.pos = conflictPoint;
309  minTTC.type = type;
310  }
311 
312  DRACspan.push_back(drac);
313  if (drac != INVALID && (drac > maxDRAC.value || maxDRAC.value == INVALID)) {
314  maxDRAC.value = drac;
315  maxDRAC.time = time;
316  maxDRAC.pos = conflictPoint;
317  maxDRAC.type = type;
318  }
319 
320  if (pet.first != INVALID && (PET.value >= pet.second || PET.value == INVALID)) {
321  PET.value = pet.second;
322  PET.time = pet.first;
323  PET.pos = conflictPoint;
324  PET.type = type;
325  }
326 }
327 
328 
329 void
331  remainingExtraTime = value;
332 }
333 
334 
335 void
337  remainingExtraTime -= amount;
338 }
339 
340 
341 double
343  return remainingExtraTime;
344 }
345 
346 
348  encounter(e),
350  conflictPoint(Position::invalidPosition()),
351  egoConflictEntryDist(INVALID),
352  foeConflictEntryDist(INVALID),
353  egoConflictExitDist(INVALID),
354  foeConflictExitDist(INVALID),
355  egoEstimatedConflictEntryTime(INVALID),
356  foeEstimatedConflictEntryTime(INVALID),
357  egoEstimatedConflictExitTime(INVALID),
358  foeEstimatedConflictExitTime(INVALID),
359  egoConflictAreaLength(INVALID),
360  foeConflictAreaLength(INVALID),
361  egoLeftConflict(false),
362  foeLeftConflict(false),
363  ttc(INVALID),
364  drac(INVALID),
365  pet(std::make_pair(INVALID, INVALID)) {
366 }
367 
368 
369 void
371  if (myHolder.isOnRoad()) {
372  update();
373  // Write out past conflicts
374  flushConflicts();
375  } else {
376  resetEncounters();
377  // Write out past conflicts
378  flushConflicts(true);
379  }
380 }
381 
382 void
384 #ifdef DEBUG_SSM
385  std::cout << "\n" << SIMTIME << " Device '" << getID() << "' update()\n"
386  << "Size of myActiveEncounters: " << myActiveEncounters.size()
387  << "\nSize of myPastConflicts: " << myPastConflicts.size()
388  << std::endl;
389 #endif
390  // Scan surroundings for other vehicles
391  FoeInfoMap foes;
393 
394 #ifdef DEBUG_SSM
395  if (foes.size() > 0) {
396  std::cout << "Scanned surroundings: Found potential foes:\n";
397  for (FoeInfoMap::const_iterator i = foes.begin(); i != foes.end(); ++i) {
398  std::cout << i->first->getID() << " ";
399  }
400  std::cout << std::endl;
401  } else {
402  std::cout << "Scanned surroundings: No potential conflict could be identified." << std::endl;
403  }
404 #endif
405 
406  // Update encounters and conflicts -> removes all foes (and deletes corresponding FoeInfos) for which already a corresponding encounter exists
407  processEncounters(foes);
408 
409  // Make new encounters for all foes, which were not removed by processEncounters (and deletes corresponding FoeInfos)
410  createEncounters(foes);
411  foes.clear();
412 }
413 
414 
415 void
417 #ifdef DEBUG_SSM
418  std::cout << "\n" << SIMTIME << " Device '" << getID() << "' createEncounters()" << std::endl;
419  std::cout << "New foes:\n";
420  for (FoeInfoMap::const_iterator vi = foes.begin(); vi != foes.end(); ++vi) {
421  std::cout << vi->first->getID() << "\n";
422  }
423  std::cout << std::endl;
424 #endif
425 
426  for (FoeInfoMap::const_iterator foe = foes.begin(); foe != foes.end(); ++foe) {
427  std::pair<MSLane*, MSLane*> conflictLanes;
428  Encounter* e = new Encounter(myHolderMS, foe->first, SIMTIME, myExtraTime);
429  updateEncounter(e, foe->second); // deletes foe->second
431  assert(myActiveEncounters.empty());
433  }
434  assert(myOldestActiveEncounterBegin <= e->begin);
435  myActiveEncounters.push_back(e);
436  }
437 }
438 
439 void
441  // Call processEncounters() with empty vehicle set
442  FoeInfoMap foes;
443  // processEncounters with empty argument closes all encounters
444  processEncounters(foes, true);
445 }
446 
447 void
449 #ifdef DEBUG_SSM
450  std::cout << "\n" << SIMTIME << " Device '" << getID() << "' processEncounters()" << std::endl;
451  std::cout << "Currently present foes:\n";
452  for (FoeInfoMap::const_iterator vi = foes.begin(); vi != foes.end(); ++vi) {
453  std::cout << vi->first->getID() << "\n";
454  }
455  std::cout << std::endl;
456 #endif
457 
458  // Run through active encounters. If corresponding foe is still present in foes update and
459  // remove foe from foes. If the foe has disappeared close the encounter (check if it qualifies
460  // as a conflict and in case transfer it to myPastConflicts).
461  // Afterwards run through remaining elements in foes and create new encounters for them.
462 
463  EncounterVector::iterator ei = myActiveEncounters.begin();
464  while (ei != myActiveEncounters.end()) {
465  Encounter* e = *ei;
466  // check whether foe is still on net
467  bool foeExists = !(MSNet::getInstance()->getVehicleControl().getVehicle(e->foeID) == 0);
468  if (!foeExists) {
469  e->foe = 0;
470  }
471  if (foes.find(e->foe) != foes.end()) {
472  FoeInfo* foeInfo = foes[e->foe];
473  // Update encounter
474  updateEncounter(e, foeInfo); // deletes foeInfo
475  // Erase foes which were already encountered
476  foes.erase(e->foe);
477  } else {
478  if (e->getRemainingExtraTime() <= 0. || forceClose || !foeExists) {
479  // Close encounter, extra time has expired (deletes e if it does not qualify as conflict)
480  e->closingRequested = true;
481  } else {
482  updateEncounter(e, 0); // counts down extra time
483  }
484  }
485 
486  if (e->closingRequested) {
487  double eBegin = e->begin;
488  closeEncounter(e);
489  ei = myActiveEncounters.erase(ei);
490  if (myActiveEncounters.empty()) {
492  } else if (eBegin == myOldestActiveEncounterBegin) {
493  // Erased the oldest encounter, update myOldestActiveEncounterBegin
494  auto i = myActiveEncounters.begin();
495  myOldestActiveEncounterBegin = (*i++)->begin;
496  while (i != myActiveEncounters.end()) {
498  }
499  }
500  } else {
501  ++ei;
502  }
503  }
504 }
505 
506 
507 bool
509  // Check if conflict measure thresholds are exceeded (to decide whether to keep the encounter for writing out)
510 #ifdef DEBUG_SSM
511  std::cout << SIMTIME << " qualifiesAsConflict() for encounter of vehicles '"
512  << e->egoID << "' and '" << e->foeID
513  << "'" << std::endl;
514 #endif
515 
516  if (myComputePET && e->PET.value != INVALID && e->PET.value <= myThresholds["PET"]) {
517  return true;
518  }
519  if (myComputeTTC && e->minTTC.value != INVALID && e->minTTC.value <= myThresholds["TTC"]) {
520  return true;
521  }
522  if (myComputeDRAC && e->maxDRAC.value != INVALID && e->maxDRAC.value >= myThresholds["DRAC"]) {
523  return true;
524  }
525  return false;
526 }
527 
528 
529 void
531  // erase pointers (encounter is stored before being destroyed and pointers could become invalid)
532  e->ego = 0;
533  e->foe = 0;
534  e->end = e->timeSpan.back();
535  bool wasConflict = qualifiesAsConflict(e);
536  if (wasConflict) {
537  myPastConflicts.push(e);
538  } else {
539  delete e;
540  }
541 #ifdef DEBUG_SSM
542  std::cout << SIMTIME << " closeEncounter() of vehicles '"
543  << e->egoID << "' and '" << e->foeID
544  << "' (was ranked as " << (wasConflict ? "conflict" : "non-conflict") << ")" << std::endl;
545 #endif
546 
547  return;
548 }
549 
550 
551 void
553 #ifdef DEBUG_SSM
554  std::cout << SIMTIME << " updateEncounter() of vehicles '"
555  << e->egoID << "' and '" << e->foeID
556  << "'" << std::endl;
557 #endif
558  assert(e->foe != 0);
559 
560  // Struct storing distances (determined in classifyEncounter()) and times to potential conflict entry / exit (in estimateConflictTimes())
561  EncounterApproachInfo eInfo(e);
562 
563  // Classify encounter type based on the present information
564  // More details on follower/lead relation are determined in a second step below, see estimateConflictTimes()
565  // If a crossing situation is ongoing (i.e. one of the vehicles entered the conflict area already in the last step,
566  // this is handled by passedEncounter by only tracing the vehicle's movements)
567  // The further developement of the encounter type is done in checkConflictEntryAndExit()
568  eInfo.type = classifyEncounter(foeInfo, eInfo);
569 
570  if (eInfo.type == ENCOUNTER_TYPE_NOCONFLICT_AHEAD) {
571  // At this state, eInfo.type == ENCOUNTER_TYPE_NOCONFLICT_AHEAD implies that the foe
572  // is either out of the device's range or its route does not interfere with the ego's route.
573 #ifdef DEBUG_SSM
574  std::cout << SIMTIME << " Encounter of vehicles '"
575  << e->egoID << "' and '" << e->foeID
576  << "' does not imply any conflict." << std::endl;
577 #endif
578  updatePassedEncounter(e, foeInfo, eInfo);
579 // return;
585  // Ongoing encounter. Treat with update passed encounter (trace covered distances)
586  // eInfo.type only holds the previous type
587  updatePassedEncounter(e, foeInfo, eInfo);
588 
589  // Estimate times until a possible conflict / collision
590  estimateConflictTimes(eInfo);
591 
592  } else {
593  // Estimate times until a possible conflict / collision
594  // Not all are used for all types of encounters:
595  // Follow/lead situation doesn't need them at all, currently (might change if more SSMs are implemented).
596  // Crossing / Merging calculates entry times to determine leader/follower and calculates the exit time for the leader.
597  estimateConflictTimes(eInfo);
598 
599  // reset the remaining extra time (foe could have re-entered the device range after beginning extra time countdown already)
601  }
602 
603  // update entry/exit times for conflict area
605 
606  // update (x,y)-coords of conflict point
607  determineConflictPoint(eInfo);
608 
609  // Compute SSMs
610  computeSSMs(eInfo);
611 
612  // Add current states to trajectories and update type
613  e->add(SIMTIME, eInfo.type, e->ego->getPosition(), e->ego->getVelocityVector(), e->foe->getPosition(), e->foe->getVelocityVector(),
614  eInfo.conflictPoint, eInfo.egoConflictEntryDist, eInfo.foeConflictEntryDist, eInfo.ttc, eInfo.drac, eInfo.pet);
615 
616  // free foeInfo
617  delete foeInfo;
618 
619 }
620 
621 
622 void
624  /* Calculates the (x,y)-coordinate for the eventually predicted conflict point and stores the result in
625  * eInfo.conflictPoint. In case of MERGING and CROSSING, this is the entry point to conflict area for follower
626  * In case of FOLLOWING it is the position of leader's back. */
627 
628 #ifdef DEBUG_SSM
629  std::cout << SIMTIME << " determineConflictPoint()" << std::endl;
630 #endif
631 
632  const EncounterType& type = eInfo.type;
633  const Encounter* e = eInfo.encounter;
638  || type == ENCOUNTER_TYPE_COLLISION) {
639  eInfo.conflictPoint = e->conflictPointSpan.back();
640  } else if (type == ENCOUNTER_TYPE_CROSSING_FOLLOWER
644  } else if (type == ENCOUNTER_TYPE_CROSSING_LEADER
648  } else if (type == ENCOUNTER_TYPE_FOLLOWING_FOLLOWER) {
649  eInfo.conflictPoint = e->foe->getPosition(-e->foe->getLength());
650  } else if (type == ENCOUNTER_TYPE_FOLLOWING_LEADER) {
651  eInfo.conflictPoint = e->ego->getPosition(-e->ego->getLength());
652  } else {
653 #ifdef DEBUG_SSM
654  std::cout << "No conflict point associated with encounter type " << type << std::endl;
655 #endif
656  return;
657  }
658 
659 #ifdef DEBUG_SSM
660  std::cout << " Conflict at " << eInfo.conflictPoint << std::endl;
661 #endif
662 }
663 
664 
665 void
667 
668  EncounterType& type = eInfo.type;
669  Encounter* e = eInfo.encounter;
670 
671  assert(type != ENCOUNTER_TYPE_NOCONFLICT_AHEAD); // arrival times not defined, if no conflict is ahead.
672 #ifdef DEBUG_SSM
673  std::cout << SIMTIME << " estimateConflictTimes() for ego '" << e->egoID << "' and foe '" << e->foeID << "'\n"
674  << " encounter type: " << eInfo.type << "\n"
675  << " egoConflictEntryDist=" << (eInfo.egoConflictEntryDist == INVALID ? "NA" : toString(eInfo.egoConflictEntryDist))
676  << ", foeConflictEntryDist=" << (eInfo.foeConflictEntryDist == INVALID ? "NA" : toString(eInfo.foeConflictEntryDist))
677  << "\n ego speed=" << e->ego->getSpeed()
678  << ", foe speed=" << e->foe->getSpeed()
679  << std::endl;
680 #endif
681 
683  // No need to know the times until ...ConflictDistEntry, currently. They would correspond to an estimated time headway or similar.
684  // TTC must take into account the movement of the leader, as would DRAC, PET doesn't need the time either, since it uses aposteriori
685  // values.
686 #ifdef DEBUG_SSM
687  std::cout << " encouter type " << type << " -> no entry/exit times to be calculated."
688  << std::endl;
689 #endif
690  return;
691  }
692 
693  assert(type == ENCOUNTER_TYPE_MERGING || type == ENCOUNTER_TYPE_CROSSING
700 
701  // Determine exit distances
702  if (type == ENCOUNTER_TYPE_MERGING) {
703  assert(type == ENCOUNTER_TYPE_MERGING);
706  } else {
709  }
710 
711  // Estimate entry times to stipulate a leader / follower relation for the encounter.
712  if (eInfo.egoConflictEntryDist > 0.) {
714  assert(eInfo.egoEstimatedConflictEntryTime > 0.);
715  } else {
716  // ego already entered conflict area
718  }
719  if (eInfo.foeConflictEntryDist > 0.) {
721  assert(eInfo.foeEstimatedConflictEntryTime > 0.);
722  } else {
723  // foe already entered conflict area
725  }
726 
727 #ifdef DEBUG_SSM
728  std::cout << " Potential conflict type: " << (type == ENCOUNTER_TYPE_CROSSING ? "CROSSING" : "MERGING") << "\n"
729  << " egoConflictEntryTime=" << (eInfo.egoEstimatedConflictEntryTime == INVALID ? "INVALID" : toString(eInfo.egoEstimatedConflictEntryTime))
730  << ", foeConflictEntryTime=" << (eInfo.foeEstimatedConflictEntryTime == INVALID ? "INVALID" : toString(eInfo.foeEstimatedConflictEntryTime))
731  << std::endl;
732 #endif
733 
734  // Estimate exit times from conflict area for leader / follower.
735  if (eInfo.egoConflictExitDist >= 0.) {
737  } else {
738  eInfo.egoEstimatedConflictExitTime = 0.;
739  }
740  if (eInfo.foeConflictExitDist >= 0.) {
742  } else {
743  eInfo.foeEstimatedConflictExitTime = 0.;
744  }
745 
746 
747  if (type != ENCOUNTER_TYPE_MERGING && type != ENCOUNTER_TYPE_CROSSING) {
748  // this call is issued in context of an ongoing conflict, therefore complete type is already known for the encounter
749  // (One of EGO_ENTERED_CONFLICT_AREA, FOE_ENTERED_CONFLICT_AREA, EGO_LEFT_CONFLICT_AREA, FOE_LEFT_CONFLICT_AREA, BOTH_ENTERED_CONFLICT_AREA)
750  // --> no need to specify incomplete encounter type
751  return;
752  }
753 
754  // For merging and crossing situation, the leader/follower relation not determined by classifyEncounter()
755  // This is done below based on the estimated conflict entry times
756  if (eInfo.egoEstimatedConflictEntryTime == 0. && eInfo.foeEstimatedConflictEntryTime == 0.) {
758  std::stringstream ss;
759  ss << "SSM device of vehicle '" << e->egoID << "' detected collision with vehicle '" << e->foeID << "'";
760  WRITE_WARNING(ss.str());
762  // ego is estimated first at conflict point
763 #ifdef DEBUG_SSM
764  std::cout << " -> ego is estimated leader at conflict entry."
765  << " egoConflictExitTime=" << (eInfo.egoEstimatedConflictExitTime == INVALID ? "NA" : toString(eInfo.egoEstimatedConflictExitTime))
766  << std::endl;
767 #endif
769  } else {
770  // ego is estimated second at conflict point
771 #ifdef DEBUG_SSM
772  std::cout << " -> foe is estimated leader at conflict entry."
773  << " foeConflictExitTime=" << (eInfo.foeEstimatedConflictExitTime == INVALID ? "NA" : toString(eInfo.foeEstimatedConflictExitTime))
774  << std::endl;
775 #endif
777  }
778 
779 }
780 
781 
782 
783 void
785 #ifdef DEBUG_SSM
786  Encounter* e = eInfo.encounter;
787  std::cout << SIMTIME << " computeSSMs() for vehicles '"
788  << e->ego->getID() << "' and '" << e->foe->getID()
789  << "'" << std::endl;
790 #endif
791 
792  const EncounterType& type = eInfo.type;
793 
798  if (myComputeTTC || myComputeDRAC) {
799  determineTTCandDRAC(eInfo);
800  }
801  determinePET(eInfo);
802  } else if (type == ENCOUNTER_TYPE_BOTH_LEFT_CONFLICT_AREA) {
803  determinePET(eInfo);
804  } else if (type == ENCOUNTER_TYPE_COLLISION) {
805  // TODO: handle collision
808  // No conflict measures apply for these states, which correspond to intermediate times between
809  // one vehicle leaving the conflict area and the arrival time for the other (difference corresponds to the PET)
811  // No conflict measures apply for this state
812  } else if (type == ENCOUNTER_TYPE_MERGING_PASSED || type == ENCOUNTER_TYPE_FOLLOWING_PASSED) {
813  // No conflict measures apply for this state
814  } else if (type == ENCOUNTER_TYPE_NOCONFLICT_AHEAD) {
815  // No conflict measures apply for this state
816  } else {
817  std::stringstream ss;
818  ss << "'" << type << "'";
819  WRITE_WARNING("Unknown or undetermined encounter type at computeSSMs(): " + ss.str());
820  }
821 
822 #ifdef DEBUG_SSM
823  std::cout << "computeSSMs() for encounter of vehicles '" << e->egoID << "' and '" << e->foeID << "':\n"
824  << " ttc=" << (eInfo.ttc == INVALID ? "INVALID" : toString(eInfo.ttc))
825  << ", drac=" << (eInfo.drac == INVALID ? "INVALID" : toString(eInfo.drac))
826  << ", pet=" << (eInfo.pet.second == INVALID ? "INVALID" : toString(eInfo.pet.second))
827  << std::endl;
828 #endif
829 }
830 
831 
832 void
834  Encounter* e = eInfo.encounter;
835  if (e->size() == 0) {
836  return;
837  }
838  const EncounterType& type = eInfo.type;
839  std::pair<double, double>& pet = eInfo.pet;
840 
841 #ifdef DEBUG_SSM
842  std::cout << SIMTIME << " determinePET() for encounter of vehicles '" << e->egoID << "' and '" << e->foeID << "'"
843  << std::endl;
844 #endif
845 
847  // For a following situation, the corresponding PET-value is merely the time-headway.
848  // Determining these could be done by comparison of memorized gaps with memorized covered distances
849  // Implementation is postponed. Tracing the time gaps (in contrast to crossing PET) corresponds to
850  // a vector of values not a single value.
851  // pass
852  } else if (type == ENCOUNTER_TYPE_BOTH_LEFT_CONFLICT_AREA) {
853  EncounterType prevType = static_cast<EncounterType>(e->typeSpan.back());
854  if (prevType == ENCOUNTER_TYPE_BOTH_LEFT_CONFLICT_AREA) {
855 #ifdef DEBUG_SSM
856  std::cout << "PET for crossing encounter already calculated as " << e->PET.second
857  << std::endl;
858 #endif
859  // pet must have been calculated already
860  assert(e->PET.value != INVALID);
861  return;
862  }
863 
864  // this situation should have emerged from one of the following
865  assert(prevType == ENCOUNTER_TYPE_CROSSING_FOLLOWER
866  || prevType == ENCOUNTER_TYPE_CROSSING_LEADER
872 
873 
874 #ifdef DEBUG_SSM
875  std::cout << "e->egoDistsToConflict.back() = " << e->egoDistsToConflict.back()
876  << "\ne->egoConflictEntryTime = " << e->egoConflictEntryTime
877  << "\ne->egoConflictExitTime = " << e->egoConflictExitTime
878  << "\ne->foeDistsToConflict.back() = " << e->foeDistsToConflict.back()
879  << "\ne->foeConflictEntryTime = " << e->foeConflictEntryTime
880  << "\ne->foeConflictExitTime = " << e->foeConflictExitTime
881  << std::endl;
882 #endif
883 
884  assert(e->foeConflictEntryTime != INVALID);
885  assert(e->egoConflictEntryTime != INVALID);
886 
887  // Both have passed the conflict area
889  // both have left the conflict region already
891  pet.first = e->egoConflictEntryTime;
892  pet.second = e->egoConflictEntryTime - e->foeConflictExitTime;
893  } else if (e->foeConflictEntryTime > e->egoConflictExitTime) {
894  pet.first = e->foeConflictEntryTime;
895  pet.second = e->foeConflictEntryTime - e->egoConflictExitTime;
896  } else {
897 
898 #ifdef DEBUG_SSM
899  std::cout << "Unexpected branch in determinePET: Both passed conflict area in the same step."
900  << std::endl;
901 #endif
902  pet.first = INVALID;
903  pet.second = INVALID;
904  assert(prevType != ENCOUNTER_TYPE_EGO_LEFT_CONFLICT_AREA
906  }
907 
908  // Reset entry and exit times two allow an eventual subsequent re-use
913 
914 #ifdef DEBUG_SSM
915  std::cout << "Calculated PET = " << pet.second << " (at t=" << pet.first << ")"
916  << std::endl;
917 #endif
918  } else {
919  // other cases (merging and pre-crossing situations) do not correspond to a PET calculation.
920 #ifdef DEBUG_SSM
921  std::cout << "PET unappropriate for merging and pre-crossing situations. No calculation performed."
922  << std::endl;
923 #endif
924  return;
925  }
926 }
927 
928 
929 void
931  Encounter* e = eInfo.encounter;
932  const EncounterType& type = eInfo.type;
933  double& ttc = eInfo.ttc;
934  double& drac = eInfo.drac;
935 
936 #ifdef DEBUG_SSM
937  std::cout << SIMTIME << " determineTTCandDRAC() for encounter of vehicles '" << e->egoID << "' and '" << e->foeID << "' (type = " << eInfo.type << ")"
938  << std::endl;
939 #endif
940 
941  // Dependent on the actual encounter situation (eInfo.type) calculate the TTC.
942  // For merging and crossing, different cases occur when a collision during the merging / crossing process is predicted.
943  if (type == ENCOUNTER_TYPE_FOLLOWING_FOLLOWER) {
944  double gap = eInfo.egoConflictEntryDist;
945  if (myComputeTTC) {
946  ttc = computeTTC(gap, e->ego->getSpeed(), e->foe->getSpeed());
947  }
948  if (myComputeDRAC) {
949  drac = computeDRAC(gap, e->ego->getSpeed(), e->foe->getSpeed());
950  }
951  } else if (type == ENCOUNTER_TYPE_FOLLOWING_LEADER) {
952  double gap = eInfo.foeConflictEntryDist;
953  if (myComputeTTC) {
954  ttc = computeTTC(gap, e->foe->getSpeed(), e->ego->getSpeed());
955  }
956  if (myComputeDRAC) {
957  drac = computeDRAC(gap, e->foe->getSpeed(), e->ego->getSpeed());
958  }
959  } else if (type == ENCOUNTER_TYPE_MERGING_FOLLOWER || type == ENCOUNTER_TYPE_MERGING_LEADER) {
960  // TODO: calculate more specifically whether a following situation in the merge conflict area
961  // is predicted when assuming constant speeds or whether a side collision is predicted.
962  // Currently, we ignore any conflict area before the actual merging point of the lanes.
963 
964  // linearly extrapolated arrival times at the conflict
965  // NOTE: These differ from the estimated times stored in eInfo
966  double egoEntryTime = e->ego->getSpeed() > 0 ? eInfo.egoConflictEntryDist / e->ego->getSpeed() : INVALID;
967  double egoExitTime = e->ego->getSpeed() > 0 ? eInfo.egoConflictExitDist / e->ego->getSpeed() : INVALID;
968  double foeEntryTime = e->foe->getSpeed() > 0 ? eInfo.foeConflictEntryDist / e->foe->getSpeed() : INVALID;
969  double foeExitTime = e->foe->getSpeed() > 0 ? eInfo.foeConflictExitDist / e->foe->getSpeed() : INVALID;
970 
971 #ifdef DEBUG_SSM
972  std::cout << " Conflict times with constant speed extrapolation for merging situation:\n "
973  << " egoEntryTime=" << (egoEntryTime == INVALID ? "NA" : toString(egoEntryTime))
974  << ", egoExitTime=" << (egoExitTime == INVALID ? "NA" : toString(egoExitTime))
975  << ", foeEntryTime=" << (foeEntryTime == INVALID ? "NA" : toString(foeEntryTime))
976  << ", foeExitTime=" << (foeExitTime == INVALID ? "NA" : toString(foeExitTime))
977  << std::endl;
978 #endif
979 
980  // based on that, we obtain
981  if (egoEntryTime == INVALID || foeEntryTime == INVALID) {
982  // at least one vehicle is stopped
983  ttc = INVALID;
984  drac = INVALID;
985 #ifdef DEBUG_SSM
986  std::cout << " No TTC and DRAC computed as one vehicle is stopped." << std::endl;
987 #endif
988  return;
989  }
990  double leaderEntryTime = MIN2(egoEntryTime, foeEntryTime);
991  double followerEntryTime = MAX2(egoEntryTime, foeEntryTime);
992  double leaderExitTime = leaderEntryTime == egoEntryTime ? egoExitTime : foeExitTime;
993  //double followerExitTime = leaderEntryTime==egoEntryTime?foeExitTime:egoExitTime;
994  double leaderSpeed = leaderEntryTime == egoEntryTime ? e->ego->getSpeed() : e->foe->getSpeed();
995  double followerSpeed = leaderEntryTime == egoEntryTime ? e->foe->getSpeed() : e->ego->getSpeed();
996  double leaderConflictDist = leaderEntryTime == egoEntryTime ? eInfo.egoConflictEntryDist : eInfo.foeConflictEntryDist;
997  double followerConflictDist = leaderEntryTime == egoEntryTime ? eInfo.foeConflictEntryDist : eInfo.egoConflictEntryDist;
998  double leaderLength = leaderEntryTime == egoEntryTime ? e->ego->getLength() : e->foe->getLength();
999  if (leaderExitTime >= followerEntryTime) {
1000  // collision would occur at merge area
1001  if (myComputeTTC) {
1002  ttc = computeTTC(followerConflictDist, followerSpeed, 0.);
1003  }
1004  // TODO: Calculate more specific drac for merging case here (complete stop is not always necessary -> see calculation for crossing case)
1005  // Rather the
1006  if (myComputeDRAC) {
1007  drac = computeDRAC(followerConflictDist, followerSpeed, 0.);
1008  }
1009 // if (myComputeDRAC) drac = computeDRAC(eInfo);
1010 
1011 #ifdef DEBUG_SSM
1012  std::cout << " Extrapolation predicts collision *at* merge point with TTC=" << ttc
1013  << ", drac=" << drac << std::endl;
1014 #endif
1015 
1016  } else {
1017  // -> No collision at the merge area
1018  if (myComputeTTC) {
1019  // Check if after merge a collision would occur if speeds are hold constant.
1020  double gapAfterMerge = followerConflictDist - leaderExitTime * followerSpeed;
1021  assert(gapAfterMerge >= 0);
1022 
1023  // ttc as for following situation (assumes no collision until leader merged)
1024  double ttcAfterMerge = computeTTC(gapAfterMerge, followerSpeed, leaderSpeed);
1025  ttc = ttcAfterMerge == INVALID ? INVALID : leaderExitTime + ttcAfterMerge;
1026  }
1027  if (myComputeDRAC) {
1028  // Intitial gap. (May be negative only if the leader speed is higher than the follower speed, i.e., dv < 0)
1029  double g0 = followerConflictDist - leaderConflictDist - leaderLength;
1030  if (g0 < 0) {
1031  // Speed difference must be positive if g0<0.
1032  assert(leaderSpeed - followerSpeed > 0);
1033  // no deceleration needed for dv>0 and gap after merge >= 0
1034  drac = INVALID;
1035  } else {
1036  // compute drac as for a following situation
1037  drac = computeDRAC(g0, followerSpeed, leaderSpeed);
1038  }
1039  }
1040 #ifdef DEBUG_SSM
1041  if (ttc == INVALID) {
1042  assert(dv >= 0);
1043  assert(drac == INVALID);
1044  std::cout << " Extrapolation does not predict any collision." << std::endl;
1045  } else {
1046  std::cout << " Extrapolation predicts collision *after* merge point with TTC="
1047  << (ttc == INVALID ? "NA" : toString(ttc))
1048  << ", drac=" << (drac == INVALID ? "NA" : toString(drac)) << std::endl;
1049  }
1050 #endif
1051 
1052  }
1053 
1054  } else if (type == ENCOUNTER_TYPE_CROSSING_FOLLOWER
1056  if (myComputeDRAC) {
1057  drac = computeDRAC(eInfo);
1058  }
1060  // follower's predicted arrival at the crossing area is earlier than the leader's predicted exit -> collision predicted
1061  double gap = eInfo.egoConflictEntryDist;
1062  if (myComputeTTC) {
1063  ttc = computeTTC(gap, e->ego->getSpeed(), 0.);
1064  }
1065  } else {
1066  // encounter is expected to happen without collision
1067  ttc = INVALID;
1068  }
1069  } else if (type == ENCOUNTER_TYPE_CROSSING_LEADER
1071  if (myComputeDRAC) {
1072  drac = computeDRAC(eInfo);
1073  }
1075  // follower's predicted arrival at the crossing area is earlier than the leader's predicted exit -> collision predicted
1076  double gap = eInfo.foeConflictEntryDist;
1077  if (myComputeTTC) {
1078  ttc = computeTTC(gap, e->foe->getSpeed(), 0.);
1079  }
1080  } else {
1081  // encounter is expected to happen without collision
1082  ttc = INVALID;
1083  }
1084  } else {
1085 #ifdef DEBUG_SSM
1086  std::stringstream ss;
1087  ss << "'" << type << "'";
1088  WRITE_WARNING("Underspecified or unknown encounter type in MSDevice_SSM::determineTTCandDRAC(): " + ss.str());
1089 #endif
1090  }
1091 
1092 #ifdef DEBUG_SSM
1093  std::cout << "ttc=" << (ttc == INVALID ? "INVALID" : toString(ttc)) << ", drac=" << (drac == INVALID ? "INVALID" : toString(drac))
1094  << std::endl;
1095 #endif
1096 }
1097 
1098 
1099 double
1100 MSDevice_SSM::computeTTC(double gap, double followerSpeed, double leaderSpeed) const {
1101  // TODO: in merging situations, the TTC may be lower than the one computed here for following situations
1102  // (currently only the cross section corresponding to the target lane's begin is considered)
1103  // More specifically, the minimum has to be taken from the two if a collision at merge was predicted.
1104 #ifdef DEBUG_SSM
1105  std::cout << "computeTTC() with gap=" << gap << ", followerSpeed=" << followerSpeed << ", leaderSpeed=" << leaderSpeed
1106  << std::endl;
1107 #endif
1108  if (gap <= 0.) {
1109  return 0.; // collision already happend
1110  }
1111  double dv = followerSpeed - leaderSpeed;
1112  if (dv <= 0.) {
1113  return INVALID; // no collision
1114  }
1115 
1116  return gap / dv;
1117 }
1118 
1119 
1120 double
1121 MSDevice_SSM::computeDRAC(double gap, double followerSpeed, double leaderSpeed) {
1122 #ifdef DEBUG_SSM_DRAC
1123  std::cout << "computeDRAC() with gap=" << gap << ", followerSpeed=" << followerSpeed << ", leaderSpeed=" << leaderSpeed
1124  << std::endl;
1125 #endif
1126  if (gap <= 0.) {
1127  return INVALID; // collision!
1128  }
1129  double dv = followerSpeed - leaderSpeed;
1130  if (dv <= 0.) {
1131  return 0.0; // no need to break
1132  }
1133  assert(followerSpeed > 0.);
1134  return 0.5 * dv * dv / gap; // following Guido et al. (2011)
1135 }
1136 
1137 double
1139  // Introduce concise variable names
1140  double dEntry1 = eInfo.egoConflictEntryDist;
1141  double dEntry2 = eInfo.foeConflictEntryDist;
1142  double dExit1 = eInfo.egoConflictExitDist;
1143  double dExit2 = eInfo.foeConflictExitDist;
1144  double v1 = eInfo.encounter->ego->getSpeed();
1145  double v2 = eInfo.encounter->foe->getSpeed();
1146  double tEntry1 = eInfo.egoEstimatedConflictEntryTime;
1147  double tEntry2 = eInfo.foeEstimatedConflictEntryTime;
1148  double tExit1 = eInfo.egoEstimatedConflictExitTime;
1149  double tExit2 = eInfo.foeEstimatedConflictExitTime;
1150 #ifdef DEBUG_SSM_DRAC
1151  std::cout << SIMTIME << "computeDRAC() with"
1152  << "\ndEntry1=" << dEntry1 << ", dEntry2=" << dEntry2
1153  << ", dExit1=" << dExit1 << ", dExit2=" << dExit2
1154  << ",\nv1=" << v1 << ", v2=" << v2
1155  << "\ntEntry1=" << (tEntry1 == INVALID ? "NA" : toString(tEntry1)) << ", tEntry2=" << (tEntry2 == INVALID ? "NA" : toString(tEntry2))
1156  << ", tExit1=" << (tExit1 == INVALID ? "NA" : toString(tExit1)) << ", tExit2=" << (tExit2 == INVALID ? "NA" : toString(tExit2))
1157  << std::endl;
1158 #endif
1159  if (dExit1 <= 0. || dExit2 <= 0.) {
1160  // At least one vehicle already left or is not about to enter conflict area at all => no breaking needed.
1161 #ifdef DEBUG_SSM_DRAC
1162  std::cout << "One already left conflict area -> drac == 0." << std::endl;
1163 #endif
1164  return 0.;
1165  }
1166  if (dEntry1 <= 0. && dEntry2 <= 0.) {
1167  // collision... (both already entered conflict area but none left)
1168 #ifdef DEBUG_SSM_DRAC
1169  std::cout << "Both entered conflict area but neither left. -> collision!" << std::endl;
1170 #endif
1171  return INVALID;
1172  }
1173 
1174  double drac = std::numeric_limits<double>::max();
1175  if (dEntry1 > 0.) {
1176  // vehicle 1 could break
1177 #ifdef DEBUG_SSM_DRAC
1178  std::cout << "Ego could break..." << std::endl;
1179 #endif
1180  if (tExit2 != INVALID) {
1181  // Vehicle 2 is expected to leave conflict area at t2
1182  drac = MIN2(drac, 2 * (v1 - dEntry1 / tExit2) / tExit2);
1183 #ifdef DEBUG_SSM_DRAC
1184  std::cout << " Foe expected to leave in " << tExit2 << "-> Ego needs drac=" << drac << std::endl;
1185 #endif
1186  } else {
1187  // Vehicle 2 is expected to stop on conflict area or earlier
1188  if (tEntry2 != INVALID) {
1189  // ... on conflict area => veh1 has to stop before entry
1190  drac = MIN2(drac, computeDRAC(dEntry1, v1, 0));
1191 #ifdef DEBUG_SSM_DRAC
1192  std::cout << " Foe is expected stop on conflict area -> Ego needs drac=" << drac << std::endl;
1193 #endif
1194  } else {
1195  // ... before conflict area
1196 #ifdef DEBUG_SSM_DRAC
1197  std::cout << " Foe is expected stop before conflict area -> no drac computation for ego (will be done for foe if applicable)" << std::endl;
1198 #endif
1199  }
1200  }
1201  }
1202 
1203  if (dEntry2 > 0.) {
1204  // vehicle 2 could break
1205 #ifdef DEBUG_SSM_DRAC
1206  std::cout << "Foe could break..." << std::endl;
1207 #endif
1208  if (tExit1 != INVALID) {
1209  // Vehicle 1 is expected to leave conflict area at t1
1210 #ifdef DEBUG_SSM_DRAC
1211  std::cout << " Ego expected to leave in " << tExit1 << "-> Foe needs drac=" << (2 * (v2 - dEntry2 / tExit1) / tExit1) << std::endl;
1212 #endif
1213  drac = MIN2(drac, 2 * (v2 - dEntry2 / tExit1) / tExit1);
1214  } else {
1215  // Vehicle 1 is expected to stop on conflict area or earlier
1216  if (tEntry1 != INVALID) {
1217  // ... on conflict area => veh2 has to stop before entry
1218 #ifdef DEBUG_SSM_DRAC
1219  std::cout << " Ego is expected stop on conflict area -> Foe needs drac=" << computeDRAC(dEntry2, v2, 0) << std::endl;
1220 #endif
1221  drac = MIN2(drac, computeDRAC(dEntry2, v2, 0));
1222  } else {
1223  // ... before conflict area
1224 #ifdef DEBUG_SSM_DRAC
1225  std::cout << " Ego is expected stop before conflict area -> no drac computation for foe (done for ego if applicable)" << std::endl;
1226 #endif
1227  }
1228  }
1229  }
1230 
1231  return drac > 0 ? drac : INVALID;
1232 }
1233 
1234 void
1236  // determine exact entry and exit times
1237  Encounter* e = eInfo.encounter;
1238 
1239 #ifdef DEBUG_SSM
1240  std::cout << SIMTIME << " checkConflictEntryAndExit() for encounter of vehicles '" << e->egoID << "' and '" << e->foeID << "'" << std::endl;
1241 #endif
1242 
1243  // Distances to conflict area boundaries in previous step
1244  double prevEgoConflictEntryDist = eInfo.egoConflictEntryDist + e->ego->getLastStepDist();
1245  double prevFoeConflictEntryDist = eInfo.foeConflictEntryDist + e->foe->getLastStepDist();
1246  double prevEgoConflictExitDist = prevEgoConflictEntryDist + eInfo.egoConflictAreaLength + e->ego->getLength();
1247  double prevFoeConflictExitDist = prevFoeConflictEntryDist + eInfo.foeConflictAreaLength + e->foe->getLength();
1248  EncounterType prevType = e->currentType;
1249 
1250  if (e->timeSpan.size() == 0) {
1251  // Encounter has just been created, no data points yet.
1252  // If a vehicle was already beyond conflict area boundary in last step, we set the passing time to the previous time step
1253  prevEgoConflictEntryDist = MAX2(prevEgoConflictEntryDist, 0.);
1254  prevFoeConflictEntryDist = MAX2(prevFoeConflictEntryDist, 0.);
1255  prevEgoConflictExitDist = MAX2(prevEgoConflictExitDist, 0.);
1256  prevFoeConflictExitDist = MAX2(prevFoeConflictExitDist, 0.);
1257  }
1258 
1259 
1260  // Check if ego entered in last step
1261  if (e->egoConflictEntryTime == INVALID && eInfo.egoConflictEntryDist < 0 && prevEgoConflictEntryDist >= 0) {
1262  // ego must have entered the conflict in the last step. Determine exact entry time
1263  e->egoConflictEntryTime = SIMTIME - TS + MSCFModel::passingTime(-prevEgoConflictEntryDist, 0., -eInfo.egoConflictEntryDist, e->ego->getPreviousSpeed(), e->ego->getSpeed());
1264 #ifdef DEBUG_SSM
1265  std::cout << " ego entered conflict area at t=" << e->egoConflictEntryTime << std::endl;
1266 #endif
1267  // Update encounter type (only done here for entering, the other transitions are done in updatePassedEncounter)
1268  if (prevType == ENCOUNTER_TYPE_CROSSING_FOLLOWER
1269  || prevType == ENCOUNTER_TYPE_CROSSING_LEADER) {
1271  }
1272  }
1273 
1274  // Check if foe entered in last step
1275  if (e->foeConflictEntryTime == INVALID && eInfo.foeConflictEntryDist < 0. && prevFoeConflictEntryDist >= 0) {
1276  // foe must have entered the conflict in the last step. Determine exact entry time
1277  e->foeConflictEntryTime = SIMTIME - TS + MSCFModel::passingTime(-prevFoeConflictEntryDist, 0., -eInfo.foeConflictEntryDist, e->foe->getPreviousSpeed(), e->foe->getSpeed());
1278 #ifdef DEBUG_SSM
1279  std::cout << " foe entered conflict area at t=" << e->foeConflictEntryTime << std::endl;
1280 #endif
1281  // Update encounter type (only done here for entering, the other transitions are done in updatePassedEncounter)
1282  if (prevType == ENCOUNTER_TYPE_CROSSING_FOLLOWER
1283  || prevType == ENCOUNTER_TYPE_CROSSING_LEADER) {
1285  }
1286  }
1287 
1288  // Check if ego left conflict area
1289  if (e->egoConflictExitTime == INVALID && eInfo.egoConflictExitDist < 0 && prevEgoConflictExitDist >= 0) {
1290  // ego must have left the conflict area in the last step. Determine exact exit time
1291  e->egoConflictExitTime = SIMTIME - TS + MSCFModel::passingTime(-prevEgoConflictExitDist, 0., -eInfo.egoConflictExitDist, e->ego->getPreviousSpeed(), e->ego->getSpeed());
1292  // Add cross section to calculate PET for foe
1293 // e->foePETCrossSections.push_back(std::make_pair(eInfo.foeConflictEntryCrossSection, e->egoConflictExitTime));
1294 #ifdef DEBUG_SSM
1295  std::cout << " ego left conflict area at t=" << e->egoConflictExitTime << std::endl;
1296 #endif
1297  // Update encounter type (only done here for entering, the other transitions are done in updatePassedEncounter)
1298  if (prevType == ENCOUNTER_TYPE_CROSSING_FOLLOWER
1299  || prevType == ENCOUNTER_TYPE_CROSSING_LEADER) {
1301  }
1302  }
1303 
1304  // Check if foe left conflict area
1305  if (e->foeConflictExitTime == INVALID && eInfo.foeConflictExitDist < 0 && prevFoeConflictExitDist >= 0) {
1306  // foe must have left the conflict area in the last step. Determine exact exit time
1307  e->foeConflictExitTime = SIMTIME - TS + MSCFModel::passingTime(-prevFoeConflictExitDist, 0., -eInfo.foeConflictExitDist, e->foe->getPreviousSpeed(), e->foe->getSpeed());
1308  // Add cross section to calculate PET for ego
1309 // e->egoPETCrossSections.push_back(std::make_pair(eInfo.egoConflictEntryCrossSection, e->foeConflictExitTime));
1310 #ifdef DEBUG_SSM
1311  std::cout << " foe left conflict area at t=" << e->foeConflictExitTime << std::endl;
1312 #endif
1313  // Update encounter type (only done here for entering, the other transitions are done in updatePassedEncounter)
1314  if (prevType == ENCOUNTER_TYPE_CROSSING_FOLLOWER
1315  || prevType == ENCOUNTER_TYPE_CROSSING_LEADER) {
1317  }
1318  }
1319 }
1320 
1321 
1322 void
1324 
1325 #ifdef DEBUG_SSM
1326  std::cout << SIMTIME << " updatePassedEncounter() for vehicles '"
1327  << e->egoID << "' and '" << e->foeID << "'"
1328  << std::endl;
1329 #endif
1330 
1331  if (foeInfo == 0) {
1332  // the foe is out of the device's range, proceed counting down the remaining extra time to trace
1333  e->countDownExtraTime(TS);
1334 #ifdef DEBUG_SSM
1335  std::cout << " Foe is out of range. Counting down extra time."
1336  << " Remaining seconds before closing encounter: " << e->getRemainingExtraTime()
1337  << std::endl;
1338 #endif
1339 
1340  } else {
1341  // reset the remaining extra time (foe could have re-entered the device range after beginning extra time countdown already)
1343  }
1344 
1345  // Check, whether this was really a potential conflict at some time:
1346  // Search through typeSpan for a type other than no conflict
1347  EncounterType lastPotentialConflictType = e->typeSpan.size() > 0 ? static_cast<EncounterType>(e->typeSpan.back()) : ENCOUNTER_TYPE_NOCONFLICT_AHEAD;
1348 
1349  if (lastPotentialConflictType == ENCOUNTER_TYPE_NOCONFLICT_AHEAD) {
1350  // This encounter was no conflict in the last step -> remains so
1351 #ifdef DEBUG_SSM
1352  std::cout << " This encounter wasn't classified as a potential conflict lately." << std::endl;
1353 #endif
1354  if (foeInfo == 0) {
1355  // Encounter was either never a potential conflict and foe is out of range
1356  // or the foe has left the network
1357  // -> no use in further tracing this encounter
1358  e->closingRequested = true;
1359 #ifdef DEBUG_SSM
1360  std::cout << " Closing encounter." << std::endl;
1361 #endif
1363  }
1364  } else if (lastPotentialConflictType == ENCOUNTER_TYPE_FOLLOWING_FOLLOWER
1365  || lastPotentialConflictType == ENCOUNTER_TYPE_FOLLOWING_LEADER
1366  || lastPotentialConflictType == ENCOUNTER_TYPE_FOLLOWING_PASSED) {
1367  // if a following situation leads to a no-conflict situation this encounter switches no-conflict, since no further computations (PET) are needed.
1369 #ifdef DEBUG_SSM
1370  std::cout << " Encounter was previously classified as a follow/lead situation." << std::endl;
1371 #endif
1372  } else if (lastPotentialConflictType == ENCOUNTER_TYPE_MERGING_FOLLOWER
1373  || lastPotentialConflictType == ENCOUNTER_TYPE_MERGING_LEADER
1374  || lastPotentialConflictType == ENCOUNTER_TYPE_MERGING_PASSED) {
1375  // if a merging situation leads to a no-conflict situation the leader was either removed from the net (we disregard special treatment)
1376  // or route- or lane-changes removed the conflict.
1378 #ifdef DEBUG_SSM
1379  std::cout << " Encounter was previously classified as a merging situation." << std::endl;
1380 #endif
1381  }
1382  if (lastPotentialConflictType == ENCOUNTER_TYPE_CROSSING_FOLLOWER
1383  || lastPotentialConflictType == ENCOUNTER_TYPE_CROSSING_LEADER
1384  || lastPotentialConflictType == ENCOUNTER_TYPE_EGO_ENTERED_CONFLICT_AREA
1385  || lastPotentialConflictType == ENCOUNTER_TYPE_FOE_ENTERED_CONFLICT_AREA
1386  || lastPotentialConflictType == ENCOUNTER_TYPE_BOTH_ENTERED_CONFLICT_AREA
1387  || lastPotentialConflictType == ENCOUNTER_TYPE_EGO_LEFT_CONFLICT_AREA
1388  || lastPotentialConflictType == ENCOUNTER_TYPE_FOE_LEFT_CONFLICT_AREA
1389  || lastPotentialConflictType == ENCOUNTER_TYPE_BOTH_LEFT_CONFLICT_AREA
1390  || lastPotentialConflictType == ENCOUNTER_TYPE_COLLISION) {
1391  // Encounter has been a crossing situation.
1392 
1393 #ifdef DEBUG_SSM
1394  std::cout << " Encounter was previously classified as a crossing situation of type " << lastPotentialConflictType << "." << std::endl;
1395 #endif
1396  // For passed encounters, the xxxConflictAreaLength variables are not determined before -> we use the stored values.
1397 
1398  // TODO: This could also more precisely be calculated wrt the angle of the crossing *at the conflict point*
1399  if (eInfo.egoConflictAreaLength == INVALID) {
1400  eInfo.egoConflictAreaLength = e->foe->getWidth();
1401  }
1402  if (eInfo.foeConflictAreaLength == INVALID) {
1403  eInfo.foeConflictAreaLength = e->ego->getWidth();
1404  }
1405 
1406  eInfo.egoConflictEntryDist = e->egoDistsToConflict.back() - e->ego->getLastStepDist();
1408  eInfo.foeConflictEntryDist = e->foeDistsToConflict.back() - e->foe->getLastStepDist();
1410 
1411 #ifdef DEBUG_SSM
1412  std::cout << " egoConflictEntryDist = " << eInfo.egoConflictEntryDist
1413  << ", egoConflictExitDist = " << eInfo.egoConflictExitDist
1414  << "\n foeConflictEntryDist = " << eInfo.foeConflictEntryDist
1415  << ", foeConflictExitDist = " << eInfo.foeConflictExitDist
1416  << std::endl;
1417 #endif
1418 
1419  // Determine actual encounter type
1420  bool egoEnteredConflict = eInfo.egoConflictEntryDist < 0.;
1421  bool foeEnteredConflict = eInfo.foeConflictEntryDist < 0.;
1422  bool egoLeftConflict = eInfo.egoConflictExitDist < 0.;
1423  bool foeLeftConflict = eInfo.foeConflictExitDist < 0.;
1424 
1425  if ((!egoEnteredConflict) && !foeEnteredConflict) {
1426  // XXX: do we need to recompute the follow/lead order, here?
1427  assert(lastPotentialConflictType == ENCOUNTER_TYPE_CROSSING_FOLLOWER
1428  || lastPotentialConflictType == ENCOUNTER_TYPE_CROSSING_LEADER);
1429  eInfo.type = lastPotentialConflictType;
1430  } else if (egoEnteredConflict && !foeEnteredConflict) {
1432  } else if ((!egoEnteredConflict) && foeEnteredConflict) {
1434  } else { // (egoEnteredConflict && foeEnteredConflict) {
1436  }
1437 
1438  if ((!egoLeftConflict) && !foeLeftConflict) {
1441  }
1442  } else if (egoLeftConflict && !foeLeftConflict) {
1445  }
1446  } else if ((!egoLeftConflict) && foeLeftConflict) {
1449  }
1450  } else {
1452  // It should not occur that both leave the conflict at the same step
1453  assert(lastPotentialConflictType == ENCOUNTER_TYPE_FOE_LEFT_CONFLICT_AREA
1454  || lastPotentialConflictType == ENCOUNTER_TYPE_EGO_LEFT_CONFLICT_AREA
1455  || lastPotentialConflictType == ENCOUNTER_TYPE_BOTH_ENTERED_CONFLICT_AREA
1456  || lastPotentialConflictType == ENCOUNTER_TYPE_BOTH_LEFT_CONFLICT_AREA);
1457  }
1458 
1459  // TODO: adjust the conflict distances according to lateral movement for single ENTERED-cases
1460 
1461 #ifdef DEBUG_SSM
1462  std::cout << " Updated classification: " << eInfo.type << std::endl;
1463 #endif
1464  }
1465 }
1466 
1467 
1470 #ifdef DEBUG_SSM
1471  std::cout << "classifyEncounter() called." << std::endl;
1472 #endif
1473  if (foeInfo == 0) {
1474  // foeInfo == 0 signalizes, that no corresponding foe info was returned by findSurroundingVehicles(),
1475  // i.e. the foe is actually out of range (This may also mean that it has left the network)
1477  }
1478  const Encounter* e = eInfo.encounter;
1479 
1480  // previous classification (if encounter was not just created)
1481  EncounterType prevType = e->typeSpan.size() > 0 ? static_cast<EncounterType>(e->typeSpan.back()) : ENCOUNTER_TYPE_NOCONFLICT_AHEAD;
1482  if (e->typeSpan.size() > 0
1488  // This is an ongoing crossing situation with at least one of the vehicles not
1489  // having passed the conflict area.
1490  // -> Merely trace the change of distances to the conflict entry / exit
1491  // -> Derefer this to updatePassedEncounter, where this is done anyhow.
1492 #ifdef DEBUG_SSM
1493  std::cout << " Ongoing crossing conflict will be traced by passedEncounter()." << std::endl;
1494 #endif
1495  return prevType;
1496  }
1497 
1498 
1499  // Ego's current Lane
1500  const MSLane* egoLane = e->ego->getLane();
1501  // Foe's current Lane
1502  const MSLane* foeLane = e->foe->getLane();
1503 
1504  // Ego's conflict lane is memorized in foeInfo
1505  const MSLane* egoConflictLane = foeInfo->egoConflictLane;
1506  double egoDistToConflictLane = foeInfo->egoDistToConflictLane;
1507  // Find conflicting lane and the distance to its entry link for the foe
1508  double foeDistToConflictLane;
1509  const MSLane* foeConflictLane = findFoeConflictLane(e->foe, foeInfo->egoConflictLane, foeDistToConflictLane);
1510 
1511 #ifdef DEBUG_SSM
1512  std::cout << "egoConflictLane: '" << (egoConflictLane == 0 ? "NULL" : egoConflictLane->getID()) << "'\n"
1513  << "foeConflictLane: '" << (foeConflictLane == 0 ? "NULL" : foeConflictLane->getID()) << "'"
1514  << "\nEgo's distance to conflict lane: " << egoDistToConflictLane
1515  << "\nFoe's distance to conflict lane: " << foeDistToConflictLane
1516  << std::endl;
1517 #endif
1518 
1519  // Treat different cases for foeConflictLane and egoConflictLane (internal or non-internal / equal to egoLane or to foeLane),
1520  // and thereby determine encounterType and the ego/foeEncounterDistance.
1521  // The encounter distance has a different meaning for different types of encounters:
1522  // 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.
1523  // 2) For merging encounters the encounter distance is the distance until the begin of the common target edge/lane.
1524  // (XXX: Perhaps this should be adjusted to include the entry point to the region where a simultaneous occupancy of
1525  // both merging lanes could imply a collision)
1526  // 3) For crossing encounters the encounter distances is the distance until the entry point to the conflicting lane.
1527 
1529 
1530  if (foeConflictLane == 0) {
1531  // foe vehicle is not on course towards the ego's route (see findFoeConflictLane)
1533 #ifdef DEBUG_SSM
1534  std::cout << "-> Encounter type: No conflict." << std::endl;
1535 #endif
1536  } else if (!egoConflictLane->isInternal()) {
1537  // The conflict lane is non-internal, therefore we either have no potential conflict or a lead/follow situation (i.e., no crossing or merging)
1538  if (egoConflictLane == egoLane) {
1539  // The conflict point is on the ego's current lane.
1540  if (foeLane == egoLane) {
1541  // Foe is on the same non-internal lane
1542  if (e->ego->getPositionOnLane() > e->foe->getPositionOnLane()) {
1545  } else {
1548  }
1549 #ifdef DEBUG_SSM
1550  std::cout << "-> Encounter type: Lead/follow-situation on non-internal lane '" << egoLane->getID() << "'" << std::endl;
1551 #endif
1552  } else if (&(foeLane->getEdge()) == &(egoLane->getEdge())) {
1553  // Foe is on the same non-internal edge but not on the same lane. Treat this as no conflict for now
1554  // XXX: this disregards conflicts for vehicles on adjacent lanes
1556 #ifdef DEBUG_SSM
1557  std::cout << "-> Encounter type: " << type << std::endl;
1558 #endif
1559  } else {
1560  assert(&(egoLane->getEdge()) == &(foeConflictLane->getEdge()));
1561  assert(egoDistToConflictLane <= 0);
1562  // Foe must be on a route leading into the ego's edge
1563  if (foeConflictLane == egoLane) {
1565  eInfo.foeConflictEntryDist = foeDistToConflictLane + e->ego->getBackPositionOnLane();
1566 
1567 #ifdef DEBUG_SSM
1568  std::cout << "-> Encounter type: Ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' leads foe '"
1569  << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
1570  << " (gap = " << eInfo.foeConflictEntryDist << ")"
1571  << std::endl;
1572 #endif
1573  } else {
1574  // Foe's route leads to an adjacent lane of the current lane of the ego
1576 #ifdef DEBUG_SSM
1577  std::cout << "-> Encounter type: " << type << std::endl;
1578 #endif
1579  }
1580  }
1581  } else {
1582  // 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
1583  // is located on the foe's current edge see findSurroundingVehicles()
1584  // (otherwise the foe would have had to enter the ego's route along a junction and the corresponding
1585  // conflict lane would be internal)
1586  assert(&(foeLane->getEdge()) == &(egoConflictLane->getEdge()));
1587  assert(foeDistToConflictLane <= 0);
1588  if (foeLane == egoConflictLane) {
1590  eInfo.egoConflictEntryDist = egoDistToConflictLane + e->foe->getBackPositionOnLane();
1591 #ifdef DEBUG_SSM
1592  std::cout << "-> Encounter type: Ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' follows foe '"
1593  << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
1594  << " (gap = " << eInfo.egoConflictEntryDist << ")"
1595  << std::endl;
1596 #endif
1597  } else {
1598  // Ego's route leads to an adjacent lane of the current lane of the foe
1600 #ifdef DEBUG_SSM
1601  std::cout << "-> Encounter type: " << type << std::endl;
1602 #endif
1603  }
1604  }
1605  } else {
1606  // egoConflictLane is internal, i.e., lies on a junction. Besides the lead/follow situation (which may stretch over different lanes of a connection),
1607  // merging or crossing of the conflict lanes is possible.
1608  assert(foeConflictLane->isInternal());
1609  MSLink* egoEntryLink = egoConflictLane->getEntryLink();
1610  MSLink* foeEntryLink = foeConflictLane->getEntryLink();
1611  if (&(egoEntryLink->getViaLane()->getEdge()) == &(foeEntryLink->getViaLane()->getEdge())) {
1612  if (egoEntryLink != foeEntryLink) {
1613  // XXX: this disregards conflicts for vehicles on adjacent internal lanes
1615 #ifdef DEBUG_SSM
1616  std::cout << "-> Encounter type: " << type << std::endl;
1617 #endif
1618  } else {
1619  // Lead / follow situation on connection
1620  if (egoLane == egoConflictLane && foeLane != foeConflictLane) {
1621  // ego on junction, foe not yet
1623  eInfo.foeConflictEntryDist = foeDistToConflictLane + e->ego->getBackPositionOnLane();
1624 #ifdef DEBUG_SSM
1625  std::cout << "-> Encounter type: Ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' leads foe '"
1626  << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
1627  << " (gap = " << eInfo.foeConflictEntryDist << ")"
1628  << std::endl;
1629 #endif
1630  } else if (egoLane != egoConflictLane && foeLane == foeConflictLane) {
1631  // foe on junction, ego not yet
1633  eInfo.egoConflictEntryDist = egoDistToConflictLane + e->foe->getBackPositionOnLane();
1634 #ifdef DEBUG_SSM
1635  std::cout << "-> Encounter type: Ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' follows foe '"
1636  << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
1637  << " (gap = " << eInfo.egoConflictEntryDist << ")"
1638  << std::endl;
1639 #endif
1640  } else {
1641  // Both must be already on the junction in a lead / follow situation on a connection
1642  // (since they approach via the same link, findSurroundingVehicles() would have determined a
1643  // different conflictLane if both are not on the junction)
1644  assert(egoLane == egoConflictLane);
1645  assert(foeLane == foeConflictLane);
1646  if (egoLane == foeLane) {
1647  // both on the same internal lane
1648  if (e->ego->getPositionOnLane() > e->foe->getPositionOnLane()) {
1650  eInfo.foeConflictEntryDist = foeDistToConflictLane + e->ego->getBackPositionOnLane();
1651 #ifdef DEBUG_SSM
1652  std::cout << "-> Encounter type: Ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' leads foe '"
1653  << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
1654  << " (gap = " << eInfo.foeConflictEntryDist << ")"
1655  << std::endl;
1656 #endif
1657  } else {
1659  eInfo.egoConflictEntryDist = egoDistToConflictLane + e->foe->getBackPositionOnLane();
1660 #ifdef DEBUG_SSM
1661  std::cout << "-> Encounter type: Ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' follows foe '"
1662  << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
1663  << " (gap = " << eInfo.egoConflictEntryDist << ")"
1664  << std::endl;
1665 #endif
1666  }
1667  } else {
1668  // ego and foe on distinct, consecutive internal lanes
1669 #ifdef DEBUG_SSM
1670  std::cout << " Lead/follow situation on consecutive internal lanes." << std::endl;
1671 #endif
1672  MSLane* lane = egoEntryLink->getViaLane();
1673 #ifdef _MSC_VER
1674 #pragma warning(push)
1675 #pragma warning(disable: 4127) // do not warn about constant conditional expression
1676 #endif
1677  while (true) {
1678 #ifdef _MSC_VER
1679 #pragma warning(pop)
1680 #endif
1681  // Find first of egoLane and foeLane while crossing the junction (this dertermines who's the follower)
1682  // Then set the conflict lane to the lane of the leader and adapt the follower's distance to conflict
1683  if (egoLane == lane) {
1684  // ego is follower
1686  // adapt conflict dist
1687  eInfo.egoConflictEntryDist = egoDistToConflictLane;
1688  while (lane != foeLane) {
1689  eInfo.egoConflictEntryDist += lane->getLength();
1690  lane = lane->getLinkCont()[0]->getViaLane();
1691  assert(lane != 0);
1692  }
1694  egoConflictLane = lane;
1695 #ifdef DEBUG_SSM
1696  std::cout << "-> Encounter type: Ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' follows foe '"
1697  << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
1698  << " (gap = " << eInfo.egoConflictEntryDist << ")"
1699  << std::endl;
1700 #endif
1701  break;
1702  } else if (foeLane == lane) {
1703  // ego is leader
1705  // adapt conflict dist
1706  eInfo.foeConflictEntryDist = foeDistToConflictLane;
1707  while (lane != egoLane) {
1708  eInfo.foeConflictEntryDist += lane->getLength();
1709  lane = lane->getLinkCont()[0]->getViaLane();
1710  assert(lane != 0);
1711  }
1713  foeConflictLane = lane;
1714 #ifdef DEBUG_SSM
1715  std::cout << "-> Encounter type: Ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' leads foe '"
1716  << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
1717  << " (gap = " << eInfo.foeConflictEntryDist << ")"
1718  << std::endl;
1719 #endif
1720  break;
1721  }
1722  lane = lane->getLinkCont()[0]->getViaLane();
1723  assert(lane != 0);
1724  }
1725  }
1726 #ifdef DEBUG_SSM
1727  std::cout << "-> Encounter type: Lead/follow-situation on connection from '" << egoEntryLink->getLaneBefore()->getID()
1728  << "' to '" << egoEntryLink->getLane()->getID() << "'" << std::endl;
1729 #endif
1730  }
1731  }
1732  } else {
1733  // Entry links to junctions lead to different internal edges.
1734  // There are three possibilities, either the edges cross, merge or have no conflict
1735  const std::vector<MSLink*>& egoFoeLinks = egoEntryLink->getFoeLinks();
1736  const std::vector<MSLink*>& foeFoeLinks = foeEntryLink->getFoeLinks();
1737  // Determine whether ego and foe links are foes
1738  bool crossOrMerge = (find(egoFoeLinks.begin(), egoFoeLinks.end(), foeEntryLink) != egoFoeLinks.end()
1739  || find(foeFoeLinks.begin(), foeFoeLinks.end(), egoEntryLink) != foeFoeLinks.end());
1740  if (!crossOrMerge) {
1741 // if (&(foeEntryLink->getLane()->getEdge()) == &(egoEntryLink->getLane()->getEdge())) {
1742 // // XXX: the situation of merging into adjacent lanes is disregarded for now <- the alleged situation appears to imply crossOrMerge!!!
1743 // type = ENCOUNTER_TYPE_MERGING_ADJACENT;
1744 //#ifdef DEBUG_SSM
1745 // std::cout << "-> Encounter type: No conflict (adjacent lanes)." << std::endl;
1746 //#endif
1747 // } else {
1749 #ifdef DEBUG_SSM
1750  std::cout << "-> Encounter type: No conflict." << std::endl;
1751 #endif
1752 // }
1753  } else if (&(foeEntryLink->getLane()->getEdge()) == &(egoEntryLink->getLane()->getEdge())) {
1754  if (foeEntryLink->getLane() == egoEntryLink->getLane()) {
1755  type = ENCOUNTER_TYPE_MERGING;
1756  eInfo.egoConflictEntryDist = egoDistToConflictLane + egoEntryLink->getInternalLengthsAfter();
1757  eInfo.foeConflictEntryDist = foeDistToConflictLane + foeEntryLink->getInternalLengthsAfter();
1758 #ifdef DEBUG_SSM
1759  std::cout << "-> Encounter type: Merging situation of ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' and foe '"
1760  << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
1761  << "\nDistances to merge-point: ego: " << eInfo.egoConflictEntryDist << ", foe: " << eInfo.foeConflictEntryDist
1762  << std::endl;
1763 #endif
1764  } else {
1765  // Links leading to the same edge but different lanes. XXX: Disregards conflicts on adjacent lanes
1767 #ifdef DEBUG_SSM
1768  std::cout << "-> Encounter type: No conflict: " << type << std::endl;
1769 #endif
1770  }
1771  } else {
1772  type = ENCOUNTER_TYPE_CROSSING;
1773 
1774  assert(egoConflictLane->isInternal());
1775  assert(foeConflictLane->getEdge().getToJunction() == egoConflictLane->getEdge().getToJunction());
1776 
1777  // If the conflict lanes are internal, they may not correspond to the
1778  // actually crossing parts of the corresponding connections.
1779  // Adjust the conflict lanes accordingly.
1780  // set back both to the first parts of the corresponding connections
1781  double offset = 0.;
1782  egoConflictLane = egoConflictLane->getFirstInternalInConnection(offset);
1783  egoDistToConflictLane -= offset;
1784  foeConflictLane = foeConflictLane->getFirstInternalInConnection(offset);
1785  foeDistToConflictLane -= offset;
1786  // find the distances to the conflict from the junction entry for both vehicles
1787  // Here we also determine the real crossing lanes (before the conflict lane is the first lane of the connection)
1788  // for the ego
1789  double egoDistToConflictFromJunctionEntry = INVALID;
1790  double foeInternalLaneLengthsBeforeCrossing = 0.;
1791  while (foeConflictLane != 0 && foeConflictLane->isInternal()) {
1792  egoDistToConflictFromJunctionEntry = egoEntryLink->getLengthsBeforeCrossing(foeConflictLane);
1793  if (egoDistToConflictFromJunctionEntry != INVALID) {
1794  // found correct foeConflictLane
1795  egoDistToConflictFromJunctionEntry += 0.5 * (foeConflictLane->getWidth() - e->foe->getVehicleType().getWidth());
1796  break;
1797  } else {
1798  foeInternalLaneLengthsBeforeCrossing += foeConflictLane->getLength();
1799  }
1800  foeConflictLane = foeConflictLane->getCanonicalSuccessorLane();
1801  assert(foeConflictLane != 0 && foeConflictLane->isInternal()); // this loop should be ended by the break! Otherwise the lanes do not cross, which should be the case here.
1802  }
1803  assert(egoDistToConflictFromJunctionEntry != INVALID);
1804 
1805  // for the foe
1806  double foeDistToConflictFromJunctionEntry = INVALID;
1807  double egoInternalLaneLengthsBeforeCrossing = 0.;
1808  foeDistToConflictFromJunctionEntry = INVALID;
1809  while (egoConflictLane != 0 && egoConflictLane->isInternal()) {
1810  foeDistToConflictFromJunctionEntry = foeEntryLink->getLengthsBeforeCrossing(egoConflictLane);
1811  if (foeDistToConflictFromJunctionEntry != INVALID) {
1812  // found correct egoConflictLane
1813  foeDistToConflictFromJunctionEntry += 0.5 * (egoConflictLane->getWidth() - e->ego->getVehicleType().getWidth());
1814  break;
1815  } else {
1816  egoInternalLaneLengthsBeforeCrossing += egoConflictLane->getLength();
1817  }
1818  egoConflictLane = egoConflictLane->getCanonicalSuccessorLane();
1819  assert(egoConflictLane != 0 && egoConflictLane->isInternal()); // this loop should be ended by the break! Otherwise the lanes do not cross, which should be the case here.
1820  }
1821  assert(foeDistToConflictFromJunctionEntry != INVALID);
1822 
1823  // store conflict entry information in eInfo
1824 
1825 // // TO-DO: equip these with exit times to store relevant PET sections in encounter
1826 // eInfo.egoConflictEntryCrossSection = std::make_pair(egoConflictLane, egoDistToConflictFromJunctionEntry - egoInternalLaneLengthsBeforeCrossing);
1827 // eInfo.foeConflictEntryCrossSection = std::make_pair(foeConflictLane, foeDistToConflictFromJunctionEntry - foeInternalLaneLengthsBeforeCrossing);
1828 
1829  // Take into account the lateral position for the exact determination of the conflict point
1830  // whether lateral position increases or decreases conflict distance depends on lane angles at conflict
1831  // -> conflictLaneOrientation in {-1,+1}
1832  // First, measure the angle between the two connection lines (straight lines from junction entry point to junction exit point)
1833  Position egoEntryPos = egoEntryLink->getViaLane()->getShape().front();
1834  Position egoExitPos = egoEntryLink->getCorrespondingExitLink()->getInternalLaneBefore()->getShape().back();
1835  PositionVector egoConnectionLine(egoEntryPos, egoExitPos);
1836  Position foeEntryPos = foeEntryLink->getViaLane()->getShape().front();
1837  Position foeExitPos = foeEntryLink->getCorrespondingExitLink()->getInternalLaneBefore()->getShape().back();
1838  PositionVector foeConnectionLine(foeEntryPos, foeExitPos);
1839  double angle = std::fmod(egoConnectionLine.rotationAtOffset(0.) - foeConnectionLine.rotationAtOffset(0.), (2 * M_PI));
1840  if (angle < 0) {
1841  angle += 2 * M_PI;
1842  }
1843  assert(angle >= 0);
1844  assert(angle <= 2 * M_PI);
1845  if (angle > M_PI) {
1846  angle -= 2 * M_PI;
1847  }
1848  assert(angle >= -M_PI);
1849  assert(angle <= M_PI);
1850  // Determine orientation of the connection lines. (Positive values mean that the ego vehicle approaches from the foe's left side.)
1851  double crossingOrientation = (angle < 0) - (angle > 0);
1852 
1853  // Adjust conflict dist to lateral positions
1854  // TODO: This could more precisely be calculated wrt the angle of the crossing *at the conflict point*
1855  egoDistToConflictFromJunctionEntry -= crossingOrientation * e->foe->getLateralPositionOnLane();
1856  foeDistToConflictFromJunctionEntry += crossingOrientation * e->ego->getLateralPositionOnLane();
1857 
1858  // Complete entry distances
1859  eInfo.egoConflictEntryDist = egoDistToConflictLane + egoDistToConflictFromJunctionEntry;
1860  eInfo.foeConflictEntryDist = foeDistToConflictLane + foeDistToConflictFromJunctionEntry;
1861 
1862 
1863 #ifdef DEBUG_SSM
1864  std::cout << " Determined exact conflict distances for crossing conflict."
1865  << "\n crossingOrientation=" << crossingOrientation
1866  << ", egoCrossingAngle=" << egoConnectionLine.rotationAtOffset(0.)
1867  << ", foeCrossingAngle=" << foeConnectionLine.rotationAtOffset(0.)
1868  << ", relativeAngle=" << angle
1869  << " (foe from " << (crossingOrientation > 0 ? "right)" : "left)")
1870  << "\n resulting offset for conflict entry distance:"
1871  << "\n ego=" << crossingOrientation* e->foe->getLateralPositionOnLane()
1872  << ", foe=" << crossingOrientation* e->ego->getLateralPositionOnLane()
1873  << "\n resulting entry distances:"
1874  << "\n ego=" << eInfo.egoConflictEntryDist
1875  << ", foe=" << eInfo.foeConflictEntryDist
1876  << std::endl;
1877 #endif
1878 
1879  // TODO: This could also more precisely be calculated wrt the angle of the crossing *at the conflict point*
1880  eInfo.egoConflictAreaLength = e->foe->getWidth();
1881  eInfo.foeConflictAreaLength = e->ego->getWidth();
1882 
1883  // resulting exit distances
1886 
1887 #ifdef DEBUG_SSM
1888  std::cout << "real egoConflictLane: '" << (egoConflictLane == 0 ? "NULL" : egoConflictLane->getID()) << "'\n"
1889  << "real foeConflictLane: '" << (foeConflictLane == 0 ? "NULL" : foeConflictLane->getID()) << "'\n"
1890  << "-> Encounter type: Crossing situation of ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' and foe '"
1891  << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
1892  << "\nDistances to crossing-point: ego: " << eInfo.egoConflictEntryDist << ", foe: " << eInfo.foeConflictEntryDist
1893  << std::endl;
1894 #endif
1895  }
1896  }
1897  }
1898  return type;
1899 }
1900 
1901 
1902 const MSLane*
1903 MSDevice_SSM::findFoeConflictLane(const MSVehicle* foe, const MSLane* egoConflictLane, double& distToConflictLane) const {
1904 
1905 #ifdef DEBUG_SSM
1906  std::cout << SIMTIME << " findFoeConflictLane() for foe '"
1907  << foe->getID() << "' on lane '" << foe->getLane()->getID()
1908  << "' (with egoConflictLane=" << (egoConflictLane == 0 ? "NULL" : egoConflictLane->getID())
1909  << ")\nfoeBestLanes: " << toString(foe->getBestLanesContinuation())
1910  << std::endl;
1911 #endif
1912  MSLane* foeLane = foe->getLane();
1913  std::vector<MSLane*>::const_iterator laneIter = foe->getBestLanesContinuation().begin();
1914  std::vector<MSLane*>::const_iterator foeBestLanesEnd = foe->getBestLanesContinuation().end();
1915  assert(foeLane->isInternal() || *laneIter == foeLane);
1916  distToConflictLane = -foe->getPositionOnLane();
1917 
1918  // Potential conflict lies on junction if egoConflictLane is internal
1919  const MSJunction* conflictJunction = egoConflictLane->isInternal() ? egoConflictLane->getEdge().getToJunction() : 0;
1920 #ifdef DEBUG_SSM
1921  if (conflictJunction != 0) {
1922  std::cout << "Potential conflict on junction '" << conflictJunction->getID()
1923  << std::endl;
1924  }
1925 #endif
1926  if (foeLane->isInternal() && foeLane->getEdge().getToJunction() == conflictJunction) {
1927  // foe is already on the conflict junction
1928  return foeLane;
1929  }
1930 
1931  // Foe is not on the conflict junction
1932 
1933  // Leading internal lanes in bestlanes are resembled as a single NULL-pointer skip them
1934  if (*laneIter == 0) {
1935  while (foeLane != 0 && foeLane->isInternal()) {
1936  distToConflictLane += foeLane->getLength();
1937  foeLane = foeLane->getLinkCont()[0]->getViaLane();
1938  }
1939  ++laneIter;
1940  assert(laneIter == foeBestLanesEnd || *laneIter != 0);
1941  }
1942 
1943  // Look for the junction downstream along foeBestLanes
1944  while (laneIter != foeBestLanesEnd && distToConflictLane <= myRange) {
1945  // Eventual internal lanes were skipped
1946  assert(*laneIter == foeLane || foeLane == 0);
1947  foeLane = *laneIter;
1948  assert(!foeLane->isInternal());
1949  if (&foeLane->getEdge() == &egoConflictLane->getEdge()) {
1950 #ifdef DEBUG_SSM
1951  std::cout << "Found conflict lane for foe: '" << foeLane->getID() << "'" << std::endl;
1952 #endif
1953  // found the potential conflict edge along foeBestLanes
1954  return foeLane;
1955  }
1956  // No conflict on foeLane
1957  distToConflictLane += foeLane->getLength();
1958 
1959  // set laneIter to next non internal lane along foeBestLanes
1960  ++laneIter;
1961  if (laneIter == foeBestLanesEnd) {
1962  return 0;
1963  }
1964  MSLane* nextNonInternalLane = *laneIter;
1965  MSLink* link = foeLane->getLinkTo(nextNonInternalLane);
1966  // Set foeLane to first internal lane on the next junction
1967  foeLane = link->getViaLane();
1968  assert(foeLane->isInternal());
1969  if (foeLane->getEdge().getToJunction() == conflictJunction) {
1970 #ifdef DEBUG_SSM
1971  std::cout << "Found conflict lane for foe: '" << foeLane->getID() << "'" << std::endl;
1972 #endif
1973  // found egoConflictLane, resp. the conflict junction, along foeBestLanes
1974  return foeLane;
1975  }
1976  // No conflict on junction
1977  distToConflictLane += link->getInternalLengthsAfter();
1978  foeLane = nextNonInternalLane;
1979  }
1980  // Didn't find conflicting lane on foeBestLanes within range.
1981  return 0;
1982 }
1983 
1984 void
1986 #ifdef DEBUG_SSM
1987  std::cout << "\n" << SIMTIME << " Device '" << getID() << "' flushConflicts()" << std::endl;
1988 #endif
1989  while (!myPastConflicts.empty()) {
1990  Encounter* top = myPastConflicts.top();
1991  if (flushAll || top->begin <= myOldestActiveEncounterBegin) {
1992  writeOutConflict(top);
1993  myPastConflicts.pop();
1994  delete top;
1995  } else {
1996  break;
1997  }
1998  }
1999 }
2000 
2001 void
2004 }
2005 
2006 void
2008  for (Position& x : xv) {
2009  toGeo(x);
2010  }
2011 }
2012 
2013 void
2015 #ifdef DEBUG_SSM
2016  std::cout << SIMTIME << " writeOutConflict() of vehicles '"
2017  << e->egoID << "' and '" << e->foeID
2018  << "'\ntoGeo=" << myUseGeoCoords << std::endl;
2019 #endif
2020  myOutputFile->openTag("conflict");
2021  myOutputFile->writeAttr("begin", e->begin).writeAttr("end", e->end);
2022  myOutputFile->writeAttr("ego", e->egoID).writeAttr("foe", e->foeID);
2023 
2024  if (mySaveTrajectories) {
2025  myOutputFile->openTag("timeSpan").writeAttr("values", e->timeSpan).closeTag();
2026  myOutputFile->openTag("typeSpan").writeAttr("values", e->typeSpan).closeTag();
2027 
2028  // Some useful snippets for that (from MSFCDExport.cpp):
2029  if (myUseGeoCoords) {
2030  toGeo(e->egoTrajectory.x);
2031  toGeo(e->foeTrajectory.x);
2033  }
2034 
2036  myOutputFile->openTag("egoVelocity").writeAttr("values", toString(e->egoTrajectory.v)).closeTag();
2037 
2038  myOutputFile->openTag("foePosition").writeAttr("values", toString(e->foeTrajectory.x, myUseGeoCoords ? gPrecisionGeo : gPrecision)).closeTag();
2039  myOutputFile->openTag("foeVelocity").writeAttr("values", toString(e->foeTrajectory.v)).closeTag();
2040 
2041  myOutputFile->openTag("conflictPoint").writeAttr("values", toString(e->conflictPointSpan, myUseGeoCoords ? gPrecisionGeo : gPrecision)).closeTag();
2042  }
2043 
2044  if (myComputeTTC) {
2045  if (mySaveTrajectories) {
2046  myOutputFile->openTag("TTCSpan").writeAttr("values", makeStringWithNAs(e->TTCspan, INVALID)).closeTag();
2047  }
2048  if (e->minTTC.time == INVALID) {
2049  myOutputFile->openTag("minTTC").writeAttr("time", "NA").writeAttr("position", "NA").writeAttr("type", "NA").writeAttr("value", "NA").closeTag();
2050  } else {
2051  std::string time = toString(e->minTTC.time);
2052  std::string type = toString(int(e->minTTC.type));
2053  std::string value = toString(e->minTTC.value);
2054  if (myUseGeoCoords) {
2055  toGeo(e->minTTC.pos);
2056  }
2057  std::string position = toString(e->minTTC.pos, myUseGeoCoords ? gPrecisionGeo : gPrecision);
2058  myOutputFile->openTag("minTTC").writeAttr("time", time).writeAttr("position", position).writeAttr("type", type).writeAttr("value", value).closeTag();
2059  }
2060  }
2061  if (myComputeDRAC) {
2062  if (mySaveTrajectories) {
2063  myOutputFile->openTag("DRACSpan").writeAttr("values", makeStringWithNAs(e->DRACspan, {0.0, INVALID})).closeTag();
2064  }
2065  if (e->maxDRAC.time == INVALID) {
2066  myOutputFile->openTag("maxDRAC").writeAttr("time", "NA").writeAttr("position", "NA").writeAttr("type", "NA").writeAttr("value", "NA").closeTag();
2067  } else {
2068  std::string time = toString(e->maxDRAC.time);
2069  std::string type = toString(int(e->maxDRAC.type));
2070  std::string value = toString(e->maxDRAC.value);
2071  if (myUseGeoCoords) {
2072  toGeo(e->maxDRAC.pos);
2073  }
2074  std::string position = toString(e->maxDRAC.pos, myUseGeoCoords ? gPrecisionGeo : gPrecision);
2075  myOutputFile->openTag("maxDRAC").writeAttr("time", time).writeAttr("position", position).writeAttr("type", type).writeAttr("value", value).closeTag();
2076  }
2077  }
2078  if (myComputePET) {
2079  if (e->PET.time == INVALID) {
2080  myOutputFile->openTag("PET").writeAttr("time", "NA").writeAttr("position", "NA").writeAttr("type", "NA").writeAttr("value", "NA").closeTag();
2081  } else {
2082  std::string time = toString(e->PET.time);
2083  std::string type = toString(int(e->PET.type));
2084  std::string value = toString(e->PET.value);
2085  if (myUseGeoCoords) {
2086  toGeo(e->PET.pos);
2087  }
2088  std::string position = toString(e->PET.pos, myUseGeoCoords ? gPrecisionGeo : gPrecision);
2089  myOutputFile->openTag("PET").writeAttr("time", time).writeAttr("position", position).writeAttr("type", type).writeAttr("value", value).closeTag();
2090  }
2091  }
2093 }
2094 
2095 std::string
2096 MSDevice_SSM::makeStringWithNAs(std::vector<double> v, double NA, std::string sep) {
2097  std::string res = "";
2098  for (std::vector<double>::const_iterator i = v.begin(); i != v.end(); ++i) {
2099  res += (i == v.begin() ? "" : sep) + (*i == NA ? "NA" : toString(*i));
2100  }
2101  return res;
2102 }
2103 
2104 std::string
2105 MSDevice_SSM::makeStringWithNAs(std::vector<double> v, std::vector<double> NAs, std::string sep) {
2106  std::string res = "";
2107  for (std::vector<double>::const_iterator i = v.begin(); i != v.end(); ++i) {
2108  res += (i == v.begin() ? "" : sep) + (find(NAs.begin(), NAs.end(), *i) != NAs.end() ? "NA" : toString(*i));
2109  }
2110  return res;
2111 }
2112 
2113 
2114 // ---------------------------------------------------------------------------
2115 // MSDevice_SSM-methods
2116 // ---------------------------------------------------------------------------
2117 MSDevice_SSM::MSDevice_SSM(SUMOVehicle& holder, const std::string& id, std::string outputFilename, std::map<std::string, double> thresholds,
2118  bool trajectories, double range, double extraTime, bool useGeoCoords) :
2119  MSDevice(holder, id),
2120  myThresholds(thresholds),
2121  mySaveTrajectories(trajectories),
2122  myRange(range),
2123  myExtraTime(extraTime),
2124  myUseGeoCoords(useGeoCoords),
2126  // Take care! Holder is currently being constructed. Cast occurs before completion.
2127  myHolderMS = static_cast<MSVehicle*>(&holder);
2128 
2129  myComputeTTC = myThresholds.find("TTC") != myThresholds.end();
2130  myComputeDRAC = myThresholds.find("DRAC") != myThresholds.end();
2131  myComputePET = myThresholds.find("PET") != myThresholds.end();
2132 
2135 
2136  // XXX: Who deletes the OutputDevice?
2137  myOutputFile = &OutputDevice::getDevice(outputFilename);
2138 // TODO: make xsd, include header
2139 // myOutputFile.writeXMLHeader("SSMLog", "SSMLog.xsd");
2140  if (createdOutputFiles.count(outputFilename) == 0) {
2141  myOutputFile->openTag("SSMLog");
2142  createdOutputFiles.insert(outputFilename);
2143  }
2144 
2145  // register at static instance container
2146  instances->insert(this);
2147 
2148 #ifdef DEBUG_SSM
2149  std::vector<std::string> measures;
2150  std::vector<double> threshVals;
2151  for (std::map<std::string, double>::const_iterator i = myThresholds.begin(); i != myThresholds.end(); ++i) {
2152  measures.push_back(i->first);
2153  threshVals.push_back(i->second);
2154  }
2155  std::cout << "Initialized ssm device '" << id << "' with "
2156  << "myMeasures=" << joinToString(measures, " ")
2157  << ", myThresholds=" << joinToString(threshVals, " ")
2158  << ", mySaveTrajectories=" << mySaveTrajectories
2159  << ", myRange=" << myRange << ", output file=" << outputFilename << ", extra time=" << myExtraTime << ", useGeo=" << myUseGeoCoords << "\n";
2160 #endif
2161 }
2162 
2165  // XXX: Who deletes this device?
2166  // unregister from static instance container
2167  instances->erase((MSDevice*) this);
2168  resetEncounters();
2169  flushConflicts(true);
2170 }
2171 
2172 
2173 bool
2175 #ifdef DEBUG_SSM_NOTIFICATIONS
2176  std::cout << "device '" << getID() << "' notifyEnter: reason=" << reason << " currentEdge=" << veh.getLane()->getEdge().getID() << "\n";
2177 #else
2178  UNUSED_PARAMETER(veh);
2179  UNUSED_PARAMETER(reason);
2180 #endif
2181  return true; // keep the device
2182 }
2183 
2184 bool
2185 MSDevice_SSM::notifyLeave(SUMOVehicle& veh, double /*lastPos*/,
2186  MSMoveReminder::Notification reason, const MSLane* /* enteredLane */) {
2187 #ifdef DEBUG_SSM_NOTIFICATIONS
2188  std::cout << "device '" << getID() << "' notifyLeave: reason=" << reason << " currentEdge=" << veh.getLane()->getEdge().getID() << "\n";
2189 #else
2190  UNUSED_PARAMETER(veh);
2191  UNUSED_PARAMETER(reason);
2192 #endif
2193  return true; // keep the device
2194 }
2195 
2196 bool
2197 MSDevice_SSM::notifyMove(SUMOVehicle& /* veh */, double /* oldPos */,
2198  double /* newPos */, double newSpeed) {
2199 #ifdef DEBUG_SSM_NOTIFICATIONS
2200  std::cout << "device '" << getID() << "' notifyMove: newSpeed=" << newSpeed << "\n";
2201 #else
2202  UNUSED_PARAMETER(newSpeed);
2203 #endif
2204  return true; // keep the device
2205 }
2206 
2207 
2208 void
2209 MSDevice_SSM::findSurroundingVehicles(const MSVehicle& veh, double range, FoeInfoMap& foeCollector) {
2210 #ifdef DEBUG_SSM_SURROUNDING
2211  std::cout << SIMTIME << " Looking for surrounding vehicles for ego vehicle '" << veh.getID()
2212  << "' on edge '" << veh.getLane()->getEdge().getID()
2213  << "'."
2214  << "\nVehicle's best lanes = " << toString(veh.getBestLanesContinuation())
2215  << std::endl;
2216 #endif
2217 
2218  if (!veh.isOnRoad()) {
2219  return;
2220  }
2221 
2222  // The requesting vehicle's current route
2223  // XXX: Restriction to route scanning may have to be generalized to scanning of possible continuations when
2224  // considering situations involving sudden route changes. See also the definition of the EncounterTypes.
2225  // A second problem is that following situations on deviating routes may result in closing encounters
2226  // too early if a leading foe is not traced on its new lane. (see test 'foe_leader_deviating_routes')
2227 
2228  // If veh is on an internal edge, the edgeIter points towards the last edge before the junction
2229  ConstMSEdgeVector::const_iterator edgeIter = veh.getCurrentRouteEdge();
2230  assert(*edgeIter != 0);
2231 
2232  // Best continuation lanes for the ego vehicle
2233  const std::vector<MSLane*> egoBestLanes = veh.getBestLanesContinuation();
2234  std::vector<MSLane*>::const_iterator laneIter = egoBestLanes.begin();
2235 
2236  // current lane in loop below
2237  const MSLane* lane = veh.getLane();
2238  assert(lane->isInternal() || lane == *laneIter);
2239  assert(lane != 0);
2240  // next non-internal lane on the route
2241  const MSLane* nextNonInternalLane = 0;
2242 
2243  const MSEdge* edge; // current edge in loop below
2244 
2245  // Init pos with vehicle's current position. Below pos is set to zero to denote
2246  // the beginning position of the currently considered edge
2247  double pos = veh.getPositionOnLane();
2248  // remainingRange is the range minus the distance that is already scanned downstream along the vehicles route
2249  double remainingDownstreamRange = range;
2250  // 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)
2251  double distToConflictLane = -pos;
2252  // junctions that were already scanned (break search in recurrent nets)
2253  std::set<const MSJunction*> seenJunctions;
2254 
2255  // if the current edge is internal, collect all vehicles from the junction and below range upstream (except on the vehicles own edge),
2256  // this is analogous to the code treating junctions in the loop below. Note that the distance on the junction itself is not included into
2257  // range, so vehicles farther away than range can be collected, too.
2258  if (lane->isInternal()) {
2259  edge = &(lane->getEdge());
2260 
2261 #ifdef DEBUG_SSM_SURROUNDING
2262  std::cout << SIMTIME << " Vehicle '" << veh.getID() << "' is on internal edge " << edge->getID() << "'.\n"
2263  << "Previous edge of its route: '" << (*edgeIter)->getID() << "'" << std::endl;
2264 #endif
2265 
2266  assert(edge->getToJunction() == edge->getFromJunction());
2267 
2268  const MSJunction* junction = edge->getToJunction();
2269  // Collect vehicles on the junction
2270  getVehiclesOnJunction(junction, distToConflictLane, lane, foeCollector);
2271  seenJunctions.insert(junction);
2272 
2273  // Collect vehicles on incoming edges.
2274  // Note that this includes the previous edge on the ego vehicle's route.
2275  // (The distance on the current internal edge is ignored)
2276  const ConstMSEdgeVector& incoming = junction->getIncoming();
2277  for (ConstMSEdgeVector::const_iterator ei = incoming.begin(); ei != incoming.end(); ++ei) {
2278  if ((*ei)->isInternal()) {
2279  continue;
2280  }
2281  // Upstream range is taken from the vehicle's back
2282  getUpstreamVehicles(*ei, (*ei)->getLength(), range + veh.getLength(), distToConflictLane, lane, foeCollector, seenJunctions);
2283  }
2284 
2285 // // Take into account internal distance covered on the current lane
2286 // (commented out, because upstream scanning disregards internal lanes on the last scanned junction
2287 // -- this makes the scanning symmetric between leader and follower)
2288 // remainingDownstreamRange -= lane->getLength() - pos;
2289 
2290  // Take into account non-internal lengths until next non-internal lane
2291  MSLink* link = lane->getLinkCont()[0];
2292  remainingDownstreamRange -= link->getInternalLengthsAfter();
2293  distToConflictLane += lane->getLength() + link->getInternalLengthsAfter();
2294 
2295  // The next non-internal lane
2296  pos = 0.;
2297  lane = *(++laneIter);
2298  edge = &lane->getEdge();
2299  } else {
2300  // Collect all vehicles in range behind ego vehicle
2301  edge = &(lane->getEdge());
2302  getUpstreamVehicles(edge, pos, range + veh.getLength(), distToConflictLane, lane, foeCollector, seenJunctions);
2303  }
2304 
2305  assert(lane != 0);
2306  assert(!lane->isInternal());
2307 
2308  // Advance downstream the ego vehicle's route for distance 'range'.
2309  // Collect all vehicles on the traversed Edges and on incoming edges at junctions.
2310  while (remainingDownstreamRange > 0.) {
2311 #ifdef DEBUG_SSM_SURROUNDING
2312  std::cout << SIMTIME << " Scanning downstream for vehicle '" << veh.getID() << "' on lane '" << veh.getLane()->getID() << "', position=" << pos << ".\n"
2313  << "Considering edge '" << edge->getID() << "' Remaining downstream range = " << remainingDownstreamRange
2314  << "\nbestLanes=" << toString(egoBestLanes) << "\n"
2315  << std::endl;
2316 #endif
2317  assert(!edge->isInternal());
2318  assert(!lane->isInternal());
2319  assert(pos == 0 || lane == veh.getLane());
2320  if (pos + remainingDownstreamRange < lane->getLength()) {
2321  // scan range ends on this lane
2322  getUpstreamVehicles(edge, pos + remainingDownstreamRange, remainingDownstreamRange, distToConflictLane, lane, foeCollector, seenJunctions);
2323  // scanned required downstream range
2324  break;
2325  } else {
2326  // Also need to scan area that reaches beyond the lane
2327  // Collecting vehicles on non-internal edge ahead
2328  getUpstreamVehicles(edge, edge->getLength(), edge->getLength() - pos, distToConflictLane, lane, foeCollector, seenJunctions);
2329  // account for scanned distance on lane
2330  remainingDownstreamRange -= lane->getLength() - pos;
2331  distToConflictLane += lane->getLength();
2332  pos = 0.;
2333 
2334  // proceed to next non-internal lane
2335  ++laneIter;
2336  assert(laneIter == egoBestLanes.end() || *laneIter != 0);
2337 
2338  // If the vehicle's best lanes go on, collect vehicles on the upcoming junction
2339  if (laneIter != egoBestLanes.end()) {
2340  // Upcoming junction
2341  const MSJunction* junction = lane->getEdge().getToJunction();
2342 
2343  // Find connection for ego on the junction
2344  nextNonInternalLane = *laneIter;
2345  MSLink* link = lane->getLinkTo(nextNonInternalLane);
2346  assert(link != 0);
2347  // First lane of the connection
2348  lane = link->getViaLane();
2349  assert(lane != 0);
2350 
2351  if (seenJunctions.count(junction) == 0) {
2352  // Collect vehicles on the junction, if it wasn't considered already
2353  getVehiclesOnJunction(junction, distToConflictLane, lane, foeCollector);
2354  seenJunctions.insert(junction);
2355  // Collect vehicles on incoming edges (except the last edge, where we already collected). Use full range.
2356  const ConstMSEdgeVector& incoming = junction->getIncoming();
2357  for (ConstMSEdgeVector::const_iterator ei = incoming.begin(); ei != incoming.end(); ++ei) {
2358  if (*ei == edge || (*ei)->isInternal()) {
2359  continue;
2360  }
2361  getUpstreamVehicles(*ei, (*ei)->getLength(), range, distToConflictLane, lane, foeCollector, seenJunctions);
2362  }
2363  // account for scanned distance on junction
2364  double linkLength = link->getInternalLengthsAfter();
2365  remainingDownstreamRange -= linkLength;
2366  distToConflictLane += linkLength;
2367 #ifdef DEBUG_SSM_SURROUNDING
2368  std::cout << " Downstream Scan for vehicle '" << veh.getID() << "' proceeded over junction '" << junction->getID()
2369  << "',\n linkLength=" << linkLength << ", remainingDownstreamRange=" << remainingDownstreamRange
2370  << std::endl;
2371 #endif
2372 
2373  // update ego's lane to next non internal edge
2374  lane = nextNonInternalLane;
2375  edge = &(lane->getEdge());
2376  } else {
2377 #ifdef DEBUG_SSM_SURROUNDING
2378  std::cout << " Downstream Scan for vehicle '" << veh.getID() << "' stops at junction '" << junction->getID()
2379  << "', which has already been scanned."
2380  << std::endl;
2381 #endif
2382  break;
2383  }
2384  } else {
2385  // Further vehicle path unknown, break search
2386  break;
2387  }
2388  }
2389  }
2390  // remove ego vehicle
2391  foeCollector.erase(&veh);
2392 }
2393 
2394 void
2395 MSDevice_SSM::getUpstreamVehicles(const MSEdge* edge, double pos, double range, double egoDistToConflictLane, const MSLane* const egoConflictLane, FoeInfoMap& foeCollector, std::set<const MSJunction*> seenJunctions) {
2396 #ifdef DEBUG_SSM_SURROUNDING
2397  std::cout << SIMTIME << " getUpstreamVehicles() for edge '" << edge->getID() << "'"
2398  << " pos = " << pos << " range = " << range
2399  << "\nFound vehicles:"
2400  << std::endl;
2401 #endif
2402  if (range <= 0) {
2403  return;
2404  }
2405 
2406  const std::vector<MSLane*>& lanes = edge->getLanes();
2407  // Collect vehicles on the given edge with position in [pos-range,pos]
2408  for (std::vector<MSLane*>::const_iterator li = lanes.begin(); li != lanes.end(); ++li) {
2409  MSLane* lane = *li;
2410  const MSLane::VehCont& vehicles = lane->getVehiclesSecure();
2411  for (MSLane::VehCont::const_iterator vi = vehicles.begin(); vi != vehicles.end(); ++vi) {
2412  MSVehicle* veh = *vi;
2413  if (veh->getPositionOnLane() - veh->getLength() <= pos && veh->getPositionOnLane() >= pos - range) {
2414 #ifdef DEBUG_SSM
2415  std::cout << veh->getID() << "\n";
2416 #endif
2417  FoeInfo* c = new FoeInfo(); // c is deleted in updateEncounter()
2418  c->egoDistToConflictLane = egoDistToConflictLane;
2419  c->egoConflictLane = egoConflictLane;
2420  foeCollector[veh] = c;
2421  }
2422  }
2423  lane->releaseVehicles();
2424  }
2425 
2426 #ifdef DEBUG_SSM
2427  std::cout << std::endl;
2428 #endif
2429 
2430  // TODO: Gather vehicles from opposite direction. This should happen in any case, where opposite direction overtaking is possible.
2431  // If it isn't it might still be nicer to trace oncoming vehicles for the resulting trajectories in the encounters
2432  // if (edge->hasOpposite...)
2433 
2434  if (range <= pos) {
2435  return;
2436  }
2437 
2438  // Here we have: range > pos, i.e. we proceed collecting vehicles on preceding edges
2439  range -= pos;
2440 
2441  // Junction representing the origin of 'edge'
2442  const MSJunction* junction = edge->getFromJunction();
2443  if (seenJunctions.count(junction) == 0) {
2444  // Collect vehicles from incoming edges of the junction
2445  if (!edge->isInternal()) {
2446  // collect vehicles on preceding junction (for internal edges this is already done in caller,
2447  // i.e. findSurroundingVehicles() or the recursive call from getUpstreamVehicles())
2448 
2449  // Collect vehicles on the junction, if it wasn't considered already
2450  getVehiclesOnJunction(junction, egoDistToConflictLane, egoConflictLane, foeCollector);
2451  seenJunctions.insert(junction);
2452  }
2453  // Collect vehicles from incoming edges from the junction representing the origin of 'edge'
2454  const ConstMSEdgeVector& incoming = junction->getIncoming();
2455  for (ConstMSEdgeVector::const_iterator ei = incoming.begin(); ei != incoming.end(); ++ei) {
2456  if ((*ei)->isInternal()) {
2457  continue;
2458  }
2459  const MSEdge* inEdge = *ei;
2460  assert(inEdge != 0);
2461  double distOnJunction = edge->isInternal() ? 0. : inEdge->getInternalFollowingLengthTo(edge);
2462  if (distOnJunction >= range) {
2463  continue;
2464  }
2465  // account for vehicles on the predecessor edge
2466  getUpstreamVehicles(inEdge, inEdge->getLength(), range - distOnJunction, egoDistToConflictLane, egoConflictLane, foeCollector, seenJunctions);
2467  }
2468  } else {
2469 #ifdef DEBUG_SSM_SURROUNDING
2470  std::cout << " Downstream Scan for stops at junction '" << junction->getID()
2471  << "', which has already been scanned."
2472  << std::endl;
2473 #endif
2474  }
2475 }
2476 
2477 void
2478 MSDevice_SSM::getVehiclesOnJunction(const MSJunction* junction, double egoDistToConflictLane, const MSLane* const egoConflictLane, FoeInfoMap& foeCollector) {
2479 #ifdef DEBUG_SSM_SURROUNDING
2480  std::cout << SIMTIME << " getVehiclesOnJunction() for junction '" << junction->getID() << "'"
2481  << "\nFound vehicles:"
2482  << std::endl;
2483 #endif
2484  // Collect vehicles on internal lanes
2485  const std::vector<MSLane*> lanes = junction->getInternalLanes();
2486  for (std::vector<MSLane*>::const_iterator li = lanes.begin(); li != lanes.end(); ++li) {
2487  MSLane* lane = *li;
2488  const MSLane::VehCont& vehicles = lane->getVehiclesSecure();
2489 
2490  // Add FoeInfos (XXX: for some situations, a vehicle may be collected twice. Then the later finding overwrites the earlier in foeCollector.
2491  // This could lead to neglecting a conflict when determining foeConflictLane later.) -> TODO: test with twice intersecting routes
2492  for (MSLane::VehCont::const_iterator vi = vehicles.begin(); vi != vehicles.end(); ++vi) {
2493  FoeInfo* c = new FoeInfo();
2494  c->egoConflictLane = egoConflictLane;
2495  c->egoDistToConflictLane = egoDistToConflictLane;
2496  foeCollector[*vi] = c;
2497 #ifdef DEBUG_SSM_SURROUNDING
2498  for (MSLane::VehCont::const_iterator vi = vehicles.begin(); vi != vehicles.end(); ++vi) {
2499  std::cout << (*vi)->getID() << "\n";
2500  }
2501 #endif
2502  }
2503  lane->releaseVehicles();
2504 
2505  // If there is an internal continuation lane, also collect vehicles on that lane
2506  if (lane->getLinkCont().size() > 1 && lane->getLinkCont()[0]->getViaLane() != 0) {
2507  // There's a second internal lane of the connection
2508  lane = lane->getLinkCont()[0]->getViaLane();
2509  // This code must be modified, if more than two-piece internal lanes are allowed. Thus, assert:
2510  assert(lane->getLinkCont().size() == 0 || lane->getLinkCont()[0]->getViaLane() == 0);
2511 
2512  // collect vehicles
2513  const MSLane::VehCont& vehicles2 = lane->getVehiclesSecure();
2514  // Add FoeInfos. This duplicates the loop for the first internal lane
2515  for (MSLane::VehCont::const_iterator vi = vehicles2.begin(); vi != vehicles2.end(); ++vi) {
2516  FoeInfo* c = new FoeInfo();
2517  c->egoConflictLane = egoConflictLane;
2518  c->egoDistToConflictLane = egoDistToConflictLane;
2519  foeCollector[*vi] = c;
2520 #ifdef DEBUG_SSM_SURROUNDING
2521  for (MSLane::VehCont::const_iterator vi = vehicles2.begin(); vi != vehicles2.end(); ++vi) {
2522  std::cout << (*vi)->getID() << "\n";
2523  }
2524 #endif
2525  }
2526  lane->releaseVehicles();
2527  }
2528  }
2529 
2530 #ifdef DEBUG_SSM_SURROUNDING
2531  std::cout << std::endl;
2532 #endif
2533 }
2534 
2535 
2536 
2537 void
2539  // This is called once at vehicle removal.
2540  // Also: flush myOutputFile? Or is this done automatically?
2541  // myOutputFile->closeTag();
2542 }
2543 
2544 // ---------------------------------------------------------------------------
2545 // Static parameter load helpers
2546 // ---------------------------------------------------------------------------
2547 std::string
2548 MSDevice_SSM::getOutputFilename(const SUMOVehicle& v, std::string deviceID) {
2549  std::string file = deviceID + ".xml";
2550  if (v.getParameter().knowsParameter("device.ssm.file")) {
2551  try {
2552  file = v.getParameter().getParameter("device.ssm.file", file);
2553  } catch (...) {
2554  WRITE_WARNING("Invalid value '" + v.getParameter().getParameter("device.ssm.file", file) + "'for vehicle parameter 'ssm.measures'");
2555  }
2556  } else if (v.getVehicleType().getParameter().knowsParameter("device.ssm.file")) {
2557  try {
2558  file = v.getVehicleType().getParameter().getParameter("device.ssm.file", file);
2559  } catch (...) {
2560  WRITE_WARNING("Invalid value '" + v.getVehicleType().getParameter().getParameter("device.ssm.file", file) + "'for vType parameter 'ssm.measures'");
2561  }
2562  } else {
2563  std::cout << "vehicle '" << v.getID() << "' does not supply vehicle parameter 'device.ssm.file'. Using default of '" << file << "'\n";
2564  }
2565  return file;
2566 }
2567 
2568 bool
2571  bool useGeo = false;
2572  if (v.getParameter().knowsParameter("device.ssm.geo")) {
2573  try {
2574  useGeo = TplConvert::_2bool(v.getParameter().getParameter("device.ssm.geo", "no").c_str());
2575  } catch (...) {
2576  WRITE_WARNING("Invalid value '" + v.getParameter().getParameter("device.ssm.geo", "no") + "'for vehicle parameter 'ssm.geo'");
2577  }
2578  } else if (v.getVehicleType().getParameter().knowsParameter("device.ssm.geo")) {
2579  try {
2580  useGeo = TplConvert::_2bool(v.getVehicleType().getParameter().getParameter("device.ssm.geo", "no").c_str());
2581  } catch (...) {
2582  WRITE_WARNING("Invalid value '" + v.getVehicleType().getParameter().getParameter("device.ssm.geo", "no") + "'for vType parameter 'ssm.geo'");
2583  }
2584  } else {
2585  useGeo = oc.getBool("device.ssm.geo");
2586 #ifdef DEBUG_SSM
2587  std::cout << "vehicle '" << v.getID() << "' does not supply vehicle parameter 'device.ssm.geo'. Using default of '" << useGeo << "'\n";
2588 #endif
2589  }
2590  return useGeo;
2591 }
2592 
2593 
2594 double
2597  double range = -INVALID;
2598  if (v.getParameter().knowsParameter("device.ssm.range")) {
2599  try {
2600  range = TplConvert::_2double(v.getParameter().getParameter("device.ssm.range", "").c_str());
2601  } catch (...) {
2602  WRITE_WARNING("Invalid value '" + v.getParameter().getParameter("device.ssm.range", "") + "'for vehicle parameter 'ssm.range'");
2603  }
2604  } else if (v.getVehicleType().getParameter().knowsParameter("device.ssm.range")) {
2605  try {
2606  range = TplConvert::_2double(v.getVehicleType().getParameter().getParameter("device.ssm.range", "").c_str());
2607  } catch (...) {
2608  WRITE_WARNING("Invalid value '" + v.getVehicleType().getParameter().getParameter("device.ssm.range", "") + "'for vType parameter 'ssm.range'");
2609  }
2610  } else {
2611  range = oc.getFloat("device.ssm.range");
2612 #ifdef DEBUG_SSM
2613  std::cout << "vehicle '" << v.getID() << "' does not supply vehicle parameter 'device.ssm.range'. Using default of '" << range << "'\n";
2614 #endif
2615  }
2616  return range;
2617 }
2618 
2619 
2620 double
2623  double extraTime = INVALID;
2624  if (v.getParameter().knowsParameter("device.ssm.extratime")) {
2625  try {
2626  extraTime = TplConvert::_2double(v.getParameter().getParameter("device.ssm.extratime", "").c_str());
2627  } catch (...) {
2628  WRITE_WARNING("Invalid value '" + v.getParameter().getParameter("device.ssm.extratime", "") + "'for vehicle parameter 'ssm.extratime'");
2629  }
2630  } else if (v.getVehicleType().getParameter().knowsParameter("device.ssm.extratime")) {
2631  try {
2632  extraTime = TplConvert::_2double(v.getVehicleType().getParameter().getParameter("device.ssm.extratime", "").c_str());
2633  } catch (...) {
2634  WRITE_WARNING("Invalid value '" + v.getVehicleType().getParameter().getParameter("device.ssm.extratime", "") + "'for vType parameter 'ssm.extratime'");
2635  }
2636  } else {
2637  extraTime = oc.getFloat("device.ssm.extratime");
2638 #ifdef DEBUG_SSM
2639  std::cout << "vehicle '" << v.getID() << "' does not supply vehicle parameter 'device.ssm.extratime'. Using default of '" << extraTime << "'\n";
2640 #endif
2641  }
2642  if (extraTime < 0.) {
2643  extraTime = DEFAULT_EXTRA_TIME;
2644  WRITE_WARNING("Negative (or no) value encountered for vehicle parameter 'device.ssm.extratime' in vehicle '" + v.getID() + "' using default value " + toString(extraTime) + " instead");
2645  }
2646  return extraTime;
2647 }
2648 
2649 
2650 bool
2653  bool trajectories = false;
2654  if (v.getParameter().knowsParameter("device.ssm.trajectories")) {
2655  try {
2656  trajectories = TplConvert::_2bool(v.getParameter().getParameter("device.ssm.trajectories", "no").c_str());
2657  } catch (...) {
2658  WRITE_WARNING("Invalid value '" + v.getParameter().getParameter("device.ssm.trajectories", "no") + "'for vehicle parameter 'ssm.trajectories'");
2659  }
2660  } else if (v.getVehicleType().getParameter().knowsParameter("device.ssm.trajectories")) {
2661  try {
2662  trajectories = TplConvert::_2bool(v.getVehicleType().getParameter().getParameter("device.ssm.trajectories", "no").c_str());
2663  } catch (...) {
2664  WRITE_WARNING("Invalid value '" + v.getVehicleType().getParameter().getParameter("device.ssm.trajectories", "no") + "'for vType parameter 'ssm.trajectories'");
2665  }
2666  } else {
2667  trajectories = oc.getBool("device.ssm.trajectories");
2668 #ifdef DEBUG_SSM
2669  std::cout << "vehicle '" << v.getID() << "' does not supply vehicle parameter 'device.ssm.trajectories'. Using default of '" << trajectories << "'\n";
2670 #endif
2671  }
2672  return trajectories;
2673 }
2674 
2675 
2676 bool
2677 MSDevice_SSM::getMeasuresAndThresholds(const SUMOVehicle& v, std::string deviceID, std::map<std::string, double>& thresholds) {
2679 
2680  // Measures
2681  std::string measures_str = "";
2682  if (v.getParameter().knowsParameter("device.ssm.measures")) {
2683  try {
2684  measures_str = v.getParameter().getParameter("device.ssm.measures", "");
2685  } catch (...) {
2686  WRITE_WARNING("Invalid value '" + v.getParameter().getParameter("device.ssm.measures", "") + "'for vehicle parameter 'ssm.measures'");
2687  }
2688  } else if (v.getVehicleType().getParameter().knowsParameter("device.ssm.measures")) {
2689  try {
2690  measures_str = v.getVehicleType().getParameter().getParameter("device.ssm.measures", "");
2691  } catch (...) {
2692  WRITE_WARNING("Invalid value '" + v.getVehicleType().getParameter().getParameter("device.ssm.measures", "") + "'for vType parameter 'ssm.measures'");
2693  }
2694  } else {
2695  measures_str = oc.getString("device.ssm.measures");
2696  std::cout << "vehicle '" << v.getID() << "' does not supply vehicle parameter 'device.ssm.measures'. Using default of '" << measures_str << "'\n";
2697  }
2698 
2699  // Check retrieved measures
2700  if (measures_str == "") {
2701  WRITE_WARNING("No measures specified for ssm device of vehicle '" + v.getID() + "'. Registering all available SSMs.");
2702  measures_str = AVAILABLE_SSMS;
2703  }
2705  std::vector<std::string> available = st.getVector();
2706  st = StringTokenizer(measures_str);
2707  std::vector<std::string> measures = st.getVector();
2708  for (std::vector<std::string>::const_iterator i = measures.begin(); i != measures.end(); ++i) {
2709  if (std::find(available.begin(), available.end(), *i) == available.end()) {
2710  // Given identifier is unknown
2711  WRITE_ERROR("SSM identifier '" + *i + "' is not supported. Aborting construction of SSM device '" + deviceID + "'.");
2712  return false;
2713  }
2714  }
2715 
2716  // Thresholds
2717  std::string thresholds_str = "";
2718  if (v.getParameter().knowsParameter("device.ssm.thresholds")) {
2719  try {
2720  thresholds_str = v.getParameter().getParameter("device.ssm.thresholds", "");
2721  } catch (...) {
2722  WRITE_WARNING("Invalid value '" + v.getParameter().getParameter("device.ssm.thresholds", "") + "'for vehicle parameter 'ssm.thresholds'");
2723  }
2724  } else if (v.getVehicleType().getParameter().knowsParameter("device.ssm.thresholds")) {
2725  try {
2726  thresholds_str = v.getVehicleType().getParameter().getParameter("device.ssm.thresholds", "");
2727  } catch (...) {
2728  WRITE_WARNING("Invalid value '" + v.getVehicleType().getParameter().getParameter("device.ssm.thresholds", "") + "'for vType parameter 'ssm.thresholds'");
2729  }
2730  } else {
2731  thresholds_str = oc.getString("device.ssm.thresholds");
2732 #ifdef DEBUG_SSM
2733  std::cout << "vehicle '" << v.getID() << "' does not supply vehicle parameter 'device.ssm.thresholds'. Using default of '" << thresholds_str << "'\n";
2734 #endif
2735  }
2736 
2737  // Parse vector of doubles from threshold_str
2738  int count = 0;
2739  if (thresholds_str != "") {
2740  st = StringTokenizer(thresholds_str);
2741  while (count < (int)measures.size() && st.hasNext()) {
2742  double thresh = TplConvert::_2double(st.next().c_str());
2743  thresholds.insert(std::make_pair(measures[count], thresh));
2744  ++count;
2745  }
2746  if (thresholds.size() < measures.size() || st.hasNext()) {
2747  WRITE_ERROR("Given list of thresholds ('" + thresholds_str + "') is not of the same size as the list of measures ('" + measures_str + "').\nPlease specify exactly one threshold for each measure.");
2748  return false;
2749  }
2750  } else {
2751  // assume default thresholds if none are given
2752  for (std::vector<std::string>::const_iterator i = measures.begin(); i != measures.end(); ++i) {
2753  if (*i == "TTC") {
2754  thresholds.insert(std::make_pair(*i, DEFAULT_THRESHOLD_TTC));
2755  } else if (*i == "DRAC") {
2756  thresholds.insert(std::make_pair(*i, DEFAULT_THRESHOLD_DRAC));
2757  } else if (*i == "PET") {
2758  thresholds.insert(std::make_pair(*i, DEFAULT_THRESHOLD_PET));
2759  } else {
2760  WRITE_ERROR("Unknown SSM identifier '" + (*i) + "'. Aborting construction of ssm device."); // should never occur
2761  return false;
2762  }
2763  }
2764  }
2765  return true;
2766 }
2767 
2768 
2769 
2770 /****************************************************************************/
2771 
bool myUseGeoCoords
Whether to use the original coordinate system for output.
Definition: MSDevice_SSM.h:626
void doRegister(const std::string &name, Option *v)
Adds an option under the given name.
Definition: OptionsCont.cpp:81
OutputDevice & writeAttr(const SumoXMLAttr attr, const T &val)
writes a named attribute
Definition: OutputDevice.h:260
static void determineConflictPoint(EncounterApproachInfo &eInfo)
Calculates the (x,y)-coordinate for the eventually predicted conflict point and stores the result in ...
#define AVAILABLE_SSMS
ENCOUNTER_TYPE_MERGING_ADJACENT.
Definition: MSDevice_SSM.h:93
const std::string egoID
Definition: MSDevice_SSM.h:190
static const std::set< MSDevice * > & getInstances()
returns all currently existing SSM devices
double getLength() const
Returns the vehicle&#39;s length.
MSEdge & getEdge() const
Returns the lane&#39;s edge.
Definition: MSLane.h:607
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
double getPreviousSpeed() const
Returns the vehicle&#39;s speed before the previous time step.
Definition: MSVehicle.h:490
#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.
ConflictPointInfo minTTC
Definition: MSDevice_SSM.h:232
Position getVelocityVector() const
Returns the vehicle&#39;s direction in radians.
Definition: MSVehicle.h:692
ConflictPointInfo maxDRAC
Definition: MSDevice_SSM.h:233
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:71
MSLane * getLane() const
Returns the lane the vehicle is on.
Definition: MSVehicle.h:564
PositionVector conflictPointSpan
Predicted location of the conflict: In case of MERGING and CROSSING: entry point to conflict area for...
Definition: MSDevice_SSM.h:219
static bool _2bool(const E *const data)
converts a 0-terminated char-type array into the boolean value described by it
Definition: TplConvert.h:388
void updatePassedEncounter(Encounter *e, FoeInfo *foeInfo, EncounterApproachInfo &eInfo)
Updates an encounter, which was classified as ENCOUNTER_TYPE_NOCONFLICT_AHEAD this may be the case be...
static void insertOptions(OptionsCont &oc)
Inserts MSDevice_SSM-options.
MSVehicle * myHolderMS
Definition: MSDevice_SSM.h:629
SUMOVehicle & myHolder
The vehicle that stores the device.
Definition: MSDevice.h:188
int gPrecision
the precision for floating point outputs
Definition: StdDefs.cpp:29
double egoConflictEntryTime
Times when the ego vehicle entered/left the conflict area. Currently only applies for crossing situat...
Definition: MSDevice_SSM.h:199
void determinePET(EncounterApproachInfo &eInfo) const
Discriminates between different encounter types and correspondingly determines the PET for those case...
A device which collects info on the vehicle trip (mainly on departure and arrival) ...
Definition: MSDevice_SSM.h:62
void determineTTCandDRAC(EncounterApproachInfo &eInfo) const
Discriminates between different encounter types and correspondingly determines TTC and DRAC for those...
The base class for an intersection.
Definition: MSJunction.h:64
const MSVehicle * foe
Definition: MSDevice_SSM.h:189
double getPositionOnLane() const
Get the vehicle&#39;s position along the lane.
Definition: MSVehicle.h:402
EncounterVector myActiveEncounters
Definition: MSDevice_SSM.h:636
#define INVALID
SUMOVehicle * getVehicle(const std::string &id) const
Returns the vehicle with the given id.
Notification
Definition of a vehicle state.
ENCOUNTER_TYPE_FOLLOWING_PASSED.
Definition: MSDevice_SSM.h:117
virtual MSLane * getLane() const =0
Returns the lane the vehicle is on.
double computeTTC(double gap, double followerSpeed, double leaderSpeed) const
Computes the time to collision (in seconds) for two vehicles with a given initial gap under the assum...
EncounterType classifyEncounter(const FoeInfo *foeInfo, EncounterApproachInfo &eInfo) const
Classifies the current type of the encounter provided some information on the opponents.
const std::vector< MSLane * > & getLanes() const
Returns this edge&#39;s lanes.
Definition: MSEdge.h:167
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
Definition: MSNet.cpp:167
T MAX2(T a, T b)
Definition: StdDefs.h:73
double getLength() const
Returns the lane&#39;s length.
Definition: MSLane.h:497
virtual const VehCont & getVehiclesSecure() const
Returns the vehicles container; locks it for microsimulation.
Definition: MSLane.h:392
const PositionVector & getShape() const
Returns this lane&#39;s shape.
Definition: MSLane.h:439
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:1881
Position getPosition(const double offset=0) const
Return current position (x/y, cartesian)
Definition: MSVehicle.cpp:901
double rotationAtOffset(double pos) const
Returns the rotation at the given length.
static double getDetectionRange(const SUMOVehicle &v)
ENCOUNTER_TYPE_COLLISION.
Definition: MSDevice_SSM.h:121
static void toGeo(Position &x)
convert SUMO-positions to geo coordinates (in place)
std::vector< double > TTCspan
All values for TTC.
Definition: MSDevice_SSM.h:222
ENCOUNTER_TYPE_EGO_ENTERED_CONFLICT_AREA.
Definition: MSDevice_SSM.h:104
Position pos
Predicted location of the conflict: In case of MERGING and CROSSING: entry point to conflict area for...
Definition: MSDevice_SSM.h:145
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:74
ENCOUNTER_TYPE_CROSSING.
Definition: MSDevice_SSM.h:96
const std::string foeID
Definition: MSDevice_SSM.h:191
#define TS
Definition: SUMOTime.h:51
#define DEFAULT_THRESHOLD_PET
double getLength() const
return the length of the edge
Definition: MSEdge.h:569
double time
time point of the conflict
Definition: MSDevice_SSM.h:141
const MSJunction * getToJunction() const
Definition: MSEdge.h:352
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.
void resetExtraTime(double value)
resets remainingExtraTime to the given value
MSDevice_SSM(SUMOVehicle &holder, const std::string &id, std::string outputFilename, std::map< std::string, double > thresholds, bool trajectories, double range, double extraTime, bool useGeoCoords)
Constructor.
ENCOUNTER_TYPE_CROSSING_FOLLOWER.
Definition: MSDevice_SSM.h:102
double getWidth() const
Returns the lane&#39;s width.
Definition: MSLane.h:513
#define UNUSED_PARAMETER(x)
Definition: StdDefs.h:39
ENCOUNTER_TYPE_BOTH_ENTERED_CONFLICT_AREA.
Definition: MSDevice_SSM.h:112
Position getPositionAlongBestLanes(double offset) const
Return the (x,y)-position, which the vehicle would reach if it continued along its best continuation ...
Definition: MSVehicle.cpp:933
#define WRITE_WARNING(msg)
Definition: MsgHandler.h:199
MSLane * getCanonicalSuccessorLane() const
Definition: MSLane.cpp:2315
static OptionsCont & getOptions()
Retrieves the options.
Definition: OptionsCont.cpp:64
#define SIMTIME
Definition: SUMOTime.h:71
static bool requestsTrajectories(const SUMOVehicle &v)
Encounter(const MSVehicle *_ego, const MSVehicle *const _foe, double _begin, double extraTime)
Constructor.
ENCOUNTER_TYPE_CROSSING_LEADER.
Definition: MSDevice_SSM.h:99
double getMaxSpeedOnLane() const
Returns the maximal speed for the vehicle on its current lane (including speed factor and deviation...
Definition: MSVehicle.h:574
EncounterType currentType
Definition: MSDevice_SSM.h:193
std::vector< double > DRACspan
All values for DRAC.
Definition: MSDevice_SSM.h:224
ENCOUNTER_TYPE_FOLLOWING_LEADER.
Definition: MSDevice_SSM.h:80
void cartesian2geo(Position &cartesian) const
Converts the given cartesian (shifted) position to its geo (lat/long) representation.
bool isInternal() const
Definition: MSLane.cpp:1774
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:586
ENCOUNTER_TYPE_MERGING_FOLLOWER.
Definition: MSDevice_SSM.h:91
const MSCFModel & getCarFollowModel() const
Returns the vehicle&#39;s car following model definition.
Definition: MSVehicle.h:882
ENCOUNTER_TYPE_MERGING.
Definition: MSDevice_SSM.h:85
std::size_t size() const
Returns the number of trajectory points stored.
Definition: MSDevice_SSM.h:166
static bool getMeasuresAndThresholds(const SUMOVehicle &v, std::string deviceID, std::map< std::string, double > &thresholds)
ENCOUNTER_TYPE_BOTH_LEFT_CONFLICT_AREA.
Definition: MSDevice_SSM.h:114
MSLink * getEntryLink() const
Returns the entry link if this is an internal lane, else 0.
Definition: MSLane.cpp:1902
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:55
Representation of a vehicle.
Definition: SUMOVehicle.h:66
static double computeDRAC(double gap, double followerSpeed, double leaderSpeed)
Computes the DRAC (deceleration to avoid a collision) for a lead/follow situation as defined...
void processEncounters(FoeInfoMap &foes, bool forceClose=false)
Finds encounters for which the foe vehicle has disappeared from range. remainingExtraTime is decrease...
#define DEFAULT_THRESHOLD_DRAC
A point in 2D or 3D with translation and scaling methods.
Definition: Position.h:45
bool myComputeTTC
Flags for switching on / off comutation of different SSMs, derived from myMeasures.
Definition: MSDevice_SSM.h:628
int gPrecisionGeo
Definition: StdDefs.cpp:30
A list of positions.
OutputDevice * myOutputFile
Output device.
Definition: MSDevice_SSM.h:644
ENCOUNTER_TYPE_FOE_LEFT_CONFLICT_AREA.
Definition: MSDevice_SSM.h:110
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:4026
MSVehicleControl & getVehicleControl()
Returns the vehicle control.
Definition: MSNet.h:306
double getRemainingExtraTime() const
returns the remaining extra time
double myRange
Detection range. For vehicles closer than this distance from the ego vehicle, SSMs are traced...
Definition: MSDevice_SSM.h:622
std::vector< double > egoDistsToConflict
Evolution of the ego vehicle&#39;s distance to the conflict point.
Definition: MSDevice_SSM.h:212
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:78
ENCOUNTER_TYPE_NOCONFLICT_AHEAD.
Definition: MSDevice_SSM.h:73
#define DEFAULT_EXTRA_TIME
static void getUpstreamVehicles(const MSEdge *edge, double pos, double range, double egoDistToConflictLane, const MSLane *const egoConflictLane, FoeInfoMap &foeCollector, std::set< const MSJunction *> seenJunctions)
Collects all vehicles within range &#39;range&#39; upstream of the position &#39;pos&#39; on the edge &#39;edge&#39; into foe...
bool notifyEnter(SUMOVehicle &veh, MSMoveReminder::Notification reason, const MSLane *enteredLane=0)
Called whenever the holder enteres a lane.
void computeSSMs(EncounterApproachInfo &e) const
Compute current values of the logged SSMs (myMeasures) for the given encounter &#39;e&#39; and update &#39;e&#39; acc...
const ConstMSEdgeVector & getIncoming() const
Definition: MSJunction.h:105
bool qualifiesAsConflict(Encounter *e)
Tests if the SSM values exceed the threshold for qualification as conflict.
T MIN2(T a, T b)
Definition: StdDefs.h:67
static void checkConflictEntryAndExit(EncounterApproachInfo &eInfo)
Checks whether ego or foe have entered or left the conflict area in the last step and eventually writ...
ENCOUNTER_TYPE_MERGING_LEADER.
Definition: MSDevice_SSM.h:88
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:101
std::ostream & operator<<(std::ostream &out, MSDevice_SSM::EncounterType type)
Nicer output for EncounterType enum.
ENCOUNTER_TYPE_FOE_ENTERED_CONFLICT_AREA.
Definition: MSDevice_SSM.h:106
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...
ConflictPointInfo PET
Definition: MSDevice_SSM.h:234
const MSLane * findFoeConflictLane(const MSVehicle *foe, const MSLane *egoConflictLane, double &distToConflictLane) const
Computes the conflict lane for the foe.
EncounterType type
Type of the conflict.
Definition: MSDevice_SSM.h:147
double myOldestActiveEncounterBegin
begin time of the oldest active encounter
Definition: MSDevice_SSM.h:638
bool isInternal() const
return whether this edge is an internal edge
Definition: MSEdge.h:229
virtual const std::vector< MSLane * > getInternalLanes() const
Returns all internal lanes on the junction.
Definition: MSJunction.h:115
ENCOUNTER_TYPE_FOLLOWING.
Definition: MSDevice_SSM.h:76
bool mySaveTrajectories
This determines whether the whole trajectories of the vehicles (position, speed, ssms) shall be saved...
Definition: MSDevice_SSM.h:620
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:89
const MSVehicle * ego
Definition: MSDevice_SSM.h:188
static double getExtraTime(const SUMOVehicle &v)
void flushConflicts(bool all=false)
Writes out all past conflicts that have begun earlier than the oldest active encounter.
static std::string makeStringWithNAs(std::vector< double > v, double NA, std::string sep=" ")
make a string of a double vector and treat a special value as invalid ("NA")
double getLateralPositionOnLane() const
Get the vehicle&#39;s lateral position on the lane.
Definition: MSVehicle.h:439
std::vector< double > foeDistsToConflict
Evolution of the foe vehicle&#39;s distance to the conflict point.
Definition: MSDevice_SSM.h:214
bool closingRequested
this flag is set by updateEncounter() or directly in processEncounters(), where encounters are closed...
Definition: MSDevice_SSM.h:238
std::vector< std::string > getVector()
#define WRITE_ERROR(msg)
Definition: MsgHandler.h:205
Abstract in-vehicle device.
Definition: MSDevice.h:70
bool myComputeDRAC
Definition: MSDevice_SSM.h:628
ENCOUNTER_TYPE_FOLLOWING_PASSED.
Definition: MSDevice_SSM.h:119
static double passingTime(const double lastPos, const double passedPos, const double currentPos, const double lastSpeed, const double currentSpeed)
Calculates the time at which the position passedPosition has been passed In case of a ballistic updat...
Definition: MSCFModel.cpp:552
std::vector< Encounter * > EncounterVector
Definition: MSDevice_SSM.h:291
double getWidth() const
Get the width which vehicles of this class shall have when being drawn.
static bool equippedByDefaultAssignmentOptions(const OptionsCont &oc, const std::string &deviceName, SUMOVehicle &v)
Determines whether a vehicle should get a certain device.
Definition: MSDevice.cpp:115
static bool useGeoCoords(const SUMOVehicle &v)
void countDownExtraTime(double amount)
decreases myRemaingExtraTime by given amount in seconds
double foeConflictEntryTime
Times when the foe vehicle entered/left the conflict area. Currently only applies for crossing situat...
Definition: MSDevice_SSM.h:201
const MSVehicleType & getVehicleType() const
Returns the vehicle&#39;s type definition.
#define M_PI
Definition: odrSpiral.cpp:40
std::map< const MSVehicle *, FoeInfo * > FoeInfoMap
Definition: MSDevice_SSM.h:292
const MSLane * egoConflictLane
Definition: MSDevice_SSM.h:279
Trajectory egoTrajectory
Trajectory of the ego vehicle.
Definition: MSDevice_SSM.h:208
const MSLane * getFirstInternalInConnection(double &offset) const
Returns 0 if the lane is not internal. Otherwise the first part of the connection (sequence of intern...
Definition: MSLane.cpp:1621
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:66
~MSDevice_SSM()
Destructor.
const MSJunction * getFromJunction() const
Definition: MSEdge.h:348
static double estimateArrivalTime(double dist, double speed, double maxSpeed, double accel)
Computes the time needed to travel a distance dist given an initial speed and constant acceleration...
Definition: MSCFModel.cpp:344
double getAcceleration() const
Returns the vehicle&#39;s acceleration in m/s (this is computed as the last step&#39;s mean acceleration in c...
Definition: MSVehicle.h:499
Structure to collect some info on the encounter needed during ssm calculation by various functions...
Definition: MSDevice_SSM.h:250
std::vector< double > timeSpan
time points corresponding to the trajectories
Definition: MSDevice_SSM.h:204
A storage for options typed value containers)
Definition: OptionsCont.h:98
static double _2double(const E *const data)
converts a char-type array into the double value described by it
Definition: TplConvert.h:311
const std::string getParameter(const std::string &key, const std::string &defaultValue="") const
Returns the value for a given key.
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:676
ENCOUNTER_TYPE_EGO_LEFT_CONFLICT_AREA.
Definition: MSDevice_SSM.h:108
static const GeoConvHelper & getFinal()
the coordinate transformation for writing the location element and for tracking the original coordina...
std::pair< double, double > pet
Definition: MSDevice_SSM.h:269
double getLength() const
Get vehicle&#39;s length [m].
std::map< std::string, double > myThresholds
Definition: MSDevice_SSM.h:617
double getBackPositionOnLane(const MSLane *lane) const
Get the vehicle&#39;s position relative to the given lane.
Definition: MSVehicle.cpp:3120
double getLastStepDist() const
Get the distance the vehicle covered in the previous timestep.
Definition: MSVehicle.h:409
Static storage of an output device and its base (abstract) implementation.
Definition: OutputDevice.h:70
An encounter is an episode involving two vehicles, which are closer to each other than some specified...
Definition: MSDevice_SSM.h:127
bool closeTag()
Closes the most recently opened tag.
#define DEFAULT_THRESHOLD_TTC
double myExtraTime
Extra time in seconds to be logged after a conflict is over.
Definition: MSDevice_SSM.h:624
std::vector< int > typeSpan
Evolution of the encounter classification (.
Definition: MSDevice_SSM.h:206
double remainingExtraTime
Remaining extra time (decreases after an encounter ended)
Definition: MSDevice_SSM.h:196
ENCOUNTER_TYPE_ON_ADJACENT_LANES.
Definition: MSDevice_SSM.h:82
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:1874
static void estimateConflictTimes(EncounterApproachInfo &eInfo)
Estimates the time until conflict for the vehicles based on the distance to the conflict entry points...
double getSpeed() const
Returns the vehicle&#39;s current speed.
Definition: MSVehicle.h:482
void writeOutConflict(Encounter *e)
const MSRouteIterator & getCurrentRouteEdge() const
Returns an iterator pointing to the current edge in this vehicles route.
virtual void releaseVehicles() const
Allows to use the container for microsimulation again.
Definition: MSLane.h:419
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 std::set< std::string > createdOutputFiles
remember which files were created already (don&#39;t duplicate xml root-elements)
Definition: MSDevice_SSM.h:647
static double fn[10]
Definition: odrSpiral.cpp:82
static bool gUseMesoSim
Definition: MSGlobals.h:97
Representation of a lane in the micro simulation.
Definition: MSLane.h:77
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:236
EncounterQueue myPastConflicts
Past encounters that where qualified as conflicts and are not yet flushed to the output file...
Definition: MSDevice_SSM.h:640
OutputDevice & openTag(const std::string &xmlElement)
Opens an XML tag.
double value
value of the corresponding SSM
Definition: MSDevice_SSM.h:149
static void buildVehicleDevices(SUMOVehicle &v, std::vector< MSDevice *> &into)
Build devices for the given vehicle, if needed.
double getWidth() const
Returns the vehicle&#39;s width.
Trajectory foeTrajectory
Trajectory of the foe vehicle.
Definition: MSDevice_SSM.h:210
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:290
void add(double time, EncounterType type, Position egoX, Position egoV, Position foeX, Position foeV, Position conflictPoint, double egoDistToConflict, double foeDistToConflict, double ttc, double drac, std::pair< double, double > pet)
add a new data point and update encounter type