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 /****************************************************************************/
18 // An SSM-device logs encounters / conflicts of the carrying vehicle with other surrounding vehicles
19 // XXX: Preliminary implementation. Use with care. Especially rerouting vehicles could be problematic.
20 // TODO: implement SSM time-gap (estimated conflict entry and exit times are already calculated for PET calculation)
21 /****************************************************************************/
22 
23 // ===========================================================================
24 // included modules
25 // ===========================================================================
26 #include <config.h>
27 
28 #include <iostream>
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 
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: " << toString(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: " << toString(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 const MSLane*
2393 MSDevice_SSM::findFoeConflictLane(const MSVehicle* foe, const MSLane* egoConflictLane, double& distToConflictLane) const {
2394 
2395 #ifdef DEBUG_SSM
2396  if (DEBUG_COND(myHolderMS))
2397  std::cout << SIMTIME << " findFoeConflictLane() for foe '"
2398  << foe->getID() << "' on lane '" << foe->getLane()->getID()
2399  << "' (with egoConflictLane=" << (egoConflictLane == 0 ? "NULL" : egoConflictLane->getID())
2400  << ")\nfoeBestLanes: " << ::toString(foe->getBestLanesContinuation())
2401  << std::endl;
2402 #endif
2403  if (foe->getLaneChangeModel().isOpposite()) {
2404  // distinguish three cases
2405  // 1) foe is driving in the same direction as ego and ego is driving in lane direction -> ENCOUNTER_TYPE_ON_ADJACENT_LANES
2406  // 2) foe is driving in the same direction as ego and ego is also driving in the opposite direction -> ENCOUNTER_TYPE_FOLLOWING
2407  // 3) foe is driving in the opposite direction as ego and both are driving way from each other -> ENCOUNTER_TYPE_NOCONFLICT_AHEAD
2408  // 3) foe is driving in the opposite direction as ego and both are driving towards each other -> ENCOUNTER_TYPE_ONCOMING
2409 #ifdef DEBUG_SSM_OPPOSITE
2410 #endif
2411  auto egoIt = std::find(myHolder.getCurrentRouteEdge(), myHolder.getRoute().end(), foe->getEdge());
2412  if (egoIt != myHolder.getRoute().end()) {
2413  // same direction, foe is leader
2415  return foe->getLane();
2416  } else {
2417  // adjacent
2418  return nullptr;
2419  }
2420  }
2421  auto foeIt = std::find(foe->getCurrentRouteEdge(), foe->getRoute().end(), myHolder.getEdge());
2422  if (foeIt != foe->getRoute().end()) {
2423  // same direction, ego is leader
2425  return egoConflictLane;
2426  } else {
2427  // adjacent
2428  return nullptr;
2429  }
2430  }
2431  auto egoIt2 = std::find(myHolder.getCurrentRouteEdge(), myHolder.getRoute().end(), foe->getEdge()->getOppositeEdge());
2432  if (egoIt2 != myHolder.getRoute().end()) {
2433  // opposite direction, driving towards each other
2434  return egoConflictLane;
2435  } else {
2436  // opposite direction, driving away from each other
2437  return nullptr;
2438  }
2439  }
2440 
2441  MSLane* foeLane = foe->getLane();
2442  std::vector<MSLane*>::const_iterator laneIter = foe->getBestLanesContinuation().begin();
2443  std::vector<MSLane*>::const_iterator foeBestLanesEnd = foe->getBestLanesContinuation().end();
2444  assert(foeLane->isInternal() || *laneIter == foeLane);
2445  distToConflictLane = -foe->getPositionOnLane();
2446 
2447  // Potential conflict lies on junction if egoConflictLane is internal
2448  const MSJunction* conflictJunction = egoConflictLane->isInternal() ? egoConflictLane->getEdge().getToJunction() : nullptr;
2449 #ifdef DEBUG_SSM
2450  if (DEBUG_COND(myHolderMS))
2451  if (conflictJunction != 0) {
2452  std::cout << "Potential conflict on junction '" << conflictJunction->getID()
2453  << std::endl;
2454  }
2455 #endif
2456  if (foeLane->isInternal() && foeLane->getEdge().getToJunction() == conflictJunction) {
2457  // foe is already on the conflict junction
2458  if (egoConflictLane != nullptr && egoConflictLane->isInternal() && egoConflictLane->getLinkCont()[0]->getViaLane() == foeLane) {
2459  distToConflictLane += egoConflictLane->getLength();
2460  }
2461  return foeLane;
2462  }
2463 
2464  // Foe is not on the conflict junction
2465 
2466  // Leading internal lanes in bestlanes are resembled as a single NULL-pointer skip them
2467  if (*laneIter == nullptr) {
2468  while (foeLane != nullptr && foeLane->isInternal()) {
2469  distToConflictLane += foeLane->getLength();
2470  foeLane = foeLane->getLinkCont()[0]->getViaLane();
2471  }
2472  ++laneIter;
2473  assert(laneIter == foeBestLanesEnd || *laneIter != 0);
2474  }
2475 
2476  // Look for the junction downstream along foeBestLanes
2477  while (laneIter != foeBestLanesEnd && distToConflictLane <= myRange) {
2478  // Eventual internal lanes were skipped
2479  assert(*laneIter == foeLane || foeLane == 0);
2480  foeLane = *laneIter;
2481  assert(!foeLane->isInternal());
2482  if (&foeLane->getEdge() == &egoConflictLane->getEdge()) {
2483 #ifdef DEBUG_SSM
2484  if (DEBUG_COND(myHolderMS)) {
2485  std::cout << "Found conflict lane for foe: '" << foeLane->getID() << "'" << std::endl;
2486  }
2487 #endif
2488  // found the potential conflict edge along foeBestLanes
2489  return foeLane;
2490  }
2491  // No conflict on foeLane
2492  distToConflictLane += foeLane->getLength();
2493 
2494  // set laneIter to next non internal lane along foeBestLanes
2495  ++laneIter;
2496  if (laneIter == foeBestLanesEnd) {
2497  return nullptr;
2498  }
2499  MSLane* nextNonInternalLane = *laneIter;
2500  MSLink* link = foeLane->getLinkTo(nextNonInternalLane);
2501  // Set foeLane to first internal lane on the next junction
2502  foeLane = link->getViaLane();
2503  assert(foeLane == 0 || foeLane->isInternal());
2504  if (foeLane == nullptr) {
2505  foeLane = nextNonInternalLane;
2506  continue;
2507  }
2508  if (foeLane->getEdge().getToJunction() == conflictJunction) {
2509  assert(foeLane != 0);
2510 #ifdef DEBUG_SSM
2511  if (DEBUG_COND(myHolderMS)) {
2512  std::cout << "Found conflict lane for foe: '" << foeLane->getID() << "'" << std::endl;
2513  }
2514 #endif
2515  // found egoConflictLane, resp. the conflict junction, along foeBestLanes
2516  return foeLane;
2517  }
2518  // No conflict on junction
2519  distToConflictLane += link->getInternalLengthsAfter();
2520  foeLane = nextNonInternalLane;
2521  }
2522  // Didn't find conflicting lane on foeBestLanes within range.
2523  return nullptr;
2524 }
2525 
2526 void
2528 #ifdef DEBUG_SSM
2529  if (DEBUG_COND(myHolderMS)) {
2530  std::cout << "\n" << SIMTIME << " Device '" << getID() << "' flushConflicts past=" << myPastConflicts.size()
2531  << " oldestActive=" << (myOldestActiveEncounterBegin == INVALID ? -1 : myOldestActiveEncounterBegin)
2532  << " topBegin=" << (myPastConflicts.size() > 0 ? myPastConflicts.top()->begin : -1)
2533  << "\n";
2534  }
2535 #endif
2536  while (!myPastConflicts.empty()) {
2537  Encounter* top = myPastConflicts.top();
2538  if (flushAll || top->begin <= myOldestActiveEncounterBegin) {
2539  writeOutConflict(top);
2540  myPastConflicts.pop();
2541  delete top;
2542  } else {
2543  break;
2544  }
2545  }
2546 }
2547 
2548 void
2550  std::string egoID = myHolderMS->getID();
2551 #ifdef DEBUG_SSM
2552  if (DEBUG_COND(myHolderMS))
2553  std::cout << SIMTIME << " flushGlobalMeasures() of vehicle '"
2554  << egoID << "'"
2555  << "'\ntoGeo=" << myUseGeoCoords << std::endl;
2556 #endif
2558  myOutputFile->openTag("globalMeasures");
2559  myOutputFile->writeAttr("ego", egoID);
2561  if (myComputeBR) {
2562  myOutputFile->openTag("BRSpan").writeAttr("values", myBRspan).closeTag();
2563 
2564  if (myMaxBR.second != 0.0) {
2565  if (myUseGeoCoords) {
2566  toGeo(myMaxBR.first.second);
2567  }
2568  myOutputFile->openTag("maxBR").writeAttr("time", myMaxBR.first.first).writeAttr("position", ::toString(myMaxBR.first.second)).writeAttr("value", myMaxBR.second).closeTag();
2569  }
2570  }
2571 
2572  if (myComputeSGAP) {
2574  if (myMinSGAP.second != "") {
2575  if (myUseGeoCoords) {
2576  toGeo(myMinSGAP.first.first.second);
2577  }
2578  myOutputFile->openTag("minSGAP").writeAttr("time", myMinSGAP.first.first.first)
2579  .writeAttr("position", ::toString(myMinSGAP.first.first.second))
2580  .writeAttr("value", myMinSGAP.first.second)
2581  .writeAttr("leader", myMinSGAP.second).closeTag();
2582  }
2583  }
2584 
2585  if (myComputeTGAP) {
2587  if (myMinTGAP.second != "") {
2588  if (myUseGeoCoords) {
2589  toGeo(myMinTGAP.first.first.second);
2590  }
2591  myOutputFile->openTag("minTGAP").writeAttr("time", myMinTGAP.first.first.first)
2592  .writeAttr("position", ::toString(myMinTGAP.first.first.second))
2593  .writeAttr("value", myMinTGAP.first.second)
2594  .writeAttr("leader", myMinTGAP.second).closeTag();
2595  }
2596  }
2597  // close globalMeasures
2599  }
2600 }
2601 
2602 void
2605 }
2606 
2607 void
2609  for (Position& x : xv) {
2610  toGeo(x);
2611  }
2612 }
2613 
2614 void
2616 #ifdef DEBUG_SSM
2617  if (DEBUG_COND(myHolderMS))
2618  std::cout << SIMTIME << " writeOutConflict() of vehicles '"
2619  << e->egoID << "' and '" << e->foeID
2620  << "'\ntoGeo=" << myUseGeoCoords << std::endl;
2621 #endif
2622  myOutputFile->openTag("conflict");
2623  myOutputFile->writeAttr("begin", e->begin).writeAttr("end", e->end);
2624  myOutputFile->writeAttr("ego", e->egoID).writeAttr("foe", e->foeID);
2625 
2626  if (mySaveTrajectories) {
2627  myOutputFile->openTag("timeSpan").writeAttr("values", e->timeSpan).closeTag();
2628  myOutputFile->openTag("typeSpan").writeAttr("values", e->typeSpan).closeTag();
2629 
2630  // Some useful snippets for that (from MSFCDExport.cpp):
2631  if (myUseGeoCoords) {
2632  toGeo(e->egoTrajectory.x);
2633  toGeo(e->foeTrajectory.x);
2635  }
2636 
2638  myOutputFile->openTag("egoVelocity").writeAttr("values", ::toString(e->egoTrajectory.v)).closeTag();
2639 
2641  myOutputFile->openTag("foeVelocity").writeAttr("values", ::toString(e->foeTrajectory.v)).closeTag();
2642 
2644  }
2645 
2646  if (myComputeTTC) {
2647  if (mySaveTrajectories) {
2648  myOutputFile->openTag("TTCSpan").writeAttr("values", makeStringWithNAs(e->TTCspan, INVALID)).closeTag();
2649  }
2650  if (e->minTTC.time == INVALID) {
2651  myOutputFile->openTag("minTTC").writeAttr("time", "NA").writeAttr("position", "NA").writeAttr("type", "NA").writeAttr("value", "NA").closeTag();
2652  } else {
2653  std::string time = ::toString(e->minTTC.time);
2654  std::string type = ::toString(int(e->minTTC.type));
2655  std::string value = ::toString(e->minTTC.value);
2656  if (myUseGeoCoords) {
2657  toGeo(e->minTTC.pos);
2658  }
2659  std::string position = ::toString(e->minTTC.pos, myUseGeoCoords ? gPrecisionGeo : gPrecision);
2660  myOutputFile->openTag("minTTC").writeAttr("time", time).writeAttr("position", position).writeAttr("type", type).writeAttr("value", value).closeTag();
2661  }
2662  }
2663  if (myComputeDRAC) {
2664  if (mySaveTrajectories) {
2665  myOutputFile->openTag("DRACSpan").writeAttr("values", makeStringWithNAs(e->DRACspan, {0.0, INVALID})).closeTag();
2666  }
2667  if (e->maxDRAC.time == INVALID) {
2668  myOutputFile->openTag("maxDRAC").writeAttr("time", "NA").writeAttr("position", "NA").writeAttr("type", "NA").writeAttr("value", "NA").closeTag();
2669  } else {
2670  std::string time = ::toString(e->maxDRAC.time);
2671  std::string type = ::toString(int(e->maxDRAC.type));
2672  std::string value = ::toString(e->maxDRAC.value);
2673  if (myUseGeoCoords) {
2674  toGeo(e->maxDRAC.pos);
2675  }
2676  std::string position = ::toString(e->maxDRAC.pos, myUseGeoCoords ? gPrecisionGeo : gPrecision);
2677  myOutputFile->openTag("maxDRAC").writeAttr("time", time).writeAttr("position", position).writeAttr("type", type).writeAttr("value", value).closeTag();
2678  }
2679  }
2680  if (myComputePET) {
2681  if (e->PET.time == INVALID) {
2682  myOutputFile->openTag("PET").writeAttr("time", "NA").writeAttr("position", "NA").writeAttr("type", "NA").writeAttr("value", "NA").closeTag();
2683  } else {
2684  std::string time = ::toString(e->PET.time);
2685  std::string type = ::toString(int(e->PET.type));
2686  std::string value = ::toString(e->PET.value);
2687  if (myUseGeoCoords) {
2688  toGeo(e->PET.pos);
2689  }
2690  std::string position = ::toString(e->PET.pos, myUseGeoCoords ? gPrecisionGeo : gPrecision);
2691  myOutputFile->openTag("PET").writeAttr("time", time).writeAttr("position", position).writeAttr("type", type).writeAttr("value", value).closeTag();
2692  }
2693  }
2695 }
2696 
2697 std::string
2698 MSDevice_SSM::makeStringWithNAs(std::vector<double> v, double NA, std::string sep) {
2699  std::string res = "";
2700  for (std::vector<double>::const_iterator i = v.begin(); i != v.end(); ++i) {
2701  res += (i == v.begin() ? "" : sep) + (*i == NA ? "NA" : ::toString(*i));
2702  }
2703  return res;
2704 }
2705 
2706 std::string
2707 MSDevice_SSM::makeStringWithNAs(std::vector<double> v, std::vector<double> NAs, std::string sep) {
2708  std::string res = "";
2709  for (std::vector<double>::const_iterator i = v.begin(); i != v.end(); ++i) {
2710  res += (i == v.begin() ? "" : sep) + (find(NAs.begin(), NAs.end(), *i) != NAs.end() ? "NA" : ::toString(*i));
2711  }
2712  return res;
2713 }
2714 
2715 
2716 // ---------------------------------------------------------------------------
2717 // MSDevice_SSM-methods
2718 // ---------------------------------------------------------------------------
2719 MSDevice_SSM::MSDevice_SSM(SUMOVehicle& holder, const std::string& id, std::string outputFilename, std::map<std::string, double> thresholds,
2720  bool trajectories, double range, double extraTime, bool useGeoCoords) :
2721  MSVehicleDevice(holder, id),
2722  myThresholds(thresholds),
2723  mySaveTrajectories(trajectories),
2724  myRange(range),
2725  myExtraTime(extraTime),
2728  myMaxBR(std::make_pair(-1, Position(0., 0.)), 0.0),
2729  myMinSGAP(std::make_pair(std::make_pair(-1, Position(0., 0.)), std::numeric_limits<double>::max()), ""),
2730  myMinTGAP(std::make_pair(std::make_pair(-1, Position(0., 0.)), std::numeric_limits<double>::max()), "") {
2731  // Take care! Holder is currently being constructed. Cast occurs before completion.
2732  myHolderMS = static_cast<MSVehicle*>(&holder);
2733 
2734  myComputeTTC = myThresholds.find("TTC") != myThresholds.end();
2735  myComputeDRAC = myThresholds.find("DRAC") != myThresholds.end();
2736  myComputePET = myThresholds.find("PET") != myThresholds.end();
2737 
2738  myComputeBR = myThresholds.find("BR") != myThresholds.end();
2739  myComputeSGAP = myThresholds.find("SGAP") != myThresholds.end();
2740  myComputeTGAP = myThresholds.find("TGAP") != myThresholds.end();
2741 
2744 
2745  // XXX: Who deletes the OutputDevice?
2746  myOutputFile = &OutputDevice::getDevice(outputFilename);
2747 // TODO: make xsd, include header
2748 // myOutputFile.writeXMLHeader("SSMLog", "SSMLog.xsd");
2749  if (createdOutputFiles.count(outputFilename) == 0) {
2750  myOutputFile->writeXMLHeader("SSMLog", "");
2751  createdOutputFiles.insert(outputFilename);
2752  }
2753  // register at static instance container
2754  myInstances->insert(this);
2755 
2756 #ifdef DEBUG_SSM
2757  if (DEBUG_COND(myHolderMS)) {
2758  std::vector<std::string> measures;
2759  std::vector<double> threshVals;
2760  for (std::map<std::string, double>::const_iterator i = myThresholds.begin(); i != myThresholds.end(); ++i) {
2761  measures.push_back(i->first);
2762  threshVals.push_back(i->second);
2763  }
2764  std::cout << "Initialized ssm device '" << id << "' with "
2765  << "myMeasures=" << joinToString(measures, " ")
2766  << ", myThresholds=" << joinToString(threshVals, " ")
2767  << ", mySaveTrajectories=" << mySaveTrajectories
2768  << ", myRange=" << myRange << ", output file=" << outputFilename << ", extra time=" << myExtraTime << ", useGeo=" << myUseGeoCoords << "\n";
2769  }
2770 #endif
2771 }
2772 
2775  // Deleted in ~BaseVehicle()
2776  // unregister from static instance container
2777  myInstances->erase(this);
2778  resetEncounters();
2779  flushConflicts(true);
2781 }
2782 
2783 
2784 bool
2786  assert(veh.isVehicle());
2787 #ifdef DEBUG_SSM_NOTIFICATIONS
2788  MSBaseVehicle* v = (MSBaseVehicle*) &veh;
2789  std::cout << "device '" << getID() << "' notifyEnter: reason=" << reason << " currentEdge=" << v->getLane()->getEdge().getID() << "\n";
2790 #else
2791  UNUSED_PARAMETER(veh);
2792  UNUSED_PARAMETER(reason);
2793 #endif
2794  return true; // keep the device
2795 }
2796 
2797 bool
2799  MSMoveReminder::Notification reason, const MSLane* /* enteredLane */) {
2800  assert(veh.isVehicle());
2801 #ifdef DEBUG_SSM_NOTIFICATIONS
2802  MSBaseVehicle* v = (MSBaseVehicle*) &veh;
2803  std::cout << "device '" << getID() << "' notifyLeave: reason=" << reason << " currentEdge=" << v->getLane()->getEdge().getID() << "\n";
2804 #else
2805  UNUSED_PARAMETER(veh);
2806  UNUSED_PARAMETER(reason);
2807 #endif
2808  return true; // keep the device
2809 }
2810 
2811 bool
2812 MSDevice_SSM::notifyMove(SUMOTrafficObject& /* veh */, double /* oldPos */,
2813  double /* newPos */, double newSpeed) {
2814 #ifdef DEBUG_SSM_NOTIFICATIONS
2815  std::cout << "device '" << getID() << "' notifyMove: newSpeed=" << newSpeed << "\n";
2816 #else
2817  UNUSED_PARAMETER(newSpeed);
2818 #endif
2819  return true; // keep the device
2820 }
2821 
2822 
2823 void
2824 MSDevice_SSM::findSurroundingVehicles(const MSVehicle& veh, double range, FoeInfoMap& foeCollector) {
2825  if (!veh.isOnRoad()) {
2826  return;
2827  }
2828 #ifdef DEBUG_SSM_SURROUNDING
2829  gDebugFlag3 = DEBUG_COND(&veh);
2830  if (gDebugFlag3) {
2831  std::cout << SIMTIME << " Looking for surrounding vehicles for ego vehicle '" << veh.getID()
2832  << "' on edge '" << veh.getLane()->getEdge().getID()
2833  << "'."
2834  << "\nVehicle's best lanes = " << ::toString(veh.getBestLanesContinuation())
2835  << std::endl;
2836  }
2837 #endif
2838 
2839 
2840  // The requesting vehicle's current route
2841  // XXX: Restriction to route scanning may have to be generalized to scanning of possible continuations when
2842  // considering situations involving sudden route changes. See also the definition of the EncounterTypes.
2843  // A second problem is that following situations on deviating routes may result in closing encounters
2844  // too early if a leading foe is not traced on its new lane. (see test 'foe_leader_deviating_routes')
2845 
2846  // If veh is on an internal edge, the edgeIter points towards the last edge before the junction
2847  //ConstMSEdgeVector::const_iterator edgeIter = veh.getCurrentRouteEdge();
2848  //assert(*edgeIter != 0);
2849 
2850  // Best continuation lanes for the ego vehicle
2851  std::vector<MSLane*> egoBestLanes = veh.getBestLanesContinuation();
2852  const bool isOpposite = veh.getLaneChangeModel().isOpposite();
2853  if (isOpposite) {
2854  for (int i = 0; i < (int)egoBestLanes.size(); i++) {
2855  if (egoBestLanes[i] != nullptr && egoBestLanes[i]->getEdge().getOppositeEdge() != nullptr) {
2856  egoBestLanes[i] = egoBestLanes[i]->getEdge().getOppositeEdge()->getLanes().back();
2857  }
2858  }
2859  }
2860  std::vector<MSLane*>::const_iterator laneIter = egoBestLanes.begin();
2861 
2862  // current lane in loop below
2863  const MSLane* lane = veh.getLane();
2864  assert(lane->isInternal() || lane == *laneIter);
2865  assert(lane != 0);
2866  // next non-internal lane on the route
2867  const MSLane* nextNonInternalLane = nullptr;
2868 
2869  const MSEdge* edge; // current edge in loop below
2870 
2871  // Init pos with vehicle's current position. Below pos is set to zero to denote
2872  // the beginning position of the currently considered edge
2873  double pos = veh.getPositionOnLane();
2874  // remainingDownstreamRange is the range minus the distance that is already scanned downstream along the vehicles route
2875  double remainingDownstreamRange = range;
2876  // 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)
2877  double distToConflictLane = isOpposite ? pos - veh.getLane()->getLength() : -pos;
2878  // junctions that were encountered during downstream scan. Memorized to break search at re-scan in recurrent nets.
2879  std::set<const MSJunction*> seenJunctions;
2880  // Starting points for upstream scans to be executed after downstream scan is complete.
2881  // Holds pairs (starting edge, starting position on edge)
2882  std::vector<UpstreamScanStartInfo> upstreamScanStartPositions;
2883 
2884 
2885  // if the current edge is internal, collect all vehicles from the junction and within upstream range (except on the vehicles own edge),
2886  // this is analogous to the code treating junctions in the loop below. Note that the distance on the junction itself is not included into
2887  // range, so vehicles farther away than range can be collected, too.
2888  if (lane->isInternal()) {
2889  edge = &(lane->getEdge());
2890 
2891 #ifdef DEBUG_SSM_SURROUNDING
2892  if (gDebugFlag3) {
2893  std::cout << SIMTIME << " Vehicle '" << veh.getID() << "' is on internal edge " << edge->getID() << "'." << std::endl;
2894 // << "Previous edge of its route: '" << (*edgeIter)->getID() << "'" << std::endl;
2895  }
2896 #endif
2897 
2898  assert(edge->getToJunction() == edge->getFromJunction());
2899 
2900  const MSJunction* junction = edge->getToJunction();
2901  // Collect vehicles on the junction
2902  getVehiclesOnJunction(junction, distToConflictLane, lane, foeCollector);
2903 
2904  // Collect vehicles on incoming edges.
2905  // Note that this includes the previous edge on the ego vehicle's route.
2906  // (The distance on the current internal edge is ignored)
2907  const ConstMSEdgeVector& incoming = junction->getIncoming();
2908  for (ConstMSEdgeVector::const_iterator ei = incoming.begin(); ei != incoming.end(); ++ei) {
2909  if ((*ei)->isInternal()) {
2910  continue;
2911  }
2912  // Upstream range is taken from the vehicle's back
2913  upstreamScanStartPositions.push_back(UpstreamScanStartInfo(*ei, (*ei)->getLength(), range + veh.getLength(), distToConflictLane, lane));
2914  }
2915 
2916 // // Take into account internal distance covered on the current lane
2917 // (commented out, because upstream scanning disregards internal lanes on the last scanned junction
2918 // -- this makes the scanning symmetric between leader and follower)
2919 // remainingDownstreamRange -= lane->getLength() - pos;
2920 
2921  // Take into account non-internal lengths until next non-internal lane
2922  MSLink* link = lane->getLinkCont()[0];
2923  remainingDownstreamRange -= link->getInternalLengthsAfter();
2924  distToConflictLane += lane->getLength() + link->getInternalLengthsAfter();
2925 
2926  // The next non-internal lane
2927  pos = 0.;
2928  lane = *(++laneIter);
2929  edge = &lane->getEdge();
2930  } else {
2931  // Collect all vehicles in range behind ego vehicle
2932  edge = &(lane->getEdge());
2933  upstreamScanStartPositions.push_back(UpstreamScanStartInfo(edge, pos, range + veh.getLength(), distToConflictLane, lane));
2934  }
2935 
2936  assert(lane != 0);
2937  assert(!lane->isInternal());
2938 
2939  // Advance downstream the ego vehicle's route for distance 'range'.
2940  // Collect all vehicles on the traversed Edges and on incoming edges at junctions
2941  // and starting points for upstream vehicle collection strated below after downstream scan.
2942  while (remainingDownstreamRange > 0.) {
2943 #ifdef DEBUG_SSM_SURROUNDING
2944  if (gDebugFlag3) {
2945  std::cout << SIMTIME << " Scanning downstream for vehicle '" << veh.getID() << "' on lane '" << veh.getLane()->getID() << "', position=" << pos << ".\n"
2946  << "Considering edge '" << edge->getID() << "' Remaining downstream range = " << remainingDownstreamRange
2947  << "\nbestLanes=" << ::toString(egoBestLanes) << "\n"
2948  << std::endl;
2949  }
2950 #endif
2951  assert(!edge->isInternal());
2952  assert(!lane->isInternal());
2953  assert(pos == 0 || lane == veh.getLane());
2954  if (pos + remainingDownstreamRange < lane->getLength()) {
2955  // scan range ends on this lane
2956  upstreamScanStartPositions.push_back(UpstreamScanStartInfo(edge, pos + remainingDownstreamRange, remainingDownstreamRange, distToConflictLane, lane));
2957  // scanned required downstream range
2958  break;
2959  } else {
2960  // Also need to scan area that reaches beyond the lane
2961  // Collecting vehicles on non-internal edge ahead
2962  upstreamScanStartPositions.push_back(UpstreamScanStartInfo(edge, edge->getLength(), edge->getLength() - pos, distToConflictLane, lane));
2963  // account for scanned distance on lane
2964  remainingDownstreamRange -= lane->getLength() - pos;
2965  distToConflictLane += lane->getLength();
2966  pos = 0.;
2967 
2968  // proceed to next non-internal lane
2969  ++laneIter;
2970  assert(laneIter == egoBestLanes.end() || *laneIter != 0);
2971 
2972  // If the vehicle's best lanes go on, collect vehicles on the upcoming junction
2973  if (laneIter != egoBestLanes.end()) {
2974  // Upcoming junction
2975  const MSJunction* junction = lane->getEdge().getToJunction();
2976 
2977  // Find connection for ego on the junction
2978  nextNonInternalLane = *laneIter;
2979  MSLink* link = lane->getLinkTo(nextNonInternalLane);
2980  if (isOpposite && link == nullptr) {
2981  link = nextNonInternalLane->getLinkTo(lane);
2982  if (link == nullptr) {
2983  link = lane->getOpposite()->getLinkTo(nextNonInternalLane);
2984  }
2985  }
2986  assert(link != 0 || link->getLength() == 0.);
2987 
2988  // First lane of the connection
2989  lane = link->getViaLane();
2990  if (lane == nullptr) {
2991  // link without internal lane
2992  lane = nextNonInternalLane;
2993  edge = &(lane->getEdge());
2994  if (seenJunctions.count(junction) == 0) {
2995  seenJunctions.insert(junction);
2996  continue;
2997  } else {
2998  break;
2999  }
3000  }
3001 
3002  if (seenJunctions.count(junction) == 0) {
3003  // Collect vehicles on the junction, if it wasn't considered already
3004  getVehiclesOnJunction(junction, distToConflictLane, lane, foeCollector);
3005  seenJunctions.insert(junction);
3006  // Collect vehicles on incoming edges (except the last edge, where we already collected). Use full range.
3007  const ConstMSEdgeVector& incoming = junction->getIncoming();
3008  for (ConstMSEdgeVector::const_iterator ei = incoming.begin(); ei != incoming.end(); ++ei) {
3009  if (*ei == edge || (*ei)->isInternal()) {
3010  continue;
3011  }
3012  upstreamScanStartPositions.push_back(UpstreamScanStartInfo(*ei, (*ei)->getLength(), range, distToConflictLane, lane));
3013  }
3014  // account for scanned distance on junction
3015  double linkLength = link->getInternalLengthsAfter();
3016  remainingDownstreamRange -= linkLength;
3017  distToConflictLane += linkLength;
3018 #ifdef DEBUG_SSM_SURROUNDING
3019  if (gDebugFlag3) {
3020  std::cout << " Downstream Scan for vehicle '" << veh.getID() << "' proceeded over junction '" << junction->getID()
3021  << "',\n linkLength=" << linkLength << ", remainingDownstreamRange=" << remainingDownstreamRange
3022  << std::endl;
3023  }
3024 #endif
3025 
3026  // update ego's lane to next non internal edge
3027  lane = nextNonInternalLane;
3028  edge = &(lane->getEdge());
3029  } else {
3030 #ifdef DEBUG_SSM_SURROUNDING
3031  if (gDebugFlag3) {
3032  std::cout << " Downstream Scan for vehicle '" << veh.getID() << "' stops at junction '" << junction->getID()
3033  << "', which has already been scanned."
3034  << std::endl;
3035  }
3036 #endif
3037  break;
3038  }
3039  } else {
3040  // Further vehicle path unknown, break search
3041  break;
3042  }
3043  }
3044  }
3045  // Scan upstream branches from collected starting points
3046  for (UpstreamScanStartInfo& i : upstreamScanStartPositions) {
3047  getUpstreamVehicles(i, foeCollector, seenJunctions);
3048  }
3049 
3050  // remove ego vehicle
3051  foeCollector.erase(&veh);
3052  gDebugFlag3 = false;
3053 }
3054 
3055 void
3056 MSDevice_SSM::getUpstreamVehicles(const UpstreamScanStartInfo& scanStart, FoeInfoMap& foeCollector, std::set<const MSJunction*>& seenJunctions) {
3057 #ifdef DEBUG_SSM_SURROUNDING
3058  if (gDebugFlag3) {
3059  std::cout << SIMTIME << " getUpstreamVehicles() for edge '" << scanStart.edge->getID() << "'"
3060  << " pos = " << scanStart.pos << " range = " << scanStart.range
3061  << "\nFound vehicles:"
3062  << std::endl;
3063  }
3064 #endif
3065  if (scanStart.range <= 0) {
3066  return;
3067  }
3068 
3069  const std::vector<MSLane*>& lanes = scanStart.edge->getLanes();
3070  // Collect vehicles on the given edge with position in [pos-range,pos]
3071  for (std::vector<MSLane*>::const_iterator li = lanes.begin(); li != lanes.end(); ++li) {
3072  MSLane* lane = *li;
3073  const MSLane::VehCont& vehicles = lane->getVehiclesSecure();
3074  for (MSLane::VehCont::const_iterator vi = vehicles.begin(); vi != vehicles.end(); ++vi) {
3075  MSVehicle* veh = *vi;
3076  if (foeCollector.find(veh) != foeCollector.end()) {
3077  // vehicle already recognized, earlier recognized conflict has priority
3078  continue;
3079  }
3080  if (veh->getPositionOnLane() - veh->getLength() <= scanStart.pos && veh->getPositionOnLane() >= scanStart.pos - scanStart.range) {
3081 #ifdef DEBUG_SSM_SURROUNDING
3082  if (gDebugFlag3) {
3083  std::cout << veh->getID() << "\n";
3084  }
3085 #endif
3086  FoeInfo* c = new FoeInfo(); // c is deleted in updateEncounter()
3088  c->egoConflictLane = scanStart.egoConflictLane;
3089  foeCollector[veh] = c;
3090  }
3091  }
3092  lane->releaseVehicles();
3093  }
3094 
3095 #ifdef DEBUG_SSM_SURROUNDING
3096  if (gDebugFlag3) {
3097  std::cout << std::endl;
3098  }
3099 #endif
3100 
3101  // TODO: Gather vehicles from opposite direction. This should happen in any case, where opposite direction overtaking is possible.
3102  // If it isn't it might still be nicer to trace oncoming vehicles for the resulting trajectories in the encounters
3103  // if (edge->hasOpposite...)
3104 
3105  if (scanStart.range <= scanStart.pos) {
3106  return;
3107  }
3108 
3109  // Here we have: range > pos, i.e. we proceed collecting vehicles on preceding edges
3110  double remainingRange = scanStart.range - scanStart.pos;
3111 
3112  // Junction representing the origin of 'edge'
3113  const MSJunction* junction = scanStart.edge->getFromJunction();
3114  if (seenJunctions.count(junction) == 0) {
3115  // Collect vehicles from incoming edges of the junction
3116  if (!scanStart.edge->isInternal()) {
3117  // collect vehicles on preceding junction (for internal edges this is already done in caller,
3118  // i.e. findSurroundingVehicles() or the recursive call from getUpstreamVehicles())
3119 
3120  // Collect vehicles on the junction, if it wasn't considered already
3121  getVehiclesOnJunction(junction, scanStart.egoDistToConflictLane, scanStart.egoConflictLane, foeCollector);
3122  seenJunctions.insert(junction);
3123  }
3124  // Collect vehicles from incoming edges from the junction representing the origin of 'edge'
3125  const ConstMSEdgeVector& incoming = junction->getIncoming();
3126  for (ConstMSEdgeVector::const_iterator ei = incoming.begin(); ei != incoming.end(); ++ei) {
3127  if ((*ei)->isInternal()) {
3128  continue;
3129  }
3130  const MSEdge* inEdge = *ei;
3131  assert(inEdge != 0);
3132  double distOnJunction = scanStart.edge->isInternal() ? 0. : inEdge->getInternalFollowingLengthTo(scanStart.edge);
3133  if (distOnJunction >= remainingRange) {
3134  continue;
3135  }
3136  // account for vehicles on the predecessor edge
3137  UpstreamScanStartInfo nextInfo(inEdge, inEdge->getLength(), remainingRange - distOnJunction, scanStart.egoDistToConflictLane, scanStart.egoConflictLane);
3138  getUpstreamVehicles(nextInfo, foeCollector, seenJunctions);
3139  }
3140  } else {
3141 #ifdef DEBUG_SSM_SURROUNDING
3142  if (gDebugFlag3) {
3143  std::cout << " Downstream Scan for stops at junction '" << junction->getID()
3144  << "', which has already been scanned."
3145  << std::endl;
3146  }
3147 #endif
3148  }
3149 }
3150 
3151 void
3152 MSDevice_SSM::getVehiclesOnJunction(const MSJunction* junction, double egoDistToConflictLane, const MSLane* const egoConflictLane, FoeInfoMap& foeCollector) {
3153 #ifdef DEBUG_SSM_SURROUNDING
3154  if (gDebugFlag3) {
3155  std::cout << SIMTIME << " getVehiclesOnJunction() for junction '" << junction->getID() << "'"
3156  << "\nFound vehicles:"
3157  << std::endl;
3158  }
3159 #endif
3160  // FoeInfo creation
3161  auto collectFoeInfos = [&](const MSLane::VehCont & vehicles) {
3162  for (MSVehicle* veh : vehicles) {
3163  if (foeCollector.find(veh) != foeCollector.end()) {
3164  delete foeCollector[veh];
3165  }
3166  FoeInfo* c = new FoeInfo();
3167  c->egoConflictLane = egoConflictLane;
3168  c->egoDistToConflictLane = egoDistToConflictLane;
3169  foeCollector[veh] = c;
3170 #ifdef DEBUG_SSM_SURROUNDING
3171  if (gDebugFlag3) {
3172  for (MSVehicle* veh : vehicles) {
3173  std::cout << veh->getID() << "\n";
3174  }
3175  }
3176 #endif
3177  }
3178  };
3179 
3180  // Collect vehicles on internal lanes
3181  const std::vector<MSLane*> lanes = junction->getInternalLanes();
3182  for (std::vector<MSLane*>::const_iterator li = lanes.begin(); li != lanes.end(); ++li) {
3183  MSLane* lane = *li;
3184  const MSLane::VehCont& vehicles = lane->getVehiclesSecure();
3185 
3186  // Add FoeInfos (XXX: for some situations, a vehicle may be collected twice. Then the later finding overwrites the earlier in foeCollector.
3187  // This could lead to neglecting a conflict when determining foeConflictLane later.) -> TODO: test with twice intersecting routes
3188  collectFoeInfos(vehicles);
3189 
3190  lane->releaseVehicles();
3191 
3192  // If there is an internal continuation lane, also collect vehicles on that lane
3193  if (lane->getLinkCont().size() > 1 && lane->getLinkCont()[0]->getViaLane() != nullptr) {
3194  // There's a second internal lane of the connection
3195  lane = lane->getLinkCont()[0]->getViaLane();
3196  // This code must be modified, if more than two-piece internal lanes are allowed. Thus, assert:
3197  assert(lane->getLinkCont().size() == 0 || lane->getLinkCont()[0]->getViaLane() == 0);
3198 
3199  // collect vehicles
3200  const MSLane::VehCont& vehicles2 = lane->getVehiclesSecure();
3201  // Add FoeInfos for the first internal lane
3202  collectFoeInfos(vehicles2);
3203  lane->releaseVehicles();
3204  }
3205  }
3206 
3207 #ifdef DEBUG_SSM_SURROUNDING
3208  if (gDebugFlag3) {
3209  std::cout << std::endl;
3210  }
3211 #endif
3212 }
3213 
3214 
3215 
3216 void
3218  // This is called once at vehicle removal.
3219  // Also: flush myOutputFile? Or is this done automatically?
3220  // myOutputFile->closeTag();
3221 }
3222 
3223 // ---------------------------------------------------------------------------
3224 // Static parameter load helpers
3225 // ---------------------------------------------------------------------------
3226 std::string
3227 MSDevice_SSM::getOutputFilename(const SUMOVehicle& v, std::string deviceID) {
3229  std::string file = deviceID + ".xml";
3230  if (v.getParameter().knowsParameter("device.ssm.file")) {
3231  try {
3232  file = v.getParameter().getParameter("device.ssm.file", file);
3233  } catch (...) {
3234  WRITE_WARNING("Invalid value '" + v.getParameter().getParameter("device.ssm.file", file) + "'for vehicle parameter 'ssm.measures'");
3235  }
3236  } else if (v.getVehicleType().getParameter().knowsParameter("device.ssm.file")) {
3237  try {
3238  file = v.getVehicleType().getParameter().getParameter("device.ssm.file", file);
3239  } catch (...) {
3240  WRITE_WARNING("Invalid value '" + v.getVehicleType().getParameter().getParameter("device.ssm.file", file) + "'for vType parameter 'ssm.measures'");
3241  }
3242  } else {
3243  file = oc.getString("device.ssm.file") == "" ? file : oc.getString("device.ssm.file");
3244  if (!oc.isSet("device.ssm.file") && (issuedParameterWarnFlags & SSM_WARN_FILE) == 0) {
3245  std::cout << "vehicle '" << v.getID() << "' does not supply vehicle parameter 'device.ssm.file'. Using default of '" << file << "'\n";
3247  }
3248  }
3249  return file;
3250 }
3251 
3252 bool
3255  bool useGeo = false;
3256  if (v.getParameter().knowsParameter("device.ssm.geo")) {
3257  try {
3258  useGeo = StringUtils::toBool(v.getParameter().getParameter("device.ssm.geo", "no"));
3259  } catch (...) {
3260  WRITE_WARNING("Invalid value '" + v.getParameter().getParameter("device.ssm.geo", "no") + "'for vehicle parameter 'ssm.geo'");
3261  }
3262  } else if (v.getVehicleType().getParameter().knowsParameter("device.ssm.geo")) {
3263  try {
3264  useGeo = StringUtils::toBool(v.getVehicleType().getParameter().getParameter("device.ssm.geo", "no"));
3265  } catch (...) {
3266  WRITE_WARNING("Invalid value '" + v.getVehicleType().getParameter().getParameter("device.ssm.geo", "no") + "'for vType parameter 'ssm.geo'");
3267  }
3268  } else {
3269  useGeo = oc.getBool("device.ssm.geo");
3270  if (!oc.isSet("device.ssm.geo") && (issuedParameterWarnFlags & SSM_WARN_GEO) == 0) {
3271  std::cout << "vehicle '" << v.getID() << "' does not supply vehicle parameter 'device.ssm.geo'. Using default of '" << ::toString(useGeo) << "'\n";
3273  }
3274  }
3275  return useGeo;
3276 }
3277 
3278 
3279 double
3282  double range = -INVALID;
3283  if (v.getParameter().knowsParameter("device.ssm.range")) {
3284  try {
3285  range = StringUtils::toDouble(v.getParameter().getParameter("device.ssm.range", ""));
3286  } catch (...) {
3287  WRITE_WARNING("Invalid value '" + v.getParameter().getParameter("device.ssm.range", "") + "'for vehicle parameter 'ssm.range'");
3288  }
3289  } else if (v.getVehicleType().getParameter().knowsParameter("device.ssm.range")) {
3290  try {
3291  range = StringUtils::toDouble(v.getVehicleType().getParameter().getParameter("device.ssm.range", ""));
3292  } catch (...) {
3293  WRITE_WARNING("Invalid value '" + v.getVehicleType().getParameter().getParameter("device.ssm.range", "") + "'for vType parameter 'ssm.range'");
3294  }
3295  } else {
3296  range = oc.getFloat("device.ssm.range");
3297  if (!oc.isSet("device.ssm.range") && (issuedParameterWarnFlags & SSM_WARN_RANGE) == 0) {
3298  std::cout << "vehicle '" << v.getID() << "' does not supply vehicle parameter 'device.ssm.range'. Using default of '" << range << "'\n";
3300  }
3301  }
3302  return range;
3303 }
3304 
3305 
3306 double
3309  double extraTime = INVALID;
3310  if (v.getParameter().knowsParameter("device.ssm.extratime")) {
3311  try {
3312  extraTime = StringUtils::toDouble(v.getParameter().getParameter("device.ssm.extratime", ""));
3313  } catch (...) {
3314  WRITE_WARNING("Invalid value '" + v.getParameter().getParameter("device.ssm.extratime", "") + "'for vehicle parameter 'ssm.extratime'");
3315  }
3316  } else if (v.getVehicleType().getParameter().knowsParameter("device.ssm.extratime")) {
3317  try {
3318  extraTime = StringUtils::toDouble(v.getVehicleType().getParameter().getParameter("device.ssm.extratime", ""));
3319  } catch (...) {
3320  WRITE_WARNING("Invalid value '" + v.getVehicleType().getParameter().getParameter("device.ssm.extratime", "") + "'for vType parameter 'ssm.extratime'");
3321  }
3322  } else {
3323  extraTime = oc.getFloat("device.ssm.extratime");
3324  if (!oc.isSet("device.ssm.extratime") && (issuedParameterWarnFlags & SSM_WARN_EXTRATIME) == 0) {
3325  std::cout << "vehicle '" << v.getID() << "' does not supply vehicle parameter 'device.ssm.extratime'. Using default of '" << extraTime << "'\n";
3327  }
3328  }
3329  if (extraTime < 0.) {
3330  extraTime = DEFAULT_EXTRA_TIME;
3331  WRITE_WARNING("Negative (or no) value encountered for vehicle parameter 'device.ssm.extratime' in vehicle '" + v.getID() + "' using default value " + ::toString(extraTime) + " instead");
3332  }
3333  return extraTime;
3334 }
3335 
3336 
3337 bool
3340  bool trajectories = false;
3341  if (v.getParameter().knowsParameter("device.ssm.trajectories")) {
3342  try {
3343  trajectories = StringUtils::toBool(v.getParameter().getParameter("device.ssm.trajectories", "no"));
3344  } catch (...) {
3345  WRITE_WARNING("Invalid value '" + v.getParameter().getParameter("device.ssm.trajectories", "no") + "'for vehicle parameter 'ssm.trajectories'");
3346  }
3347  } else if (v.getVehicleType().getParameter().knowsParameter("device.ssm.trajectories")) {
3348  try {
3349  trajectories = StringUtils::toBool(v.getVehicleType().getParameter().getParameter("device.ssm.trajectories", "no"));
3350  } catch (...) {
3351  WRITE_WARNING("Invalid value '" + v.getVehicleType().getParameter().getParameter("device.ssm.trajectories", "no") + "'for vType parameter 'ssm.trajectories'");
3352  }
3353  } else {
3354  trajectories = oc.getBool("device.ssm.trajectories");
3355  if (!oc.isSet("device.ssm.trajectories") && (issuedParameterWarnFlags & SSM_WARN_TRAJECTORIES) == 0) {
3356  std::cout << "vehicle '" << v.getID() << "' does not supply vehicle parameter 'device.ssm.trajectories'. Using default of '" << ::toString(trajectories) << "'\n";
3358  }
3359  }
3360  return trajectories;
3361 }
3362 
3363 
3364 bool
3365 MSDevice_SSM::getMeasuresAndThresholds(const SUMOVehicle& v, std::string deviceID, std::map<std::string, double>& thresholds) {
3367 
3368  // Measures
3369  std::string measures_str = "";
3370  if (v.getParameter().knowsParameter("device.ssm.measures")) {
3371  try {
3372  measures_str = v.getParameter().getParameter("device.ssm.measures", "");
3373  } catch (...) {
3374  WRITE_WARNING("Invalid value '" + v.getParameter().getParameter("device.ssm.measures", "") + "'for vehicle parameter 'ssm.measures'");
3375  }
3376  } else if (v.getVehicleType().getParameter().knowsParameter("device.ssm.measures")) {
3377  try {
3378  measures_str = v.getVehicleType().getParameter().getParameter("device.ssm.measures", "");
3379  } catch (...) {
3380  WRITE_WARNING("Invalid value '" + v.getVehicleType().getParameter().getParameter("device.ssm.measures", "") + "'for vType parameter 'ssm.measures'");
3381  }
3382  } else {
3383  measures_str = oc.getString("device.ssm.measures");
3384  if (!oc.isSet("device.ssm.measures") && (issuedParameterWarnFlags & SSM_WARN_MEASURES) == 0) {
3385  std::cout << "vehicle '" << v.getID() << "' does not supply vehicle parameter 'device.ssm.measures'. Using default of '" << measures_str << "'\n";
3387  }
3388  }
3389 
3390  // Check retrieved measures
3391  if (measures_str == "") {
3392  WRITE_WARNING("No measures specified for ssm device of vehicle '" + v.getID() + "'. Registering all available SSMs.");
3393  measures_str = AVAILABLE_SSMS;
3394  }
3396  std::vector<std::string> available = st.getVector();
3397  st = StringTokenizer(measures_str);
3398  std::vector<std::string> measures = st.getVector();
3399  for (std::vector<std::string>::const_iterator i = measures.begin(); i != measures.end(); ++i) {
3400  if (std::find(available.begin(), available.end(), *i) == available.end()) {
3401  // Given identifier is unknown
3402  WRITE_ERROR("SSM identifier '" + *i + "' is not supported. Aborting construction of SSM device '" + deviceID + "'.");
3403  return false;
3404  }
3405  }
3406 
3407  // Thresholds
3408  std::string thresholds_str = "";
3409  if (v.getParameter().knowsParameter("device.ssm.thresholds")) {
3410  try {
3411  thresholds_str = v.getParameter().getParameter("device.ssm.thresholds", "");
3412  } catch (...) {
3413  WRITE_WARNING("Invalid value '" + v.getParameter().getParameter("device.ssm.thresholds", "") + "'for vehicle parameter 'ssm.thresholds'");
3414  }
3415  } else if (v.getVehicleType().getParameter().knowsParameter("device.ssm.thresholds")) {
3416  try {
3417  thresholds_str = v.getVehicleType().getParameter().getParameter("device.ssm.thresholds", "");
3418  } catch (...) {
3419  WRITE_WARNING("Invalid value '" + v.getVehicleType().getParameter().getParameter("device.ssm.thresholds", "") + "'for vType parameter 'ssm.thresholds'");
3420  }
3421  } else {
3422  thresholds_str = oc.getString("device.ssm.thresholds");
3423  if (!oc.isSet("device.ssm.thresholds") && (issuedParameterWarnFlags & SSM_WARN_THRESHOLDS) == 0) {
3424  std::cout << "vehicle '" << v.getID() << "' does not supply vehicle parameter 'device.ssm.thresholds'. Using default of '" << thresholds_str << "'\n";
3426  }
3427  }
3428 
3429  // Parse vector of doubles from threshold_str
3430  int count = 0;
3431  if (thresholds_str != "") {
3432  st = StringTokenizer(thresholds_str);
3433  while (count < (int)measures.size() && st.hasNext()) {
3434  double thresh = StringUtils::toDouble(st.next());
3435  thresholds.insert(std::make_pair(measures[count], thresh));
3436  ++count;
3437  }
3438  if (thresholds.size() < measures.size() || st.hasNext()) {
3439  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.");
3440  return false;
3441  }
3442  } else {
3443  // assume default thresholds if none are given
3444  for (std::vector<std::string>::const_iterator i = measures.begin(); i != measures.end(); ++i) {
3445  if (*i == "TTC") {
3446  thresholds.insert(std::make_pair(*i, DEFAULT_THRESHOLD_TTC));
3447  } else if (*i == "DRAC") {
3448  thresholds.insert(std::make_pair(*i, DEFAULT_THRESHOLD_DRAC));
3449  } else if (*i == "PET") {
3450  thresholds.insert(std::make_pair(*i, DEFAULT_THRESHOLD_PET));
3451  } else if (*i == "BR") {
3452  thresholds.insert(std::make_pair(*i, DEFAULT_THRESHOLD_BR));
3453  } else if (*i == "SGAP") {
3454  thresholds.insert(std::make_pair(*i, DEFAULT_THRESHOLD_SGAP));
3455  } else if (*i == "TGAP") {
3456  thresholds.insert(std::make_pair(*i, DEFAULT_THRESHOLD_TGAP));
3457  } else {
3458  WRITE_ERROR("Unknown SSM identifier '" + (*i) + "'. Aborting construction of ssm device."); // should never occur
3459  return false;
3460  }
3461  }
3462  }
3463  return true;
3464 }
3465 
3466 
3467 
3468 /****************************************************************************/
3469 
MSLane::releaseVehicles
virtual void releaseVehicles() const
Allows to use the container for microsimulation again.
Definition: MSLane.h:458
MSDevice_SSM::EncounterApproachInfo::foeConflictAreaLength
double foeConflictAreaLength
Definition: MSDevice_SSM.h:315
MSDevice_SSM::UpstreamScanStartInfo::range
double range
Definition: MSDevice_SSM.h:349
OptionsCont::isSet
bool isSet(const std::string &name, bool failOnNonExistant=true) const
Returns the information whether the named option is set.
Definition: OptionsCont.cpp:136
UNUSED_PARAMETER
#define UNUSED_PARAMETER(x)
Definition: StdDefs.h:32
MSDevice_SSM::Encounter::TTCspan
std::vector< double > TTCspan
All values for TTC.
Definition: MSDevice_SSM.h:273
SUMOTrafficObject
Representation of a vehicle or person.
Definition: SUMOTrafficObject.h:48
MSDevice_SSM::Encounter::countDownExtraTime
void countDownExtraTime(double amount)
decreases myRemaingExtraTime by given amount in seconds
Definition: MSDevice_SSM.cpp:360
MSLane::getVehiclesSecure
virtual const VehCont & getVehiclesSecure() const
Returns the vehicles container; locks it for microsimulation.
Definition: MSLane.h:428
MSDevice_SSM::getExtraTime
static double getExtraTime(const SUMOVehicle &v)
Definition: MSDevice_SSM.cpp:3307
MSDevice_SSM::myTGAPspan
std::vector< double > myTGAPspan
All values for time gap.
Definition: MSDevice_SSM.h:730
MSVehicle::getPreviousSpeed
double getPreviousSpeed() const
Returns the vehicle's speed before the previous time step.
Definition: MSVehicle.h:485
MSDevice_SSM::ENCOUNTER_TYPE_FOLLOWING_PASSED
ENCOUNTER_TYPE_FOLLOWING_PASSED.
Definition: MSDevice_SSM.h:112
MSDevice_SSM::Encounter::timeSpan
std::vector< double > timeSpan
time points corresponding to the trajectories
Definition: MSDevice_SSM.h:255
MIN2
T MIN2(T a, T b)
Definition: StdDefs.h:74
MSDevice_SSM::Encounter::foeID
const std::string foeID
Definition: MSDevice_SSM.h:242
MSDevice_SSM::UpstreamScanStartInfo::egoDistToConflictLane
double egoDistToConflictLane
Definition: MSDevice_SSM.h:350
MSDevice_SSM::Encounter::ConflictPointInfo::type
EncounterType type
Type of the conflict.
Definition: MSDevice_SSM.h:194
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:342
WRITE_WARNING
#define WRITE_WARNING(msg)
Definition: MsgHandler.h:239
MSNet.h
MSLane
Representation of a lane in the micro simulation.
Definition: MSLane.h:83
MSDevice_SSM
A device which collects info on the vehicle trip (mainly on departure and arrival)
Definition: MSDevice_SSM.h:57
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:259
MSVehicle::isOnRoad
bool isOnRoad() const
Returns the information whether the vehicle is on a road (is simulated)
Definition: MSVehicle.h:583
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:61
OutputDevice
Static storage of an output device and its base (abstract) implementation.
Definition: OutputDevice.h:64
MSRoute::end
MSRouteIterator end() const
Returns the end of the list of edges to pass.
Definition: MSRoute.cpp:76
MSEdge::getOppositeEdge
const MSEdge * getOppositeEdge() const
Returns the opposite direction edge if on exists else a nullptr.
Definition: MSEdge.cpp:1096
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:192
MSDevice_SSM::FoeInfo::egoConflictLane
const MSLane * egoConflictLane
Definition: MSDevice_SSM.h:330
MSLane::getCanonicalSuccessorLane
MSLane * getCanonicalSuccessorLane() const
Definition: MSLane.cpp:2598
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:95
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:313
MSDevice_SSM::EncounterApproachInfo::ttc
double ttc
Definition: MSDevice_SSM.h:318
MSDevice_SSM::Encounter::Trajectory::x
PositionVector x
Definition: MSDevice_SSM.h:180
MSDevice_SSM::Encounter::egoID
const std::string egoID
Definition: MSDevice_SSM.h:241
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:2698
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:389
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:724
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:743
MSDevice_SSM::ENCOUNTER_TYPE_BOTH_LEFT_CONFLICT_AREA
ENCOUNTER_TYPE_BOTH_LEFT_CONFLICT_AREA.
Definition: MSDevice_SSM.h:109
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:699
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:202
MSBaseVehicle::getLength
double getLength() const
Returns the vehicle's length.
Definition: MSBaseVehicle.h:394
MSDevice_SSM::SSM_WARN_GEO
Definition: MSDevice_SSM.h:755
MSDevice_SSM::ENCOUNTER_TYPE_COLLISION
ENCOUNTER_TYPE_COLLISION.
Definition: MSDevice_SSM.h:118
MSJunction::getInternalLanes
virtual const std::vector< MSLane * > getInternalLanes() const
Returns all internal lanes on the junction.
Definition: MSJunction.h:114
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:733
MSDevice_SSM::SSM_WARN_RANGE
Definition: MSDevice_SSM.h:752
SUMOVehicle
Representation of a vehicle.
Definition: SUMOVehicle.h:61
MSLane::VehCont
std::vector< MSVehicle * > VehCont
Container for vehicles.
Definition: MSLane.h:93
MSDevice_SSM::ENCOUNTER_TYPE_CROSSING_LEADER
ENCOUNTER_TYPE_CROSSING_LEADER.
Definition: MSDevice_SSM.h:94
MSBaseVehicle::getRoute
const MSRoute & getRoute() const
Returns the current route.
Definition: MSBaseVehicle.h:110
GeoConvHelper.h
ConstMSEdgeVector
std::vector< const MSEdge * > ConstMSEdgeVector
Definition: MSEdge.h:73
MSDevice_SSM::toGeo
static void toGeo(Position &x)
convert SUMO-positions to geo coordinates (in place)
Definition: MSDevice_SSM.cpp:2603
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:223
MSDevice_SSM::EncounterApproachInfo::egoConflictAreaLength
double egoConflictAreaLength
Definition: MSDevice_SSM.h:314
MSDevice_SSM::myThresholds
std::map< std::string, double > myThresholds
Definition: MSDevice_SSM.h:694
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:305
OptionsCont::getOptions
static OptionsCont & getOptions()
Retrieves the options.
Definition: OptionsCont.cpp:58
StringTokenizer::next
std::string next()
returns the next substring when it exists. Otherwise the behaviour is undefined
Definition: StringTokenizer.cpp:100
MSGlobals::gUseMesoSim
static bool gUseMesoSim
Definition: MSGlobals.h:91
DEFAULT_THRESHOLD_TGAP
#define DEFAULT_THRESHOLD_TGAP
Definition: MSDevice_SSM.cpp:87
MSDevice_SSM::myOutputFile
OutputDevice * myOutputFile
Output device.
Definition: MSDevice_SSM.h:740
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:747
MSDevice_SSM::flushGlobalMeasures
void flushGlobalMeasures()
Write out all non-encounter specific measures as headways and braking rates.
Definition: MSDevice_SSM.cpp:2549
MSDevice_SSM::myComputeTTC
bool myComputeTTC
Flags for switching on / off comutation of different SSMs, derived from myMeasures.
Definition: MSDevice_SSM.h:705
MSVehicleDevice::myHolder
SUMOVehicle & myHolder
The vehicle that stores the device.
Definition: MSVehicleDevice.h:85
PositionVector
A list of positions.
Definition: PositionVector.h:46
MSDevice_SSM::UpstreamScanStartInfo
Auxiliary structure used to handle upstream scanning start points Upstream scan has to be started aft...
Definition: MSDevice_SSM.h:344
MSEdge::getLength
double getLength() const
return the length of the edge
Definition: MSEdge.h:582
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.
Definition: MSDevice_SSM.h:99
MSVehicle::getCarFollowModel
const MSCFModel & getCarFollowModel() const
Returns the vehicle's car following model definition.
Definition: MSVehicle.h:894
MSDevice_SSM::Encounter::typeSpan
std::vector< int > typeSpan
Evolution of the encounter classification (.
Definition: MSDevice_SSM.h:257
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.
Definition: MSDevice_SSM.h:101
MSDevice_SSM::writeOutConflict
void writeOutConflict(Encounter *e)
Definition: MSDevice_SSM.cpp:2615
gPrecisionGeo
int gPrecisionGeo
Definition: StdDefs.cpp:28
MSJunction::getIncoming
const ConstMSEdgeVector & getIncoming() const
Definition: MSJunction.h:102
MSLane::getIncomingLanes
const std::vector< IncomingLaneInfo > & getIncomingLanes() const
Definition: MSLane.h:819
MSDevice_SSM::myMinTGAP
std::pair< std::pair< std::pair< double, Position >, double >, std::string > myMinTGAP
Definition: MSDevice_SSM.h:735
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:71
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:270
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:254
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:434
MSDevice_SSM::FoeInfo
Definition: MSDevice_SSM.h:329
MSDevice_SSM::myMinSGAP
std::pair< std::pair< std::pair< double, Position >, double >, std::string > myMinSGAP
Definition: MSDevice_SSM.h:734
MSBaseVehicle::getEdge
const MSEdge * getEdge() const
Returns the edge the vehicle is currently at.
Definition: MSBaseVehicle.cpp:170
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:252
MSDevice_SSM::SSM_WARN_EXTRATIME
Definition: MSDevice_SSM.h:753
MAX2
T MAX2(T a, T b)
Definition: StdDefs.h:80
MSEdge::isInternal
bool isInternal() const
return whether this edge is an internal edge
Definition: MSEdge.h:233
MSVehicle::getPosition
Position getPosition(const double offset=0) const
Return current position (x/y, cartesian)
Definition: MSVehicle.cpp:1280
AVAILABLE_SSMS
#define AVAILABLE_SSMS
Definition: MSDevice_SSM.cpp:79
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:256
MSDevice_SSM::EncounterApproachInfo::egoConflictExitDist
double egoConflictExitDist
Definition: MSDevice_SSM.h:308
MSEdge::getFromJunction
const MSJunction * getFromJunction() const
Definition: MSEdge.h:357
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:243
DEFAULT_RANGE
#define DEFAULT_RANGE
Definition: MSDevice_SSM.cpp:73
MSDevice_SSM::UpstreamScanStartInfo::edge
const MSEdge * edge
Definition: MSDevice_SSM.h:346
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:473
SIMTIME
#define SIMTIME
Definition: SUMOTime.h:64
MSVehicleControl::getVehicle
SUMOVehicle * getVehicle(const std::string &id) const
Returns the vehicle with the given id.
Definition: MSVehicleControl.cpp:235
MSVehicle::getPositionOnLane
double getPositionOnLane() const
Get the vehicle's position along the lane.
Definition: MSVehicle.h:397
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:5059
MSDevice_SSM::~MSDevice_SSM
~MSDevice_SSM()
Destructor.
Definition: MSDevice_SSM.cpp:2774
MSDevice_SSM::getDetectionRange
static double getDetectionRange(const SUMOVehicle &v)
Definition: MSDevice_SSM.cpp:3280
MSDevice_SSM::Encounter::foeDistsToConflict
std::vector< double > foeDistsToConflict
Evolution of the foe vehicle's distance to the conflict point.
Definition: MSDevice_SSM.h:265
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:313
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:2527
GeoConvHelper::getFinal
static const GeoConvHelper & getFinal()
the coordinate transformation for writing the location element and for tracking the original coordina...
Definition: GeoConvHelper.h:106
MSDevice_SSM::ENCOUNTER_TYPE_EGO_LEFT_CONFLICT_AREA
ENCOUNTER_TYPE_EGO_LEFT_CONFLICT_AREA.
Definition: MSDevice_SSM.h:103
MSVehicleType::getWidth
double getWidth() const
Get the width which vehicles of this class shall have when being drawn.
Definition: MSVehicleType.h:247
MSDevice_SSM::myActiveEncounters
EncounterVector myActiveEncounters
Definition: MSDevice_SSM.h:713
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:44
MSDevice_SSM::Encounter::minTTC
ConflictPointInfo minTTC
Definition: MSDevice_SSM.h:283
StringTokenizer
Definition: StringTokenizer.h:62
DEFAULT_EXTRA_TIME
#define DEFAULT_EXTRA_TIME
Definition: MSDevice_SSM.cpp:89
MSDevice_SSM::Encounter::egoConflictExitTime
double egoConflictExitTime
Definition: MSDevice_SSM.h:250
MSDevice_SSM::generateOutput
void generateOutput() const
Finalizes output. Called on vehicle removal.
Definition: MSDevice_SSM.cpp:3217
MSDevice_SSM::EncounterVector
std::vector< Encounter * > EncounterVector
Definition: MSDevice_SSM.h:355
MSDevice_SSM::update
void update()
Definition: MSDevice_SSM.cpp:413
MSDevice_SSM::Encounter::PET
ConflictPointInfo PET
Definition: MSDevice_SSM.h:285
MSDevice_SSM::ENCOUNTER_TYPE_MERGING
ENCOUNTER_TYPE_MERGING.
Definition: MSDevice_SSM.h:80
MSDevice_SSM::Encounter::end
double end
Definition: MSDevice_SSM.h:243
MSLane::getLength
double getLength() const
Returns the lane's length.
Definition: MSLane.h:541
gDebugFlag3
bool gDebugFlag3
Definition: StdDefs.cpp:35
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:697
MSDevice_SSM::ENCOUNTER_TYPE_BOTH_ENTERED_CONFLICT_AREA
ENCOUNTER_TYPE_BOTH_ENTERED_CONFLICT_AREA.
Definition: MSDevice_SSM.h:107
OptionsCont::doRegister
void doRegister(const std::string &name, Option *v)
Adds an option under the given name.
Definition: OptionsCont.cpp:75
MSVehicle::getVelocityVector
Position getVelocityVector() const
Returns the vehicle's direction in radians.
Definition: MSVehicle.h:689
MSDevice_SSM::EncounterApproachInfo
Structure to collect some info on the encounter needed during ssm calculation by various functions.
Definition: MSDevice_SSM.h:301
DEBUG_COND_ENCOUNTER
#define DEBUG_COND_ENCOUNTER(e)
Definition: MSDevice_SSM.cpp:63
MSVehicle::getLaneChangeModel
MSAbstractLaneChangeModel & getLaneChangeModel()
Definition: MSVehicle.cpp:4609
MSAbstractLaneChangeModel::isOpposite
bool isOpposite() const
Definition: MSAbstractLaneChangeModel.h:557
GeoConvHelper::cartesian2geo
void cartesian2geo(Position &cartesian) const
Converts the given cartesian (shifted) position to its geo (lat/long) representation.
Definition: GeoConvHelper.cpp:294
Position
A point in 2D or 3D with translation and scaling methods.
Definition: Position.h:39
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:706
MSDevice_SSM::EncounterApproachInfo::foeEstimatedConflictEntryTime
double foeEstimatedConflictEntryTime
Definition: MSDevice_SSM.h:311
OptionsCont
A storage for options typed value containers)
Definition: OptionsCont.h:90
MSDevice_SSM::ENCOUNTER_TYPE_CROSSING_FOLLOWER
ENCOUNTER_TYPE_CROSSING_FOLLOWER.
Definition: MSDevice_SSM.h:97
MSEdge
A road/street connecting two junctions.
Definition: MSEdge.h:76
MSDevice_SSM::ENCOUNTER_TYPE_FOLLOWING_FOLLOWER
ENCOUNTER_TYPE_FOLLOWING_FOLLOWER.
Definition: MSDevice_SSM.h:73
MSDevice_SSM::UpstreamScanStartInfo::egoConflictLane
const MSLane * egoConflictLane
Definition: MSDevice_SSM.h:351
MSEdge::getToJunction
const MSJunction * getToJunction() const
Definition: MSEdge.h:361
MSVehicle::getLastStepDist
double getLastStepDist() const
Get the distance the vehicle covered in the previous timestep.
Definition: MSVehicle.h:404
MSDevice_SSM::Encounter::currentType
EncounterType currentType
Definition: MSDevice_SSM.h:244
MSDevice_SSM::Encounter::foeConflictExitTime
double foeConflictExitTime
Definition: MSDevice_SSM.h:252
MSDevice_SSM::FoeInfo::egoDistToConflictLane
double egoDistToConflictLane
Definition: MSDevice_SSM.h:332
MSBaseVehicle::getVehicleType
const MSVehicleType & getVehicleType() const
Returns the vehicle's type definition.
Definition: MSBaseVehicle.h:118
MSDevice_SSM::myInstances
static std::set< MSDevice_SSM *, ComparatorNumericalIdLess > * myInstances
All currently existing SSM devices.
Definition: MSDevice_SSM.h:61
MSDevice_SSM::getUpstreamVehicles
static void getUpstreamVehicles(const UpstreamScanStartInfo &scanStart, FoeInfoMap &foeCollector, std::set< const MSJunction * > &seenJunctions)
Collects all vehicles within range 'range' upstream of the position 'pos' on the edge 'edge' into foe...
Definition: MSDevice_SSM.cpp:3056
MSLane::getLinkCont
const MSLinkCont & getLinkCont() const
returns the container with all links !!!
Definition: MSLane.cpp:2099
MSDevice_SSM::ENCOUNTER_TYPE_MERGING_LEADER
ENCOUNTER_TYPE_MERGING_LEADER.
Definition: MSDevice_SSM.h:83
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:319
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:726
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:3499
MSDevice_SSM::ENCOUNTER_TYPE_ONCOMING
Definition: MSDevice_SSM.h:116
OptionsCont::addOptionSubTopic
void addOptionSubTopic(const std::string &topic)
Adds an option subtopic.
Definition: OptionsCont.cpp:523
MSDevice_SSM::ENCOUNTER_TYPE_FOLLOWING_LEADER
ENCOUNTER_TYPE_FOLLOWING_LEADER.
Definition: MSDevice_SSM.h:75
MSVehicle::getLane
MSLane * getLane() const
Returns the lane the vehicle is on.
Definition: MSVehicle.h:561
MSDevice_SSM::myBRspan
std::vector< double > myBRspan
All values for brake rate.
Definition: MSDevice_SSM.h:726
MSLane::getEdge
MSEdge & getEdge() const
Returns the lane's edge.
Definition: MSLane.h:670
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:1837
MSDevice_SSM::Encounter::size
std::size_t size() const
Returns the number of trajectory points stored.
Definition: MSDevice_SSM.h:213
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:2812
OptionsCont::getFloat
double getFloat(const std::string &name) const
Returns the double-value of the named option (only for Option_Float)
Definition: OptionsCont.cpp:209
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:240
MSDevice_SSM::getMeasuresAndThresholds
static bool getMeasuresAndThresholds(const SUMOVehicle &v, std::string deviceID, std::map< std::string, double > &thresholds)
Definition: MSDevice_SSM.cpp:3365
StringUtils.h
MSDevice_SSM::Encounter::DRACspan
std::vector< double > DRACspan
All values for DRAC.
Definition: MSDevice_SSM.h:275
MSLane::getShape
const PositionVector & getShape() const
Returns this lane's shape.
Definition: MSLane.h:478
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:2798
MSDevice_SSM::requestsTrajectories
static bool requestsTrajectories(const SUMOVehicle &v)
Definition: MSDevice_SSM.cpp:3338
OutputDevice::getDevice
static OutputDevice & getDevice(const std::string &name)
Returns the described OutputDevice.
Definition: OutputDevice.cpp:55
MSDevice_SSM::toString
static std::string toString(EncounterType type)
Definition: MSDevice_SSM.h:121
MSDevice_SSM::EncounterQueue
std::priority_queue< Encounter *, std::vector< Encounter * >, Encounter::compare > EncounterQueue
Definition: MSDevice_SSM.h:354
MSNet::getInstance
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
Definition: MSNet.cpp:168
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:204
MSDevice_SSM::UpstreamScanStartInfo::pos
double pos
Definition: MSDevice_SSM.h:348
MSDevice_SSM::EncounterApproachInfo::encounter
Encounter * encounter
Definition: MSDevice_SSM.h:303
M_PI
#define M_PI
Definition: odrSpiral.cpp:40
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:701
PositionVector::rotationAtOffset
double rotationAtOffset(double pos) const
Returns the rotation at the given length.
Definition: PositionVector.cpp:286
MSVehicleType::getLength
double getLength() const
Get vehicle's length [m].
Definition: MSVehicleType.h:110
MSDevice_SSM::Encounter::ConflictPointInfo::value
double value
value of the corresponding SSM
Definition: MSDevice_SSM.h:196
MSDevice_SSM::EncounterApproachInfo::egoEstimatedConflictExitTime
double egoEstimatedConflictExitTime
Definition: MSDevice_SSM.h:312
MSDevice_SSM::myComputeTGAP
bool myComputeTGAP
Definition: MSDevice_SSM.h:705
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:2824
MSDevice_SSM::ENCOUNTER_TYPE_NOCONFLICT_AHEAD
ENCOUNTER_TYPE_NOCONFLICT_AHEAD.
Definition: MSDevice_SSM.h:68
MSDevice_SSM::Encounter::ConflictPointInfo::time
double time
time point of the conflict
Definition: MSDevice_SSM.h:188
MSDevice_SSM::ENCOUNTER_TYPE_CROSSING
ENCOUNTER_TYPE_CROSSING.
Definition: MSDevice_SSM.h:91
MSLane::getLinkTo
MSLink * getLinkTo(const MSLane *) const
returns the link to the given lane or 0, if it is not connected
Definition: MSLane.cpp:2106
MSBaseVehicle::getCurrentRouteEdge
const MSRouteIterator & getCurrentRouteEdge() const
Returns an iterator pointing to the current edge in this vehicles route.
Definition: MSBaseVehicle.h:200
MSDevice_SSM::Encounter::egoDistsToConflict
std::vector< double > egoDistsToConflict
Evolution of the ego vehicle's distance to the conflict point.
Definition: MSDevice_SSM.h:263
MSDevice_SSM::getOutputFilename
static std::string getOutputFilename(const SUMOVehicle &v, std::string deviceID)
Definition: MSDevice_SSM.cpp:3227
MSVehicleType::getParameter
const SUMOVTypeParameter & getParameter() const
Definition: MSVehicleType.h:556
MSBaseVehicle
The base class for microscopic and mesoscopic vehicles.
Definition: MSBaseVehicle.h:52
MSVehicle::getBestLanesContinuation
const std::vector< MSLane * > & getBestLanesContinuation() const
Returns the best sequence of lanes to continue the route starting at myLane.
Definition: MSVehicle.cpp:4987
MSDevice_SSM::Encounter
An encounter is an episode involving two vehicles, which are closer to each other than some specified...
Definition: MSDevice_SSM.h:174
joinToString
std::string joinToString(const std::vector< T > &v, const T_BETWEEN &between, std::streamsize accuracy=gPrecision)
Definition: ToString.h:247
MSBaseVehicle::getID
const std::string & getID() const
Returns the name of the vehicle.
Definition: MSBaseVehicle.cpp:137
MSDevice_SSM::SSM_WARN_MEASURES
Definition: MSDevice_SSM.h:749
MSDevice_SSM::SSM_WARN_TRAJECTORIES
Definition: MSDevice_SSM.h:751
MSEdge::getLanes
const std::vector< MSLane * > & getLanes() const
Returns this edge's lanes.
Definition: MSEdge.h:165
MSDevice_SSM::myComputeDRAC
bool myComputeDRAC
Definition: MSDevice_SSM.h:705
MSVehicle::getBackPositionOnLane
double getBackPositionOnLane(const MSLane *lane) const
Get the vehicle's position relative to the given lane.
Definition: MSVehicle.cpp:3993
MSDevice_SSM::EncounterApproachInfo::egoEstimatedConflictEntryTime
double egoEstimatedConflictEntryTime
Definition: MSDevice_SSM.h:310
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:494
MSDevice_SSM::useGeoCoords
static bool useGeoCoords(const SUMOVehicle &v)
Definition: MSDevice_SSM.cpp:3253
MSLane::getWidth
double getWidth() const
Returns the lane's width.
Definition: MSLane.h:557
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:320
MSBaseVehicle::getWidth
double getWidth() const
Returns the vehicle's width.
Definition: MSBaseVehicle.h:402
MSDevice_SSM::EncounterApproachInfo::foeConflictExitDist
double foeConflictExitDist
Definition: MSDevice_SSM.h:309
StringTokenizer::getVector
std::vector< std::string > getVector()
return vector of strings
Definition: StringTokenizer.cpp:192
DEBUG_COND
#define DEBUG_COND(ego)
Definition: MSDevice_SSM.cpp:61
MSDevice_SSM::myOldestActiveEncounterBegin
double myOldestActiveEncounterBegin
begin time of the oldest active encounter
Definition: MSDevice_SSM.h:715
MSDevice_SSM.h
config.h
MSDevice_SSM::EncounterApproachInfo::type
EncounterType type
Definition: MSDevice_SSM.h:304
MSDevice_SSM::ENCOUNTER_TYPE_MERGING_PASSED
ENCOUNTER_TYPE_FOLLOWING_PASSED.
Definition: MSDevice_SSM.h:114
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:597
MSLane::isInternal
bool isInternal() const
Definition: MSLane.cpp:1999
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:717
MSDevice_SSM::Encounter::ego
const MSVehicle * ego
Definition: MSDevice_SSM.h:239
MSDevice_SSM::mySGAPspan
std::vector< double > mySGAPspan
All values for space gap.
Definition: MSDevice_SSM.h:728
MSVehicle::getSpeed
double getSpeed() const
Returns the vehicle's current speed.
Definition: MSVehicle.h:477
StringTokenizer.h
MSDevice_SSM::Encounter::foeTrajectory
Trajectory foeTrajectory
Trajectory of the foe vehicle.
Definition: MSDevice_SSM.h:261
MSDevice_SSM::getVehiclesOnJunction
static void getVehiclesOnJunction(const MSJunction *, double egoDistToConflictLane, const MSLane *const egoConflictLane, FoeInfoMap &foeCollector)
Collects all vehicles on the junction into foeCollector.
Definition: MSDevice_SSM.cpp:3152
MSDevice_SSM::ENCOUNTER_TYPE_ON_ADJACENT_LANES
ENCOUNTER_TYPE_ON_ADJACENT_LANES.
Definition: MSDevice_SSM.h:77
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:2393
gPrecision
int gPrecision
the precision for floating point outputs
Definition: StdDefs.cpp:27
MSDevice_SSM::myComputePET
bool myComputePET
Definition: MSDevice_SSM.h:705
MSDevice_SSM::FoeInfoMap
std::map< const MSVehicle *, FoeInfo * > FoeInfoMap
Definition: MSDevice_SSM.h:356
MSDevice_SSM::ENCOUNTER_TYPE_MERGING_FOLLOWER
ENCOUNTER_TYPE_MERGING_FOLLOWER.
Definition: MSDevice_SSM.h:86
fn
static double fn[10]
Definition: odrSpiral.cpp:82
MSDevice_SSM::Encounter::Trajectory::v
PositionVector v
Definition: MSDevice_SSM.h:182
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.
Definition: MSDevice_SSM.h:71
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:2785
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:228
MSDevice_SSM::EncounterApproachInfo::egoConflictEntryDist
double egoConflictEntryDist
Definition: MSDevice_SSM.h:306
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:250
MSDevice_SSM::EncounterApproachInfo::foeConflictEntryDist
double foeConflictEntryDist
Definition: MSDevice_SSM.h:307
MSDevice_SSM::myComputeBR
bool myComputeBR
Definition: MSDevice_SSM.h:705
MSDevice_SSM::Encounter::maxDRAC
ConflictPointInfo maxDRAC
Definition: MSDevice_SSM.h:284
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:2719
MSDevice_SSM::SSM_WARN_THRESHOLDS
Definition: MSDevice_SSM.h:750
Named::getID
const std::string & getID() const
Returns the id.
Definition: Named.h:77
MSMoveReminder::Notification
Notification
Definition of a vehicle state.
Definition: MSMoveReminder.h:89
WRITE_ERROR
#define WRITE_ERROR(msg)
Definition: MsgHandler.h:245
MSDevice_SSM::myUseGeoCoords
bool myUseGeoCoords
Whether to use the original coordinate system for output.
Definition: MSDevice_SSM.h:703
MSDevice_SSM::ENCOUNTER_TYPE_FOE_LEFT_CONFLICT_AREA
ENCOUNTER_TYPE_FOE_LEFT_CONFLICT_AREA.
Definition: MSDevice_SSM.h:105
MSNet::getVehicleControl
MSVehicleControl & getVehicleControl()
Returns the vehicle control.
Definition: MSNet.h:337
MSDevice_SSM::Encounter::closingRequested
bool closingRequested
this flag is set by updateEncounter() or directly in processEncounters(), where encounters are closed...
Definition: MSDevice_SSM.h:289
MSDevice_SSM::myComputeSGAP
bool myComputeSGAP
Definition: MSDevice_SSM.h:705
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:1313
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:65
MSAbstractLaneChangeModel.h
MSDevice_SSM::EncounterType
EncounterType
Different types of encounters corresponding to relative positions of the vehicles....
Definition: MSDevice_SSM.h:66
MSDevice_SSM::ENCOUNTER_TYPE_MERGING_ADJACENT
ENCOUNTER_TYPE_MERGING_ADJACENT.
Definition: MSDevice_SSM.h:88
MSLane::getEntryLink
MSLink * getEntryLink() const
Returns the entry link if this is an internal lane, else 0.
Definition: MSLane.cpp:2127
MSVehicle
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:80
MSVehicle::getMaxSpeedOnLane
double getMaxSpeedOnLane() const
Returns the maximal speed for the vehicle on its current lane (including speed factor and deviation,...
Definition: MSVehicle.h:571
MSDevice_SSM::SSM_WARN_FILE
Definition: MSDevice_SSM.h:754
MSVehicleDevice
Abstract in-vehicle device.
Definition: MSVehicleDevice.h:55
MSDevice_SSM::Encounter::foe
const MSVehicle * foe
Definition: MSDevice_SSM.h:240