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