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