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