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