Eclipse 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-2019 German Aerospace Center (DLR) and others.
4 // This program and the accompanying materials
5 // are made available under the terms of the Eclipse Public License v2.0
6 // which accompanies this distribution, and is available at
7 // http://www.eclipse.org/legal/epl-v20.html
8 // SPDX-License-Identifier: EPL-2.0
9 /****************************************************************************/
17 // An SSM-device logs encounters / conflicts of the carrying vehicle with other surrounding vehicles
18 // XXX: Preliminary implementation. Use with care. Especially rerouting vehicles could be problematic.
19 // TODO: implement SSM time-gap (estimated conflict entry and exit times are already calculated for PET calculation)
20 /****************************************************************************/
21 
22 // ===========================================================================
23 // included modules
24 // ===========================================================================
25 #include <config.h>
26 
27 #include <iostream>
28 #include <algorithm>
30 #include <utils/geom/GeomHelper.h>
35 #include <microsim/MSNet.h>
36 #include <microsim/MSJunction.h>
37 #include <microsim/MSLane.h>
38 #include <microsim/MSEdge.h>
39 #include <microsim/MSVehicle.h>
42 #include <utils/geom/Position.h>
44 #include "MSDevice_SSM.h"
45 
46 // ===========================================================================
47 // Debug constants
48 // ===========================================================================
49 //#define DEBUG_SSM
50 //#define DEBUG_SSM_OPPOSITE
51 //#define DEBUG_ENCOUNTER
52 #define DEBUG_SSM_SURROUNDING
53 //#define DEBUG_SSM_DRAC
54 //#define DEBUG_SSM_NOTIFICATIONS
55 //#define DEBUG_COND(ego) MSNet::getInstance()->getCurrentTimeStep() > 308000
56 #define DEBUG_COND(ego) (ego!=nullptr && ego->isSelected())
57 #define DEBUG_COND_FIND(ego) (ego.isSelected())
58 #define DEBUG_EGO_ID "EW.3"
59 #define DEBUG_FOE_ID "WE.0"
60 
61 //#define DEBUG_COND(ego) ((ego)!=nullptr && (ego)->getID() == DEBUG_EGO_ID)
62 
63 //#define DEBUG_COND_ENCOUNTER(e) ((DEBUG_EGO_ID == std::string("") || e->egoID == DEBUG_EGO_ID) && (DEBUG_FOE_ID == std::string("") || e->foeID == DEBUG_FOE_ID))
64 //#define DEBUG_COND_ENCOUNTER(e) (e->ego != nullptr && e->ego->isSelected() && e->foe != nullptr && e->foe->isSelected())
65 
66 // ===========================================================================
67 // Constants
68 // ===========================================================================
69 // value indicating an invalid double parameter
70 #define INVALID std::numeric_limits<double>::max()
71 
72 // default value for the detection range of potential opponents
73 #define DEFAULT_RANGE 50.0
74 
75 // list of implemented SSMs (NOTE: To add more SSMs, identifiers are added to AVAILABLE_SSMS
76 // and a default threshold must be defined. A corresponding
77 // case should be added to the switch in buildVehicleDevices,
78 // and in computeSSMs(), the SSM-value should be computed.)
79 #define AVAILABLE_SSMS "TTC DRAC PET BR SGAP TGAP"
80 
81 #define DEFAULT_THRESHOLD_TTC 3. // in [s.], events get logged if time to collision is 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])
82 #define DEFAULT_THRESHOLD_DRAC 3. // in [m/s^2], events get logged if "deceleration to avoid a crash" is 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])
83 #define DEFAULT_THRESHOLD_PET 2. // in seconds, events get logged if post encroachment time is below threshold
84 
85 #define DEFAULT_THRESHOLD_BR 0.0 // in [m/s^2], events get logged if brake rate is above threshold
86 #define DEFAULT_THRESHOLD_SGAP 0.2 // in [m.], events get logged if the space headway is below threshold.
87 #define DEFAULT_THRESHOLD_TGAP 0.5 // in [m.], events get logged if the time headway is below threshold.
88 
89 #define DEFAULT_EXTRA_TIME 5. // in seconds, events get logged for extra time even if encounter is over
90 
91 // ===========================================================================
92 // method definitions
93 // ===========================================================================
94 
95 
96 
98 std::ostream& operator<<(std::ostream& out, MSDevice_SSM::EncounterType type) {
99  switch (type) {
101  out << "NOCONFLICT_AHEAD";
102  break;
104  out << "FOLLOWING";
105  break;
107  out << "FOLLOWING_FOLLOWER";
108  break;
110  out << "FOLLOWING_LEADER";
111  break;
113  out << "ON_ADJACENT_LANES";
114  break;
116  out << "MERGING";
117  break;
119  out << "MERGING_LEADER";
120  break;
122  out << "MERGING_FOLLOWER";
123  break;
125  out << "MERGING_ADJACENT";
126  break;
128  out << "CROSSING";
129  break;
131  out << "CROSSING_LEADER";
132  break;
134  out << "CROSSING_FOLLOWER";
135  break;
137  out << "EGO_ENTERED_CONFLICT_AREA";
138  break;
140  out << "FOE_ENTERED_CONFLICT_AREA";
141  break;
143  out << "BOTH_ENTERED_CONFLICT_AREA";
144  break;
146  out << "EGO_LEFT_CONFLICT_AREA";
147  break;
149  out << "FOE_LEFT_CONFLICT_AREA";
150  break;
152  out << "BOTH_LEFT_CONFLICT_AREA";
153  break;
155  out << "FOLLOWING_PASSED";
156  break;
158  out << "MERGING_PASSED";
159  break;
160  // Collision (currently unused, might be differentiated further)
162  out << "COLLISION";
163  break;
165  out << "ONCOMING";
166  break;
167  default:
168  out << "unknown type (" << int(type) << ")";
169  break;
170  }
171  return out;
172 }
173 
174 
175 // ---------------------------------------------------------------------------
176 // static initialisation methods
177 // ---------------------------------------------------------------------------
178 
179 std::set<MSDevice_SSM*, ComparatorNumericalIdLess>* MSDevice_SSM::myInstances = new std::set<MSDevice_SSM*, ComparatorNumericalIdLess>();
180 
181 std::set<std::string> MSDevice_SSM::createdOutputFiles;
182 
184 
185 const std::set<MSDevice_SSM*, ComparatorNumericalIdLess>&
187  return *myInstances;
188 }
189 
190 void
192  // Close current encounters and flush conflicts to file for all existing devices
193  if (myInstances != nullptr) {
194  for (MSDevice_SSM* device : *myInstances) {
195  device->resetEncounters();
196  device->flushConflicts(true);
197  device->flushGlobalMeasures();
198  }
199  myInstances->clear();
200  }
201  for (auto& fn : createdOutputFiles) {
203  file->closeTag();
204  }
205 }
206 
207 void
209  oc.addOptionSubTopic("SSM Device");
210  insertDefaultAssignmentOptions("ssm", "SSM Device", oc);
211 
212  // custom options
213  oc.doRegister("device.ssm.measures", Option::makeUnsetWithDefault<Option_String, std::string>(""));
214  oc.addDescription("device.ssm.measures", "SSM Device", "Specifies which measures will be logged (as a space separated sequence of IDs in ('TTC', 'DRAC', 'PET')).");
215  oc.doRegister("device.ssm.thresholds", Option::makeUnsetWithDefault<Option_String, std::string>(""));
216  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.");
217  oc.doRegister("device.ssm.trajectories", Option::makeUnsetWithDefault<Option_Bool, bool>(false));
218  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).");
219  oc.doRegister("device.ssm.range", Option::makeUnsetWithDefault<Option_Float, double>(DEFAULT_RANGE));
220  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.");
221  oc.doRegister("device.ssm.extratime", Option::makeUnsetWithDefault<Option_Float, double>(DEFAULT_EXTRA_TIME));
222  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.");
223  oc.doRegister("device.ssm.file", Option::makeUnsetWithDefault<Option_String, std::string>(""));
224  oc.addDescription("device.ssm.file", "SSM Device", "Give a global default filename for the SSM output.");
225  oc.doRegister("device.ssm.geo", Option::makeUnsetWithDefault<Option_Bool, bool>(false));
226  oc.addDescription("device.ssm.geo", "SSM Device", "Whether to use coordinates of the original reference system in output (default is false).");
227 }
228 
229 void
230 MSDevice_SSM::buildVehicleDevices(SUMOVehicle& v, std::vector<MSVehicleDevice*>& into) {
233  WRITE_WARNING("SSM Device for vehicle '" + v.getID() + "' will not be built. (SSMs not supported in MESO)");
234  return;
235  }
236  // ID for the device
237  std::string deviceID = "ssm_" + v.getID();
238 
239  // Load parameters:
240 
241  // Measures and thresholds
242  std::map<std::string, double> thresholds;
243  bool success = getMeasuresAndThresholds(v, deviceID, thresholds);
244  if (!success) {
245  return;
246  }
247 
248  // TODO: modify trajectory option: "all", "conflictPoints", ("position" && "speed" == "vehState"), "SSMs"!
249  // Trajectories
250  bool trajectories = requestsTrajectories(v);
251 
252  // detection range
253  double range = getDetectionRange(v);
254 
255  // extra time
256  double extraTime = getExtraTime(v);
257 
258  // File
259  std::string file = getOutputFilename(v, deviceID);
260 
261  const bool useGeo = useGeoCoords(v);
262 
263  // Build the device (XXX: who deletes it?)
264  MSDevice_SSM* device = new MSDevice_SSM(v, deviceID, file, thresholds, trajectories, range, extraTime, useGeo);
265  into.push_back(device);
266  }
267 }
268 
269 
270 MSDevice_SSM::Encounter::Encounter(const MSVehicle* _ego, const MSVehicle* const _foe, double _begin, double extraTime) :
271  ego(_ego),
272  foe(_foe),
273  egoID(_ego->getID()),
274  foeID(_foe->getID()),
275  begin(_begin),
276  end(-INVALID),
277  currentType(ENCOUNTER_TYPE_NOCONFLICT_AHEAD),
278  remainingExtraTime(extraTime),
279  egoConflictEntryTime(INVALID),
280  egoConflictExitTime(INVALID),
281  foeConflictEntryTime(INVALID),
282  foeConflictExitTime(INVALID),
283  minTTC(INVALID, Position::invalidPosition(), ENCOUNTER_TYPE_NOCONFLICT_AHEAD, INVALID),
284  maxDRAC(INVALID, Position::invalidPosition(), ENCOUNTER_TYPE_NOCONFLICT_AHEAD, INVALID),
285  PET(INVALID, Position::invalidPosition(), ENCOUNTER_TYPE_NOCONFLICT_AHEAD, INVALID),
286  closingRequested(false) {
287 #ifdef DEBUG_ENCOUNTER
288  if (DEBUG_COND_ENCOUNTER(this)) {
289  std::cout << "\n" << SIMTIME << " Constructing encounter of '" << ego->getID() << "' and '" << foe->getID() << "'" << std::endl;
290  }
291 #endif
292 }
293 
295 #ifdef DEBUG_ENCOUNTER
296  if (DEBUG_COND_ENCOUNTER(this)) {
297  std::cout << "\n" << SIMTIME << " Destroying encounter of '" << egoID << "' and '" << foeID << "' (begin was " << begin << ")" << std::endl;
298  }
299 #endif
300 }
301 
302 
303 void
304 MSDevice_SSM::Encounter::add(double time, const EncounterType type, Position egoX, Position egoV, Position foeX, Position foeV,
305  Position conflictPoint, double egoDistToConflict, double foeDistToConflict, double ttc, double drac, std::pair<double, double> pet) {
306 #ifdef DEBUG_ENCOUNTER
307  if (DEBUG_COND_ENCOUNTER(this))
308  std::cout << time << " Adding data point for encounter of '" << egoID << "' and '" << foeID << "':\n"
309  << "type=" << type << ", egoDistToConflict=" << (egoDistToConflict == INVALID ? "NA" : ::toString(egoDistToConflict))
310  << ", foeDistToConflict=" << (foeDistToConflict == INVALID ? "NA" : ::toString(foeDistToConflict))
311  << ",\nttc=" << (ttc == INVALID ? "NA" : ::toString(ttc))
312  << ", drac=" << (drac == INVALID ? "NA" : ::toString(drac))
313  << ", pet=" << (pet.second == INVALID ? "NA" : ::toString(pet.second))
314  << std::endl;
315 #endif
316  currentType = type;
317 
318  timeSpan.push_back(time);
319  typeSpan.push_back(type);
320  egoTrajectory.x.push_back(egoX);
321  egoTrajectory.v.push_back(egoV);
322  foeTrajectory.x.push_back(foeX);
323  foeTrajectory.v.push_back(foeV);
324  conflictPointSpan.push_back(conflictPoint);
325  egoDistsToConflict.push_back(egoDistToConflict);
326  foeDistsToConflict.push_back(foeDistToConflict);
327 
328  TTCspan.push_back(ttc);
329  if (ttc != INVALID && (ttc < minTTC.value || minTTC.value == INVALID)) {
330  minTTC.value = ttc;
331  minTTC.time = time;
332  minTTC.pos = conflictPoint;
333  minTTC.type = type;
334  }
335 
336  DRACspan.push_back(drac);
337  if (drac != INVALID && (drac > maxDRAC.value || maxDRAC.value == INVALID)) {
338  maxDRAC.value = drac;
339  maxDRAC.time = time;
340  maxDRAC.pos = conflictPoint;
341  maxDRAC.type = type;
342  }
343 
344  if (pet.first != INVALID && (PET.value >= pet.second || PET.value == INVALID)) {
345  PET.value = pet.second;
346  PET.time = pet.first;
347  PET.pos = conflictPoint;
348  PET.type = type;
349  }
350 }
351 
352 
353 void
355  remainingExtraTime = value;
356 }
357 
358 
359 void
361  remainingExtraTime -= amount;
362 }
363 
364 
365 double
367  return remainingExtraTime;
368 }
369 
370 
372  encounter(e),
374  conflictPoint(Position::invalidPosition()),
375  egoConflictEntryDist(INVALID),
376  foeConflictEntryDist(INVALID),
377  egoConflictExitDist(INVALID),
378  foeConflictExitDist(INVALID),
379  egoEstimatedConflictEntryTime(INVALID),
380  foeEstimatedConflictEntryTime(INVALID),
381  egoEstimatedConflictExitTime(INVALID),
382  foeEstimatedConflictExitTime(INVALID),
383  egoConflictAreaLength(INVALID),
384  foeConflictAreaLength(INVALID),
385  egoLeftConflict(false),
386  foeLeftConflict(false),
387  ttc(INVALID),
388  drac(INVALID),
389  pet(std::make_pair(INVALID, INVALID)) {
390 }
391 
392 
393 void
395  if (myHolder.isOnRoad()) {
396  update();
397  // Write out past conflicts
398  flushConflicts();
399  } else {
400 #ifdef DEBUG_SSM
401  if (DEBUG_COND(myHolderMS))
402  std::cout << "\n" << SIMTIME << " Device '" << getID() << "' updateAndWriteOutput()\n"
403  << " Holder is off-road! Calling resetEncounters()."
404  << std::endl;
405 #endif
406  resetEncounters();
407  // Write out past conflicts
408  flushConflicts(true);
409  }
410 }
411 
412 void
414 #ifdef DEBUG_SSM
415  if (DEBUG_COND(myHolderMS))
416  std::cout << "\n" << SIMTIME << " Device '" << getID() << "' update()\n"
417  << "Size of myActiveEncounters: " << myActiveEncounters.size()
418  << "\nSize of myPastConflicts: " << myPastConflicts.size()
419  << std::endl;
420 #endif
421  // Scan surroundings for other vehicles
422  FoeInfoMap foes;
424 
425 #ifdef DEBUG_SSM
426  if (DEBUG_COND(myHolderMS)) {
427  if (foes.size() > 0) {
428  std::cout << "Scanned surroundings: Found potential foes:\n";
429  for (FoeInfoMap::const_iterator i = foes.begin(); i != foes.end(); ++i) {
430  std::cout << i->first->getID() << " ";
431  }
432  std::cout << std::endl;
433  } else {
434  std::cout << "Scanned surroundings: No potential conflict could be identified." << std::endl;
435  }
436  }
437 #endif
438 
439  // Update encounters and conflicts -> removes all foes (and deletes corresponding FoeInfos) for which already a corresponding encounter exists
440  processEncounters(foes);
441 
442  // Make new encounters for all foes, which were not removed by processEncounters (and deletes corresponding FoeInfos)
443  createEncounters(foes);
444  foes.clear();
445 
446  // Compute "global SSMs" (only computed once per time-step)
448 
449 }
450 
451 
452 void
456  if (myComputeBR) {
457  double br = MAX2(-myHolderMS->getAcceleration(), 0.0);
458  if (br > myMaxBR.second) {
459  myMaxBR = std::make_pair(std::make_pair(SIMTIME, myHolderMS->getPosition()), br);
460  }
461  myBRspan.push_back(br);
462  }
463 
464  double leaderSearchDist = 0;
465  std::pair<const MSVehicle*, double> leader(nullptr, 0.);
466  if (myComputeSGAP) {
467  leaderSearchDist = myThresholds["SGAP"];
468  }
469  if (myComputeTGAP) {
470  leaderSearchDist = MAX2(leaderSearchDist, myThresholds["TGAP"] * myHolderMS->getSpeed());
471  }
472 
473  if (leaderSearchDist > 0.) {
474  leader = myHolderMS->getLeader(leaderSearchDist);
475  }
476 
477  if (myComputeSGAP) {
478  if (leader.first == nullptr) {
479  mySGAPspan.push_back(INVALID);
480  } else {
481  double sgap = leader.second + leader.first->getVehicleType().getMinGap();
482  mySGAPspan.push_back(sgap);
483  if (sgap < myMinSGAP.first.second) {
484  myMinSGAP = std::make_pair(std::make_pair(std::make_pair(SIMTIME, myHolderMS->getPosition()), sgap), leader.first->getID());
485  }
486  }
487  }
488 
489  if (myComputeTGAP) {
490  if (leader.first == nullptr || myHolderMS->getSpeed() == 0.) {
491  myTGAPspan.push_back(INVALID);
492  } else {
493  const double tgap = (leader.second + leader.first->getVehicleType().getMinGap()) / myHolderMS->getSpeed();
494  myTGAPspan.push_back(tgap);
495  if (tgap < myMinTGAP.first.second) {
496  myMinTGAP = std::make_pair(std::make_pair(std::make_pair(SIMTIME, myHolderMS->getPosition()), tgap), leader.first->getID());
497  }
498  }
499  }
500 
501  }
502 }
503 
504 
505 void
507 #ifdef DEBUG_SSM
508  if (DEBUG_COND(myHolderMS)) {
509  std::cout << "\n" << SIMTIME << " Device '" << getID() << "' createEncounters()" << std::endl;
510  std::cout << "New foes:\n";
511  for (FoeInfoMap::const_iterator vi = foes.begin(); vi != foes.end(); ++vi) {
512  std::cout << vi->first->getID() << "\n";
513  }
514  std::cout << std::endl;
515  }
516 #endif
517 
518  for (FoeInfoMap::const_iterator foe = foes.begin(); foe != foes.end(); ++foe) {
519  Encounter* e = new Encounter(myHolderMS, foe->first, SIMTIME, myExtraTime);
520  if (updateEncounter(e, foe->second)) {
522  assert(myActiveEncounters.empty());
524  }
525  assert(myOldestActiveEncounterBegin <= e->begin);
526  myActiveEncounters.push_back(e);
527  } else {
528  // Discard encounters, where one vehicle already left the conflict area
529  delete e;
530  }
531  // free foeInfo
532  delete foe->second;
533  }
534 }
535 
536 void
538  // Call processEncounters() with empty vehicle set
539  FoeInfoMap foes;
540  // processEncounters with empty argument closes all encounters
541  processEncounters(foes, true);
542 }
543 
544 void
546 #ifdef DEBUG_SSM
547  if (DEBUG_COND(myHolderMS)) {
548  std::cout << "\n" << SIMTIME << " Device '" << getID() << "' processEncounters(forceClose = " << forceClose << ")" << std::endl;
549  std::cout << "Currently present foes:\n";
550  for (FoeInfoMap::const_iterator vi = foes.begin(); vi != foes.end(); ++vi) {
551  std::cout << vi->first->getID() << "\n";
552  }
553  std::cout << std::endl;
554  }
555 #endif
556 
557  // Run through active encounters. If corresponding foe is still present in foes update and
558  // remove foe from foes. If the foe has disappeared close the encounter (check if it qualifies
559  // as a conflict and in case transfer it to myPastConflicts).
560  // Afterwards run through remaining elements in foes and create new encounters for them.
561  EncounterVector::iterator ei = myActiveEncounters.begin();
562  while (ei != myActiveEncounters.end()) {
563  Encounter* e = *ei;
564  // check whether foe is still on net
565  bool foeExists = !(MSNet::getInstance()->getVehicleControl().getVehicle(e->foeID) == nullptr);
566  if (!foeExists) {
567  e->foe = nullptr;
568  }
569  if (foes.find(e->foe) != foes.end()) {
570  FoeInfo* foeInfo = foes[e->foe];
571  EncounterType prevType = e->currentType;
572  // Update encounter
573  updateEncounter(e, foeInfo);
576  // The encounter classification switched from BOTH_LEFT to another
577  // => Start new encounter (i.e. don't erase the foe, don't delete the foeInfo and request closing)
578  // Note that updateEncounter did not add another trajectory point in this case.
579 #ifdef DEBUG_SSM
580  if (DEBUG_COND(myHolderMS)) {
581  std::cout << " Requesting encounter closure because both left conflict area of previous encounter but another encounter lies ahead." << std::endl;
582  }
583 #endif
584  e->closingRequested = true;
585  } else {
586  // Erase foes which were already encountered and should not be used to open a new conflict
587  delete foeInfo;
588  foes.erase(e->foe);
589  }
590  } else {
591  if (e->getRemainingExtraTime() <= 0. || forceClose || !foeExists) {
592  // Close encounter, extra time has expired (deletes e if it does not qualify as conflict)
593 #ifdef DEBUG_SSM
594  if (DEBUG_COND(myHolderMS)) {
595  std::cout << " Requesting encounter closure because..." << std::endl;
596  if (e->getRemainingExtraTime() <= 0.) {
597  std::cout << " ... extra time elapsed." << std::endl;
598  } else if (forceClose) {
599  std::cout << " ... closing was forced." << std::endl;
600  } else {
601  std::cout << " ... foe disappeared." << std::endl;
602  }
603  }
604 #endif
605  e->closingRequested = true;
606  } else {
607  updateEncounter(e, nullptr); // counts down extra time
608  }
609  }
610 
611  if (e->closingRequested) {
612  double eBegin = e->begin;
613  closeEncounter(e);
614  ei = myActiveEncounters.erase(ei);
615  if (myActiveEncounters.empty()) {
617  } else if (eBegin == myOldestActiveEncounterBegin) {
618  // Erased the oldest encounter, update myOldestActiveEncounterBegin
619  auto i = myActiveEncounters.begin();
620  myOldestActiveEncounterBegin = (*i++)->begin;
621  while (i != myActiveEncounters.end()) {
623  }
624  }
625  } else {
626  ++ei;
627  }
628  }
629 }
630 
631 
632 bool
634  // Check if conflict measure thresholds are exceeded (to decide whether to keep the encounter for writing out)
635 #ifdef DEBUG_SSM
636  if (DEBUG_COND(myHolderMS))
637  std::cout << SIMTIME << " qualifiesAsConflict() for encounter of vehicles '"
638  << e->egoID << "' and '" << e->foeID
639  << "'" << std::endl;
640 #endif
641 
642  if (myComputePET && e->PET.value != INVALID && e->PET.value <= myThresholds["PET"]) {
643  return true;
644  }
645  if (myComputeTTC && e->minTTC.value != INVALID && e->minTTC.value <= myThresholds["TTC"]) {
646  return true;
647  }
648  if (myComputeDRAC && e->maxDRAC.value != INVALID && e->maxDRAC.value >= myThresholds["DRAC"]) {
649  return true;
650  }
651  return false;
652 }
653 
654 
655 void
657  assert(e->size() > 0);
658  // erase pointers (encounter is stored before being destroyed and pointers could become invalid)
659  e->ego = nullptr;
660  e->foe = nullptr;
661  e->end = e->timeSpan.back();
662  bool wasConflict = qualifiesAsConflict(e);
663 #ifdef DEBUG_SSM
664  if (DEBUG_COND(myHolderMS)) {
665  std::cout << SIMTIME << " closeEncounter() of vehicles '"
666  << e->egoID << "' and '" << e->foeID
667  << "' (was ranked as " << (wasConflict ? "conflict" : "non-conflict") << ")" << std::endl;
668  }
669 #endif
670  if (wasConflict) {
671  myPastConflicts.push(e);
672 #ifdef DEBUG_SSM
673  if (!myPastConflicts.empty()) {
674  if (DEBUG_COND(myHolderMS)) {
675  std::cout << "pastConflictsQueue of veh '" << myHolderMS->getID() << "':\n";
676  }
677  auto myPastConflicts_bak = myPastConflicts;
678  double lastBegin = myPastConflicts.top()->begin;
679  while (!myPastConflicts.empty()) {
680  auto c = myPastConflicts.top();
681  myPastConflicts.pop();
682  if (DEBUG_COND(myHolderMS)) {
683  std::cout << " Conflict with foe '" << c->foe << "' (time " << c->begin << "-" << c->end << ")\n";
684  }
685  if (c->begin < lastBegin) {
686  std::cout << " Queue corrupt...\n";
687  assert(false);
688  }
689  lastBegin = c->begin;
690  }
691  std::cout << std::endl;
692  myPastConflicts = myPastConflicts_bak;
693  }
694 #endif
695  } else {
696  delete e;
697  }
698  return;
699 }
700 
701 
702 bool
704 #ifdef DEBUG_ENCOUNTER
705  if (DEBUG_COND_ENCOUNTER(e)) {
706  std::cout << SIMTIME << " updateEncounter() of vehicles '" << e->egoID << "' and '" << e->foeID << "'\n";
707  }
708 #endif
709  assert(e->foe != 0);
710 
711  // Struct storing distances (determined in classifyEncounter()) and times to potential conflict entry / exit (in estimateConflictTimes())
712  EncounterApproachInfo eInfo(e);
713 
714  // Classify encounter type based on the present information
715  // More details on follower/lead relation are determined in a second step below, see estimateConflictTimes()
716  // If a crossing situation is ongoing (i.e. one of the vehicles entered the conflict area already in the last step,
717  // this is handled by passedEncounter by only tracing the vehicle's movements)
718  // The further development of the encounter type is done in checkConflictEntryAndExit()
719  eInfo.type = classifyEncounter(foeInfo, eInfo);
720 
721  // Discard new encounters, where one vehicle has already left the conflict area
722  if (eInfo.encounter->size() == 0) {
725  // Signalize to discard
726  return false;
727  }
728  }
729 
730  if (eInfo.type == ENCOUNTER_TYPE_NOCONFLICT_AHEAD) {
731  // At this state, eInfo.type == ENCOUNTER_TYPE_NOCONFLICT_AHEAD implies that the foe
732  // is either out of the device's range or its route does not interfere with the ego's route.
733 #ifdef DEBUG_ENCOUNTER
734  if (DEBUG_COND_ENCOUNTER(e)) {
735  std::cout << SIMTIME << " Encounter of vehicles '" << e->egoID << "' and '" << e->foeID << "' does not imply any conflict.\n";
736  }
737 #endif
738  updatePassedEncounter(e, foeInfo, eInfo);
739 // return;
746  // Ongoing encounter. Treat with update passed encounter (trace covered distances)
747  // eInfo.type only holds the previous type
748  updatePassedEncounter(e, foeInfo, eInfo);
749 
750  // Estimate times until a possible conflict / collision
751  estimateConflictTimes(eInfo);
752 
753  } else {
754  // Estimate times until a possible conflict / collision
755  // Not all are used for all types of encounters:
756  // Follow/lead situation doesn't need them at all, currently (might change if more SSMs are implemented).
757  // Crossing / Merging calculates entry times to determine leader/follower and calculates the exit time for the leader.
758  estimateConflictTimes(eInfo);
759 
760  // reset the remaining extra time (foe could have re-entered the device range after beginning extra time countdown already)
762  }
763 
764  // update entry/exit times for conflict area
766  if (e->size() == 0) {
767 #ifdef DEBUG_ENCOUNTER
768  if (DEBUG_COND_ENCOUNTER(e)) {
769  std::cout << SIMTIME << " type when creating encounter: " << eInfo.type << "\n";
770  }
771 #endif
777  return false;
778  }
779  }
780 
781  // update (x,y)-coords of conflict point
782  determineConflictPoint(eInfo);
783 
784  // Compute SSMs
785  computeSSMs(eInfo);
786 
789  // Don't add a point which switches back to a different encounter type from a passed encounter.
790  // For this situation this encounter will be closed and a new encounter will be created,
791  // @see correspondingly conditionalized code in processEncounters()
792  e->currentType = eInfo.type;
793  } else {
794  // Add current states to trajectories and update type
795  e->add(SIMTIME, eInfo.type, e->ego->getPosition(), e->ego->getVelocityVector(), e->foe->getPosition(), e->foe->getVelocityVector(),
796  eInfo.conflictPoint, eInfo.egoConflictEntryDist, eInfo.foeConflictEntryDist, eInfo.ttc, eInfo.drac, eInfo.pet);
797  }
798  // Keep encounter
799  return true;
800 }
801 
802 
803 void
805  /* Calculates the (x,y)-coordinate for the eventually predicted conflict point and stores the result in
806  * eInfo.conflictPoint. In case of MERGING and CROSSING, this is the entry point to conflict area for follower
807  * In case of FOLLOWING it is the position of leader's back. */
808 
809 #ifdef DEBUG_SSM
810  if (DEBUG_COND(eInfo.encounter->ego)) {
811  std::cout << SIMTIME << " determineConflictPoint()" << std::endl;
812  }
813 #endif
814 
815  const EncounterType& type = eInfo.type;
816  const Encounter* e = eInfo.encounter;
819  || type == ENCOUNTER_TYPE_COLLISION) {
820  // Both vehicles have already past the conflict entry.
821  assert(e->size() > 0); // A new encounter should not be created if both vehicles already entered the conflict area
822  eInfo.conflictPoint = e->conflictPointSpan.back();
823  } else if (type == ENCOUNTER_TYPE_CROSSING_FOLLOWER
828  } else if (type == ENCOUNTER_TYPE_CROSSING_LEADER
833  } else if (type == ENCOUNTER_TYPE_FOLLOWING_FOLLOWER) {
834  eInfo.conflictPoint = e->foe->getPosition(-e->foe->getLength());
835  } else if (type == ENCOUNTER_TYPE_FOLLOWING_LEADER) {
836  eInfo.conflictPoint = e->ego->getPosition(-e->ego->getLength());
837  } else if (type == ENCOUNTER_TYPE_ONCOMING) {
838  eInfo.conflictPoint = (e->ego->getPosition() + e->foe->getPosition()) * 0.5;
839  } else {
840 #ifdef DEBUG_SSM
841  if (DEBUG_COND(eInfo.encounter->ego)) {
842  std::cout << "No conflict point associated with encounter type " << type << std::endl;
843  }
844 #endif
845  return;
846  }
847 
848 #ifdef DEBUG_SSM
849  if (DEBUG_COND(eInfo.encounter->ego)) {
850  std::cout << " Conflict at " << eInfo.conflictPoint << std::endl;
851  }
852 #endif
853 }
854 
855 
856 void
858 
859  EncounterType& type = eInfo.type;
860  Encounter* e = eInfo.encounter;
861 
862  assert(type != ENCOUNTER_TYPE_NOCONFLICT_AHEAD); // arrival times not defined, if no conflict is ahead.
863 #ifdef DEBUG_SSM
864  if (DEBUG_COND(e->ego))
865  std::cout << SIMTIME << " estimateConflictTimes() for ego '" << e->egoID << "' and foe '" << e->foeID << "'\n"
866  << " encounter type: " << eInfo.type << "\n"
867  << " egoConflictEntryDist=" << (eInfo.egoConflictEntryDist == INVALID ? "NA" : ::toString(eInfo.egoConflictEntryDist))
868  << ", foeConflictEntryDist=" << (eInfo.foeConflictEntryDist == INVALID ? "NA" : ::toString(eInfo.foeConflictEntryDist))
869  << "\n ego speed=" << e->ego->getSpeed()
870  << ", foe speed=" << e->foe->getSpeed()
871  << std::endl;
872 #endif
873 
875  // No need to know the times until ...ConflictDistEntry, currently. They would correspond to an estimated time headway or similar.
876  // TTC must take into account the movement of the leader, as would DRAC, PET doesn't need the time either, since it uses aposteriori
877  // values.
878 #ifdef DEBUG_SSM
879  if (DEBUG_COND(e->ego))
880  std::cout << " encouter type " << type << " -> no entry/exit times to be calculated."
881  << std::endl;
882 #endif
883  return;
884  }
885 
886  assert(type == ENCOUNTER_TYPE_MERGING || type == ENCOUNTER_TYPE_CROSSING
893  || type == ENCOUNTER_TYPE_ONCOMING);
894 
895  // Determine exit distances
896  if (type == ENCOUNTER_TYPE_MERGING || type == ENCOUNTER_TYPE_ONCOMING) {
899  } else {
902  }
903 
904  // Estimate entry times to stipulate a leader / follower relation for the encounter.
905  if (eInfo.egoConflictEntryDist > 0.) {
907  assert(eInfo.egoEstimatedConflictEntryTime > 0.);
908  } else {
909  // ego already entered conflict area
911  }
912  if (eInfo.foeConflictEntryDist > 0.) {
914  assert(eInfo.foeEstimatedConflictEntryTime > 0.);
915  } else {
916  // foe already entered conflict area
918  }
919 
920  if (type == ENCOUNTER_TYPE_ONCOMING) {
923  }
924 
925 #ifdef DEBUG_SSM
926  if (DEBUG_COND(e->ego))
927  std::cout << " Conflict type: " << encounterToString(type) << "\n"
928  << " egoConflictEntryTime=" << (eInfo.egoEstimatedConflictEntryTime == INVALID ? "INVALID" : ::toString(eInfo.egoEstimatedConflictEntryTime))
929  << ", foeConflictEntryTime=" << (eInfo.foeEstimatedConflictEntryTime == INVALID ? "INVALID" : ::toString(eInfo.foeEstimatedConflictEntryTime))
930  << std::endl;
931 #endif
932 
933  // Estimate exit times from conflict area for leader / follower.
934  if (eInfo.egoConflictExitDist >= 0.) {
936  } else {
937  eInfo.egoEstimatedConflictExitTime = 0.;
938  }
939  if (eInfo.foeConflictExitDist >= 0.) {
941  } else {
942  eInfo.foeEstimatedConflictExitTime = 0.;
943  }
944 
945  if (type == ENCOUNTER_TYPE_ONCOMING) {
948  }
949 
950  if (type != ENCOUNTER_TYPE_MERGING && type != ENCOUNTER_TYPE_CROSSING) {
951  // this call is issued in context of an ongoing conflict, therefore complete type is already known for the encounter
952  // (One of EGO_ENTERED_CONFLICT_AREA, FOE_ENTERED_CONFLICT_AREA, EGO_LEFT_CONFLICT_AREA, FOE_LEFT_CONFLICT_AREA, BOTH_ENTERED_CONFLICT_AREA)
953  // --> no need to specify incomplete encounter type
954  return;
955  }
956 
957  // For merging and crossing situation, the leader/follower relation not determined by classifyEncounter()
958  // This is done below based on the estimated conflict entry times
959  if (eInfo.egoEstimatedConflictEntryTime == 0. && eInfo.foeEstimatedConflictEntryTime == 0. &&
960  eInfo.egoConflictExitDist >= 0 && eInfo.foeConflictExitDist >= 0) {
962  std::stringstream ss;
963  ss << "SSM device of vehicle '" << e->egoID << "' detected collision with vehicle '" << e->foeID << "' at time " << SIMTIME;
964  WRITE_WARNING(ss.str());
966  // ego is estimated first at conflict point
967 #ifdef DEBUG_SSM
968  if (DEBUG_COND(e->ego))
969  std::cout << " -> ego is estimated leader at conflict entry."
970  << " egoConflictExitTime=" << (eInfo.egoEstimatedConflictExitTime == INVALID ? "NA" : ::toString(eInfo.egoEstimatedConflictExitTime))
971  << std::endl;
972 #endif
974  } else {
975  // ego is estimated second at conflict point
976 #ifdef DEBUG_SSM
977  if (DEBUG_COND(e->ego))
978  std::cout << " -> foe is estimated leader at conflict entry."
979  << " foeConflictExitTime=" << (eInfo.foeEstimatedConflictExitTime == INVALID ? "NA" : ::toString(eInfo.foeEstimatedConflictExitTime))
980  << std::endl;
981 #endif
983  }
984 
985 }
986 
987 
988 
989 void
991 #ifdef DEBUG_SSM
992  if (DEBUG_COND(myHolderMS)) {
993  Encounter* e = eInfo.encounter;
994  std::cout << SIMTIME << " computeSSMs() for vehicles '"
995  << e->ego->getID() << "' and '" << e->foe->getID()
996  << "'" << std::endl;
997  }
998 #endif
999 
1000  const EncounterType& type = eInfo.type;
1001 
1006  || type == ENCOUNTER_TYPE_ONCOMING) {
1007  if (myComputeTTC || myComputeDRAC) {
1008  determineTTCandDRAC(eInfo);
1009  }
1010  determinePET(eInfo);
1011  } else if (type == ENCOUNTER_TYPE_BOTH_LEFT_CONFLICT_AREA) {
1012  determinePET(eInfo);
1013  } else if (type == ENCOUNTER_TYPE_COLLISION) {
1014  // TODO: handle collision
1017  // No conflict measures apply for these states, which correspond to intermediate times between
1018  // one vehicle leaving the conflict area and the arrival time for the other (difference corresponds to the PET)
1019  } else if (type == ENCOUNTER_TYPE_ON_ADJACENT_LANES || type == ENCOUNTER_TYPE_MERGING_ADJACENT) {
1020  // No conflict measures apply for this state
1021  } else if (type == ENCOUNTER_TYPE_MERGING_PASSED || type == ENCOUNTER_TYPE_FOLLOWING_PASSED) {
1022  // No conflict measures apply for this state
1023  } else if (type == ENCOUNTER_TYPE_NOCONFLICT_AHEAD) {
1024  // No conflict measures apply for this state
1025  } else {
1026  std::stringstream ss;
1027  ss << "'" << type << "'";
1028  WRITE_WARNING("Unknown or undetermined encounter type at computeSSMs(): " + ss.str());
1029  }
1030 
1031 #ifdef DEBUG_SSM
1032  if (DEBUG_COND(myHolderMS)) {
1033  Encounter* e = eInfo.encounter;
1034  std::cout << "computeSSMs() for encounter of vehicles '" << e->egoID << "' and '" << e->foeID << "':\n"
1035  << " ttc=" << (eInfo.ttc == INVALID ? "INVALID" : ::toString(eInfo.ttc))
1036  << ", drac=" << (eInfo.drac == INVALID ? "INVALID" : ::toString(eInfo.drac))
1037  << ", pet=" << (eInfo.pet.second == INVALID ? "INVALID" : ::toString(eInfo.pet.second))
1038  << std::endl;
1039  }
1040 #endif
1041 }
1042 
1043 
1044 void
1046  Encounter* e = eInfo.encounter;
1047  if (e->size() == 0) {
1048  return;
1049  }
1050  const EncounterType& type = eInfo.type;
1051  std::pair<double, double>& pet = eInfo.pet;
1052 
1053 #ifdef DEBUG_SSM
1054  if (DEBUG_COND(myHolderMS))
1055  std::cout << SIMTIME << " determinePET() for encounter of vehicles '" << e->egoID << "' and '" << e->foeID << "'"
1056  << "(type: " << encounterToString(static_cast<EncounterType>(e->typeSpan.back())) << ")" << std::endl;
1057 #endif
1058 
1060  // For a following situation, the corresponding PET-value is merely the time-headway.
1061  // Determining these could be done by comparison of memorized gaps with memorized covered distances
1062  // Implementation is postponed. Tracing the time gaps (in contrast to crossing PET) corresponds to
1063  // a vector of values not a single value.
1064  // pass
1065  } else if (type == ENCOUNTER_TYPE_BOTH_LEFT_CONFLICT_AREA) {
1066  EncounterType prevType = static_cast<EncounterType>(e->typeSpan.back());
1067  if (prevType == ENCOUNTER_TYPE_BOTH_LEFT_CONFLICT_AREA) {
1068 #ifdef DEBUG_SSM
1069  if (DEBUG_COND(myHolderMS))
1070  std::cout << "PET for crossing encounter already calculated as " << e->PET.value
1071  << std::endl;
1072 #endif
1073  // pet must have been calculated already
1074  assert(e->PET.value != INVALID);
1075  return;
1076  }
1077 
1078  // this situation should have emerged from one of the following
1079  assert(prevType == ENCOUNTER_TYPE_CROSSING_FOLLOWER
1080  || prevType == ENCOUNTER_TYPE_CROSSING_LEADER
1086 
1087 
1088 #ifdef DEBUG_SSM
1089  if (DEBUG_COND(myHolderMS))
1090  std::cout << "e->egoDistsToConflict.back() = " << e->egoDistsToConflict.back()
1091  << "\ne->egoConflictEntryTime = " << e->egoConflictEntryTime
1092  << "\ne->egoConflictExitTime = " << e->egoConflictExitTime
1093  << "\ne->foeDistsToConflict.back() = " << e->foeDistsToConflict.back()
1094  << "\ne->foeConflictEntryTime = " << e->foeConflictEntryTime
1095  << "\ne->foeConflictExitTime = " << e->foeConflictExitTime
1096  << std::endl;
1097 #endif
1098 
1099  // But both have passed the conflict area
1101 
1102  // Both have left the conflict region
1103  // (Conflict may have started as one was already within the conflict area - thus the check for INVALID entry times)
1105  pet.first = e->egoConflictEntryTime;
1106  pet.second = e->egoConflictEntryTime - e->foeConflictExitTime;
1108  pet.first = e->foeConflictEntryTime;
1109  pet.second = e->foeConflictEntryTime - e->egoConflictExitTime;
1110  } else {
1111 #ifdef DEBUG_SSM
1112  if (DEBUG_COND(myHolderMS))
1113  std::cout << "Unexpected branch in determinePET: Both passed conflict area in the same step."
1114  << std::endl;
1115 #endif
1116  pet.first = INVALID;
1117  pet.second = INVALID;
1118  assert(prevType != ENCOUNTER_TYPE_EGO_LEFT_CONFLICT_AREA
1119  && prevType != ENCOUNTER_TYPE_FOE_LEFT_CONFLICT_AREA);
1120  assert(false); // let this fail for debug build
1121  }
1122 
1123  // Reset entry and exit times two allow an eventual subsequent re-use
1128 
1129 #ifdef DEBUG_SSM
1130  if (DEBUG_COND(myHolderMS))
1131  std::cout << "Calculated PET = " << pet.second << " (at t=" << pet.first << ")"
1132  << std::endl;
1133 #endif
1134  } else {
1135  // other cases (merging and pre-crossing situations) do not correspond to a PET calculation.
1136 #ifdef DEBUG_SSM
1137  if (DEBUG_COND(myHolderMS))
1138  std::cout << "PET unappropriate for merging and pre-crossing situations. No calculation performed."
1139  << std::endl;
1140 #endif
1141  return;
1142  }
1143 }
1144 
1145 
1146 void
1148  Encounter* e = eInfo.encounter;
1149  const EncounterType& type = eInfo.type;
1150  double& ttc = eInfo.ttc;
1151  double& drac = eInfo.drac;
1152 
1153 #ifdef DEBUG_SSM
1154  if (DEBUG_COND(myHolderMS))
1155  std::cout << SIMTIME << " determineTTCandDRAC() for encounter of vehicles '" << e->egoID << "' and '" << e->foeID << "' (type = " << eInfo.type << ")"
1156  << std::endl;
1157 #endif
1158 
1159  // Dependent on the actual encounter situation (eInfo.type) calculate the TTC.
1160  // For merging and crossing, different cases occur when a collision during the merging / crossing process is predicted.
1161  if (type == ENCOUNTER_TYPE_FOLLOWING_FOLLOWER) {
1162  double gap = eInfo.egoConflictEntryDist;
1163  if (myComputeTTC) {
1164  ttc = computeTTC(gap, e->ego->getSpeed(), e->foe->getSpeed());
1165  }
1166  if (myComputeDRAC) {
1167  drac = computeDRAC(gap, e->ego->getSpeed(), e->foe->getSpeed());
1168  }
1169  } else if (type == ENCOUNTER_TYPE_FOLLOWING_LEADER) {
1170  double gap = eInfo.foeConflictEntryDist;
1171  if (myComputeTTC) {
1172  ttc = computeTTC(gap, e->foe->getSpeed(), e->ego->getSpeed());
1173  }
1174  if (myComputeDRAC) {
1175  drac = computeDRAC(gap, e->foe->getSpeed(), e->ego->getSpeed());
1176  }
1177  } else if (type == ENCOUNTER_TYPE_ONCOMING) {
1178  if (myComputeTTC) {
1179  const double dv = e->ego->getSpeed() + e->foe->getSpeed();
1180  if (dv > 0) {
1181  ttc = eInfo.egoConflictEntryDist / dv;
1182  }
1183  }
1184  } else if (type == ENCOUNTER_TYPE_MERGING_FOLLOWER || type == ENCOUNTER_TYPE_MERGING_LEADER) {
1185  // TODO: calculate more specifically whether a following situation in the merge conflict area
1186  // is predicted when assuming constant speeds or whether a side collision is predicted.
1187  // Currently, we ignore any conflict area before the actual merging point of the lanes.
1188 
1189  // linearly extrapolated arrival times at the conflict
1190  // NOTE: These differ from the estimated times stored in eInfo
1191  double egoEntryTime = e->ego->getSpeed() > 0 ? eInfo.egoConflictEntryDist / e->ego->getSpeed() : INVALID;
1192  double egoExitTime = e->ego->getSpeed() > 0 ? eInfo.egoConflictExitDist / e->ego->getSpeed() : INVALID;
1193  double foeEntryTime = e->foe->getSpeed() > 0 ? eInfo.foeConflictEntryDist / e->foe->getSpeed() : INVALID;
1194  double foeExitTime = e->foe->getSpeed() > 0 ? eInfo.foeConflictExitDist / e->foe->getSpeed() : INVALID;
1195 
1196 #ifdef DEBUG_SSM
1197  if (DEBUG_COND(myHolderMS))
1198  std::cout << " Conflict times with constant speed extrapolation for merging situation:\n "
1199  << " egoEntryTime=" << (egoEntryTime == INVALID ? "NA" : ::toString(egoEntryTime))
1200  << ", egoExitTime=" << (egoExitTime == INVALID ? "NA" : ::toString(egoExitTime))
1201  << ", foeEntryTime=" << (foeEntryTime == INVALID ? "NA" : ::toString(foeEntryTime))
1202  << ", foeExitTime=" << (foeExitTime == INVALID ? "NA" : ::toString(foeExitTime))
1203  << std::endl;
1204 #endif
1205 
1206  // based on that, we obtain
1207  if (egoEntryTime == INVALID || foeEntryTime == INVALID) {
1208  // at least one vehicle is stopped
1209  ttc = INVALID;
1210  drac = INVALID;
1211 #ifdef DEBUG_SSM
1212  if (DEBUG_COND(myHolderMS)) {
1213  std::cout << " No TTC and DRAC computed as one vehicle is stopped." << std::endl;
1214  }
1215 #endif
1216  return;
1217  }
1218  double leaderEntryTime = MIN2(egoEntryTime, foeEntryTime);
1219  double followerEntryTime = MAX2(egoEntryTime, foeEntryTime);
1220  double leaderExitTime = leaderEntryTime == egoEntryTime ? egoExitTime : foeExitTime;
1221  //double followerExitTime = leaderEntryTime==egoEntryTime?foeExitTime:egoExitTime;
1222  double leaderSpeed = leaderEntryTime == egoEntryTime ? e->ego->getSpeed() : e->foe->getSpeed();
1223  double followerSpeed = leaderEntryTime == egoEntryTime ? e->foe->getSpeed() : e->ego->getSpeed();
1224  double leaderConflictDist = leaderEntryTime == egoEntryTime ? eInfo.egoConflictEntryDist : eInfo.foeConflictEntryDist;
1225  double followerConflictDist = leaderEntryTime == egoEntryTime ? eInfo.foeConflictEntryDist : eInfo.egoConflictEntryDist;
1226  double leaderLength = leaderEntryTime == egoEntryTime ? e->ego->getLength() : e->foe->getLength();
1227  if (leaderExitTime >= followerEntryTime) {
1228  // collision would occur at merge area
1229  if (myComputeTTC) {
1230  ttc = computeTTC(followerConflictDist, followerSpeed, 0.);
1231  }
1232  // TODO: Calculate more specific drac for merging case here (complete stop is not always necessary -> see calculation for crossing case)
1233  // Rather the
1234  if (myComputeDRAC) {
1235  drac = computeDRAC(followerConflictDist, followerSpeed, 0.);
1236  }
1237 // if (myComputeDRAC) drac = computeDRAC(eInfo);
1238 
1239 #ifdef DEBUG_SSM
1240  if (DEBUG_COND(myHolderMS))
1241  std::cout << " Extrapolation predicts collision *at* merge point with TTC=" << ttc
1242  << ", drac=" << drac << std::endl;
1243 #endif
1244 
1245  } else {
1246  // -> No collision at the merge area
1247  if (myComputeTTC) {
1248  // Check if after merge a collision would occur if speeds are hold constant.
1249  double gapAfterMerge = followerConflictDist - leaderExitTime * followerSpeed;
1250  assert(gapAfterMerge >= 0);
1251 
1252  // ttc as for following situation (assumes no collision until leader merged)
1253  double ttcAfterMerge = computeTTC(gapAfterMerge, followerSpeed, leaderSpeed);
1254  ttc = ttcAfterMerge == INVALID ? INVALID : leaderExitTime + ttcAfterMerge;
1255  }
1256  if (myComputeDRAC) {
1257  // Intitial gap. (May be negative only if the leader speed is higher than the follower speed, i.e., dv < 0)
1258  double g0 = followerConflictDist - leaderConflictDist - leaderLength;
1259  if (g0 < 0) {
1260  // Speed difference must be positive if g0<0.
1261  assert(leaderSpeed - followerSpeed > 0);
1262  // no deceleration needed for dv>0 and gap after merge >= 0
1263  drac = INVALID;
1264  } else {
1265  // compute drac as for a following situation
1266  drac = computeDRAC(g0, followerSpeed, leaderSpeed);
1267  }
1268  }
1269 #ifdef DEBUG_SSM
1270  if (DEBUG_COND(myHolderMS)) {
1271  if (ttc == INVALID) {
1272  // assert(dv >= 0);
1273  assert(drac == INVALID || drac == 0.0);
1274  std::cout << " Extrapolation does not predict any collision." << std::endl;
1275  } else {
1276  std::cout << " Extrapolation predicts collision *after* merge point with TTC="
1277  << (ttc == INVALID ? "NA" : ::toString(ttc))
1278  << ", drac=" << (drac == INVALID ? "NA" : ::toString(drac)) << std::endl;
1279  }
1280  }
1281 #endif
1282 
1283  }
1284 
1285  } else if (type == ENCOUNTER_TYPE_CROSSING_FOLLOWER
1287  if (myComputeDRAC) {
1288  drac = computeDRAC(eInfo);
1289  }
1291  // follower's predicted arrival at the crossing area is earlier than the leader's predicted exit -> collision predicted
1292  double gap = eInfo.egoConflictEntryDist;
1293  if (myComputeTTC) {
1294  ttc = computeTTC(gap, e->ego->getSpeed(), 0.);
1295  }
1296  } else {
1297  // encounter is expected to happen without collision
1298  ttc = INVALID;
1299  }
1300  } else if (type == ENCOUNTER_TYPE_CROSSING_LEADER
1302  if (myComputeDRAC) {
1303  drac = computeDRAC(eInfo);
1304  }
1306  // follower's predicted arrival at the crossing area is earlier than the leader's predicted exit -> collision predicted
1307  double gap = eInfo.foeConflictEntryDist;
1308  if (myComputeTTC) {
1309  ttc = computeTTC(gap, e->foe->getSpeed(), 0.);
1310  }
1311  } else {
1312  // encounter is expected to happen without collision
1313  ttc = INVALID;
1314  }
1315  } else {
1316 #ifdef DEBUG_SSM
1317  if (DEBUG_COND(myHolderMS)) {
1318  std::stringstream ss;
1319  ss << "'" << type << "'";
1320  WRITE_WARNING("Underspecified or unknown encounter type in MSDevice_SSM::determineTTCandDRAC(): " + ss.str());
1321  }
1322 #endif
1323  }
1324 
1325 #ifdef DEBUG_SSM
1326  if (DEBUG_COND(myHolderMS))
1327  std::cout << "ttc=" << (ttc == INVALID ? "INVALID" : ::toString(ttc)) << ", drac=" << (drac == INVALID ? "INVALID" : ::toString(drac))
1328  << std::endl;
1329 #endif
1330 }
1331 
1332 
1333 double
1334 MSDevice_SSM::computeTTC(double gap, double followerSpeed, double leaderSpeed) const {
1335  // TODO: in merging situations, the TTC may be lower than the one computed here for following situations
1336  // (currently only the cross section corresponding to the target lane's begin is considered)
1337  // More specifically, the minimum has to be taken from the two if a collision at merge was predicted.
1338 #ifdef DEBUG_SSM
1339  if (DEBUG_COND(myHolderMS))
1340  std::cout << "computeTTC() with gap=" << gap << ", followerSpeed=" << followerSpeed << ", leaderSpeed=" << leaderSpeed
1341  << std::endl;
1342 #endif
1343  if (gap <= 0.) {
1344  return 0.; // collision already happend
1345  }
1346  double dv = followerSpeed - leaderSpeed;
1347  if (dv <= 0.) {
1348  return INVALID; // no collision
1349  }
1350 
1351  return gap / dv;
1352 }
1353 
1354 
1355 double
1356 MSDevice_SSM::computeDRAC(double gap, double followerSpeed, double leaderSpeed) {
1357 //#ifdef DEBUG_SSM_DRAC
1358 // if (DEBUG_COND)
1359 // std::cout << "computeDRAC() with gap=" << gap << ", followerSpeed=" << followerSpeed << ", leaderSpeed=" << leaderSpeed
1360 // << std::endl;
1361 //#endif
1362  if (gap <= 0.) {
1363  return INVALID; // collision!
1364  }
1365  double dv = followerSpeed - leaderSpeed;
1366  if (dv <= 0.) {
1367  return 0.0; // no need to break
1368  }
1369  assert(followerSpeed > 0.);
1370  return 0.5 * dv * dv / gap; // following Guido et al. (2011)
1371 }
1372 
1373 double
1375  // Introduce concise variable names
1376  double dEntry1 = eInfo.egoConflictEntryDist;
1377  double dEntry2 = eInfo.foeConflictEntryDist;
1378  double dExit1 = eInfo.egoConflictExitDist;
1379  double dExit2 = eInfo.foeConflictExitDist;
1380  double v1 = eInfo.encounter->ego->getSpeed();
1381  double v2 = eInfo.encounter->foe->getSpeed();
1382  double tEntry1 = eInfo.egoEstimatedConflictEntryTime;
1383  double tEntry2 = eInfo.foeEstimatedConflictEntryTime;
1384  double tExit1 = eInfo.egoEstimatedConflictExitTime;
1385  double tExit2 = eInfo.foeEstimatedConflictExitTime;
1386 #ifdef DEBUG_SSM_DRAC
1387  if (DEBUG_COND(eInfo.encounter->ego))
1388  std::cout << SIMTIME << "computeDRAC() with"
1389  << "\ndEntry1=" << dEntry1 << ", dEntry2=" << dEntry2
1390  << ", dExit1=" << dExit1 << ", dExit2=" << dExit2
1391  << ",\nv1=" << v1 << ", v2=" << v2
1392  << "\ntEntry1=" << (tEntry1 == INVALID ? "NA" : ::toString(tEntry1)) << ", tEntry2=" << (tEntry2 == INVALID ? "NA" : ::toString(tEntry2))
1393  << ", tExit1=" << (tExit1 == INVALID ? "NA" : ::toString(tExit1)) << ", tExit2=" << (tExit2 == INVALID ? "NA" : ::toString(tExit2))
1394  << std::endl;
1395 #endif
1396  if (dExit1 <= 0. || dExit2 <= 0.) {
1397  // At least one vehicle already left or is not about to enter conflict area at all => no breaking needed.
1398 #ifdef DEBUG_SSM_DRAC
1399  if (DEBUG_COND(eInfo.encounter->ego)) {
1400  std::cout << "One already left conflict area -> drac == 0." << std::endl;
1401  }
1402 #endif
1403  return 0.;
1404  }
1405  if (dEntry1 <= 0. && dEntry2 <= 0.) {
1406  // collision... (both already entered conflict area but none left)
1407 #ifdef DEBUG_SSM_DRAC
1408  if (DEBUG_COND(eInfo.encounter->ego)) {
1409  std::cout << "Both entered conflict area but neither left. -> collision!" << std::endl;
1410  }
1411 #endif
1412  return INVALID;
1413  }
1414 
1415  double drac = std::numeric_limits<double>::max();
1416  if (dEntry1 > 0.) {
1417  // vehicle 1 could break
1418 #ifdef DEBUG_SSM_DRAC
1419  if (DEBUG_COND(eInfo.encounter->ego)) {
1420  std::cout << "Ego could break..." << std::endl;
1421  }
1422 #endif
1423  if (tExit2 != INVALID) {
1424  // Vehicle 2 is expected to leave conflict area at t2
1425  drac = MIN2(drac, 2 * (v1 - dEntry1 / tExit2) / tExit2);
1426 #ifdef DEBUG_SSM_DRAC
1427  if (DEBUG_COND(eInfo.encounter->ego)) {
1428  std::cout << " Foe expected to leave in " << tExit2 << "-> Ego needs drac=" << drac << std::endl;
1429  }
1430 #endif
1431  } else {
1432  // Vehicle 2 is expected to stop on conflict area or earlier
1433  if (tEntry2 != INVALID) {
1434  // ... on conflict area => veh1 has to stop before entry
1435  drac = MIN2(drac, computeDRAC(dEntry1, v1, 0));
1436 #ifdef DEBUG_SSM_DRAC
1437  if (DEBUG_COND(eInfo.encounter->ego)) {
1438  std::cout << " Foe is expected stop on conflict area -> Ego needs drac=" << drac << std::endl;
1439  }
1440 #endif
1441  } else {
1442  // ... before conflict area
1443 #ifdef DEBUG_SSM_DRAC
1444  if (DEBUG_COND(eInfo.encounter->ego)) {
1445  std::cout << " Foe is expected stop before conflict area -> no drac computation for ego (will be done for foe if applicable)" << std::endl;
1446  }
1447 #endif
1448  }
1449  }
1450  }
1451 
1452  if (dEntry2 > 0.) {
1453  // vehicle 2 could break
1454 #ifdef DEBUG_SSM_DRAC
1455  if (DEBUG_COND(eInfo.encounter->ego)) {
1456  std::cout << "Foe could break..." << std::endl;
1457  }
1458 #endif
1459  if (tExit1 != INVALID) {
1460  // Vehicle 1 is expected to leave conflict area at t1
1461 #ifdef DEBUG_SSM_DRAC
1462  if (DEBUG_COND(eInfo.encounter->ego)) {
1463  std::cout << " Ego expected to leave in " << tExit1 << "-> Foe needs drac=" << (2 * (v2 - dEntry2 / tExit1) / tExit1) << std::endl;
1464  }
1465 #endif
1466  drac = MIN2(drac, 2 * (v2 - dEntry2 / tExit1) / tExit1);
1467  } else {
1468  // Vehicle 1 is expected to stop on conflict area or earlier
1469  if (tEntry1 != INVALID) {
1470  // ... on conflict area => veh2 has to stop before entry
1471 #ifdef DEBUG_SSM_DRAC
1472  if (DEBUG_COND(eInfo.encounter->ego)) {
1473  std::cout << " Ego is expected stop on conflict area -> Foe needs drac=" << computeDRAC(dEntry2, v2, 0) << std::endl;
1474  }
1475 #endif
1476  drac = MIN2(drac, computeDRAC(dEntry2, v2, 0));
1477  } else {
1478  // ... before conflict area
1479 #ifdef DEBUG_SSM_DRAC
1480  if (DEBUG_COND(eInfo.encounter->ego)) {
1481  std::cout << " Ego is expected stop before conflict area -> no drac computation for foe (done for ego if applicable)" << std::endl;
1482  }
1483 #endif
1484  }
1485  }
1486  }
1487 
1488  return drac > 0 ? drac : INVALID;
1489 }
1490 
1491 void
1493  // determine exact entry and exit times
1494  Encounter* e = eInfo.encounter;
1495 
1496 
1497  const bool foePastConflictEntry = eInfo.foeConflictEntryDist < 0.0;
1498  const bool egoPastConflictEntry = eInfo.egoConflictEntryDist < 0.0;
1499  const bool foePastConflictExit = eInfo.foeConflictExitDist < 0.0;
1500  const bool egoPastConflictExit = eInfo.egoConflictExitDist < 0.0;
1501 
1502 #ifdef DEBUG_ENCOUNTER
1503  if (DEBUG_COND_ENCOUNTER(e)) {
1504  std::cout << SIMTIME << " checkConflictEntryAndExit() for encounter of vehicles '" << e->egoID << "' and '" << e->foeID << "'"
1505  << " foeEntryDist=" << eInfo.foeConflictEntryDist
1506  << " egoEntryDist=" << eInfo.egoConflictEntryDist
1507  << " foeExitDist=" << eInfo.foeConflictExitDist
1508  << " egoExitDist=" << eInfo.egoConflictExitDist
1509  << "\n";
1510  }
1511 #endif
1512 
1513 
1514  if (e->size() == 0) {
1515  // This is a new conflict (are a conflict that was considered earlier
1516  // but disregarded due to being 'over')
1517 
1518  if (egoPastConflictExit) {
1519  if (foePastConflictExit) {
1521  } else if (foePastConflictEntry) {
1523  } else {
1525  }
1526  } else if (foePastConflictExit) {
1527  if (egoPastConflictEntry) {
1529  } else {
1531  }
1532  } else {
1533  // No one left conflict area
1534  if (egoPastConflictEntry) {
1535  if (foePastConflictEntry) {
1537  } else {
1539  }
1540  } else if (foePastConflictEntry) {
1542  }
1543  // else: both before conflict, keep current type
1544  }
1545  return;
1546  }
1547 
1548  // Distances to conflict area boundaries in previous step
1549  double prevEgoConflictEntryDist = eInfo.egoConflictEntryDist + e->ego->getLastStepDist();
1550  double prevFoeConflictEntryDist = eInfo.foeConflictEntryDist + e->foe->getLastStepDist();
1551  double prevEgoConflictExitDist = prevEgoConflictEntryDist + eInfo.egoConflictAreaLength + e->ego->getLength();
1552  double prevFoeConflictExitDist = prevFoeConflictEntryDist + eInfo.foeConflictAreaLength + e->foe->getLength();
1553  EncounterType prevType = e->currentType;
1554 
1555 //#ifdef DEBUG_ENCOUNTER
1556 // if (DEBUG_COND_ENCOUNTER(eInfo.encounter))
1557 // std::cout << "\nEgo's prev distance to conflict entry: " << prevEgoConflictEntryDist
1558 // << "\nEgo's prev distance to conflict exit: " << prevEgoConflictExitDist
1559 // << "\nFoe's prev distance to conflict entry: " << prevFoeConflictEntryDist
1560 // << "\nFoe's prev distance to conflict exit: " << prevFoeConflictExitDist
1561 // << std::endl;
1562 //#endif
1563 
1564  // Check if ego entered in last step
1565  if (e->egoConflictEntryTime == INVALID && egoPastConflictEntry && prevEgoConflictEntryDist >= 0) {
1566  // ego must have entered the conflict in the last step. Determine exact entry time
1567  e->egoConflictEntryTime = SIMTIME - TS + MSCFModel::passingTime(-prevEgoConflictEntryDist, 0., -eInfo.egoConflictEntryDist, e->ego->getPreviousSpeed(), e->ego->getSpeed());
1568 #ifdef DEBUG_ENCOUNTER
1569  if (DEBUG_COND_ENCOUNTER(e)) {
1570  std::cout << " ego entered conflict area at t=" << e->egoConflictEntryTime << std::endl;
1571  }
1572 #endif
1573  // Update encounter type (only done here for entering, the other transitions are done in updatePassedEncounter)
1574  if (prevType == ENCOUNTER_TYPE_CROSSING_FOLLOWER
1575  || prevType == ENCOUNTER_TYPE_CROSSING_LEADER) {
1577  }
1578  }
1579 
1580  // Check if foe entered in last step
1581  if (e->foeConflictEntryTime == INVALID && foePastConflictEntry && prevFoeConflictEntryDist >= 0) {
1582  // foe must have entered the conflict in the last step. Determine exact entry time
1583  e->foeConflictEntryTime = SIMTIME - TS + MSCFModel::passingTime(-prevFoeConflictEntryDist, 0., -eInfo.foeConflictEntryDist, e->foe->getPreviousSpeed(), e->foe->getSpeed());
1584 #ifdef DEBUG_ENCOUNTER
1585  if (DEBUG_COND_ENCOUNTER(e)) {
1586  std::cout << " foe entered conflict area at t=" << e->foeConflictEntryTime << std::endl;
1587  }
1588 #endif
1589  // Update encounter type (only done here for entering, the other transitions are done in updatePassedEncounter)
1590  if (prevType == ENCOUNTER_TYPE_CROSSING_FOLLOWER
1591  || prevType == ENCOUNTER_TYPE_CROSSING_LEADER) {
1593  }
1594  }
1595 
1596  // Check if ego left conflict area
1597  if (e->egoConflictExitTime == INVALID && eInfo.egoConflictExitDist < 0 && prevEgoConflictExitDist >= 0) {
1598  // ego must have left the conflict area in the last step. Determine exact exit time
1599  e->egoConflictExitTime = SIMTIME - TS + MSCFModel::passingTime(-prevEgoConflictExitDist, 0., -eInfo.egoConflictExitDist, e->ego->getPreviousSpeed(), e->ego->getSpeed());
1600  // Add cross section to calculate PET for foe
1601 // e->foePETCrossSections.push_back(std::make_pair(eInfo.foeConflictEntryCrossSection, e->egoConflictExitTime));
1602 #ifdef DEBUG_ENCOUNTER
1603  if (DEBUG_COND_ENCOUNTER(e)) {
1604  std::cout << " ego left conflict area at t=" << e->egoConflictExitTime << std::endl;
1605  }
1606 #endif
1607  // Update encounter type (only done here for entering, the other transitions are done in updatePassedEncounter)
1608  if (prevType == ENCOUNTER_TYPE_CROSSING_FOLLOWER
1609  || prevType == ENCOUNTER_TYPE_CROSSING_LEADER) {
1611  }
1612  }
1613 
1614  // Check if foe left conflict area
1615  if (e->foeConflictExitTime == INVALID && eInfo.foeConflictExitDist < 0 && prevFoeConflictExitDist >= 0) {
1616  // foe must have left the conflict area in the last step. Determine exact exit time
1617  e->foeConflictExitTime = SIMTIME - TS + MSCFModel::passingTime(-prevFoeConflictExitDist, 0., -eInfo.foeConflictExitDist, e->foe->getPreviousSpeed(), e->foe->getSpeed());
1618  // Add cross section to calculate PET for ego
1619 // e->egoPETCrossSections.push_back(std::make_pair(eInfo.egoConflictEntryCrossSection, e->foeConflictExitTime));
1620 #ifdef DEBUG_ENCOUNTER
1621  if (DEBUG_COND_ENCOUNTER(e)) {
1622  std::cout << " foe left conflict area at t=" << e->foeConflictExitTime << std::endl;
1623  }
1624 #endif
1625  // Update encounter type (only done here for entering, the other transitions are done in updatePassedEncounter)
1626  if (prevType == ENCOUNTER_TYPE_CROSSING_FOLLOWER
1627  || prevType == ENCOUNTER_TYPE_CROSSING_LEADER) {
1629  }
1630  }
1631 }
1632 
1633 
1634 void
1636 
1637 #ifdef DEBUG_ENCOUNTER
1638  if (DEBUG_COND_ENCOUNTER(e)) {
1639  std::cout << SIMTIME << " updatePassedEncounter() for vehicles '" << e->egoID << "' and '" << e->foeID << "'\n";
1640  }
1641 #endif
1642 
1643  if (foeInfo == nullptr) {
1644  // the foe is out of the device's range, proceed counting down the remaining extra time to trace
1645  e->countDownExtraTime(TS);
1646 #ifdef DEBUG_ENCOUNTER
1647  if (DEBUG_COND_ENCOUNTER(e)) std::cout << " Foe is out of range. Counting down extra time."
1648  << " Remaining seconds before closing encounter: " << e->getRemainingExtraTime() << std::endl;
1649 #endif
1650 
1651  } else {
1652  // reset the remaining extra time (foe could have re-entered the device range after beginning extra time countdown already)
1654  }
1655 
1656  // Check, whether this was really a potential conflict at some time:
1657  // Search through typeSpan for a type other than no conflict
1658  EncounterType lastPotentialConflictType = e->typeSpan.size() > 0 ? static_cast<EncounterType>(e->typeSpan.back()) : ENCOUNTER_TYPE_NOCONFLICT_AHEAD;
1659 
1660  if (lastPotentialConflictType == ENCOUNTER_TYPE_NOCONFLICT_AHEAD) {
1661  // This encounter was no conflict in the last step -> remains so
1662 #ifdef DEBUG_ENCOUNTER
1663  if (DEBUG_COND_ENCOUNTER(e)) {
1664  std::cout << " This encounter wasn't classified as a potential conflict lately.\n";
1665  }
1666 #endif
1667  if (foeInfo == nullptr) {
1668  // Encounter was either never a potential conflict and foe is out of range
1669  // or the foe has left the network
1670  // -> no use in further tracing this encounter
1671 #ifdef DEBUG_SSM
1672  if (DEBUG_COND(myHolderMS)) {
1673  std::cout << " Requesting encounter closure because foeInfo==nullptr" << std::endl;
1674  }
1675 #endif
1676  e->closingRequested = true;
1677 #ifdef DEBUG_ENCOUNTER
1678  if (DEBUG_COND_ENCOUNTER(e)) {
1679  std::cout << " Closing encounter.\n";
1680  }
1681 #endif
1683  }
1684  } else if (lastPotentialConflictType == ENCOUNTER_TYPE_FOLLOWING_FOLLOWER
1685  || lastPotentialConflictType == ENCOUNTER_TYPE_FOLLOWING_LEADER
1686  || lastPotentialConflictType == ENCOUNTER_TYPE_FOLLOWING_PASSED) {
1687  // if a following situation leads to a no-conflict situation this encounter switches no-conflict, since no further computations (PET) are needed.
1689 #ifdef DEBUG_ENCOUNTER
1690  if (DEBUG_COND_ENCOUNTER(e)) {
1691  std::cout << " Encounter was previously classified as a follow/lead situation.\n";
1692  }
1693 #endif
1694  } else if (lastPotentialConflictType == ENCOUNTER_TYPE_MERGING_FOLLOWER
1695  || lastPotentialConflictType == ENCOUNTER_TYPE_MERGING_LEADER
1696  || lastPotentialConflictType == ENCOUNTER_TYPE_MERGING_PASSED) {
1697  // if a merging situation leads to a no-conflict situation the leader was either removed from the net (we disregard special treatment)
1698  // or route- or lane-changes removed the conflict.
1700 #ifdef DEBUG_ENCOUNTER
1701  if (DEBUG_COND_ENCOUNTER(e)) {
1702  std::cout << " Encounter was previously classified as a merging situation.\n";
1703  }
1704 #endif
1705  }
1706  if (lastPotentialConflictType == ENCOUNTER_TYPE_CROSSING_FOLLOWER
1707  || lastPotentialConflictType == ENCOUNTER_TYPE_CROSSING_LEADER
1708  || lastPotentialConflictType == ENCOUNTER_TYPE_EGO_ENTERED_CONFLICT_AREA
1709  || lastPotentialConflictType == ENCOUNTER_TYPE_FOE_ENTERED_CONFLICT_AREA
1710  || lastPotentialConflictType == ENCOUNTER_TYPE_BOTH_ENTERED_CONFLICT_AREA
1711  || lastPotentialConflictType == ENCOUNTER_TYPE_EGO_LEFT_CONFLICT_AREA
1712  || lastPotentialConflictType == ENCOUNTER_TYPE_FOE_LEFT_CONFLICT_AREA
1713  || lastPotentialConflictType == ENCOUNTER_TYPE_BOTH_LEFT_CONFLICT_AREA
1714  || lastPotentialConflictType == ENCOUNTER_TYPE_COLLISION) {
1715  // Encounter has been a crossing situation.
1716 
1717 #ifdef DEBUG_ENCOUNTER
1718  if (DEBUG_COND_ENCOUNTER(e)) {
1719  std::cout << " Encounter was previously classified as a crossing situation of type " << lastPotentialConflictType << ".\n";
1720  }
1721 #endif
1722  // For passed encounters, the xxxConflictAreaLength variables are not determined before -> we use the stored values.
1723 
1724  // TODO: This could also more precisely be calculated wrt the angle of the crossing *at the conflict point*
1725  if (eInfo.egoConflictAreaLength == INVALID) {
1726  eInfo.egoConflictAreaLength = e->foe->getWidth();
1727  }
1728  if (eInfo.foeConflictAreaLength == INVALID) {
1729  eInfo.foeConflictAreaLength = e->ego->getWidth();
1730  }
1731 
1732  eInfo.egoConflictEntryDist = e->egoDistsToConflict.back() - e->ego->getLastStepDist();
1734  eInfo.foeConflictEntryDist = e->foeDistsToConflict.back() - e->foe->getLastStepDist();
1736 
1737 #ifdef DEBUG_ENCOUNTER
1738  if (DEBUG_COND_ENCOUNTER(e))
1739  std::cout << " egoConflictEntryDist = " << eInfo.egoConflictEntryDist
1740  << ", egoConflictExitDist = " << eInfo.egoConflictExitDist
1741  << "\n foeConflictEntryDist = " << eInfo.foeConflictEntryDist
1742  << ", foeConflictExitDist = " << eInfo.foeConflictExitDist
1743  << std::endl;
1744 #endif
1745 
1746  // Determine actual encounter type
1747  bool egoEnteredConflict = eInfo.egoConflictEntryDist < 0.;
1748  bool foeEnteredConflict = eInfo.foeConflictEntryDist < 0.;
1749  bool egoLeftConflict = eInfo.egoConflictExitDist < 0.;
1750  bool foeLeftConflict = eInfo.foeConflictExitDist < 0.;
1751 
1752  if ((!egoEnteredConflict) && !foeEnteredConflict) {
1753  // XXX: do we need to recompute the follow/lead order, here?
1754  assert(lastPotentialConflictType == ENCOUNTER_TYPE_CROSSING_FOLLOWER
1755  || lastPotentialConflictType == ENCOUNTER_TYPE_CROSSING_LEADER);
1756  eInfo.type = lastPotentialConflictType;
1757  } else if (egoEnteredConflict && !foeEnteredConflict) {
1759  } else if ((!egoEnteredConflict) && foeEnteredConflict) {
1761  } else { // (egoEnteredConflict && foeEnteredConflict) {
1763  }
1764 
1765  if ((!egoLeftConflict) && !foeLeftConflict) {
1768  }
1769  } else if (egoLeftConflict && !foeLeftConflict) {
1772  }
1773  } else if ((!egoLeftConflict) && foeLeftConflict) {
1776  }
1777  } else {
1779  // It should not occur that both leave the conflict at the same step
1780  assert(lastPotentialConflictType == ENCOUNTER_TYPE_FOE_LEFT_CONFLICT_AREA
1781  || lastPotentialConflictType == ENCOUNTER_TYPE_EGO_LEFT_CONFLICT_AREA
1782  || lastPotentialConflictType == ENCOUNTER_TYPE_BOTH_ENTERED_CONFLICT_AREA
1783  || lastPotentialConflictType == ENCOUNTER_TYPE_BOTH_LEFT_CONFLICT_AREA);
1784  }
1785 
1786  // TODO: adjust the conflict distances according to lateral movement for single ENTERED-cases
1787 
1788 #ifdef DEBUG_ENCOUNTER
1789  if (DEBUG_COND_ENCOUNTER(e)) {
1790  std::cout << " Updated classification: " << eInfo.type << "\n";
1791  }
1792 #endif
1793  }
1794 }
1795 
1796 
1799 #ifdef DEBUG_ENCOUNTER
1800  if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
1801  std::cout << "classifyEncounter() called.\n";
1802  }
1803 #endif
1804  if (foeInfo == nullptr) {
1805  // foeInfo == 0 signalizes, that no corresponding foe info was returned by findSurroundingVehicles(),
1806  // i.e. the foe is actually out of range (This may also mean that it has left the network)
1808  }
1809  const Encounter* e = eInfo.encounter;
1810 
1811  // previous classification (if encounter was not just created)
1812  EncounterType prevType = e->typeSpan.size() > 0 ? static_cast<EncounterType>(e->typeSpan.back()) : ENCOUNTER_TYPE_NOCONFLICT_AHEAD;
1813  if (e->typeSpan.size() > 0
1819  // This is an ongoing crossing situation with at least one of the vehicles not
1820  // having passed the conflict area.
1821  // -> Merely trace the change of distances to the conflict entry / exit
1822  // -> Derefer this to updatePassedEncounter, where this is done anyhow.
1823 #ifdef DEBUG_ENCOUNTER
1824  if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
1825  std::cout << " Ongoing crossing conflict will be traced by passedEncounter().\n";
1826  }
1827 #endif
1828  return prevType;
1829  }
1830 
1831 
1832  // Ego's current Lane
1833  const MSLane* egoLane = e->ego->getLane();
1834  // Foe's current Lane
1835  const MSLane* foeLane = e->foe->getLane();
1836 
1837  // Ego's conflict lane is memorized in foeInfo
1838  const MSLane* egoConflictLane = foeInfo->egoConflictLane;
1839  double egoDistToConflictLane = foeInfo->egoDistToConflictLane;
1840  // Find conflicting lane and the distance to its entry link for the foe
1841  double foeDistToConflictLane;
1842  const MSLane* foeConflictLane = findFoeConflictLane(e->foe, foeInfo->egoConflictLane, foeDistToConflictLane);
1843 
1844 #ifdef DEBUG_ENCOUNTER
1845  if (DEBUG_COND_ENCOUNTER(eInfo.encounter))
1846  std::cout << " egoConflictLane='" << (egoConflictLane == 0 ? "NULL" : egoConflictLane->getID()) << "'\n"
1847  << " foeConflictLane='" << (foeConflictLane == 0 ? "NULL" : foeConflictLane->getID()) << "'\n"
1848  << " egoDistToConflictLane=" << egoDistToConflictLane
1849  << " foeDistToConflictLane=" << foeDistToConflictLane
1850  << std::endl;
1851 #endif
1852 
1853  // Treat different cases for foeConflictLane and egoConflictLane (internal or non-internal / equal to egoLane or to foeLane),
1854  // and thereby determine encounterType and the ego/foeEncounterDistance.
1855  // The encounter distance has a different meaning for different types of encounters:
1856  // 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.
1857  // 2) For merging encounters the encounter distance is the distance until the begin of the common target edge/lane.
1858  // (XXX: Perhaps this should be adjusted to include the entry point to the region where a simultaneous occupancy of
1859  // both merging lanes could imply a collision)
1860  // 3) For crossing encounters the encounter distances is the distance until the entry point to the conflicting lane.
1861 
1862  EncounterType type;
1863 
1864  if (foeConflictLane == nullptr) {
1865  // foe vehicle is not on course towards the ego's route (see findFoeConflictLane)
1867 #ifdef DEBUG_ENCOUNTER
1868  if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
1869  std::cout << "-> Encounter type: No conflict.\n";
1870  }
1871 #endif
1872  } else if (!egoConflictLane->isInternal()) {
1873  // The conflict lane is non-internal, therefore we either have no potential conflict or a lead/follow situation (i.e., no crossing or merging)
1874  if (egoConflictLane == egoLane) {
1875  const bool egoOpposite = e->ego->getLaneChangeModel().isOpposite();
1876  const bool foeOpposite = e->foe->getLaneChangeModel().isOpposite();
1877  // The conflict point is on the ego's current lane.
1878  if (foeLane == egoLane) {
1879  // Foe is on the same non-internal lane
1880  if (!egoOpposite && !foeOpposite) {
1881  if (e->ego->getPositionOnLane() > e->foe->getPositionOnLane()) {
1884  } else {
1887  }
1888 #ifdef DEBUG_ENCOUNTER
1889  if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
1890  std::cout << "-> Encounter type: Lead/follow-situation on non-internal lane '" << egoLane->getID() << "'\n";
1891  }
1892 #endif
1893  } else if (egoOpposite && foeOpposite) {
1894  if (e->ego->getPositionOnLane() < e->foe->getPositionOnLane()) {
1897  } else {
1900  }
1901 #ifdef DEBUG_ENCOUNTER
1902  if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
1903  std::cout << "-> Encounter type: Lead/follow-situation while both are driving in the opposite direction on non-internal lane '" << egoLane->getID() << "'\n";
1904  }
1905 #endif
1906  } else {
1907  type = ENCOUNTER_TYPE_ONCOMING;
1908  const double gap = e->ego->getPositionOnLane() - e->foe->getPositionOnLane();
1909  if (egoOpposite) {
1910  if (e->ego->getPositionOnLane() > e->foe->getPositionOnLane()) {
1911  eInfo.egoConflictEntryDist = gap;
1912  eInfo.foeConflictEntryDist = gap;
1913  } else {
1915  }
1916  } else {
1917  if (e->ego->getPositionOnLane() < e->foe->getPositionOnLane()) {
1918  eInfo.egoConflictEntryDist = -gap;
1919  eInfo.foeConflictEntryDist = -gap;
1920  } else {
1922  }
1923  }
1924 #ifdef DEBUG_ENCOUNTER
1925  if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
1926  std::cout << "-> Encounter type: oncoming on non-internal lane '" << egoLane->getID() << "'\n";
1927  }
1928 #endif
1929 
1930  }
1931  } else if (&(foeLane->getEdge()) == &(egoLane->getEdge())) {
1932  // Foe is on the same non-internal edge but not on the same lane. Treat this as no conflict for now
1933  // XXX: this disregards conflicts for vehicles on adjacent lanes
1935 #ifdef DEBUG_ENCOUNTER
1936  if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
1937  std::cout << "-> Encounter type: " << type << std::endl;
1938  }
1939 #endif
1940  } else {
1941 
1942  if (!egoOpposite && !foeOpposite) {
1943 
1944  assert(&(egoLane->getEdge()) == &(foeConflictLane->getEdge()));
1945  assert(egoDistToConflictLane <= 0);
1946  // Foe must be on a route leading into the ego's edge
1947  if (foeConflictLane == egoLane) {
1949  eInfo.foeConflictEntryDist = foeDistToConflictLane + e->ego->getBackPositionOnLane();
1950 
1951 #ifdef DEBUG_ENCOUNTER
1952  if (DEBUG_COND_ENCOUNTER(eInfo.encounter))
1953  std::cout << "-> Encounter type: Ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' leads foe '"
1954  << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
1955  << " (gap = " << eInfo.foeConflictEntryDist << ")\n";
1956 #endif
1957  } else {
1958  // Foe's route leads to an adjacent lane of the current lane of the ego
1960 #ifdef DEBUG_ENCOUNTER
1961  if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
1962  std::cout << "-> Encounter type: " << type << std::endl;
1963  }
1964 #endif
1965  }
1966 
1967  } else if (egoOpposite && foeOpposite) {
1968  // XXX determine follower relationship by searching for the foe lane in the opposites of ego bestlanes
1970  /*
1971  if (e->ego->getPositionOnLane() < e->foe->getPositionOnLane()) {
1972  type = ENCOUNTER_TYPE_FOLLOWING_LEADER;
1973  eInfo.foeConflictEntryDist = -(e->ego->getBackPositionOnLane() - e->foe->getPositionOnLane());
1974  } else {
1975  type = ENCOUNTER_TYPE_FOLLOWING_FOLLOWER;
1976  eInfo.egoConflictEntryDist = -(e->foe->getBackPositionOnLane() - e->ego->getPositionOnLane());
1977  }
1978  */
1979 #ifdef DEBUG_ENCOUNTER
1980  if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
1981  std::cout << "-> Encounter type: Lead/follow-situation while both are driving in the opposite direction on non-internal lane '" << egoLane->getID() << "'\n";
1982  }
1983 #endif
1984  } else {
1985  type = ENCOUNTER_TYPE_ONCOMING;
1986  // XXX determine distance by searching for the foe lane in the opposites of ego bestlanes
1987  /*
1988  const double gap = e->ego->getPositionOnLane() - e->foe->getPositionOnLane();
1989  if (egoOpposite) {
1990  if (e->ego->getPositionOnLane() > e->foe->getPositionOnLane()) {
1991  eInfo.egoConflictEntryDist = gap;
1992  eInfo.foeConflictEntryDist = gap;
1993  } else {
1994  type = ENCOUNTER_TYPE_NOCONFLICT_AHEAD;
1995  }
1996  } else {
1997  if (e->ego->getPositionOnLane() < e->foe->getPositionOnLane()) {
1998  eInfo.egoConflictEntryDist = -gap;
1999  eInfo.foeConflictEntryDist = -gap;
2000  } else {
2001  type = ENCOUNTER_TYPE_NOCONFLICT_AHEAD;
2002  }
2003  }
2004  */
2005 #ifdef DEBUG_ENCOUNTER
2006  if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
2007  std::cout << "-> Encounter type: oncoming on non-internal lane '" << egoLane->getID() << "'\n";
2008  }
2009 #endif
2010 
2011  }
2012  }
2013  } else {
2014  // 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
2015  // is located on the foe's current edge see findSurroundingVehicles()
2016  // (otherwise the foe would have had to enter the ego's route along a junction and the corresponding
2017  // conflict lane would be internal)
2018  assert(&(foeLane->getEdge()) == &(egoConflictLane->getEdge()));
2019  assert(foeDistToConflictLane <= 0);
2020  if (foeLane == egoConflictLane) {
2022  eInfo.egoConflictEntryDist = egoDistToConflictLane + e->foe->getBackPositionOnLane();
2023 #ifdef DEBUG_ENCOUNTER
2024  if (DEBUG_COND_ENCOUNTER(eInfo.encounter))
2025  std::cout << "-> Encounter type: Ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' follows foe '"
2026  << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
2027  << " (gap = " << eInfo.egoConflictEntryDist << ", case1)\n";
2028 #endif
2029  } else {
2030  // Ego's route leads to an adjacent lane of the current lane of the foe
2032 #ifdef DEBUG_ENCOUNTER
2033  if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
2034  std::cout << "-> Encounter type: " << type << std::endl;
2035  }
2036 #endif
2037  }
2038  }
2039  } else {
2040  // egoConflictLane is internal, i.e., lies on a junction. Besides the lead/follow situation (which may stretch over different lanes of a connection),
2041  // merging or crossing of the conflict lanes is possible.
2042  assert(foeConflictLane->isInternal());
2043  MSLink* egoEntryLink = egoConflictLane->getEntryLink();
2044  MSLink* foeEntryLink = foeConflictLane->getEntryLink();
2045  if (&(egoEntryLink->getViaLane()->getEdge()) == &(foeEntryLink->getViaLane()->getEdge())) {
2046  if (egoEntryLink != foeEntryLink) {
2047  // XXX: this disregards conflicts for vehicles on adjacent internal lanes
2049 #ifdef DEBUG_ENCOUNTER
2050  if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
2051  std::cout << "-> Encounter type: " << type << std::endl;
2052  }
2053 #endif
2054  } else {
2055  // Lead / follow situation on connection
2056  if (egoLane == egoConflictLane && foeLane != foeConflictLane) {
2057  // ego on junction, foe not yet
2059  eInfo.foeConflictEntryDist = foeDistToConflictLane + e->ego->getBackPositionOnLane();
2060  if (e->ego->getLane()->getIncomingLanes()[0].lane->isInternal()) {
2061  eInfo.foeConflictEntryDist += e->ego->getLane()->getIncomingLanes()[0].lane->getLength();
2062  }
2063 #ifdef DEBUG_ENCOUNTER
2064  if (DEBUG_COND_ENCOUNTER(eInfo.encounter))
2065  std::cout << "-> Encounter type: Ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' leads foe '"
2066  << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
2067  << " (gap = " << eInfo.foeConflictEntryDist << ")\n";
2068 #endif
2069  } else if (egoLane != egoConflictLane && foeLane == foeConflictLane) {
2070  // foe on junction, ego not yet
2072  eInfo.egoConflictEntryDist = egoDistToConflictLane + e->foe->getBackPositionOnLane();
2073  if (e->foe->getLane()->getIncomingLanes()[0].lane->isInternal()) {
2074  eInfo.egoConflictEntryDist += e->foe->getLane()->getIncomingLanes()[0].lane->getLength();
2075  }
2076 #ifdef DEBUG_ENCOUNTER
2077  if (DEBUG_COND_ENCOUNTER(eInfo.encounter))
2078  std::cout << "-> Encounter type: Ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' follows foe '"
2079  << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
2080  << " (gap = " << eInfo.egoConflictEntryDist << ", case2)\n";
2081 #endif
2082  } else if (e->ego->getLaneChangeModel().isOpposite() || e->foe->getLaneChangeModel().isOpposite()) {
2083  type = ENCOUNTER_TYPE_MERGING;
2084  eInfo.foeConflictEntryDist = foeDistToConflictLane;
2085  eInfo.egoConflictEntryDist = egoDistToConflictLane;
2086 #ifdef DEBUG_ENCOUNTER
2087  if (DEBUG_COND_ENCOUNTER(eInfo.encounter))
2088  std::cout << "-> Encounter type: Ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' merges with foe '"
2089  << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
2090  << " (gap = " << eInfo.egoConflictEntryDist << ", case5)\n";
2091 #endif
2092 
2093  } else {
2094  // Both must be already on the junction in a lead / follow situation on a connection
2095  // (since they approach via the same link, findSurroundingVehicles() would have determined a
2096  // different conflictLane if both are not on the junction)
2097  assert(egoLane == egoConflictLane);
2098  assert(foeLane == foeConflictLane);
2099  if (egoLane == foeLane) {
2100  // both on the same internal lane
2101  if (e->ego->getPositionOnLane() > e->foe->getPositionOnLane()) {
2103  eInfo.foeConflictEntryDist = foeDistToConflictLane + e->ego->getBackPositionOnLane();
2104 #ifdef DEBUG_ENCOUNTER
2105  if (DEBUG_COND_ENCOUNTER(eInfo.encounter))
2106  std::cout << "-> Encounter type: Ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' leads foe '"
2107  << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
2108  << " (gap = " << eInfo.foeConflictEntryDist << ")"
2109  << std::endl;
2110 #endif
2111  } else {
2113  eInfo.egoConflictEntryDist = egoDistToConflictLane + e->foe->getBackPositionOnLane();
2114 #ifdef DEBUG_ENCOUNTER
2115  if (DEBUG_COND_ENCOUNTER(eInfo.encounter))
2116  std::cout << "-> Encounter type: Ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' follows foe '"
2117  << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
2118  << " (gap = " << eInfo.egoConflictEntryDist << ", case3)"
2119  << std::endl;
2120 #endif
2121  }
2122  } else {
2123  // ego and foe on distinct, consecutive internal lanes
2124 #ifdef DEBUG_ENCOUNTER
2125  if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
2126  std::cout << " Lead/follow situation on consecutive internal lanes." << std::endl;
2127  }
2128 #endif
2129  MSLane* lane = egoEntryLink->getViaLane();
2130 #ifdef _MSC_VER
2131 #pragma warning(push)
2132 #pragma warning(disable: 4127) // do not warn about constant conditional expression
2133 #endif
2134  while (true) {
2135 #ifdef _MSC_VER
2136 #pragma warning(pop)
2137 #endif
2138  // Find first of egoLane and foeLane while crossing the junction (this dertermines who's the follower)
2139  // Then set the conflict lane to the lane of the leader and adapt the follower's distance to conflict
2140  if (egoLane == lane) {
2141  // ego is follower
2143  // adapt conflict dist
2144  eInfo.egoConflictEntryDist = egoDistToConflictLane;
2145  while (lane != foeLane) {
2146  eInfo.egoConflictEntryDist += lane->getLength();
2147  lane = lane->getLinkCont()[0]->getViaLane();
2148  assert(lane != 0);
2149  }
2151  egoConflictLane = lane;
2152 #ifdef DEBUG_ENCOUNTER
2153  if (DEBUG_COND_ENCOUNTER(eInfo.encounter))
2154  std::cout << "-> Encounter type: Ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' follows foe '"
2155  << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
2156  << " (gap = " << eInfo.egoConflictEntryDist << ", case4)"
2157  << std::endl;
2158 #endif
2159  break;
2160  } else if (foeLane == lane) {
2161  // ego is leader
2163  // adapt conflict dist
2164  eInfo.foeConflictEntryDist = foeDistToConflictLane;
2165  while (lane != egoLane) {
2166  eInfo.foeConflictEntryDist += lane->getLength();
2167  lane = lane->getLinkCont()[0]->getViaLane();
2168  assert(lane != 0);
2169  }
2171  foeConflictLane = lane;
2172 #ifdef DEBUG_ENCOUNTER
2173  if (DEBUG_COND_ENCOUNTER(eInfo.encounter))
2174  std::cout << "-> Encounter type: Ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' leads foe '"
2175  << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
2176  << " (gap = " << eInfo.foeConflictEntryDist << ")"
2177  << std::endl;
2178 #endif
2179  break;
2180  }
2181  lane = lane->getLinkCont()[0]->getViaLane();
2182  assert(lane != 0);
2183  }
2184  }
2185 #ifdef DEBUG_ENCOUNTER
2186  if (DEBUG_COND_ENCOUNTER(eInfo.encounter))
2187  std::cout << "-> Encounter type: Lead/follow-situation on connection from '" << egoEntryLink->getLaneBefore()->getID()
2188  << "' to '" << egoEntryLink->getLane()->getID() << "'" << std::endl;
2189 #endif
2190  }
2191  }
2192  } else {
2193  // Entry links to junctions lead to different internal edges.
2194  // There are three possibilities, either the edges cross, merge or have no conflict
2195  const std::vector<MSLink*>& egoFoeLinks = egoEntryLink->getFoeLinks();
2196  const std::vector<MSLink*>& foeFoeLinks = foeEntryLink->getFoeLinks();
2197  // Determine whether ego and foe links are foes
2198  bool crossOrMerge = (find(egoFoeLinks.begin(), egoFoeLinks.end(), foeEntryLink) != egoFoeLinks.end()
2199  || std::find(foeFoeLinks.begin(), foeFoeLinks.end(), egoEntryLink) != foeFoeLinks.end());
2200  if (!crossOrMerge) {
2201  // if (&(foeEntryLink->getLane()->getEdge()) == &(egoEntryLink->getLane()->getEdge())) {
2202  // // XXX: the situation of merging into adjacent lanes is disregarded for now <- the alleged situation appears to imply crossOrMerge!!!
2203  // type = ENCOUNTER_TYPE_MERGING_ADJACENT;
2204  //#ifdef DEBUG_SSM
2205  // std::cout << "-> Encounter type: No conflict (adjacent lanes)." << std::endl;
2206  //#endif
2207  // } else {
2209 #ifdef DEBUG_ENCOUNTER
2210  if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
2211  std::cout << "-> Encounter type: No conflict.\n";
2212  }
2213 #endif
2214  // }
2215  } else if (&(foeEntryLink->getLane()->getEdge()) == &(egoEntryLink->getLane()->getEdge())) {
2216  if (foeEntryLink->getLane() == egoEntryLink->getLane()) {
2217  type = ENCOUNTER_TYPE_MERGING;
2218  assert(egoConflictLane->isInternal());
2219  assert(foeConflictLane->isInternal());
2220  eInfo.egoConflictEntryDist = egoDistToConflictLane + egoEntryLink->getInternalLengthsAfter();
2221  eInfo.foeConflictEntryDist = foeDistToConflictLane + foeEntryLink->getInternalLengthsAfter();
2222 
2223  MSLink* egoEntryLinkSucc = egoEntryLink->getViaLane()->getLinkCont().front();
2224  if (egoEntryLinkSucc->isInternalJunctionLink() && e->ego->getLane() == egoEntryLinkSucc->getViaLane()) {
2225  // ego is already past the internal junction
2226  eInfo.egoConflictEntryDist -= egoEntryLink->getViaLane()->getLength();
2227  eInfo.egoConflictExitDist -= egoEntryLink->getViaLane()->getLength();
2228  }
2229  MSLink* foeEntryLinkSucc = foeEntryLink->getViaLane()->getLinkCont().front();
2230  if (foeEntryLinkSucc->isInternalJunctionLink() && e->foe->getLane() == foeEntryLinkSucc->getViaLane()) {
2231  // foe is already past the internal junction
2232  eInfo.foeConflictEntryDist -= foeEntryLink->getViaLane()->getLength();
2233  eInfo.foeConflictExitDist -= foeEntryLink->getViaLane()->getLength();
2234  }
2235 
2236 #ifdef DEBUG_ENCOUNTER
2237  if (DEBUG_COND_ENCOUNTER(eInfo.encounter))
2238  std::cout << "-> Encounter type: Merging situation of ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' and foe '"
2239  << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
2240  << "\nDistances to merge-point: ego: " << eInfo.egoConflictEntryDist << ", foe: " << eInfo.foeConflictEntryDist
2241  << std::endl;
2242 #endif
2243  } else {
2244  // Links leading to the same edge but different lanes. XXX: Disregards conflicts on adjacent lanes
2246 #ifdef DEBUG_ENCOUNTER
2247  if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
2248  std::cout << "-> Encounter type: No conflict: " << type << std::endl;
2249  }
2250 #endif
2251  }
2252  } else {
2253  type = ENCOUNTER_TYPE_CROSSING;
2254 
2255  assert(egoConflictLane->isInternal());
2256  assert(foeConflictLane->getEdge().getToJunction() == egoConflictLane->getEdge().getToJunction());
2257 
2258  // If the conflict lanes are internal, they may not correspond to the
2259  // actually crossing parts of the corresponding connections.
2260  // Adjust the conflict lanes accordingly.
2261  // set back both to the first parts of the corresponding connections
2262  double offset = 0.;
2263  egoConflictLane = egoConflictLane->getFirstInternalInConnection(offset);
2264  egoDistToConflictLane -= offset;
2265  foeConflictLane = foeConflictLane->getFirstInternalInConnection(offset);
2266  foeDistToConflictLane -= offset;
2267  // find the distances to the conflict from the junction entry for both vehicles
2268  // Here we also determine the real crossing lanes (before the conflict lane is the first lane of the connection)
2269  // for the ego
2270  double egoDistToConflictFromJunctionEntry = INVALID;
2271  double foeInternalLaneLengthsBeforeCrossing = 0.;
2272  while (foeConflictLane != nullptr && foeConflictLane->isInternal()) {
2273  egoDistToConflictFromJunctionEntry = egoEntryLink->getLengthsBeforeCrossing(foeConflictLane);
2274  if (egoDistToConflictFromJunctionEntry != INVALID) {
2275  // found correct foeConflictLane
2276  egoDistToConflictFromJunctionEntry += 0.5 * (foeConflictLane->getWidth() - e->foe->getVehicleType().getWidth());
2277  break;
2278  } else {
2279  foeInternalLaneLengthsBeforeCrossing += foeConflictLane->getLength();
2280  }
2281  foeConflictLane = foeConflictLane->getCanonicalSuccessorLane();
2282  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.
2283  }
2284  assert(egoDistToConflictFromJunctionEntry != INVALID);
2285 
2286  // for the foe
2287  double foeDistToConflictFromJunctionEntry = INVALID;
2288  double egoInternalLaneLengthsBeforeCrossing = 0.;
2289  foeDistToConflictFromJunctionEntry = INVALID;
2290  while (egoConflictLane != nullptr && egoConflictLane->isInternal()) {
2291  foeDistToConflictFromJunctionEntry = foeEntryLink->getLengthsBeforeCrossing(egoConflictLane);
2292  if (foeDistToConflictFromJunctionEntry != INVALID) {
2293  // found correct egoConflictLane
2294  foeDistToConflictFromJunctionEntry += 0.5 * (egoConflictLane->getWidth() - e->ego->getVehicleType().getWidth());
2295  break;
2296  } else {
2297  egoInternalLaneLengthsBeforeCrossing += egoConflictLane->getLength();
2298  }
2299  egoConflictLane = egoConflictLane->getCanonicalSuccessorLane();
2300  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.
2301  }
2302  assert(foeDistToConflictFromJunctionEntry != INVALID);
2303 
2304  // store conflict entry information in eInfo
2305 
2306  // // TO-DO: equip these with exit times to store relevant PET sections in encounter
2307  // eInfo.egoConflictEntryCrossSection = std::make_pair(egoConflictLane, egoDistToConflictFromJunctionEntry - egoInternalLaneLengthsBeforeCrossing);
2308  // eInfo.foeConflictEntryCrossSection = std::make_pair(foeConflictLane, foeDistToConflictFromJunctionEntry - foeInternalLaneLengthsBeforeCrossing);
2309 
2310  // Take into account the lateral position for the exact determination of the conflict point
2311  // whether lateral position increases or decreases conflict distance depends on lane angles at conflict
2312  // -> conflictLaneOrientation in {-1,+1}
2313  // First, measure the angle between the two connection lines (straight lines from junction entry point to junction exit point)
2314  Position egoEntryPos = egoEntryLink->getViaLane()->getShape().front();
2315  Position egoExitPos = egoEntryLink->getCorrespondingExitLink()->getInternalLaneBefore()->getShape().back();
2316  PositionVector egoConnectionLine(egoEntryPos, egoExitPos);
2317  Position foeEntryPos = foeEntryLink->getViaLane()->getShape().front();
2318  Position foeExitPos = foeEntryLink->getCorrespondingExitLink()->getInternalLaneBefore()->getShape().back();
2319  PositionVector foeConnectionLine(foeEntryPos, foeExitPos);
2320  double angle = std::fmod(egoConnectionLine.rotationAtOffset(0.) - foeConnectionLine.rotationAtOffset(0.), (2 * M_PI));
2321  if (angle < 0) {
2322  angle += 2 * M_PI;
2323  }
2324  assert(angle >= 0);
2325  assert(angle <= 2 * M_PI);
2326  if (angle > M_PI) {
2327  angle -= 2 * M_PI;
2328  }
2329  assert(angle >= -M_PI);
2330  assert(angle <= M_PI);
2331  // Determine orientation of the connection lines. (Positive values mean that the ego vehicle approaches from the foe's left side.)
2332  double crossingOrientation = (angle < 0) - (angle > 0);
2333 
2334  // Adjust conflict dist to lateral positions
2335  // TODO: This could more precisely be calculated wrt the angle of the crossing *at the conflict point*
2336  egoDistToConflictFromJunctionEntry -= crossingOrientation * e->foe->getLateralPositionOnLane();
2337  foeDistToConflictFromJunctionEntry += crossingOrientation * e->ego->getLateralPositionOnLane();
2338 
2339  // Complete entry distances
2340  eInfo.egoConflictEntryDist = egoDistToConflictLane + egoDistToConflictFromJunctionEntry;
2341  eInfo.foeConflictEntryDist = foeDistToConflictLane + foeDistToConflictFromJunctionEntry;
2342 
2343 
2344  // TODO: This could also more precisely be calculated wrt the angle of the crossing *at the conflict point*
2345  eInfo.egoConflictAreaLength = e->foe->getWidth();
2346  eInfo.foeConflictAreaLength = e->ego->getWidth();
2347 
2348  // resulting exit distances
2351 
2352 #ifdef DEBUG_ENCOUNTER
2353  if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
2354  std::cout << " Determined exact conflict distances for crossing conflict."
2355  << "\n crossingOrientation=" << crossingOrientation
2356  << ", egoCrossingAngle=" << egoConnectionLine.rotationAtOffset(0.)
2357  << ", foeCrossingAngle=" << foeConnectionLine.rotationAtOffset(0.)
2358  << ", relativeAngle=" << angle
2359  << " (foe from " << (crossingOrientation > 0 ? "right)" : "left)")
2360  << "\n resulting offset for conflict entry distance:"
2361  << "\n ego=" << crossingOrientation* e->foe->getLateralPositionOnLane()
2362  << ", foe=" << crossingOrientation* e->ego->getLateralPositionOnLane()
2363  << "\n distToConflictLane:"
2364  << "\n ego=" << egoDistToConflictLane
2365  << ", foe=" << foeDistToConflictLane
2366  << "\n distToConflictFromJunctionEntry:"
2367  << "\n ego=" << egoDistToConflictFromJunctionEntry
2368  << ", foe=" << foeDistToConflictFromJunctionEntry
2369  << "\n resulting entry distances:"
2370  << "\n ego=" << eInfo.egoConflictEntryDist
2371  << ", foe=" << eInfo.foeConflictEntryDist
2372  << "\n resulting exit distances:"
2373  << "\n ego=" << eInfo.egoConflictExitDist
2374  << ", foe=" << eInfo.foeConflictExitDist
2375  << std::endl;
2376 
2377  std::cout << "real egoConflictLane: '" << (egoConflictLane == 0 ? "NULL" : egoConflictLane->getID()) << "'\n"
2378  << "real foeConflictLane: '" << (foeConflictLane == 0 ? "NULL" : foeConflictLane->getID()) << "'\n"
2379  << "-> Encounter type: Crossing situation of ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' and foe '"
2380  << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
2381  << "\nDistances to crossing-point: ego: " << eInfo.egoConflictEntryDist << ", foe: " << eInfo.foeConflictEntryDist
2382  << std::endl;
2383  }
2384 #endif
2385  }
2386  }
2387  }
2388  return type;
2389 }
2390 
2391 
2392 
2393 const MSLane*
2394 MSDevice_SSM::findFoeConflictLane(const MSVehicle* foe, const MSLane* egoConflictLane, double& distToConflictLane) const {
2395 
2396 #ifdef DEBUG_SSM
2397  if (DEBUG_COND(myHolderMS))
2398  std::cout << SIMTIME << " findFoeConflictLane() for foe '"
2399  << foe->getID() << "' on lane '" << foe->getLane()->getID()
2400  << "' (with egoConflictLane=" << (egoConflictLane == 0 ? "NULL" : egoConflictLane->getID())
2401  << ")\nfoeBestLanes: " << ::toString(foe->getBestLanesContinuation())
2402  << std::endl;
2403 #endif
2404  if (foe->getLaneChangeModel().isOpposite()) {
2405  // distinguish three cases
2406  // 1) foe is driving in the same direction as ego and ego is driving in lane direction -> ENCOUNTER_TYPE_ON_ADJACENT_LANES
2407  // 2) foe is driving in the same direction as ego and ego is also driving in the opposite direction -> ENCOUNTER_TYPE_FOLLOWING
2408  // 3) foe is driving in the opposite direction as ego and both are driving way from each other -> ENCOUNTER_TYPE_NOCONFLICT_AHEAD
2409  // 3) foe is driving in the opposite direction as ego and both are driving towards each other -> ENCOUNTER_TYPE_ONCOMING
2410 #ifdef DEBUG_SSM_OPPOSITE
2411 #endif
2412  auto egoIt = std::find(myHolder.getCurrentRouteEdge(), myHolder.getRoute().end(), foe->getEdge());
2413  if (egoIt != myHolder.getRoute().end()) {
2414  // same direction, foe is leader
2416  return foe->getLane();
2417  } else {
2418  // adjacent
2419  return nullptr;
2420  }
2421  }
2422  auto foeIt = std::find(foe->getCurrentRouteEdge(), foe->getRoute().end(), myHolder.getEdge());
2423  if (foeIt != foe->getRoute().end()) {
2424  // same direction, ego is leader
2426  return egoConflictLane;
2427  } else {
2428  // adjacent
2429  return nullptr;
2430  }
2431  }
2432  auto egoIt2 = std::find(myHolder.getCurrentRouteEdge(), myHolder.getRoute().end(), foe->getEdge()->getOppositeEdge());
2433  if (egoIt2 != myHolder.getRoute().end()) {
2434  // opposite direction, driving towards each other
2435  return egoConflictLane;
2436  } else {
2437  // opposite direction, driving away from each other
2438  return nullptr;
2439  }
2440  }
2441 
2442  MSLane* foeLane = foe->getLane();
2443  std::vector<MSLane*>::const_iterator laneIter = foe->getBestLanesContinuation().begin();
2444  std::vector<MSLane*>::const_iterator foeBestLanesEnd = foe->getBestLanesContinuation().end();
2445  assert(foeLane->isInternal() || *laneIter == foeLane);
2446  distToConflictLane = -foe->getPositionOnLane();
2447 
2448  // Potential conflict lies on junction if egoConflictLane is internal
2449  const MSJunction* conflictJunction = egoConflictLane->isInternal() ? egoConflictLane->getEdge().getToJunction() : nullptr;
2450 #ifdef DEBUG_SSM
2451  if (DEBUG_COND(myHolderMS))
2452  if (conflictJunction != 0) {
2453  std::cout << "Potential conflict on junction '" << conflictJunction->getID()
2454  << std::endl;
2455  }
2456 #endif
2457  if (foeLane->isInternal() && foeLane->getEdge().getToJunction() == conflictJunction) {
2458  // foe is already on the conflict junction
2459  if (egoConflictLane != nullptr && egoConflictLane->isInternal() && egoConflictLane->getLinkCont()[0]->getViaLane() == foeLane) {
2460  distToConflictLane += egoConflictLane->getLength();
2461  }
2462  return foeLane;
2463  }
2464 
2465  // Foe is not on the conflict junction
2466 
2467  // Leading internal lanes in bestlanes are resembled as a single NULL-pointer skip them
2468  if (*laneIter == nullptr) {
2469  while (foeLane != nullptr && foeLane->isInternal()) {
2470  distToConflictLane += foeLane->getLength();
2471  foeLane = foeLane->getLinkCont()[0]->getViaLane();
2472  }
2473  ++laneIter;
2474  assert(laneIter == foeBestLanesEnd || *laneIter != 0);
2475  }
2476 
2477  // Look for the junction downstream along foeBestLanes
2478  while (laneIter != foeBestLanesEnd && distToConflictLane <= myRange) {
2479  // Eventual internal lanes were skipped
2480  assert(*laneIter == foeLane || foeLane == 0);
2481  foeLane = *laneIter;
2482  assert(!foeLane->isInternal());
2483  if (&foeLane->getEdge() == &egoConflictLane->getEdge()) {
2484 #ifdef DEBUG_SSM
2485  if (DEBUG_COND(myHolderMS)) {
2486  std::cout << "Found conflict lane for foe: '" << foeLane->getID() << "'" << std::endl;
2487  }
2488 #endif
2489  // found the potential conflict edge along foeBestLanes
2490  return foeLane;
2491  }
2492  // No conflict on foeLane
2493  distToConflictLane += foeLane->getLength();
2494 
2495  // set laneIter to next non internal lane along foeBestLanes
2496  ++laneIter;
2497  if (laneIter == foeBestLanesEnd) {
2498  return nullptr;
2499  }
2500  MSLane* nextNonInternalLane = *laneIter;
2501  MSLink* link = foeLane->getLinkTo(nextNonInternalLane);
2502  // Set foeLane to first internal lane on the next junction
2503  foeLane = link->getViaLane();
2504  assert(foeLane == 0 || foeLane->isInternal());
2505  if (foeLane == nullptr) {
2506  foeLane = nextNonInternalLane;
2507  continue;
2508  }
2509  if (foeLane->getEdge().getToJunction() == conflictJunction) {
2510  assert(foeLane != 0);
2511 #ifdef DEBUG_SSM
2512  if (DEBUG_COND(myHolderMS)) {
2513  std::cout << "Found conflict lane for foe: '" << foeLane->getID() << "'" << std::endl;
2514  }
2515 #endif
2516  // found egoConflictLane, resp. the conflict junction, along foeBestLanes
2517  return foeLane;
2518  }
2519  // No conflict on junction
2520  distToConflictLane += link->getInternalLengthsAfter();
2521  foeLane = nextNonInternalLane;
2522  }
2523  // Didn't find conflicting lane on foeBestLanes within range.
2524  return nullptr;
2525 }
2526 
2527 void
2529 #ifdef DEBUG_SSM
2530  if (DEBUG_COND(myHolderMS)) {
2531  std::cout << "\n" << SIMTIME << " Device '" << getID() << "' flushConflicts past=" << myPastConflicts.size()
2532  << " oldestActive=" << (myOldestActiveEncounterBegin == INVALID ? -1 : myOldestActiveEncounterBegin)
2533  << " topBegin=" << (myPastConflicts.size() > 0 ? myPastConflicts.top()->begin : -1)
2534  << "\n";
2535  }
2536 #endif
2537  while (!myPastConflicts.empty()) {
2538  Encounter* top = myPastConflicts.top();
2539  if (flushAll || top->begin <= myOldestActiveEncounterBegin) {
2540  writeOutConflict(top);
2541  myPastConflicts.pop();
2542  delete top;
2543  } else {
2544  break;
2545  }
2546  }
2547 }
2548 
2549 void
2551  std::string egoID = myHolderMS->getID();
2552 #ifdef DEBUG_SSM
2553  if (DEBUG_COND(myHolderMS))
2554  std::cout << SIMTIME << " flushGlobalMeasures() of vehicle '"
2555  << egoID << "'"
2556  << "'\ntoGeo=" << myUseGeoCoords << std::endl;
2557 #endif
2559  myOutputFile->openTag("globalMeasures");
2560  myOutputFile->writeAttr("ego", egoID);
2562  if (myComputeBR) {
2563  myOutputFile->openTag("BRSpan").writeAttr("values", myBRspan).closeTag();
2564 
2565  if (myMaxBR.second != 0.0) {
2566  if (myUseGeoCoords) {
2567  toGeo(myMaxBR.first.second);
2568  }
2569  myOutputFile->openTag("maxBR").writeAttr("time", myMaxBR.first.first).writeAttr("position", ::toString(myMaxBR.first.second)).writeAttr("value", myMaxBR.second).closeTag();
2570  }
2571  }
2572 
2573  if (myComputeSGAP) {
2575  if (myMinSGAP.second != "") {
2576  if (myUseGeoCoords) {
2577  toGeo(myMinSGAP.first.first.second);
2578  }
2579  myOutputFile->openTag("minSGAP").writeAttr("time", myMinSGAP.first.first.first)
2580  .writeAttr("position", ::toString(myMinSGAP.first.first.second))
2581  .writeAttr("value", myMinSGAP.first.second)
2582  .writeAttr("leader", myMinSGAP.second).closeTag();
2583  }
2584  }
2585 
2586  if (myComputeTGAP) {
2588  if (myMinTGAP.second != "") {
2589  if (myUseGeoCoords) {
2590  toGeo(myMinTGAP.first.first.second);
2591  }
2592  myOutputFile->openTag("minTGAP").writeAttr("time", myMinTGAP.first.first.first)
2593  .writeAttr("position", ::toString(myMinTGAP.first.first.second))
2594  .writeAttr("value", myMinTGAP.first.second)
2595  .writeAttr("leader", myMinTGAP.second).closeTag();
2596  }
2597  }
2598  // close globalMeasures
2600  }
2601 }
2602 
2603 void
2606 }
2607 
2608 void
2610  for (Position& x : xv) {
2611  toGeo(x);
2612  }
2613 }
2614 
2615 void
2617 #ifdef DEBUG_SSM
2618  if (DEBUG_COND(myHolderMS))
2619  std::cout << SIMTIME << " writeOutConflict() of vehicles '"
2620  << e->egoID << "' and '" << e->foeID
2621  << "'\ntoGeo=" << myUseGeoCoords << std::endl;
2622 #endif
2623  myOutputFile->openTag("conflict");
2624  myOutputFile->writeAttr("begin", e->begin).writeAttr("end", e->end);
2625  myOutputFile->writeAttr("ego", e->egoID).writeAttr("foe", e->foeID);
2626 
2627  if (mySaveTrajectories) {
2628  myOutputFile->openTag("timeSpan").writeAttr("values", e->timeSpan).closeTag();
2629  myOutputFile->openTag("typeSpan").writeAttr("values", e->typeSpan).closeTag();
2630 
2631  // Some useful snippets for that (from MSFCDExport.cpp):
2632  if (myUseGeoCoords) {
2633  toGeo(e->egoTrajectory.x);
2634  toGeo(e->foeTrajectory.x);
2636  }
2637 
2639  myOutputFile->openTag("egoVelocity").writeAttr("values", ::toString(e->egoTrajectory.v)).closeTag();
2640 
2642  myOutputFile->openTag("foeVelocity").writeAttr("values", ::toString(e->foeTrajectory.v)).closeTag();
2643 
2645  }
2646 
2647  if (myComputeTTC) {
2648  if (mySaveTrajectories) {
2649  myOutputFile->openTag("TTCSpan").writeAttr("values", makeStringWithNAs(e->TTCspan, INVALID)).closeTag();
2650  }
2651  if (e->minTTC.time == INVALID) {
2652  myOutputFile->openTag("minTTC").writeAttr("time", "NA").writeAttr("position", "NA").writeAttr("type", "NA").writeAttr("value", "NA").closeTag();
2653  } else {
2654  std::string time = ::toString(e->minTTC.time);
2655  std::string type = ::toString(int(e->minTTC.type));
2656  std::string value = ::toString(e->minTTC.value);
2657  if (myUseGeoCoords) {
2658  toGeo(e->minTTC.pos);
2659  }
2660  std::string position = ::toString(e->minTTC.pos, myUseGeoCoords ? gPrecisionGeo : gPrecision);
2661  myOutputFile->openTag("minTTC").writeAttr("time", time).writeAttr("position", position).writeAttr("type", type).writeAttr("value", value).closeTag();
2662  }
2663  }
2664  if (myComputeDRAC) {
2665  if (mySaveTrajectories) {
2666  myOutputFile->openTag("DRACSpan").writeAttr("values", makeStringWithNAs(e->DRACspan, {0.0, INVALID})).closeTag();
2667  }
2668  if (e->maxDRAC.time == INVALID) {
2669  myOutputFile->openTag("maxDRAC").writeAttr("time", "NA").writeAttr("position", "NA").writeAttr("type", "NA").writeAttr("value", "NA").closeTag();
2670  } else {
2671  std::string time = ::toString(e->maxDRAC.time);
2672  std::string type = ::toString(int(e->maxDRAC.type));
2673  std::string value = ::toString(e->maxDRAC.value);
2674  if (myUseGeoCoords) {
2675  toGeo(e->maxDRAC.pos);
2676  }
2677  std::string position = ::toString(e->maxDRAC.pos, myUseGeoCoords ? gPrecisionGeo : gPrecision);
2678  myOutputFile->openTag("maxDRAC").writeAttr("time", time).writeAttr("position", position).writeAttr("type", type).writeAttr("value", value).closeTag();
2679  }
2680  }
2681  if (myComputePET) {
2682  if (e->PET.time == INVALID) {
2683  myOutputFile->openTag("PET").writeAttr("time", "NA").writeAttr("position", "NA").writeAttr("type", "NA").writeAttr("value", "NA").closeTag();
2684  } else {
2685  std::string time = ::toString(e->PET.time);
2686  std::string type = ::toString(int(e->PET.type));
2687  std::string value = ::toString(e->PET.value);
2688  if (myUseGeoCoords) {
2689  toGeo(e->PET.pos);
2690  }
2691  std::string position = ::toString(e->PET.pos, myUseGeoCoords ? gPrecisionGeo : gPrecision);
2692  myOutputFile->openTag("PET").writeAttr("time", time).writeAttr("position", position).writeAttr("type", type).writeAttr("value", value).closeTag();
2693  }
2694  }
2696 }
2697 
2698 std::string
2699 MSDevice_SSM::makeStringWithNAs(std::vector<double> v, double NA, std::string sep) {
2700  std::string res = "";
2701  for (std::vector<double>::const_iterator i = v.begin(); i != v.end(); ++i) {
2702  res += (i == v.begin() ? "" : sep) + (*i == NA ? "NA" : ::toString(*i));
2703  }
2704  return res;
2705 }
2706 
2707 std::string
2708 MSDevice_SSM::makeStringWithNAs(std::vector<double> v, std::vector<double> NAs, std::string sep) {
2709  std::string res = "";
2710  for (std::vector<double>::const_iterator i = v.begin(); i != v.end(); ++i) {
2711  res += (i == v.begin() ? "" : sep) + (find(NAs.begin(), NAs.end(), *i) != NAs.end() ? "NA" : ::toString(*i));
2712  }
2713  return res;
2714 }
2715 
2716 
2717 // ---------------------------------------------------------------------------
2718 // MSDevice_SSM-methods
2719 // ---------------------------------------------------------------------------
2720 MSDevice_SSM::MSDevice_SSM(SUMOVehicle& holder, const std::string& id, std::string outputFilename, std::map<std::string, double> thresholds,
2721  bool trajectories, double range, double extraTime, bool useGeoCoords) :
2722  MSVehicleDevice(holder, id),
2723  myThresholds(thresholds),
2724  mySaveTrajectories(trajectories),
2725  myRange(range),
2726  myExtraTime(extraTime),
2729  myMaxBR(std::make_pair(-1, Position(0., 0.)), 0.0),
2730  myMinSGAP(std::make_pair(std::make_pair(-1, Position(0., 0.)), std::numeric_limits<double>::max()), ""),
2731  myMinTGAP(std::make_pair(std::make_pair(-1, Position(0., 0.)), std::numeric_limits<double>::max()), "") {
2732  // Take care! Holder is currently being constructed. Cast occurs before completion.
2733  myHolderMS = static_cast<MSVehicle*>(&holder);
2734 
2735  myComputeTTC = myThresholds.find("TTC") != myThresholds.end();
2736  myComputeDRAC = myThresholds.find("DRAC") != myThresholds.end();
2737  myComputePET = myThresholds.find("PET") != myThresholds.end();
2738 
2739  myComputeBR = myThresholds.find("BR") != myThresholds.end();
2740  myComputeSGAP = myThresholds.find("SGAP") != myThresholds.end();
2741  myComputeTGAP = myThresholds.find("TGAP") != myThresholds.end();
2742 
2745 
2746  // XXX: Who deletes the OutputDevice?
2747  myOutputFile = &OutputDevice::getDevice(outputFilename);
2748 // TODO: make xsd, include header
2749 // myOutputFile.writeXMLHeader("SSMLog", "SSMLog.xsd");
2750  if (createdOutputFiles.count(outputFilename) == 0) {
2751  myOutputFile->writeXMLHeader("SSMLog", "");
2752  createdOutputFiles.insert(outputFilename);
2753  }
2754  // register at static instance container
2755  myInstances->insert(this);
2756 
2757 #ifdef DEBUG_SSM
2758  if (DEBUG_COND(myHolderMS)) {
2759  std::vector<std::string> measures;
2760  std::vector<double> threshVals;
2761  for (std::map<std::string, double>::const_iterator i = myThresholds.begin(); i != myThresholds.end(); ++i) {
2762  measures.push_back(i->first);
2763  threshVals.push_back(i->second);
2764  }
2765  std::cout << "Initialized ssm device '" << id << "' with "
2766  << "myMeasures=" << joinToString(measures, " ")
2767  << ", myThresholds=" << joinToString(threshVals, " ")
2768  << ", mySaveTrajectories=" << mySaveTrajectories
2769  << ", myRange=" << myRange << ", output file=" << outputFilename << ", extra time=" << myExtraTime << ", useGeo=" << myUseGeoCoords << "\n";
2770  }
2771 #endif
2772 }
2773 
2776  // Deleted in ~BaseVehicle()
2777  // unregister from static instance container
2778  myInstances->erase(this);
2779  resetEncounters();
2780  flushConflicts(true);
2782 }
2783 
2784 
2785 bool
2787  assert(veh.isVehicle());
2788 #ifdef DEBUG_SSM_NOTIFICATIONS
2789  MSBaseVehicle* v = (MSBaseVehicle*) &veh;
2790  std::cout << "device '" << getID() << "' notifyEnter: reason=" << reason << " currentEdge=" << v->getLane()->getEdge().getID() << "\n";
2791 #else
2792  UNUSED_PARAMETER(veh);
2793  UNUSED_PARAMETER(reason);
2794 #endif
2795  return true; // keep the device
2796 }
2797 
2798 bool
2800  MSMoveReminder::Notification reason, const MSLane* /* enteredLane */) {
2801  assert(veh.isVehicle());
2802 #ifdef DEBUG_SSM_NOTIFICATIONS
2803  MSBaseVehicle* v = (MSBaseVehicle*) &veh;
2804  std::cout << "device '" << getID() << "' notifyLeave: reason=" << reason << " currentEdge=" << v->getLane()->getEdge().getID() << "\n";
2805 #else
2806  UNUSED_PARAMETER(veh);
2807  UNUSED_PARAMETER(reason);
2808 #endif
2809  return true; // keep the device
2810 }
2811 
2812 bool
2813 MSDevice_SSM::notifyMove(SUMOTrafficObject& /* veh */, double /* oldPos */,
2814  double /* newPos */, double newSpeed) {
2815 #ifdef DEBUG_SSM_NOTIFICATIONS
2816  std::cout << "device '" << getID() << "' notifyMove: newSpeed=" << newSpeed << "\n";
2817 #else
2818  UNUSED_PARAMETER(newSpeed);
2819 #endif
2820  return true; // keep the device
2821 }
2822 
2823 
2824 void
2825 MSDevice_SSM::findSurroundingVehicles(const MSVehicle& veh, double range, FoeInfoMap& foeCollector) {
2826  if (!veh.isOnRoad()) {
2827  return;
2828  }
2829 #ifdef DEBUG_SSM_SURROUNDING
2830 
2832  if (gDebugFlag3) {
2833  std::cout << SIMTIME << " Looking for surrounding vehicles for ego vehicle '" << veh.getID()
2834  << "' on edge '" << veh.getLane()->getEdge().getID()
2835  << "'."
2836  << "\nVehicle's best lanes = " << ::toString(veh.getBestLanesContinuation())
2837  << std::endl;
2838  }
2839 #endif
2840 
2841 
2842  // The requesting vehicle's current route
2843  // XXX: Restriction to route scanning may have to be generalized to scanning of possible continuations when
2844  // considering situations involving sudden route changes. See also the definition of the EncounterTypes.
2845  // A second problem is that following situations on deviating routes may result in closing encounters
2846  // too early if a leading foe is not traced on its new lane. (see test 'foe_leader_deviating_routes')
2847 
2848  // If veh is on an internal edge, the edgeIter points towards the last edge before the junction
2849  //ConstMSEdgeVector::const_iterator edgeIter = veh.getCurrentRouteEdge();
2850  //assert(*edgeIter != 0);
2851 
2852  // Best continuation lanes for the ego vehicle
2853  std::vector<MSLane*> egoBestLanes = veh.getBestLanesContinuation();
2854  const bool isOpposite = veh.getLaneChangeModel().isOpposite();
2855  if (isOpposite) {
2856  for (int i = 0; i < (int)egoBestLanes.size(); i++) {
2857  if (egoBestLanes[i] != nullptr && egoBestLanes[i]->getEdge().getOppositeEdge() != nullptr) {
2858  egoBestLanes[i] = egoBestLanes[i]->getEdge().getOppositeEdge()->getLanes().back();
2859  }
2860  }
2861  }
2862  std::vector<MSLane*>::const_iterator laneIter = egoBestLanes.begin();
2863 
2864  // current lane in loop below
2865  const MSLane* lane = veh.getLane();
2866  const MSEdge* egoEdge = &(lane->getEdge());
2867  assert(lane->isInternal() || lane == *laneIter);
2868  assert(lane != 0);
2869  // next non-internal lane on the route
2870  const MSLane* nextNonInternalLane = nullptr;
2871 
2872  const MSEdge* edge; // current edge in loop below
2873 
2874  // Init pos with vehicle's current position. Below pos is set to zero to denote
2875  // the beginning position of the currently considered edge
2876  double pos = veh.getPositionOnLane();
2877  // remainingDownstreamRange is the range minus the distance that is already scanned downstream along the vehicles route
2878  double remainingDownstreamRange = range;
2879  // 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)
2880  double distToConflictLane = isOpposite ? pos - veh.getLane()->getLength() : -pos;
2881 
2882  // remember already visited lanes (no matter whether internal or not) and junctions downstream along the route
2883  std::set<const MSLane*> seenLanes;
2884  std::set<const MSJunction*> routeJunctions;
2885 
2886  // Starting points for upstream scans to be executed after downstream scan is complete.
2887  // Holds pairs (starting edge, starting position on edge)
2888  std::vector<UpstreamScanStartInfo> upstreamScanStartPositions;
2889 
2890 
2891  // if the current edge is internal, collect all vehicles from the junction and within upstream range (except on the vehicles own edge),
2892  // this is analogous to the code treating junctions in the loop below. Note that the distance on the junction itself is not included into
2893  // range, so vehicles farther away than range can be collected, too.
2894  if (lane->isInternal()) {
2895  edge = &(lane->getEdge());
2896 
2897 #ifdef DEBUG_SSM_SURROUNDING
2898  if (gDebugFlag3) {
2899  std::cout << SIMTIME << " Vehicle '" << veh.getID() << "' is on internal edge " << edge->getID() << "'." << std::endl;
2900 // << "Previous edge of its route: '" << (*edgeIter)->getID() << "'" << std::endl;
2901  }
2902 #endif
2903 
2904  assert(edge->getToJunction() == edge->getFromJunction());
2905 
2906  const MSJunction* junction = edge->getToJunction();
2907  // Collect vehicles on the junction
2908  getVehiclesOnJunction(junction, lane, distToConflictLane, lane, foeCollector, seenLanes);
2909  routeJunctions.insert(junction);
2910 
2911  // Collect vehicles on incoming edges.
2912  // Note that this includes the previous edge on the ego vehicle's route.
2913  // (The distance on the current internal edge is ignored)
2914  const ConstMSEdgeVector& incoming = junction->getIncoming();
2915  for (ConstMSEdgeVector::const_iterator ei = incoming.begin(); ei != incoming.end(); ++ei) {
2916  if ((*ei)->isInternal()) {
2917  continue;
2918  }
2919  // Upstream range is taken from the vehicle's back
2920  upstreamScanStartPositions.push_back(UpstreamScanStartInfo(*ei, (*ei)->getLength(), range + veh.getLength(), distToConflictLane, lane));
2921  }
2922 
2923 // // Take into account internal distance covered on the current lane
2924 // (commented out, because upstream scanning disregards internal lanes on the last scanned junction
2925 // -- this makes the scanning symmetric between leader and follower)
2926 // remainingDownstreamRange -= lane->getLength() - pos;
2927 
2928  // Take into account non-internal lengths until next non-internal lane
2929  MSLink* link = lane->getLinkCont()[0];
2930  remainingDownstreamRange -= link->getInternalLengthsAfter();
2931  distToConflictLane += lane->getLength() + link->getInternalLengthsAfter();
2932 
2933  // The next non-internal lane
2934  pos = 0.;
2935  lane = *(++laneIter);
2936  edge = &lane->getEdge();
2937  } else {
2938  // Collect all vehicles in range behind ego vehicle
2939  edge = &(lane->getEdge());
2940  double edgeLength = edge->getLength();
2941  double startScanPos = std::min(pos + remainingDownstreamRange, edgeLength);
2942  upstreamScanStartPositions.push_back(UpstreamScanStartInfo(edge, startScanPos, std::max(0., startScanPos - pos + range + veh.getLength()), distToConflictLane, lane));
2943  }
2944 
2945  assert(lane != 0);
2946  assert(!lane->isInternal());
2947 
2948  // Advance downstream the ego vehicle's route for distance 'range'.
2949  // Collect all vehicles on the traversed Edges and on incoming edges at junctions
2950  // and starting points for upstream vehicle collection strated below after downstream scan.
2951  while (remainingDownstreamRange > 0.) {
2952 
2953 #ifdef DEBUG_SSM_SURROUNDING
2954  if (gDebugFlag3) {
2955  std::cout << SIMTIME << " Scanning downstream for vehicle '" << veh.getID() << "' on lane '" << veh.getLane()->getID() << "', position=" << pos << ".\n"
2956  << "Considering edge '" << edge->getID() << "' Remaining downstream range = " << remainingDownstreamRange
2957  << "\nbestLanes=" << ::toString(egoBestLanes) << "\n"
2958  << std::endl;
2959  }
2960 #endif
2961  assert(!edge->isInternal());
2962  assert(!lane->isInternal());
2963  assert(pos == 0 || lane == veh.getLane());
2964  if (pos + remainingDownstreamRange < lane->getLength()) {
2965  // scan range ends on this lane
2966  if (edge->getID() != egoEdge->getID()) {
2967  upstreamScanStartPositions.push_back(UpstreamScanStartInfo(edge, pos + remainingDownstreamRange, remainingDownstreamRange, distToConflictLane, lane));
2968  }
2969  // scanned required downstream range
2970  break;
2971  } else {
2972  // Also need to scan area that reaches beyond the lane
2973  // Collecting vehicles on non-internal edge ahead
2974  if (edge->getID() != egoEdge->getID()) {
2975  upstreamScanStartPositions.push_back(UpstreamScanStartInfo(edge, edge->getLength(), edge->getLength() - pos, distToConflictLane, lane));
2976  }
2977  // account for scanned distance on lane
2978  remainingDownstreamRange -= lane->getLength() - pos;
2979  distToConflictLane += lane->getLength();
2980  pos = 0.;
2981 
2982  // proceed to next non-internal lane
2983  ++laneIter;
2984  assert(laneIter == egoBestLanes.end() || *laneIter != 0);
2985 
2986  // If the vehicle's best lanes go on, collect vehicles on the upcoming junction
2987  if (laneIter != egoBestLanes.end()) {
2988  // Upcoming junction
2989  const MSJunction* junction;
2990  if (isOpposite) {
2991  junction = lane->getOpposite()->getEdge().getToJunction();
2992  } else {
2993  junction = lane->getEdge().getToJunction();
2994  }
2995 
2996 
2997  // Find connection for ego on the junction
2998  nextNonInternalLane = *laneIter;
2999  MSLink* link = lane->getLinkTo(nextNonInternalLane);
3000  if (isOpposite && link == nullptr) {
3001  link = nextNonInternalLane->getLinkTo(lane);
3002  if (link == nullptr) {
3003  link = lane->getOpposite()->getLinkTo(nextNonInternalLane);
3004  }
3005  }
3006  assert(link != 0 || link->getLength() == 0.);
3007 
3008  // First lane of the connection
3009  lane = link->getViaLane();
3010  if (lane == nullptr) {
3011  // link without internal lane
3012  lane = nextNonInternalLane;
3013  edge = &(lane->getEdge());
3014  if (seenLanes.count(lane) == 0) {
3015  seenLanes.insert(lane);
3016  continue;
3017  } else {
3018  break;
3019  }
3020  }
3021 
3022  if (seenLanes.count(lane) == 0) {
3023  // Collect vehicles on the junction, if it wasn't considered already
3024  getVehiclesOnJunction(junction, lane, distToConflictLane, lane, foeCollector, seenLanes);
3025  routeJunctions.insert(junction);
3026 
3027  // Collect vehicles on incoming edges (except the last edge, where we already collected). Use full range.
3028  if (isOpposite) {
3029  const ConstMSEdgeVector& outgoing = junction->getOutgoing();
3030  for (ConstMSEdgeVector::const_iterator ei = outgoing.begin(); ei != outgoing.end(); ++ei) {
3031  if (*ei == edge || (*ei)->isInternal()) {
3032  continue;
3033  }
3034  upstreamScanStartPositions.push_back(UpstreamScanStartInfo(*ei, (*ei)->getLength(), range, distToConflictLane, lane));
3035  }
3036  } else {
3037  const ConstMSEdgeVector& incoming = junction->getIncoming();
3038  for (ConstMSEdgeVector::const_iterator ei = incoming.begin(); ei != incoming.end(); ++ei) {
3039  if (*ei == edge || (*ei)->isInternal()) {
3040  continue;
3041  }
3042  upstreamScanStartPositions.push_back(UpstreamScanStartInfo(*ei, (*ei)->getLength(), range, distToConflictLane, lane));
3043  }
3044  }
3045 
3046  // account for scanned distance on junction
3047  double linkLength = link->getInternalLengthsAfter();
3048  remainingDownstreamRange -= linkLength;
3049  distToConflictLane += linkLength;
3050 #ifdef DEBUG_SSM_SURROUNDING
3051  if (gDebugFlag3) {
3052  std::cout << " Downstream Scan for vehicle '" << veh.getID() << "' proceeded over junction '" << junction->getID()
3053  << "',\n linkLength=" << linkLength << ", remainingDownstreamRange=" << remainingDownstreamRange
3054  << std::endl;
3055  }
3056 #endif
3057 
3058  // update ego's lane to next non internal edge
3059  lane = nextNonInternalLane;
3060  edge = &(lane->getEdge());
3061  } else {
3062 #ifdef DEBUG_SSM_SURROUNDING
3063  if (gDebugFlag3) {
3064  std::cout << " Downstream Scan for vehicle '" << veh.getID() << "' stops at lane '" << lane->getID()
3065  << "', which has already been scanned."
3066  << std::endl;
3067  }
3068 #endif
3069  break;
3070  }
3071  } else {
3072  // Further vehicle path unknown, break search
3073  break;
3074  }
3075  }
3076  }
3077  // add junction from the end of the route
3078  routeJunctions.insert(lane->getEdge().getToJunction());
3079 
3080 
3081  // Scan upstream branches from collected starting points
3082  for (UpstreamScanStartInfo& i : upstreamScanStartPositions) {
3083  getUpstreamVehicles(i, foeCollector, seenLanes, routeJunctions);
3084  }
3085 
3086 #ifdef DEBUG_SSM_SURROUNDING
3087  if (gDebugFlag3) {
3088  for (std::pair<const MSVehicle*, FoeInfo*> foeInfo : foeCollector) {
3089  std::cout << " foe " << foeInfo.first->getID() << " conflict at " << foeInfo.second->egoConflictLane->getID() << " egoDist " << foeInfo.second->egoDistToConflictLane << std::endl;
3090  }
3091  }
3092 #endif
3093 
3094  // remove ego vehicle
3095  foeCollector.erase(&veh);
3096  gDebugFlag3 = false;
3097 }
3098 
3099 void
3100 MSDevice_SSM::getUpstreamVehicles(const UpstreamScanStartInfo& scanStart, FoeInfoMap& foeCollector, std::set<const MSLane*>& seenLanes, const std::set<const MSJunction*>& routeJunctions) {
3101 #ifdef DEBUG_SSM_SURROUNDING
3102  if (gDebugFlag3) {
3103  std::cout << SIMTIME << " getUpstreamVehicles() for edge '" << scanStart.edge->getID() << "'"
3104  << " pos = " << scanStart.pos << " range = " << scanStart.range
3105  << std::endl;
3106  }
3107 #endif
3108  if (scanStart.range <= 0) {
3109  return;
3110  }
3111 
3112  const std::vector<MSLane*>& lanes = scanStart.edge->getLanes();
3113  // Collect vehicles on the given edge with position in [pos-range,pos]
3114  for (MSLane* lane : lanes) {
3115  if (seenLanes.find(lane) != seenLanes.end()) {
3116  return;
3117  }
3118  int foundCount = 0;
3119 
3120  const MSLane::VehCont& vehicles = lane->getVehiclesSecure();
3121  for (MSLane::VehCont::const_iterator vi = vehicles.begin(); vi != vehicles.end(); ++vi) {
3122 
3123  MSVehicle* veh = *vi;
3124  if (foeCollector.find(veh) != foeCollector.end()) {
3125  // vehicle already recognized, earlier recognized conflict has priority
3126  continue;
3127  }
3128  if (veh->getPositionOnLane() - veh->getLength() <= scanStart.pos && veh->getPositionOnLane() >= scanStart.pos - scanStart.range) {
3129 #ifdef DEBUG_SSM_SURROUNDING
3130  if (gDebugFlag3) {
3131  std::cout << "\t" << veh->getID() << "\n";
3132  }
3133 #endif
3134  FoeInfo* c = new FoeInfo(); // c is deleted in updateEncounter()
3136  c->egoConflictLane = scanStart.egoConflictLane;
3137  foeCollector[veh] = c;
3138  foundCount++;
3139  }
3140  }
3141  lane->releaseVehicles();
3142 
3143 #ifdef DEBUG_SSM_SURROUNDING
3144  if (gDebugFlag3 && foundCount > 0) {
3145  std::cout << "\t" << lane->getID() << ": Found " << foundCount << "\n";
3146  }
3147 #endif
3148  seenLanes.insert(lane);
3149  }
3150 
3151 #ifdef DEBUG_SSM_SURROUNDING
3152  if (gDebugFlag3) {
3153  std::cout << std::endl;
3154  }
3155 #endif
3156 
3157  // TODO: Gather vehicles from opposite direction. This should happen in any case, where opposite direction overtaking is possible.
3158  // If it isn't it might still be nicer to trace oncoming vehicles for the resulting trajectories in the encounters
3159  // if (edge->hasOpposite...)
3160 
3161  if (scanStart.range <= scanStart.pos) {
3162  return;
3163  }
3164 
3165  // Here we have: range > pos, i.e. we proceed collecting vehicles on preceding edges
3166  double remainingRange = scanStart.range - scanStart.pos;
3167 
3168  // Junction representing the origin of 'edge'
3169  const MSJunction* junction = scanStart.edge->getFromJunction();
3170 
3171  // stop if upstream search reaches the ego route
3172  if (routeJunctions.find(junction) != routeJunctions.end()) {
3173  return;
3174  }
3175 
3176  // Collect vehicles from incoming edges of the junction
3177  int incomingEdgeCount = 0;
3178  if (!scanStart.edge->isInternal()) {
3179  // collect vehicles on preceding junction (for internal edges this is already done in caller,
3180  // i.e. findSurroundingVehicles() or the recursive call from getUpstreamVehicles())
3181 
3182  // Collect vehicles on the junction, if it wasn't considered already
3183  // run vehicle collection for all incoming connections
3184  const std::vector<MSLane*> internalLanes = junction->getInternalLanes();
3185  for (MSLane* internalLane : internalLanes) {
3186  if (internalLane->getEdge().getSuccessors()[0]->getID() == scanStart.edge->getID()) {
3187  getVehiclesOnJunction(junction, internalLane, scanStart.egoDistToConflictLane, scanStart.egoConflictLane, foeCollector, seenLanes);
3188  incomingEdgeCount++;
3189  }
3190  }
3191  }
3192  // Collect vehicles from incoming edges from the junction representing the origin of 'edge'
3193  if (incomingEdgeCount > 0) {
3194  const ConstMSEdgeVector& incoming = junction->getIncoming();
3195  for (ConstMSEdgeVector::const_iterator ei = incoming.begin(); ei != incoming.end(); ++ei) {
3196  if ((*ei)->isInternal() || (*ei)->isCrossing()) {
3197  continue;
3198  }
3199  const std::vector<MSLane*> lanes = (*ei)->getLanes();
3200  bool skip = false;
3201  for (MSLane* lane : lanes) {
3202  if (seenLanes.find(lane) != seenLanes.end()) {
3203  skip = true;
3204  break;
3205  }
3206  }
3207  if (skip) {
3208 #ifdef DEBUG_SSM_SURROUNDING
3209  //if (gDebugFlag3) std::cout << "Scan skips already seen edge " << (*ei)->getID() << "\n";
3210 #endif
3211  continue;
3212  }
3213 
3214  const MSEdge* inEdge = *ei;
3215  assert(inEdge != 0);
3216  double distOnJunction = scanStart.edge->isInternal() ? 0. : inEdge->getInternalFollowingLengthTo(scanStart.edge);
3217  if (distOnJunction >= remainingRange) {
3218 #ifdef DEBUG_SSM_SURROUNDING
3219  //if (gDebugFlag3) std::cout << "Scan stops on junction (between " << inEdge->getID() << " and " << scanStart.edge->getID() << ") at rel. dist " << distOnJunction << "\n";
3220 #endif
3221  continue;
3222  }
3223  // account for vehicles on the predecessor edge
3224  UpstreamScanStartInfo nextInfo(inEdge, inEdge->getLength(), remainingRange - distOnJunction, scanStart.egoDistToConflictLane, scanStart.egoConflictLane);
3225  getUpstreamVehicles(nextInfo, foeCollector, seenLanes, routeJunctions);
3226  }
3227  }
3228 }
3229 
3230 void
3231 MSDevice_SSM::getVehiclesOnJunction(const MSJunction* junction, const MSLane* const egoJunctionLane, double egoDistToConflictLane, const MSLane* const egoConflictLane, FoeInfoMap& foeCollector, std::set<const MSLane*>& seenLanes) {
3232 #ifdef DEBUG_SSM_SURROUNDING
3233  if (gDebugFlag3) {
3234  std::cout << SIMTIME << " getVehiclesOnJunction() for junction '" << junction->getID() << "'"
3235  << "\nFound vehicles:"
3236  << std::endl;
3237  }
3238 #endif
3239  // FoeInfo creation
3240  auto collectFoeInfos = [&](const MSLane::VehCont & vehicles) {
3241  for (MSVehicle* veh : vehicles) {
3242  if (foeCollector.find(veh) != foeCollector.end()) {
3243  delete foeCollector[veh];
3244  }
3245  FoeInfo* c = new FoeInfo();
3246  c->egoConflictLane = egoConflictLane;
3247  c->egoDistToConflictLane = egoDistToConflictLane;
3248  foeCollector[veh] = c;
3249 #ifdef DEBUG_SSM_SURROUNDING
3250  if (gDebugFlag3) {
3251  for (MSVehicle* veh : vehicles) {
3252  std::cout << "\t" << veh->getID() << "\n";
3253  }
3254  }
3255 #endif
3256  }
3257  };
3258 
3259  // stop condition
3260  if (seenLanes.find(egoJunctionLane) != seenLanes.end() || egoJunctionLane->getEdge().isCrossing()) {
3261  return;
3262  }
3263 
3264  auto scanInternalLane = [&](const MSLane * lane) {
3265  const MSLane::VehCont& vehicles = lane->getVehiclesSecure();
3266 
3267  // Add FoeInfos (XXX: for some situations, a vehicle may be collected twice. Then the later finding overwrites the earlier in foeCollector.
3268  // This could lead to neglecting a conflict when determining foeConflictLane later.) -> TODO: test with twice intersecting routes
3269  collectFoeInfos(vehicles);
3270 
3271  lane->releaseVehicles();
3272 
3273  // check additional internal link upstream in the same junction
3274  // TODO: getEntryLink returns nullptr
3275  if (lane->getCanonicalPredecessorLane()->isInternal()) {
3276  lane = lane->getCanonicalPredecessorLane();
3277 
3278  // This code must be modified, if more than two-piece internal lanes are allowed. Thus, assert:
3279  assert(!lane->getEntryLink()->fromInternalLane());
3280 
3281  // collect vehicles
3282  const MSLane::VehCont& vehicles2 = lane->getVehiclesSecure();
3283  // Add FoeInfos for the first internal lane
3284  collectFoeInfos(vehicles2);
3285  lane->releaseVehicles();
3286  }
3287 
3288 
3289  // If there is an internal continuation lane, also collect vehicles on that lane
3290  if (lane->getLinkCont().size() > 1 && lane->getLinkCont()[0]->getViaLane() != nullptr) {
3291  // There's a second internal lane of the connection
3292  lane = lane->getLinkCont()[0]->getViaLane();
3293  // This code must be modified, if more than two-piece internal lanes are allowed. Thus, assert:
3294  assert(lane->getLinkCont().size() == 0 || lane->getLinkCont()[0]->getViaLane() == 0);
3295 
3296  // collect vehicles
3297  const MSLane::VehCont& vehicles2 = lane->getVehiclesSecure();
3298  // Add FoeInfos for the first internal lane
3299  collectFoeInfos(vehicles2);
3300  lane->releaseVehicles();
3301  }
3302 
3303  };
3304 
3305  // Collect vehicles on conflicting lanes
3306  const MSLink* entryLink = egoJunctionLane->getEntryLink();
3307  if (entryLink->getFoeLanes().size() > 0) {
3308 
3309  const std::vector<MSLane*> foeLanes = junction->getFoeInternalLanes(entryLink);
3310  for (MSLane* lane : foeLanes) {
3311  if (seenLanes.find(lane) != seenLanes.end()) {
3312  continue;
3313  }
3314  scanInternalLane(lane);
3315  seenLanes.insert(lane);
3316  }
3317  }
3318  scanInternalLane(egoJunctionLane);
3319 
3320 #ifdef DEBUG_SSM_SURROUNDING
3321  if (gDebugFlag3) {
3322  std::cout << std::endl;
3323  }
3324 #endif
3325 }
3326 
3327 
3328 
3329 void
3331  // This is called once at vehicle removal.
3332  // Also: flush myOutputFile? Or is this done automatically?
3333  // myOutputFile->closeTag();
3334 }
3335 
3336 // ---------------------------------------------------------------------------
3337 // Static parameter load helpers
3338 // ---------------------------------------------------------------------------
3339 std::string
3340 MSDevice_SSM::getOutputFilename(const SUMOVehicle& v, std::string deviceID) {
3342  std::string file = deviceID + ".xml";
3343  if (v.getParameter().knowsParameter("device.ssm.file")) {
3344  try {
3345  file = v.getParameter().getParameter("device.ssm.file", file);
3346  } catch (...) {
3347  WRITE_WARNING("Invalid value '" + v.getParameter().getParameter("device.ssm.file", file) + "'for vehicle parameter 'ssm.measures'");
3348  }
3349  } else if (v.getVehicleType().getParameter().knowsParameter("device.ssm.file")) {
3350  try {
3351  file = v.getVehicleType().getParameter().getParameter("device.ssm.file", file);
3352  } catch (...) {
3353  WRITE_WARNING("Invalid value '" + v.getVehicleType().getParameter().getParameter("device.ssm.file", file) + "'for vType parameter 'ssm.measures'");
3354  }
3355  } else {
3356  file = oc.getString("device.ssm.file") == "" ? file : oc.getString("device.ssm.file");
3357  if (!oc.isSet("device.ssm.file") && (issuedParameterWarnFlags & SSM_WARN_FILE) == 0) {
3358  std::cout << "vehicle '" << v.getID() << "' does not supply vehicle parameter 'device.ssm.file'. Using default of '" << file << "'\n";
3360  }
3361  }
3362  return file;
3363 }
3364 
3365 bool
3368  bool useGeo = false;
3369  if (v.getParameter().knowsParameter("device.ssm.geo")) {
3370  try {
3371  useGeo = StringUtils::toBool(v.getParameter().getParameter("device.ssm.geo", "no"));
3372  } catch (...) {
3373  WRITE_WARNING("Invalid value '" + v.getParameter().getParameter("device.ssm.geo", "no") + "'for vehicle parameter 'ssm.geo'");
3374  }
3375  } else if (v.getVehicleType().getParameter().knowsParameter("device.ssm.geo")) {
3376  try {
3377  useGeo = StringUtils::toBool(v.getVehicleType().getParameter().getParameter("device.ssm.geo", "no"));
3378  } catch (...) {
3379  WRITE_WARNING("Invalid value '" + v.getVehicleType().getParameter().getParameter("device.ssm.geo", "no") + "'for vType parameter 'ssm.geo'");
3380  }
3381  } else {
3382  useGeo = oc.getBool("device.ssm.geo");
3383  if (!oc.isSet("device.ssm.geo") && (issuedParameterWarnFlags & SSM_WARN_GEO) == 0) {
3384  std::cout << "vehicle '" << v.getID() << "' does not supply vehicle parameter 'device.ssm.geo'. Using default of '" << ::toString(useGeo) << "'\n";
3386  }
3387  }
3388  return useGeo;
3389 }
3390 
3391 
3392 double
3395  double range = -INVALID;
3396  if (v.getParameter().knowsParameter("device.ssm.range")) {
3397  try {
3398  range = StringUtils::toDouble(v.getParameter().getParameter("device.ssm.range", ""));
3399  } catch (...) {
3400  WRITE_WARNING("Invalid value '" + v.getParameter().getParameter("device.ssm.range", "") + "'for vehicle parameter 'ssm.range'");
3401  }
3402  } else if (v.getVehicleType().getParameter().knowsParameter("device.ssm.range")) {
3403  try {
3404  range = StringUtils::toDouble(v.getVehicleType().getParameter().getParameter("device.ssm.range", ""));
3405  } catch (...) {
3406  WRITE_WARNING("Invalid value '" + v.getVehicleType().getParameter().getParameter("device.ssm.range", "") + "'for vType parameter 'ssm.range'");
3407  }
3408  } else {
3409  range = oc.getFloat("device.ssm.range");
3410  if (!oc.isSet("device.ssm.range") && (issuedParameterWarnFlags & SSM_WARN_RANGE) == 0) {
3411  std::cout << "vehicle '" << v.getID() << "' does not supply vehicle parameter 'device.ssm.range'. Using default of '" << range << "'\n";
3413  }
3414  }
3415  return range;
3416 }
3417 
3418 
3419 double
3422  double extraTime = INVALID;
3423  if (v.getParameter().knowsParameter("device.ssm.extratime")) {
3424  try {
3425  extraTime = StringUtils::toDouble(v.getParameter().getParameter("device.ssm.extratime", ""));
3426  } catch (...) {
3427  WRITE_WARNING("Invalid value '" + v.getParameter().getParameter("device.ssm.extratime", "") + "'for vehicle parameter 'ssm.extratime'");
3428  }
3429  } else if (v.getVehicleType().getParameter().knowsParameter("device.ssm.extratime")) {
3430  try {
3431  extraTime = StringUtils::toDouble(v.getVehicleType().getParameter().getParameter("device.ssm.extratime", ""));
3432  } catch (...) {
3433  WRITE_WARNING("Invalid value '" + v.getVehicleType().getParameter().getParameter("device.ssm.extratime", "") + "'for vType parameter 'ssm.extratime'");
3434  }
3435  } else {
3436  extraTime = oc.getFloat("device.ssm.extratime");
3437  if (!oc.isSet("device.ssm.extratime") && (issuedParameterWarnFlags & SSM_WARN_EXTRATIME) == 0) {
3438  std::cout << "vehicle '" << v.getID() << "' does not supply vehicle parameter 'device.ssm.extratime'. Using default of '" << extraTime << "'\n";
3440  }
3441  }
3442  if (extraTime < 0.) {
3443  extraTime = DEFAULT_EXTRA_TIME;
3444  WRITE_WARNING("Negative (or no) value encountered for vehicle parameter 'device.ssm.extratime' in vehicle '" + v.getID() + "' using default value " + ::toString(extraTime) + " instead");
3445  }
3446  return extraTime;
3447 }
3448 
3449 
3450 bool
3453  bool trajectories = false;
3454  if (v.getParameter().knowsParameter("device.ssm.trajectories")) {
3455  try {
3456  trajectories = StringUtils::toBool(v.getParameter().getParameter("device.ssm.trajectories", "no"));
3457  } catch (...) {
3458  WRITE_WARNING("Invalid value '" + v.getParameter().getParameter("device.ssm.trajectories", "no") + "'for vehicle parameter 'ssm.trajectories'");
3459  }
3460  } else if (v.getVehicleType().getParameter().knowsParameter("device.ssm.trajectories")) {
3461  try {
3462  trajectories = StringUtils::toBool(v.getVehicleType().getParameter().getParameter("device.ssm.trajectories", "no"));
3463  } catch (...) {
3464  WRITE_WARNING("Invalid value '" + v.getVehicleType().getParameter().getParameter("device.ssm.trajectories", "no") + "'for vType parameter 'ssm.trajectories'");
3465  }
3466  } else {
3467  trajectories = oc.getBool("device.ssm.trajectories");
3468  if (!oc.isSet("device.ssm.trajectories") && (issuedParameterWarnFlags & SSM_WARN_TRAJECTORIES) == 0) {
3469  std::cout << "vehicle '" << v.getID() << "' does not supply vehicle parameter 'device.ssm.trajectories'. Using default of '" << ::toString(trajectories) << "'\n";
3471  }
3472  }
3473  return trajectories;
3474 }
3475 
3476 
3477 bool
3478 MSDevice_SSM::getMeasuresAndThresholds(const SUMOVehicle& v, std::string deviceID, std::map<std::string, double>& thresholds) {
3480 
3481  // Measures
3482  std::string measures_str = "";
3483  if (v.getParameter().knowsParameter("device.ssm.measures")) {
3484  try {
3485  measures_str = v.getParameter().getParameter("device.ssm.measures", "");
3486  } catch (...) {
3487  WRITE_WARNING("Invalid value '" + v.getParameter().getParameter("device.ssm.measures", "") + "'for vehicle parameter 'ssm.measures'");
3488  }
3489  } else if (v.getVehicleType().getParameter().knowsParameter("device.ssm.measures")) {
3490  try {
3491  measures_str = v.getVehicleType().getParameter().getParameter("device.ssm.measures", "");
3492  } catch (...) {
3493  WRITE_WARNING("Invalid value '" + v.getVehicleType().getParameter().getParameter("device.ssm.measures", "") + "'for vType parameter 'ssm.measures'");
3494  }
3495  } else {
3496  measures_str = oc.getString("device.ssm.measures");
3497  if (!oc.isSet("device.ssm.measures") && (issuedParameterWarnFlags & SSM_WARN_MEASURES) == 0) {
3498  std::cout << "vehicle '" << v.getID() << "' does not supply vehicle parameter 'device.ssm.measures'. Using default of '" << measures_str << "'\n";
3500  }
3501  }
3502 
3503  // Check retrieved measures
3504  if (measures_str == "") {
3505  WRITE_WARNING("No measures specified for ssm device of vehicle '" + v.getID() + "'. Registering all available SSMs.");
3506  measures_str = AVAILABLE_SSMS;
3507  }
3509  std::vector<std::string> available = st.getVector();
3510  st = StringTokenizer(measures_str);
3511  std::vector<std::string> measures = st.getVector();
3512  for (std::vector<std::string>::const_iterator i = measures.begin(); i != measures.end(); ++i) {
3513  if (std::find(available.begin(), available.end(), *i) == available.end()) {
3514  // Given identifier is unknown
3515  WRITE_ERROR("SSM identifier '" + *i + "' is not supported. Aborting construction of SSM device '" + deviceID + "'.");
3516  return false;
3517  }
3518  }
3519 
3520  // Thresholds
3521  std::string thresholds_str = "";
3522  if (v.getParameter().knowsParameter("device.ssm.thresholds")) {
3523  try {
3524  thresholds_str = v.getParameter().getParameter("device.ssm.thresholds", "");
3525  } catch (...) {
3526  WRITE_WARNING("Invalid value '" + v.getParameter().getParameter("device.ssm.thresholds", "") + "'for vehicle parameter 'ssm.thresholds'");
3527  }
3528  } else if (v.getVehicleType().getParameter().knowsParameter("device.ssm.thresholds")) {
3529  try {
3530  thresholds_str = v.getVehicleType().getParameter().getParameter("device.ssm.thresholds", "");
3531  } catch (...) {
3532  WRITE_WARNING("Invalid value '" + v.getVehicleType().getParameter().getParameter("device.ssm.thresholds", "") + "'for vType parameter 'ssm.thresholds'");
3533  }
3534  } else {
3535  thresholds_str = oc.getString("device.ssm.thresholds");
3536  if (!oc.isSet("device.ssm.thresholds") && (issuedParameterWarnFlags & SSM_WARN_THRESHOLDS) == 0) {
3537  std::cout << "vehicle '" << v.getID() << "' does not supply vehicle parameter 'device.ssm.thresholds'. Using default of '" << thresholds_str << "'\n";
3539  }
3540  }
3541 
3542  // Parse vector of doubles from threshold_str
3543  int count = 0;
3544  if (thresholds_str != "") {
3545  st = StringTokenizer(thresholds_str);
3546  while (count < (int)measures.size() && st.hasNext()) {
3547  double thresh = StringUtils::toDouble(st.next());
3548  thresholds.insert(std::make_pair(measures[count], thresh));
3549  ++count;
3550  }
3551  if (thresholds.size() < measures.size() || st.hasNext()) {
3552  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.");
3553  return false;
3554  }
3555  } else {
3556  // assume default thresholds if none are given
3557  for (std::vector<std::string>::const_iterator i = measures.begin(); i != measures.end(); ++i) {
3558  if (*i == "TTC") {
3559  thresholds.insert(std::make_pair(*i, DEFAULT_THRESHOLD_TTC));
3560  } else if (*i == "DRAC") {
3561  thresholds.insert(std::make_pair(*i, DEFAULT_THRESHOLD_DRAC));
3562  } else if (*i == "PET") {
3563  thresholds.insert(std::make_pair(*i, DEFAULT_THRESHOLD_PET));
3564  } else if (*i == "BR") {
3565  thresholds.insert(std::make_pair(*i, DEFAULT_THRESHOLD_BR));
3566  } else if (*i == "SGAP") {
3567  thresholds.insert(std::make_pair(*i, DEFAULT_THRESHOLD_SGAP));
3568  } else if (*i == "TGAP") {
3569  thresholds.insert(std::make_pair(*i, DEFAULT_THRESHOLD_TGAP));
3570  } else {
3571  WRITE_ERROR("Unknown SSM identifier '" + (*i) + "'. Aborting construction of ssm device."); // should never occur
3572  return false;
3573  }
3574  }
3575  }
3576  return true;
3577 }
3578 
3579 
3580 std::string
3581 MSDevice_SSM::getParameter(const std::string& key) const {
3582  if (key == "minTTC" && !myComputeTTC) {
3583  throw InvalidArgument("Measure TTC is not tracked by ssm device");
3584  }
3585  if (key == "maxDRAC" && !myComputeDRAC) {
3586  throw InvalidArgument("Measure DRAC is not tracked by ssm device");
3587  }
3588  if (key == "minPET" && !myComputePET) {
3589  throw InvalidArgument("Measure PET is not tracked by ssm device");
3590  }
3591  if (key == "minTTC" ||
3592  key == "maxDRAC" ||
3593  key == "minPET") {
3594  double value = INVALID_DOUBLE;
3595  double minTTC = INVALID_DOUBLE;
3596  double minPET = INVALID_DOUBLE;
3597  double maxDRAC = -INVALID_DOUBLE;
3598  for (Encounter* e : myActiveEncounters) {
3599  minTTC = MIN2(minTTC, e->minTTC.value);
3600  minPET = MIN2(minPET, e->PET.value);
3601  maxDRAC = MAX2(maxDRAC, e->maxDRAC.value);
3602  }
3603  if (key == "minTTC") {
3604  value = minTTC;
3605  } else if (key == "maxDRAC") {
3606  value = maxDRAC;
3607  } else if (key == "minPET") {
3608  value = minPET;
3609  }
3610  if (fabs(value) == INVALID_DOUBLE) {
3611  return "";
3612  } else {
3613  return toString(value);
3614  }
3615  }
3616  throw InvalidArgument("Parameter '" + key + "' is not supported for device of type '" + deviceName() + "'");
3617 }
3618 
3619 
3620 void
3621 MSDevice_SSM::setParameter(const std::string& key, const std::string& value) {
3622  double doubleValue;
3623  try {
3624  doubleValue = StringUtils::toDouble(value);
3625  } catch (NumberFormatException&) {
3626  throw InvalidArgument("Setting parameter '" + key + "' requires a number for device of type '" + deviceName() + "'");
3627  }
3628  if (false || key == "foo") {
3629  UNUSED_PARAMETER(doubleValue);
3630  } else {
3631  throw InvalidArgument("Setting parameter '" + key + "' is not supported for device of type '" + deviceName() + "'");
3632  }
3633 }
3634 
3635 /****************************************************************************/
3636 
MSDevice_SSM::EncounterApproachInfo::foeConflictAreaLength
double foeConflictAreaLength
Definition: MSDevice_SSM.h:314
MSDevice_SSM::UpstreamScanStartInfo::range
double range
Definition: MSDevice_SSM.h:348
OptionsCont::isSet
bool isSet(const std::string &name, bool failOnNonExistant=true) const
Returns the information whether the named option is set.
Definition: OptionsCont.cpp:135
UNUSED_PARAMETER
#define UNUSED_PARAMETER(x)
Definition: StdDefs.h:31
MSDevice_SSM::Encounter::TTCspan
std::vector< double > TTCspan
All values for TTC.
Definition: MSDevice_SSM.h:272
SUMOTrafficObject
Representation of a vehicle or person.
Definition: SUMOTrafficObject.h:47
MSDevice_SSM::Encounter::countDownExtraTime
void countDownExtraTime(double amount)
decreases myRemaingExtraTime by given amount in seconds
Definition: MSDevice_SSM.cpp:360
MSDevice_SSM::getExtraTime
static double getExtraTime(const SUMOVehicle &v)
Definition: MSDevice_SSM.cpp:3420
MSDevice_SSM::myTGAPspan
std::vector< double > myTGAPspan
All values for time gap.
Definition: MSDevice_SSM.h:735
MSVehicle::getPreviousSpeed
double getPreviousSpeed() const
Returns the vehicle's speed before the previous time step.
Definition: MSVehicle.h:484
MSDevice_SSM::ENCOUNTER_TYPE_FOLLOWING_PASSED
@ ENCOUNTER_TYPE_FOLLOWING_PASSED
ENCOUNTER_TYPE_FOLLOWING_PASSED.
Definition: MSDevice_SSM.h:111
MSJunction::getFoeInternalLanes
virtual const std::vector< MSLane * > & getFoeInternalLanes(const MSLink *const) const
Definition: MSJunction.h:97
MSDevice_SSM::Encounter::timeSpan
std::vector< double > timeSpan
time points corresponding to the trajectories
Definition: MSDevice_SSM.h:254
MIN2
T MIN2(T a, T b)
Definition: StdDefs.h:73
MSDevice_SSM::getParameter
std::string getParameter(const std::string &key) const
try to retrieve the given parameter from this device. Throw exception for unsupported key
Definition: MSDevice_SSM.cpp:3581
MSDevice_SSM::Encounter::foeID
const std::string foeID
Definition: MSDevice_SSM.h:241
MSDevice_SSM::UpstreamScanStartInfo::egoDistToConflictLane
double egoDistToConflictLane
Definition: MSDevice_SSM.h:349
MSDevice_SSM::Encounter::ConflictPointInfo::type
EncounterType type
Type of the conflict.
Definition: MSDevice_SSM.h:193
StringUtils::toBool
static bool toBool(const std::string &sData)
converts a string into the bool value described by it by calling the char-type converter
Definition: StringUtils.cpp:374
WRITE_WARNING
#define WRITE_WARNING(msg)
Definition: MsgHandler.h:275
MSNet.h
MSLane
Representation of a lane in the micro simulation.
Definition: MSLane.h:82
MSDevice_SSM
A device which collects info on the vehicle trip (mainly on departure and arrival)
Definition: MSDevice_SSM.h:56
MSDevice_SSM::updatePassedEncounter
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...
Definition: MSDevice_SSM.cpp:1635
MSDevice_SSM::Encounter::egoTrajectory
Trajectory egoTrajectory
Trajectory of the ego vehicle.
Definition: MSDevice_SSM.h:258
MSVehicle::isOnRoad
bool isOnRoad() const
Returns the information whether the vehicle is on a road (is simulated)
Definition: MSVehicle.h:582
MSDevice_SSM::closeEncounter
void closeEncounter(Encounter *e)
Finalizes the encounter and calculates SSM values.
Definition: MSDevice_SSM.cpp:656
SUMOVehicle::getParameter
virtual const SUMOVehicleParameter & getParameter() const =0
Returns the vehicle's parameter (including departure definition)
MSJunction
The base class for an intersection.
Definition: MSJunction.h:60
OutputDevice
Static storage of an output device and its base (abstract) implementation.
Definition: OutputDevice.h:63
MSRoute::end
MSRouteIterator end() const
Returns the end of the list of edges to pass.
Definition: MSRoute.cpp:75
MSEdge::getOppositeEdge
const MSEdge * getOppositeEdge() const
Returns the opposite direction edge if on exists else a nullptr.
Definition: MSEdge.cpp:1038
MSDevice_SSM::Encounter::ConflictPointInfo::pos
Position pos
Predicted location of the conflict: In case of MERGING and CROSSING: entry point to conflict area for...
Definition: MSDevice_SSM.h:191
MSDevice_SSM::FoeInfo::egoConflictLane
const MSLane * egoConflictLane
Definition: MSDevice_SSM.h:329
MSLane::getCanonicalSuccessorLane
MSLane * getCanonicalSuccessorLane() const
Definition: MSLane.cpp:2609
OptionsCont.h
SUMOTrafficObject::getEdge
virtual const MSEdge * getEdge() const =0
Returns the edge the vehicle is currently at.
StringTokenizer::hasNext
bool hasNext()
returns the information whether further substrings exist
Definition: StringTokenizer.cpp:94
SUMOTrafficObject::getVehicleType
virtual const MSVehicleType & getVehicleType() const =0
Returns the vehicle's type.
SUMOTrafficObject::isVehicle
virtual bool isVehicle() const =0
Get the vehicle's ID.
StringUtils::toDouble
static double toDouble(const std::string &sData)
converts a string into the double value described by it by calling the char-type converter
Definition: StringUtils.cpp:345
MSDevice_SSM::EncounterApproachInfo::ttc
double ttc
Definition: MSDevice_SSM.h:317
MSDevice_SSM::Encounter::Trajectory::x
PositionVector x
Definition: MSDevice_SSM.h:179
MSDevice_SSM::Encounter::egoID
const std::string egoID
Definition: MSDevice_SSM.h:240
MSDevice_SSM::computeGlobalMeasures
void computeGlobalMeasures()
Stores measures, that are not associated to a specific encounter as headways and brake rates.
Definition: MSDevice_SSM.cpp:453
SUMOTrafficObject::getID
virtual const std::string & getID() const =0
Get the vehicle's ID.
MSDevice_SSM::makeStringWithNAs
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")
Definition: MSDevice_SSM.cpp:2699
MSDevice_SSM::resetEncounters
void resetEncounters()
Closes all current Encounters and moves conflicts to myPastConflicts,.
Definition: MSDevice_SSM.cpp:537
DEFAULT_THRESHOLD_DRAC
#define DEFAULT_THRESHOLD_DRAC
Definition: MSDevice_SSM.cpp:82
MSCFModel::estimateArrivalTime
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:388
SUMOVehicle::getCurrentRouteEdge
virtual const ConstMSEdgeVector::const_iterator & getCurrentRouteEdge() const =0
Returns an iterator pointing to the current edge in this vehicles route.
MSDevice_SSM::myGlobalMeasuresTimeSpan
std::vector< double > myGlobalMeasuresTimeSpan
Definition: MSDevice_SSM.h:729
MSDevice_SSM::createdOutputFiles
static std::set< std::string > createdOutputFiles
remember which files were created already (don't duplicate xml root-elements)
Definition: MSDevice_SSM.h:748
MSDevice_SSM::ENCOUNTER_TYPE_BOTH_LEFT_CONFLICT_AREA
@ ENCOUNTER_TYPE_BOTH_LEFT_CONFLICT_AREA
ENCOUNTER_TYPE_BOTH_LEFT_CONFLICT_AREA.
Definition: MSDevice_SSM.h:108
MSDevice_SSM::updateAndWriteOutput
void updateAndWriteOutput()
This is called once per time step in MSNet::writeOutput() and collects the surrounding vehicles,...
Definition: MSDevice_SSM.cpp:394
MSDevice_SSM::myRange
double myRange
Detection range. For vehicles closer than this distance from the ego vehicle, SSMs are traced.
Definition: MSDevice_SSM.h:704
OptionsCont::getString
std::string getString(const std::string &name) const
Returns the string-value of the named option (only for Option_String)
Definition: OptionsCont.cpp:201
MSBaseVehicle::getLength
double getLength() const
Returns the vehicle's length.
Definition: MSBaseVehicle.h:410
MSDevice_SSM::SSM_WARN_GEO
@ SSM_WARN_GEO
Definition: MSDevice_SSM.h:760
MSDevice_SSM::ENCOUNTER_TYPE_COLLISION
@ ENCOUNTER_TYPE_COLLISION
ENCOUNTER_TYPE_COLLISION.
Definition: MSDevice_SSM.h:117
MSJunction::getInternalLanes
virtual const std::vector< MSLane * > getInternalLanes() const
Returns all internal lanes on the junction.
Definition: MSJunction.h:113
MSDevice_SSM::myMaxBR
std::pair< std::pair< double, Position >, double > myMaxBR
Extremal values for the global measures (as <<<time, Position>, value>, [leaderID]>-pairs)
Definition: MSDevice_SSM.h:738
MSDevice_SSM::SSM_WARN_RANGE
@ SSM_WARN_RANGE
Definition: MSDevice_SSM.h:757
SUMOVehicle
Representation of a vehicle.
Definition: SUMOVehicle.h:60
MSLane::VehCont
std::vector< MSVehicle * > VehCont
Container for vehicles.
Definition: MSLane.h:92
MSDevice_SSM::ENCOUNTER_TYPE_CROSSING_LEADER
@ ENCOUNTER_TYPE_CROSSING_LEADER
ENCOUNTER_TYPE_CROSSING_LEADER.
Definition: MSDevice_SSM.h:93
MSBaseVehicle::getRoute
const MSRoute & getRoute() const
Returns the current route.
Definition: MSBaseVehicle.h:115
GeoConvHelper.h
ConstMSEdgeVector
std::vector< const MSEdge * > ConstMSEdgeVector
Definition: MSEdge.h:75
MSDevice_SSM::toGeo
static void toGeo(Position &x)
convert SUMO-positions to geo coordinates (in place)
Definition: MSDevice_SSM.cpp:2604
SUMOVehicle::isOnRoad
virtual bool isOnRoad() const =0
Returns the information whether the vehicle is on a road (is simulated)
OptionsCont::getBool
bool getBool(const std::string &name) const
Returns the boolean-value of the named option (only for Option_Bool)
Definition: OptionsCont.cpp:222
MSDevice_SSM::EncounterApproachInfo::egoConflictAreaLength
double egoConflictAreaLength
Definition: MSDevice_SSM.h:313
MSDevice_SSM::myThresholds
std::map< std::string, double > myThresholds
Definition: MSDevice_SSM.h:699
MSDevice_SSM::cleanup
static void cleanup()
Clean up remaining devices instances.
Definition: MSDevice_SSM.cpp:191
MSDevice_SSM::EncounterApproachInfo::conflictPoint
Position conflictPoint
Definition: MSDevice_SSM.h:304
OptionsCont::getOptions
static OptionsCont & getOptions()
Retrieves the options.
Definition: OptionsCont.cpp:57
StringTokenizer::next
std::string next()
returns the next substring when it exists. Otherwise the behaviour is undefined
Definition: StringTokenizer.cpp:99
MSGlobals::gUseMesoSim
static bool gUseMesoSim
Definition: MSGlobals.h:90
DEFAULT_THRESHOLD_TGAP
#define DEFAULT_THRESHOLD_TGAP
Definition: MSDevice_SSM.cpp:87
MSDevice_SSM::myOutputFile
OutputDevice * myOutputFile
Output device.
Definition: MSDevice_SSM.h:745
MSDevice_SSM::getInstances
static const std::set< MSDevice_SSM *, ComparatorNumericalIdLess > & getInstances()
returns all currently existing SSM devices
Definition: MSDevice_SSM.cpp:186
MSEdge.h
MSDevice_SSM::issuedParameterWarnFlags
static int issuedParameterWarnFlags
bitset storing info whether warning has already been issued about unset parameter (warn only once!...
Definition: MSDevice_SSM.h:752
MSDevice_SSM::flushGlobalMeasures
void flushGlobalMeasures()
Write out all non-encounter specific measures as headways and braking rates.
Definition: MSDevice_SSM.cpp:2550
MSDevice_SSM::myComputeTTC
bool myComputeTTC
Flags for switching on / off comutation of different SSMs, derived from myMeasures.
Definition: MSDevice_SSM.h:710
MSVehicleDevice::myHolder
SUMOVehicle & myHolder
The vehicle that stores the device.
Definition: MSVehicleDevice.h:84
PositionVector
A list of positions.
Definition: PositionVector.h:45
MSDevice_SSM::UpstreamScanStartInfo
Auxiliary structure used to handle upstream scanning start points Upstream scan has to be started aft...
Definition: MSDevice_SSM.h:343
MSEdge::getLength
double getLength() const
return the length of the edge
Definition: MSEdge.h:589
MSDevice_SSM::createEncounters
void createEncounters(FoeInfoMap &foes)
Makes new encounters for all given vehicles (these should be the ones entering the device's range in ...
Definition: MSDevice_SSM.cpp:506
MSDevice_SSM::computeSSMs
void computeSSMs(EncounterApproachInfo &e) const
Compute current values of the logged SSMs (myMeasures) for the given encounter 'e' and update 'e' acc...
Definition: MSDevice_SSM.cpp:990
MSDevice_SSM::ENCOUNTER_TYPE_EGO_ENTERED_CONFLICT_AREA
@ ENCOUNTER_TYPE_EGO_ENTERED_CONFLICT_AREA
ENCOUNTER_TYPE_EGO_ENTERED_CONFLICT_AREA.
Definition: MSDevice_SSM.h:98
MSVehicle::getCarFollowModel
const MSCFModel & getCarFollowModel() const
Returns the vehicle's car following model definition.
Definition: MSVehicle.h:893
MSDevice_SSM::Encounter::typeSpan
std::vector< int > typeSpan
Evolution of the encounter classification (.
Definition: MSDevice_SSM.h:256
MSDevice_SSM::Encounter::~Encounter
~Encounter()
Destructor.
Definition: MSDevice_SSM.cpp:294
MSDevice_SSM::ENCOUNTER_TYPE_FOE_ENTERED_CONFLICT_AREA
@ ENCOUNTER_TYPE_FOE_ENTERED_CONFLICT_AREA
ENCOUNTER_TYPE_FOE_ENTERED_CONFLICT_AREA.
Definition: MSDevice_SSM.h:100
MSDevice_SSM::writeOutConflict
void writeOutConflict(Encounter *e)
Definition: MSDevice_SSM.cpp:2616
gPrecisionGeo
int gPrecisionGeo
Definition: StdDefs.cpp:27
MSJunction::getIncoming
const ConstMSEdgeVector & getIncoming() const
Definition: MSJunction.h:101
MSLane::getIncomingLanes
const std::vector< IncomingLaneInfo > & getIncomingLanes() const
Definition: MSLane.h:818
MSDevice_SSM::myMinTGAP
std::pair< std::pair< std::pair< double, Position >, double >, std::string > myMinTGAP
Definition: MSDevice_SSM.h:740
Parameterised::getParameter
const std::string getParameter(const std::string &key, const std::string &defaultValue="") const
Returns the value for a given key.
Definition: Parameterised.cpp:72
MSDevice_SSM::Encounter::conflictPointSpan
PositionVector conflictPointSpan
Predicted location of the conflict: In case of MERGING and CROSSING: entry point to conflict area for...
Definition: MSDevice_SSM.h:269
MSVehicle.h
DEFAULT_THRESHOLD_BR
#define DEFAULT_THRESHOLD_BR
Definition: MSDevice_SSM.cpp:85
OutputDevice::closeTag
bool closeTag(const std::string &comment="")
Closes the most recently opened tag and optionally adds a comment.
Definition: OutputDevice.cpp:253
MSJunction::getOutgoing
const ConstMSEdgeVector & getOutgoing() const
Definition: MSJunction.h:107
INVALID
#define INVALID
Definition: MSDevice_SSM.cpp:70
DEFAULT_THRESHOLD_TTC
#define DEFAULT_THRESHOLD_TTC
Definition: MSDevice_SSM.cpp:81
MSVehicle::getLateralPositionOnLane
double getLateralPositionOnLane() const
Get the vehicle's lateral position on the lane.
Definition: MSVehicle.h:429
MSDevice_SSM::FoeInfo
Definition: MSDevice_SSM.h:328
MSDevice_SSM::myMinSGAP
std::pair< std::pair< std::pair< double, Position >, double >, std::string > myMinSGAP
Definition: MSDevice_SSM.h:739
MSBaseVehicle::getEdge
const MSEdge * getEdge() const
Returns the edge the vehicle is currently at.
Definition: MSBaseVehicle.cpp:181
MSDevice_SSM::Encounter::foeConflictEntryTime
double foeConflictEntryTime
Times when the foe vehicle entered/left the conflict area. Currently only applies for crossing situat...
Definition: MSDevice_SSM.h:251
MSDevice_SSM::SSM_WARN_EXTRATIME
@ SSM_WARN_EXTRATIME
Definition: MSDevice_SSM.h:758
MAX2
T MAX2(T a, T b)
Definition: StdDefs.h:79
MSEdge::isInternal
bool isInternal() const
return whether this edge is an internal edge
Definition: MSEdge.h:235
MSVehicle::getPosition
Position getPosition(const double offset=0) const
Return current position (x/y, cartesian)
Definition: MSVehicle.cpp:1269
AVAILABLE_SSMS
#define AVAILABLE_SSMS
Definition: MSDevice_SSM.cpp:79
NumberFormatException
Definition: UtilExceptions.h:95
MSDevice_SSM::classifyEncounter
EncounterType classifyEncounter(const FoeInfo *foeInfo, EncounterApproachInfo &eInfo) const
Classifies the current type of the encounter provided some information on the opponents.
Definition: MSDevice_SSM.cpp:1798
SUMOVehicle::getRoute
virtual const MSRoute & getRoute() const =0
Returns the current route.
OutputDevice::writeAttr
OutputDevice & writeAttr(const SumoXMLAttr attr, const T &val)
writes a named attribute
Definition: OutputDevice.h:255
MSDevice_SSM::EncounterApproachInfo::egoConflictExitDist
double egoConflictExitDist
Definition: MSDevice_SSM.h:307
MSEdge::getFromJunction
const MSJunction * getFromJunction() const
Definition: MSEdge.h:359
MSDevice_SSM::updateEncounter
bool updateEncounter(Encounter *e, FoeInfo *foeInfo)
Updates the encounter (adds a new trajectory point).
Definition: MSDevice_SSM.cpp:703
SUMOVehicle.h
MSDevice_SSM::Encounter::begin
double begin
Definition: MSDevice_SSM.h:242
DEFAULT_RANGE
#define DEFAULT_RANGE
Definition: MSDevice_SSM.cpp:73
MSEdge::isCrossing
bool isCrossing() const
return whether this edge is a pedestrian crossing
Definition: MSEdge.h:240
MSDevice_SSM::UpstreamScanStartInfo::edge
const MSEdge * edge
Definition: MSDevice_SSM.h:345
OptionsCont::addDescription
void addDescription(const std::string &name, const std::string &subtopic, const std::string &description)
Adds a description for an option.
Definition: OptionsCont.cpp:469
SIMTIME
#define SIMTIME
Definition: SUMOTime.h:63
MSVehicleControl::getVehicle
SUMOVehicle * getVehicle(const std::string &id) const
Returns the vehicle with the given id.
Definition: MSVehicleControl.cpp:240
MSVehicle::getPositionOnLane
double getPositionOnLane() const
Get the vehicle's position along the lane.
Definition: MSVehicle.h:392
MSVehicle::getLeader
std::pair< const MSVehicle *const, double > getLeader(double dist=0) const
Returns the leader of the vehicle looking for a fixed distance.
Definition: MSVehicle.cpp:5130
MSDevice_SSM::~MSDevice_SSM
~MSDevice_SSM()
Destructor.
Definition: MSDevice_SSM.cpp:2775
MSDevice_SSM::getDetectionRange
static double getDetectionRange(const SUMOVehicle &v)
Definition: MSDevice_SSM.cpp:3393
MSDevice_SSM::Encounter::foeDistsToConflict
std::vector< double > foeDistsToConflict
Evolution of the foe vehicle's distance to the conflict point.
Definition: MSDevice_SSM.h:264
MSDevice::insertDefaultAssignmentOptions
static void insertDefaultAssignmentOptions(const std::string &deviceName, const std::string &optionsTopic, OptionsCont &oc, const bool isPerson=false)
Adds common command options that allow to assign devices to vehicles.
Definition: MSDevice.cpp:126
MSDevice_SSM::Encounter::add
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
Definition: MSDevice_SSM.cpp:304
MSDevice_SSM::EncounterApproachInfo::foeEstimatedConflictExitTime
double foeEstimatedConflictExitTime
Definition: MSDevice_SSM.h:312
MSDevice_SSM::EncounterApproachInfo::EncounterApproachInfo
EncounterApproachInfo(Encounter *e)
Definition: MSDevice_SSM.cpp:371
MSDevice_SSM::flushConflicts
void flushConflicts(bool all=false)
Writes out all past conflicts that have begun earlier than the oldest active encounter.
Definition: MSDevice_SSM.cpp:2528
GeoConvHelper::getFinal
static const GeoConvHelper & getFinal()
the coordinate transformation for writing the location element and for tracking the original coordina...
Definition: GeoConvHelper.h:105
MSDevice_SSM::ENCOUNTER_TYPE_EGO_LEFT_CONFLICT_AREA
@ ENCOUNTER_TYPE_EGO_LEFT_CONFLICT_AREA
ENCOUNTER_TYPE_EGO_LEFT_CONFLICT_AREA.
Definition: MSDevice_SSM.h:102
MSVehicleType::getWidth
double getWidth() const
Get the width which vehicles of this class shall have when being drawn.
Definition: MSVehicleType.h:246
MSDevice_SSM::myActiveEncounters
EncounterVector myActiveEncounters
Definition: MSDevice_SSM.h:718
operator<<
std::ostream & operator<<(std::ostream &out, MSDevice_SSM::EncounterType type)
Nicer output for EncounterType enum.
Definition: MSDevice_SSM.cpp:98
MSJunction.h
MSDevice_SSM::buildVehicleDevices
static void buildVehicleDevices(SUMOVehicle &v, std::vector< MSVehicleDevice * > &into)
Build devices for the given vehicle, if needed.
Definition: MSDevice_SSM.cpp:230
TS
#define TS
Definition: SUMOTime.h:43
MSDevice_SSM::Encounter::minTTC
ConflictPointInfo minTTC
Definition: MSDevice_SSM.h:282
StringTokenizer
Definition: StringTokenizer.h:61
DEFAULT_EXTRA_TIME
#define DEFAULT_EXTRA_TIME
Definition: MSDevice_SSM.cpp:89
MSDevice_SSM::Encounter::egoConflictExitTime
double egoConflictExitTime
Definition: MSDevice_SSM.h:249
MSDevice_SSM::generateOutput
void generateOutput() const
Finalizes output. Called on vehicle removal.
Definition: MSDevice_SSM.cpp:3330
MSDevice_SSM::EncounterVector
std::vector< Encounter * > EncounterVector
Definition: MSDevice_SSM.h:354
SUMOVehicle::getLane
virtual MSLane * getLane() const =0
Returns the lane the vehicle is on.
MSDevice_SSM::update
void update()
Definition: MSDevice_SSM.cpp:413
MSDevice_SSM::Encounter::PET
ConflictPointInfo PET
Definition: MSDevice_SSM.h:284
MSDevice_SSM::ENCOUNTER_TYPE_MERGING
@ ENCOUNTER_TYPE_MERGING
ENCOUNTER_TYPE_MERGING.
Definition: MSDevice_SSM.h:79
MSDevice_SSM::Encounter::end
double end
Definition: MSDevice_SSM.h:242
MSLane::getLength
double getLength() const
Returns the lane's length.
Definition: MSLane.h:540
gDebugFlag3
bool gDebugFlag3
Definition: StdDefs.cpp:34
OutputDevice.h
MSDevice_SSM::mySaveTrajectories
bool mySaveTrajectories
This determines whether the whole trajectories of the vehicles (position, speed, ssms) shall be saved...
Definition: MSDevice_SSM.h:702
MSDevice_SSM::ENCOUNTER_TYPE_BOTH_ENTERED_CONFLICT_AREA
@ ENCOUNTER_TYPE_BOTH_ENTERED_CONFLICT_AREA
ENCOUNTER_TYPE_BOTH_ENTERED_CONFLICT_AREA.
Definition: MSDevice_SSM.h:106
OptionsCont::doRegister
void doRegister(const std::string &name, Option *v)
Adds an option under the given name.
Definition: OptionsCont.cpp:74
MSVehicle::getVelocityVector
Position getVelocityVector() const
Returns the vehicle's direction in radians.
Definition: MSVehicle.h:688
MSDevice_SSM::EncounterApproachInfo
Structure to collect some info on the encounter needed during ssm calculation by various functions.
Definition: MSDevice_SSM.h:300
MSDevice_SSM::deviceName
const std::string deviceName() const
return the name for this type of device
Definition: MSDevice_SSM.h:478
MSVehicle::getLaneChangeModel
MSAbstractLaneChangeModel & getLaneChangeModel()
Definition: MSVehicle.cpp:4680
MSAbstractLaneChangeModel::isOpposite
bool isOpposite() const
Definition: MSAbstractLaneChangeModel.h:535
GeoConvHelper::cartesian2geo
void cartesian2geo(Position &cartesian) const
Converts the given cartesian (shifted) position to its geo (lat/long) representation.
Definition: GeoConvHelper.cpp:293
Position
A point in 2D or 3D with translation and scaling methods.
Definition: Position.h:38
MSDevice_SSM::insertOptions
static void insertOptions(OptionsCont &oc)
Inserts MSDevice_SSM-options.
Definition: MSDevice_SSM.cpp:208
MSDevice_SSM::myHolderMS
MSVehicle * myHolderMS
Definition: MSDevice_SSM.h:711
MSDevice_SSM::EncounterApproachInfo::foeEstimatedConflictEntryTime
double foeEstimatedConflictEntryTime
Definition: MSDevice_SSM.h:310
OptionsCont
A storage for options typed value containers)
Definition: OptionsCont.h:89
MSDevice_SSM::ENCOUNTER_TYPE_CROSSING_FOLLOWER
@ ENCOUNTER_TYPE_CROSSING_FOLLOWER
ENCOUNTER_TYPE_CROSSING_FOLLOWER.
Definition: MSDevice_SSM.h:96
MSEdge
A road/street connecting two junctions.
Definition: MSEdge.h:78
MSDevice_SSM::ENCOUNTER_TYPE_FOLLOWING_FOLLOWER
@ ENCOUNTER_TYPE_FOLLOWING_FOLLOWER
ENCOUNTER_TYPE_FOLLOWING_FOLLOWER.
Definition: MSDevice_SSM.h:72
MSDevice_SSM::UpstreamScanStartInfo::egoConflictLane
const MSLane * egoConflictLane
Definition: MSDevice_SSM.h:350
MSEdge::getToJunction
const MSJunction * getToJunction() const
Definition: MSEdge.h:363
MSVehicle::getLastStepDist
double getLastStepDist() const
Get the distance the vehicle covered in the previous timestep.
Definition: MSVehicle.h:399
MSDevice_SSM::Encounter::currentType
EncounterType currentType
Definition: MSDevice_SSM.h:243
MSDevice_SSM::Encounter::foeConflictExitTime
double foeConflictExitTime
Definition: MSDevice_SSM.h:251
MSDevice_SSM::FoeInfo::egoDistToConflictLane
double egoDistToConflictLane
Definition: MSDevice_SSM.h:331
MSBaseVehicle::getVehicleType
const MSVehicleType & getVehicleType() const
Returns the vehicle's type definition.
Definition: MSBaseVehicle.h:123
MSDevice_SSM::myInstances
static std::set< MSDevice_SSM *, ComparatorNumericalIdLess > * myInstances
All currently existing SSM devices.
Definition: MSDevice_SSM.h:60
MSLane::getLinkCont
const MSLinkCont & getLinkCont() const
returns the container with all links !!!
Definition: MSLane.cpp:2110
MSDevice_SSM::ENCOUNTER_TYPE_MERGING_LEADER
@ ENCOUNTER_TYPE_MERGING_LEADER
ENCOUNTER_TYPE_MERGING_LEADER.
Definition: MSDevice_SSM.h:82
MSDevice_SSM::Encounter::Encounter
Encounter(const MSVehicle *_ego, const MSVehicle *const _foe, double _begin, double extraTime)
Constructor.
Definition: MSDevice_SSM.cpp:270
MSDevice_SSM::EncounterApproachInfo::drac
double drac
Definition: MSDevice_SSM.h:318
MSDevice_SSM::computeTTC
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...
Definition: MSDevice_SSM.cpp:1334
MSDevice_SSM::qualifiesAsConflict
bool qualifiesAsConflict(Encounter *e)
Tests if the SSM values exceed the threshold for qualification as conflict.
Definition: MSDevice_SSM.cpp:633
MSEdge::getInternalFollowingLengthTo
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:687
DEFAULT_THRESHOLD_PET
#define DEFAULT_THRESHOLD_PET
Definition: MSDevice_SSM.cpp:83
MSDevice_SSM::checkConflictEntryAndExit
static void checkConflictEntryAndExit(EncounterApproachInfo &eInfo)
Checks whether ego or foe have entered or left the conflict area in the last step and eventually writ...
Definition: MSDevice_SSM.cpp:1492
MSLane::getOpposite
MSLane * getOpposite() const
return the opposite direction lane for lane changing or 0
Definition: MSLane.cpp:3518
MSDevice_SSM::ENCOUNTER_TYPE_ONCOMING
@ ENCOUNTER_TYPE_ONCOMING
Definition: MSDevice_SSM.h:115
OptionsCont::addOptionSubTopic
void addOptionSubTopic(const std::string &topic)
Adds an option subtopic.
Definition: OptionsCont.cpp:519
MSDevice_SSM::ENCOUNTER_TYPE_FOLLOWING_LEADER
@ ENCOUNTER_TYPE_FOLLOWING_LEADER
ENCOUNTER_TYPE_FOLLOWING_LEADER.
Definition: MSDevice_SSM.h:74
MSVehicle::getLane
MSLane * getLane() const
Returns the lane the vehicle is on.
Definition: MSVehicle.h:560
MSDevice_SSM::myBRspan
std::vector< double > myBRspan
All values for brake rate.
Definition: MSDevice_SSM.h:731
MSLane::getEdge
MSEdge & getEdge() const
Returns the lane's edge.
Definition: MSLane.h:669
MSLane::getFirstInternalInConnection
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:1848
MSDevice_SSM::Encounter::size
std::size_t size() const
Returns the number of trajectory points stored.
Definition: MSDevice_SSM.h:212
MSDevice_SSM::notifyMove
bool notifyMove(SUMOTrafficObject &veh, double oldPos, double newPos, double newSpeed)
Checks for waiting steps when the vehicle moves.
Definition: MSDevice_SSM.cpp:2813
OptionsCont::getFloat
double getFloat(const std::string &name) const
Returns the double-value of the named option (only for Option_Float)
Definition: OptionsCont.cpp:208
MSDevice_SSM::determinePET
void determinePET(EncounterApproachInfo &eInfo) const
Discriminates between different encounter types and correspondingly determines the PET for those case...
Definition: MSDevice_SSM.cpp:1045
Position.h
OutputDevice::openTag
OutputDevice & openTag(const std::string &xmlElement)
Opens an XML tag.
Definition: OutputDevice.cpp:239
toString
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:47
MSDevice_SSM::getMeasuresAndThresholds
static bool getMeasuresAndThresholds(const SUMOVehicle &v, std::string deviceID, std::map< std::string, double > &thresholds)
Definition: MSDevice_SSM.cpp:3478
StringUtils.h
MSDevice_SSM::Encounter::DRACspan
std::vector< double > DRACspan
All values for DRAC.
Definition: MSDevice_SSM.h:274
MSLane::getShape
const PositionVector & getShape() const
Returns this lane's shape.
Definition: MSLane.h:477
MSDevice_SSM::notifyLeave
bool notifyLeave(SUMOTrafficObject &veh, double lastPos, MSMoveReminder::Notification reason, const MSLane *enteredLane=0)
Called whenever the holder leaves a lane.
Definition: MSDevice_SSM.cpp:2799
MSDevice_SSM::requestsTrajectories
static bool requestsTrajectories(const SUMOVehicle &v)
Definition: MSDevice_SSM.cpp:3451
OutputDevice::getDevice
static OutputDevice & getDevice(const std::string &name)
Returns the described OutputDevice.
Definition: OutputDevice.cpp:54
MSDevice_SSM::EncounterQueue
std::priority_queue< Encounter *, std::vector< Encounter * >, Encounter::compare > EncounterQueue
Definition: MSDevice_SSM.h:353
MSNet::getInstance
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
Definition: MSNet.cpp:167
MSDevice::equippedByDefaultAssignmentOptions
static bool equippedByDefaultAssignmentOptions(const OptionsCont &oc, const std::string &deviceName, DEVICEHOLDER &v, bool outputOptionSet, const bool isPerson=false)
Determines whether a vehicle should get a certain device.
Definition: MSDevice.h:203
MSDevice_SSM::getUpstreamVehicles
static void getUpstreamVehicles(const UpstreamScanStartInfo &scanStart, FoeInfoMap &foeCollector, std::set< const MSLane * > &seenLanes, const std::set< const MSJunction * > &routeJunctions)
Collects all vehicles within range 'range' upstream of the position 'pos' on the edge 'edge' into foe...
Definition: MSDevice_SSM.cpp:3100
MSDevice_SSM::UpstreamScanStartInfo::pos
double pos
Definition: MSDevice_SSM.h:347
MSDevice_SSM::EncounterApproachInfo::encounter
Encounter * encounter
Definition: MSDevice_SSM.h:302
M_PI
#define M_PI
Definition: odrSpiral.cpp:40
MSDevice_SSM::getVehiclesOnJunction
static void getVehiclesOnJunction(const MSJunction *, const MSLane *egoJunctionLane, double egoDistToConflictLane, const MSLane *const egoConflictLane, FoeInfoMap &foeCollector, std::set< const MSLane * > &seenLanes)
Collects all vehicles on the junction into foeCollector.
Definition: MSDevice_SSM.cpp:3231
DEFAULT_THRESHOLD_SGAP
#define DEFAULT_THRESHOLD_SGAP
Definition: MSDevice_SSM.cpp:86
MSDevice_SSM::myExtraTime
double myExtraTime
Extra time in seconds to be logged after a conflict is over.
Definition: MSDevice_SSM.h:706
PositionVector::rotationAtOffset
double rotationAtOffset(double pos) const
Returns the rotation at the given length.
Definition: PositionVector.cpp:294
MSVehicleType::getLength
double getLength() const
Get vehicle's length [m].
Definition: MSVehicleType.h:109
MSDevice_SSM::Encounter::ConflictPointInfo::value
double value
value of the corresponding SSM
Definition: MSDevice_SSM.h:195
InvalidArgument
Definition: UtilExceptions.h:56
MSDevice_SSM::EncounterApproachInfo::egoEstimatedConflictExitTime
double egoEstimatedConflictExitTime
Definition: MSDevice_SSM.h:311
MSDevice_SSM::myComputeTGAP
bool myComputeTGAP
Definition: MSDevice_SSM.h:710
MSDevice_SSM::setParameter
void setParameter(const std::string &key, const std::string &value)
try to set the given parameter for this device. Throw exception for unsupported key
Definition: MSDevice_SSM.cpp:3621
MSDevice_SSM::findSurroundingVehicles
static void findSurroundingVehicles(const MSVehicle &veh, double range, FoeInfoMap &foeCollector)
Returns all vehicles, which are within the given range of the given vehicle.
Definition: MSDevice_SSM.cpp:2825
MSDevice_SSM::ENCOUNTER_TYPE_NOCONFLICT_AHEAD
@ ENCOUNTER_TYPE_NOCONFLICT_AHEAD
ENCOUNTER_TYPE_NOCONFLICT_AHEAD.
Definition: MSDevice_SSM.h:67
MSDevice_SSM::Encounter::ConflictPointInfo::time
double time
time point of the conflict
Definition: MSDevice_SSM.h:187
MSDevice_SSM::ENCOUNTER_TYPE_CROSSING
@ ENCOUNTER_TYPE_CROSSING
ENCOUNTER_TYPE_CROSSING.
Definition: MSDevice_SSM.h:90
MSLane::getLinkTo
MSLink * getLinkTo(const MSLane *) const
returns the link to the given lane or 0, if it is not connected
Definition: MSLane.cpp:2117
MSBaseVehicle::getCurrentRouteEdge
const MSRouteIterator & getCurrentRouteEdge() const
Returns an iterator pointing to the current edge in this vehicles route.
Definition: MSBaseVehicle.h:205
MSDevice_SSM::Encounter::egoDistsToConflict
std::vector< double > egoDistsToConflict
Evolution of the ego vehicle's distance to the conflict point.
Definition: MSDevice_SSM.h:262
INVALID_DOUBLE
const double INVALID_DOUBLE
Definition: StdDefs.h:62
MSDevice_SSM::getOutputFilename
static std::string getOutputFilename(const SUMOVehicle &v, std::string deviceID)
Definition: MSDevice_SSM.cpp:3340
MSVehicleType::getParameter
const SUMOVTypeParameter & getParameter() const
Definition: MSVehicleType.h:560
MSBaseVehicle
The base class for microscopic and mesoscopic vehicles.
Definition: MSBaseVehicle.h:51
MSVehicle::getBestLanesContinuation
const std::vector< MSLane * > & getBestLanesContinuation() const
Returns the best sequence of lanes to continue the route starting at myLane.
Definition: MSVehicle.cpp:5058
MSDevice_SSM::Encounter
An encounter is an episode involving two vehicles, which are closer to each other than some specified...
Definition: MSDevice_SSM.h:173
joinToString
std::string joinToString(const std::vector< T > &v, const T_BETWEEN &between, std::streamsize accuracy=gPrecision)
Definition: ToString.h:246
MSBaseVehicle::getID
const std::string & getID() const
Returns the name of the vehicle.
Definition: MSBaseVehicle.cpp:138
MSDevice_SSM::SSM_WARN_MEASURES
@ SSM_WARN_MEASURES
Definition: MSDevice_SSM.h:754
MSDevice_SSM::SSM_WARN_TRAJECTORIES
@ SSM_WARN_TRAJECTORIES
Definition: MSDevice_SSM.h:756
MSEdge::getLanes
const std::vector< MSLane * > & getLanes() const
Returns this edge's lanes.
Definition: MSEdge.h:167
MSDevice_SSM::myComputeDRAC
bool myComputeDRAC
Definition: MSDevice_SSM.h:710
MSVehicle::getBackPositionOnLane
double getBackPositionOnLane(const MSLane *lane) const
Get the vehicle's position relative to the given lane.
Definition: MSVehicle.cpp:4057
MSDevice_SSM::EncounterApproachInfo::egoEstimatedConflictEntryTime
double egoEstimatedConflictEntryTime
Definition: MSDevice_SSM.h:309
MSVehicle::getAcceleration
double getAcceleration() const
Returns the vehicle's acceleration in m/s (this is computed as the last step's mean acceleration in c...
Definition: MSVehicle.h:493
MSDevice_SSM::useGeoCoords
static bool useGeoCoords(const SUMOVehicle &v)
Definition: MSDevice_SSM.cpp:3366
MSLane::getWidth
double getWidth() const
Returns the lane's width.
Definition: MSLane.h:556
MSDevice_SSM::Encounter::getRemainingExtraTime
double getRemainingExtraTime() const
returns the remaining extra time
Definition: MSDevice_SSM.cpp:366
MSDevice_SSM::EncounterApproachInfo::pet
std::pair< double, double > pet
Definition: MSDevice_SSM.h:319
MSBaseVehicle::getWidth
double getWidth() const
Returns the vehicle's width.
Definition: MSBaseVehicle.h:418
MSDevice_SSM::EncounterApproachInfo::foeConflictExitDist
double foeConflictExitDist
Definition: MSDevice_SSM.h:308
StringTokenizer::getVector
std::vector< std::string > getVector()
return vector of strings
Definition: StringTokenizer.cpp:191
DEBUG_COND
#define DEBUG_COND(ego)
Definition: MSDevice_SSM.cpp:56
MSDevice_SSM::myOldestActiveEncounterBegin
double myOldestActiveEncounterBegin
begin time of the oldest active encounter
Definition: MSDevice_SSM.h:720
MSDevice_SSM.h
config.h
DEBUG_COND_FIND
#define DEBUG_COND_FIND(ego)
Definition: MSDevice_SSM.cpp:57
MSDevice_SSM::EncounterApproachInfo::type
EncounterType type
Definition: MSDevice_SSM.h:303
MSDevice_SSM::ENCOUNTER_TYPE_MERGING_PASSED
@ ENCOUNTER_TYPE_MERGING_PASSED
ENCOUNTER_TYPE_FOLLOWING_PASSED.
Definition: MSDevice_SSM.h:113
MSCFModel::passingTime
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:596
MSLane::isInternal
bool isInternal() const
Definition: MSLane.cpp:2010
GeomHelper.h
MSDevice_SSM::myPastConflicts
EncounterQueue myPastConflicts
Past encounters that where qualified as conflicts and are not yet flushed to the output file.
Definition: MSDevice_SSM.h:722
MSDevice_SSM::Encounter::ego
const MSVehicle * ego
Definition: MSDevice_SSM.h:238
MSDevice_SSM::mySGAPspan
std::vector< double > mySGAPspan
All values for space gap.
Definition: MSDevice_SSM.h:733
MSVehicle::getSpeed
double getSpeed() const
Returns the vehicle's current speed.
Definition: MSVehicle.h:476
StringTokenizer.h
MSDevice_SSM::Encounter::foeTrajectory
Trajectory foeTrajectory
Trajectory of the foe vehicle.
Definition: MSDevice_SSM.h:260
MSDevice_SSM::ENCOUNTER_TYPE_ON_ADJACENT_LANES
@ ENCOUNTER_TYPE_ON_ADJACENT_LANES
ENCOUNTER_TYPE_ON_ADJACENT_LANES.
Definition: MSDevice_SSM.h:76
MSDevice_SSM::findFoeConflictLane
const MSLane * findFoeConflictLane(const MSVehicle *foe, const MSLane *egoConflictLane, double &distToConflictLane) const
Computes the conflict lane for the foe.
Definition: MSDevice_SSM.cpp:2394
gPrecision
int gPrecision
the precision for floating point outputs
Definition: StdDefs.cpp:26
MSDevice_SSM::myComputePET
bool myComputePET
Definition: MSDevice_SSM.h:710
MSDevice_SSM::FoeInfoMap
std::map< const MSVehicle *, FoeInfo * > FoeInfoMap
Definition: MSDevice_SSM.h:355
MSDevice_SSM::ENCOUNTER_TYPE_MERGING_FOLLOWER
@ ENCOUNTER_TYPE_MERGING_FOLLOWER
ENCOUNTER_TYPE_MERGING_FOLLOWER.
Definition: MSDevice_SSM.h:85
fn
static double fn[10]
Definition: odrSpiral.cpp:82
MSDevice_SSM::Encounter::Trajectory::v
PositionVector v
Definition: MSDevice_SSM.h:181
MSDevice_SSM::computeDRAC
static double computeDRAC(double gap, double followerSpeed, double leaderSpeed)
Computes the DRAC (deceleration to avoid a collision) for a lead/follow situation as defined,...
Definition: MSDevice_SSM.cpp:1356
MSDevice_SSM::ENCOUNTER_TYPE_FOLLOWING
@ ENCOUNTER_TYPE_FOLLOWING
ENCOUNTER_TYPE_FOLLOWING.
Definition: MSDevice_SSM.h:70
MSDevice_SSM::notifyEnter
bool notifyEnter(SUMOTrafficObject &veh, MSMoveReminder::Notification reason, const MSLane *enteredLane=0)
Called whenever the holder enteres a lane.
Definition: MSDevice_SSM.cpp:2786
MSLane.h
OutputDevice::writeXMLHeader
bool writeXMLHeader(const std::string &rootElement, const std::string &schemaFile, std::map< SumoXMLAttr, std::string > attrs=std::map< SumoXMLAttr, std::string >())
Writes an XML header with optional configuration.
Definition: OutputDevice.cpp:227
MSDevice_SSM::EncounterApproachInfo::egoConflictEntryDist
double egoConflictEntryDist
Definition: MSDevice_SSM.h:305
MSDevice_SSM::Encounter::egoConflictEntryTime
double egoConflictEntryTime
Times when the ego vehicle entered/left the conflict area. Currently only applies for crossing situat...
Definition: MSDevice_SSM.h:249
MSDevice_SSM::EncounterApproachInfo::foeConflictEntryDist
double foeConflictEntryDist
Definition: MSDevice_SSM.h:306
MSDevice_SSM::myComputeBR
bool myComputeBR
Definition: MSDevice_SSM.h:710
MSDevice_SSM::Encounter::maxDRAC
ConflictPointInfo maxDRAC
Definition: MSDevice_SSM.h:283
MSDevice_SSM::Encounter::resetExtraTime
void resetExtraTime(double value)
resets remainingExtraTime to the given value
Definition: MSDevice_SSM.cpp:354
MSDevice_SSM::estimateConflictTimes
static void estimateConflictTimes(EncounterApproachInfo &eInfo)
Estimates the time until conflict for the vehicles based on the distance to the conflict entry points...
Definition: MSDevice_SSM.cpp:857
MSDevice_SSM::determineTTCandDRAC
void determineTTCandDRAC(EncounterApproachInfo &eInfo) const
Discriminates between different encounter types and correspondingly determines TTC and DRAC for those...
Definition: MSDevice_SSM.cpp:1147
MSVehicleControl.h
MSDevice_SSM::MSDevice_SSM
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.
Definition: MSDevice_SSM.cpp:2720
MSDevice_SSM::SSM_WARN_THRESHOLDS
@ SSM_WARN_THRESHOLDS
Definition: MSDevice_SSM.h:755
Named::getID
const std::string & getID() const
Returns the id.
Definition: Named.h:76
MSMoveReminder::Notification
Notification
Definition of a vehicle state.
Definition: MSMoveReminder.h:91
WRITE_ERROR
#define WRITE_ERROR(msg)
Definition: MsgHandler.h:283
MSDevice_SSM::myUseGeoCoords
bool myUseGeoCoords
Whether to use the original coordinate system for output.
Definition: MSDevice_SSM.h:708
MSDevice_SSM::ENCOUNTER_TYPE_FOE_LEFT_CONFLICT_AREA
@ ENCOUNTER_TYPE_FOE_LEFT_CONFLICT_AREA
ENCOUNTER_TYPE_FOE_LEFT_CONFLICT_AREA.
Definition: MSDevice_SSM.h:104
MSNet::getVehicleControl
MSVehicleControl & getVehicleControl()
Returns the vehicle control.
Definition: MSNet.h:336
MSDevice_SSM::Encounter::closingRequested
bool closingRequested
this flag is set by updateEncounter() or directly in processEncounters(), where encounters are closed...
Definition: MSDevice_SSM.h:288
MSDevice_SSM::encounterToString
static std::string encounterToString(EncounterType type)
Definition: MSDevice_SSM.h:120
MSDevice_SSM::myComputeSGAP
bool myComputeSGAP
Definition: MSDevice_SSM.h:710
MSVehicle::getPositionAlongBestLanes
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:1302
MSDevice_SSM::determineConflictPoint
static void determineConflictPoint(EncounterApproachInfo &eInfo)
Calculates the (x,y)-coordinate for the eventually predicted conflict point and stores the result in ...
Definition: MSDevice_SSM.cpp:804
MSDevice_SSM::processEncounters
void processEncounters(FoeInfoMap &foes, bool forceClose=false)
Finds encounters for which the foe vehicle has disappeared from range. remainingExtraTime is decrease...
Definition: MSDevice_SSM.cpp:545
Parameterised::knowsParameter
bool knowsParameter(const std::string &key) const
Returns whether the parameter is known.
Definition: Parameterised.cpp:66
MSAbstractLaneChangeModel.h
MSDevice_SSM::EncounterType
EncounterType
Different types of encounters corresponding to relative positions of the vehicles....
Definition: MSDevice_SSM.h:65
MSDevice_SSM::ENCOUNTER_TYPE_MERGING_ADJACENT
@ ENCOUNTER_TYPE_MERGING_ADJACENT
ENCOUNTER_TYPE_MERGING_ADJACENT.
Definition: MSDevice_SSM.h:87
MSLane::getEntryLink
MSLink * getEntryLink() const
Returns the entry link if this is an internal lane, else 0.
Definition: MSLane.cpp:2138
MSVehicle
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:79
MSVehicle::getMaxSpeedOnLane
double getMaxSpeedOnLane() const
Returns the maximal speed for the vehicle on its current lane (including speed factor and deviation,...
Definition: MSVehicle.h:570
MSDevice_SSM::SSM_WARN_FILE
@ SSM_WARN_FILE
Definition: MSDevice_SSM.h:759
MSVehicleDevice
Abstract in-vehicle device.
Definition: MSVehicleDevice.h:54
MSDevice_SSM::Encounter::foe
const MSVehicle * foe
Definition: MSDevice_SSM.h:239