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