58 #define DEBUG_EGO_ID "EW.3" 59 #define DEBUG_FOE_ID "WE.0" 61 #define DEBUG_COND(ego) ((ego)!=nullptr && (ego)->getID() == DEBUG_EGO_ID) 63 #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)) 70 #define INVALID std::numeric_limits<double>::max() 73 #define DEFAULT_RANGE 50.0 79 #define AVAILABLE_SSMS "TTC DRAC PET BR SGAP TGAP" 81 #define DEFAULT_THRESHOLD_TTC 3. // in [s.], events get logged if time to collision is below threshold (1.5s. is an appropriate criticality threshold according to Van der Horst, A. R. A. (1991). Time-to-collision as a Cue for Decision-making in Braking [also see Guido et al. 2011]) 82 #define DEFAULT_THRESHOLD_DRAC 3. // in [m/s^2], events get logged if "deceleration to avoid a crash" is above threshold (3.4s. is an appropriate criticality threshold according to American Association of State Highway and Transportation Officials (2004). A Policy on Geometric Design of Highways and Streets [also see Guido et al. 2011]) 83 #define DEFAULT_THRESHOLD_PET 2. // in seconds, events get logged if post encroachment time is below threshold 85 #define DEFAULT_THRESHOLD_BR 0.0 // in [m/s^2], events get logged if brake rate is above threshold 86 #define DEFAULT_THRESHOLD_SGAP 0.2 // in [m.], events get logged if the space headway is below threshold. 87 #define DEFAULT_THRESHOLD_TGAP 0.5 // in [m.], events get logged if the time headway is below threshold. 89 #define DEFAULT_EXTRA_TIME 5. // in seconds, events get logged for extra time even if encounter is over 101 out <<
"NOCONFLICT_AHEAD";
107 out <<
"FOLLOWING_FOLLOWER";
110 out <<
"FOLLOWING_LEADER";
113 out <<
"ON_ADJACENT_LANES";
119 out <<
"MERGING_LEADER";
122 out <<
"MERGING_FOLLOWER";
125 out <<
"MERGING_ADJACENT";
131 out <<
"CROSSING_LEADER";
134 out <<
"CROSSING_FOLLOWER";
137 out <<
"EGO_ENTERED_CONFLICT_AREA";
140 out <<
"FOE_ENTERED_CONFLICT_AREA";
143 out <<
"BOTH_ENTERED_CONFLICT_AREA";
146 out <<
"EGO_LEFT_CONFLICT_AREA";
149 out <<
"FOE_LEFT_CONFLICT_AREA";
152 out <<
"BOTH_LEFT_CONFLICT_AREA";
155 out <<
"FOLLOWING_PASSED";
158 out <<
"MERGING_PASSED";
168 out <<
"unknown type (" << int(type) <<
")";
179 std::set<MSDevice_SSM*, ComparatorNumericalIdLess>*
MSDevice_SSM::myInstances =
new std::set<MSDevice_SSM*, ComparatorNumericalIdLess>();
185 const std::set<MSDevice_SSM*, ComparatorNumericalIdLess>&
195 device->resetEncounters();
196 device->flushConflicts(
true);
197 device->flushGlobalMeasures();
199 myInstances->clear();
213 oc.
doRegister(
"device.ssm.measures", Option::makeUnsetWithDefault<Option_String, std::string>(
""));
214 oc.
addDescription(
"device.ssm.measures",
"SSM Device",
"Specifies which measures will be logged (as a space separated sequence of IDs in ('TTC', 'DRAC', 'PET')).");
215 oc.
doRegister(
"device.ssm.thresholds", Option::makeUnsetWithDefault<Option_String, std::string>(
""));
216 oc.
addDescription(
"device.ssm.thresholds",
"SSM Device",
"Specifies thresholds corresponding to the specified measures (see documentation and watch the order!). Only events exceeding the thresholds will be logged.");
217 oc.
doRegister(
"device.ssm.trajectories", Option::makeUnsetWithDefault<Option_Bool, bool>(
false));
218 oc.
addDescription(
"device.ssm.trajectories",
"SSM Device",
"Specifies whether trajectories will be logged (if false, only the extremal values and times are reported, this is the default).");
220 oc.
addDescription(
"device.ssm.range",
"SSM Device",
"Specifies the detection range in meters (default is " + ::
toString(
DEFAULT_RANGE) +
"m.). For vehicles below this distance from the equipped vehicle, SSM values are traced.");
222 oc.
addDescription(
"device.ssm.extratime",
"SSM Device",
"Specifies the time in seconds to be logged after a conflict is over (default is " + ::
toString(
DEFAULT_EXTRA_TIME) +
"secs.). Required >0 if PET is to be calculated for crossing conflicts.");
223 oc.
doRegister(
"device.ssm.file", Option::makeUnsetWithDefault<Option_String, std::string>(
""));
224 oc.
addDescription(
"device.ssm.file",
"SSM Device",
"Give a global default filename for the SSM output.");
225 oc.
doRegister(
"device.ssm.geo", Option::makeUnsetWithDefault<Option_Bool, bool>(
false));
226 oc.
addDescription(
"device.ssm.geo",
"SSM Device",
"Whether to use coordinates of the original reference system in output (default is false).");
233 WRITE_WARNING(
"SSM Device for vehicle '" + v.
getID() +
"' will not be built. (SSMs not supported in MESO)");
237 std::string deviceID =
"ssm_" + v.
getID();
242 std::map<std::string, double> thresholds;
265 into.push_back(device);
273 egoID(_ego->
getID()),
274 foeID(_foe->
getID()),
278 remainingExtraTime(extraTime),
286 closingRequested(false) {
287 #ifdef DEBUG_ENCOUNTER 289 std::cout <<
"\n" <<
SIMTIME <<
" Constructing encounter of '" <<
ego->
getID() <<
"' and '" <<
foe->
getID() <<
"'" << std::endl;
295 #ifdef DEBUG_ENCOUNTER 297 std::cout <<
"\n" <<
SIMTIME <<
" Destroying encounter of '" <<
egoID <<
"' and '" <<
foeID <<
"' (begin was " <<
begin <<
")" << std::endl;
305 Position conflictPoint,
double egoDistToConflict,
double foeDistToConflict,
double ttc,
double drac, std::pair<double, double> pet) {
306 #ifdef DEBUG_ENCOUNTER 308 std::cout << time <<
" Adding data point for encounter of '" <<
egoID <<
"' and '" <<
foeID <<
"':\n" 309 <<
"type=" << type <<
", egoDistToConflict=" << (egoDistToConflict ==
INVALID ?
"NA" :
::toString(egoDistToConflict))
310 <<
", foeDistToConflict=" << (foeDistToConflict ==
INVALID ?
"NA" : ::
toString(foeDistToConflict))
374 conflictPoint(
Position::invalidPosition()),
379 egoEstimatedConflictEntryTime(
INVALID),
380 foeEstimatedConflictEntryTime(
INVALID),
381 egoEstimatedConflictExitTime(
INVALID),
382 foeEstimatedConflictExitTime(
INVALID),
383 egoConflictAreaLength(
INVALID),
384 foeConflictAreaLength(
INVALID),
385 egoLeftConflict(false),
386 foeLeftConflict(false),
402 std::cout <<
"\n" <<
SIMTIME <<
" Device '" <<
getID() <<
"' updateAndWriteOutput()\n" 403 <<
" Holder is off-road! Calling resetEncounters()." 416 std::cout <<
"\n" <<
SIMTIME <<
" Device '" <<
getID() <<
"' update()\n" 427 if (foes.size() > 0) {
428 std::cout <<
"Scanned surroundings: Found potential foes:\n";
429 for (FoeInfoMap::const_iterator i = foes.begin(); i != foes.end(); ++i) {
430 std::cout << i->first->getID() <<
" ";
432 std::cout << std::endl;
434 std::cout <<
"Scanned surroundings: No potential conflict could be identified." << std::endl;
464 double leaderSearchDist = 0;
465 std::pair<const MSVehicle*, double> leader(
nullptr, 0.);
473 if (leaderSearchDist > 0.) {
478 if (leader.first ==
nullptr) {
481 double sgap = leader.second + leader.first->getVehicleType().getMinGap();
493 const double tgap = (leader.second + leader.first->getVehicleType().getMinGap()) /
myHolderMS->
getSpeed();
509 std::cout <<
"\n" <<
SIMTIME <<
" Device '" <<
getID() <<
"' createEncounters()" << std::endl;
510 std::cout <<
"New foes:\n";
511 for (FoeInfoMap::const_iterator vi = foes.begin(); vi != foes.end(); ++vi) {
512 std::cout << vi->first->getID() <<
"\n";
514 std::cout << std::endl;
518 for (FoeInfoMap::const_iterator foe = foes.begin(); foe != foes.end(); ++foe) {
525 assert(myOldestActiveEncounterBegin <= e->begin);
548 std::cout <<
"\n" <<
SIMTIME <<
" Device '" <<
getID() <<
"' processEncounters(forceClose = " << forceClose <<
")" << std::endl;
549 std::cout <<
"Currently present foes:\n";
550 for (FoeInfoMap::const_iterator vi = foes.begin(); vi != foes.end(); ++vi) {
551 std::cout << vi->first->getID() <<
"\n";
553 std::cout << std::endl;
569 if (foes.find(e->
foe) != foes.end()) {
581 std::cout <<
" Requesting encounter closure because both left conflict area of previous encounter but another encounter lies ahead." << std::endl;
595 std::cout <<
" Requesting encounter closure because..." << std::endl;
597 std::cout <<
" ... extra time elapsed." << std::endl;
598 }
else if (forceClose) {
599 std::cout <<
" ... closing was forced." << std::endl;
601 std::cout <<
" ... foe disappeared." << std::endl;
612 double eBegin = e->
begin;
637 std::cout <<
SIMTIME <<
" qualifiesAsConflict() for encounter of vehicles '" 657 assert(e->
size() > 0);
665 std::cout <<
SIMTIME <<
" closeEncounter() of vehicles '" 667 <<
"' (was ranked as " << (wasConflict ?
"conflict" :
"non-conflict") <<
")" << std::endl;
675 std::cout <<
"pastConflictsQueue of veh '" <<
myHolderMS->
getID() <<
"':\n";
683 std::cout <<
" Conflict with foe '" << c->foe <<
"' (time " << c->begin <<
"-" << c->end <<
")\n";
685 if (c->begin < lastBegin) {
686 std::cout <<
" Queue corrupt...\n";
689 lastBegin = c->begin;
691 std::cout << std::endl;
704 #ifdef DEBUG_ENCOUNTER 706 std::cout <<
SIMTIME <<
" updateEncounter() of vehicles '" << e->
egoID <<
"' and '" << e->
foeID <<
"'\n";
733 #ifdef DEBUG_ENCOUNTER 735 std::cout <<
SIMTIME <<
" Encounter of vehicles '" << e->
egoID <<
"' and '" << e->
foeID <<
"' does not imply any conflict.\n";
766 if (e->
size() == 0) {
767 #ifdef DEBUG_ENCOUNTER 769 std::cout <<
SIMTIME <<
" type when creating encounter: " << eInfo.
type <<
"\n";
811 std::cout <<
SIMTIME <<
" determineConflictPoint()" << std::endl;
821 assert(e->
size() > 0);
842 std::cout <<
"No conflict point associated with encounter type " << type << std::endl;
850 std::cout <<
" Conflict at " << eInfo.
conflictPoint << std::endl;
865 std::cout <<
SIMTIME <<
" estimateConflictTimes() for ego '" << e->
egoID <<
"' and foe '" << e->
foeID <<
"'\n" 866 <<
" encounter type: " << eInfo.
type <<
"\n" 880 std::cout <<
" encouter type " << type <<
" -> no entry/exit times to be calculated." 927 std::cout <<
" Conflict type: " <<
toString(type) <<
"\n" 962 std::stringstream ss;
963 ss <<
"SSM device of vehicle '" << e->
egoID <<
"' detected collision with vehicle '" << e->
foeID <<
"' at time " <<
SIMTIME;
969 std::cout <<
" -> ego is estimated leader at conflict entry." 978 std::cout <<
" -> foe is estimated leader at conflict entry." 994 std::cout <<
SIMTIME <<
" computeSSMs() for vehicles '" 1026 std::stringstream ss;
1027 ss <<
"'" << type <<
"'";
1028 WRITE_WARNING(
"Unknown or undetermined encounter type at computeSSMs(): " + ss.str());
1034 std::cout <<
"computeSSMs() for encounter of vehicles '" << e->
egoID <<
"' and '" << e->
foeID <<
"':\n" 1047 if (e->
size() == 0) {
1051 std::pair<double, double>&
pet = eInfo.
pet;
1055 std::cout <<
SIMTIME <<
" determinePET() for encounter of vehicles '" << e->
egoID <<
"' and '" << e->
foeID <<
"'" 1056 <<
"(type: " <<
toString(static_cast<EncounterType>(e->
typeSpan.back())) <<
")" << std::endl;
1070 std::cout <<
"PET for crossing encounter already calculated as " << e->
PET.
value 1113 std::cout <<
"Unexpected branch in determinePET: Both passed conflict area in the same step." 1131 std::cout <<
"Calculated PET = " << pet.second <<
" (at t=" << pet.first <<
")" 1138 std::cout <<
"PET unappropriate for merging and pre-crossing situations. No calculation performed." 1155 std::cout <<
SIMTIME <<
" determineTTCandDRAC() for encounter of vehicles '" << e->
egoID <<
"' and '" << e->
foeID <<
"' (type = " << eInfo.
type <<
")" 1198 std::cout <<
" Conflict times with constant speed extrapolation for merging situation:\n " 1199 <<
" egoEntryTime=" << (egoEntryTime ==
INVALID ?
"NA" :
::toString(egoEntryTime))
1200 <<
", egoExitTime=" << (egoExitTime ==
INVALID ?
"NA" : ::
toString(egoExitTime))
1201 <<
", foeEntryTime=" << (foeEntryTime ==
INVALID ?
"NA" : ::
toString(foeEntryTime))
1202 <<
", foeExitTime=" << (foeExitTime ==
INVALID ?
"NA" : ::
toString(foeExitTime))
1213 std::cout <<
" No TTC and DRAC computed as one vehicle is stopped." << std::endl;
1218 double leaderEntryTime =
MIN2(egoEntryTime, foeEntryTime);
1219 double followerEntryTime =
MAX2(egoEntryTime, foeEntryTime);
1220 double leaderExitTime = leaderEntryTime == egoEntryTime ? egoExitTime : foeExitTime;
1227 if (leaderExitTime >= followerEntryTime) {
1230 ttc =
computeTTC(followerConflictDist, followerSpeed, 0.);
1235 drac =
computeDRAC(followerConflictDist, followerSpeed, 0.);
1241 std::cout <<
" Extrapolation predicts collision *at* merge point with TTC=" << ttc
1242 <<
", drac=" << drac << std::endl;
1249 double gapAfterMerge = followerConflictDist - leaderExitTime * followerSpeed;
1250 assert(gapAfterMerge >= 0);
1253 double ttcAfterMerge =
computeTTC(gapAfterMerge, followerSpeed, leaderSpeed);
1254 ttc = ttcAfterMerge ==
INVALID ?
INVALID : leaderExitTime + ttcAfterMerge;
1258 double g0 = followerConflictDist - leaderConflictDist - leaderLength;
1261 assert(leaderSpeed - followerSpeed > 0);
1266 drac =
computeDRAC(g0, followerSpeed, leaderSpeed);
1273 assert(drac ==
INVALID || drac == 0.0);
1274 std::cout <<
" Extrapolation does not predict any collision." << std::endl;
1276 std::cout <<
" Extrapolation predicts collision *after* merge point with TTC=" 1278 <<
", drac=" << (drac ==
INVALID ?
"NA" : ::
toString(drac)) << std::endl;
1318 std::stringstream ss;
1319 ss <<
"'" << type <<
"'";
1320 WRITE_WARNING(
"Underspecified or unknown encounter type in MSDevice_SSM::determineTTCandDRAC(): " + ss.str());
1340 std::cout <<
"computeTTC() with gap=" << gap <<
", followerSpeed=" << followerSpeed <<
", leaderSpeed=" << leaderSpeed
1346 double dv = followerSpeed - leaderSpeed;
1365 double dv = followerSpeed - leaderSpeed;
1369 assert(followerSpeed > 0.);
1370 return 0.5 * dv * dv / gap;
1386 #ifdef DEBUG_SSM_DRAC 1388 std::cout <<
SIMTIME <<
"computeDRAC() with" 1389 <<
"\ndEntry1=" << dEntry1 <<
", dEntry2=" << dEntry2
1390 <<
", dExit1=" << dExit1 <<
", dExit2=" << dExit2
1391 <<
",\nv1=" << v1 <<
", v2=" << v2
1396 if (dExit1 <= 0. || dExit2 <= 0.) {
1398 #ifdef DEBUG_SSM_DRAC 1400 std::cout <<
"One already left conflict area -> drac == 0." << std::endl;
1405 if (dEntry1 <= 0. && dEntry2 <= 0.) {
1407 #ifdef DEBUG_SSM_DRAC 1409 std::cout <<
"Both entered conflict area but neither left. -> collision!" << std::endl;
1415 double drac = std::numeric_limits<double>::max();
1418 #ifdef DEBUG_SSM_DRAC 1420 std::cout <<
"Ego could break..." << std::endl;
1425 drac =
MIN2(drac, 2 * (v1 - dEntry1 / tExit2) / tExit2);
1426 #ifdef DEBUG_SSM_DRAC 1428 std::cout <<
" Foe expected to leave in " << tExit2 <<
"-> Ego needs drac=" << drac << std::endl;
1436 #ifdef DEBUG_SSM_DRAC 1438 std::cout <<
" Foe is expected stop on conflict area -> Ego needs drac=" << drac << std::endl;
1443 #ifdef DEBUG_SSM_DRAC 1445 std::cout <<
" Foe is expected stop before conflict area -> no drac computation for ego (will be done for foe if applicable)" << std::endl;
1454 #ifdef DEBUG_SSM_DRAC 1456 std::cout <<
"Foe could break..." << std::endl;
1461 #ifdef DEBUG_SSM_DRAC 1463 std::cout <<
" Ego expected to leave in " << tExit1 <<
"-> Foe needs drac=" << (2 * (v2 - dEntry2 / tExit1) / tExit1) << std::endl;
1466 drac =
MIN2(drac, 2 * (v2 - dEntry2 / tExit1) / tExit1);
1471 #ifdef DEBUG_SSM_DRAC 1473 std::cout <<
" Ego is expected stop on conflict area -> Foe needs drac=" <<
computeDRAC(dEntry2, v2, 0) << std::endl;
1479 #ifdef DEBUG_SSM_DRAC 1481 std::cout <<
" Ego is expected stop before conflict area -> no drac computation for foe (done for ego if applicable)" << std::endl;
1502 #ifdef DEBUG_ENCOUNTER 1504 std::cout <<
SIMTIME <<
" checkConflictEntryAndExit() for encounter of vehicles '" << e->
egoID <<
"' and '" << e->
foeID <<
"'" 1514 if (e->
size() == 0) {
1518 if (egoPastConflictExit) {
1519 if (foePastConflictExit) {
1521 }
else if (foePastConflictEntry) {
1526 }
else if (foePastConflictExit) {
1527 if (egoPastConflictEntry) {
1534 if (egoPastConflictEntry) {
1535 if (foePastConflictEntry) {
1540 }
else if (foePastConflictEntry) {
1568 #ifdef DEBUG_ENCOUNTER 1584 #ifdef DEBUG_ENCOUNTER 1602 #ifdef DEBUG_ENCOUNTER 1620 #ifdef DEBUG_ENCOUNTER 1637 #ifdef DEBUG_ENCOUNTER 1639 std::cout <<
SIMTIME <<
" updatePassedEncounter() for vehicles '" << e->
egoID <<
"' and '" << e->
foeID <<
"'\n";
1643 if (foeInfo ==
nullptr) {
1646 #ifdef DEBUG_ENCOUNTER 1662 #ifdef DEBUG_ENCOUNTER 1664 std::cout <<
" This encounter wasn't classified as a potential conflict lately.\n";
1667 if (foeInfo ==
nullptr) {
1673 std::cout <<
" Requesting encounter closure because foeInfo==nullptr" << std::endl;
1677 #ifdef DEBUG_ENCOUNTER 1679 std::cout <<
" Closing encounter.\n";
1689 #ifdef DEBUG_ENCOUNTER 1691 std::cout <<
" Encounter was previously classified as a follow/lead situation.\n";
1700 #ifdef DEBUG_ENCOUNTER 1702 std::cout <<
" Encounter was previously classified as a merging situation.\n";
1717 #ifdef DEBUG_ENCOUNTER 1719 std::cout <<
" Encounter was previously classified as a crossing situation of type " << lastPotentialConflictType <<
".\n";
1737 #ifdef DEBUG_ENCOUNTER 1752 if ((!egoEnteredConflict) && !foeEnteredConflict) {
1756 eInfo.
type = lastPotentialConflictType;
1757 }
else if (egoEnteredConflict && !foeEnteredConflict) {
1759 }
else if ((!egoEnteredConflict) && foeEnteredConflict) {
1765 if ((!egoLeftConflict) && !foeLeftConflict) {
1769 }
else if (egoLeftConflict && !foeLeftConflict) {
1773 }
else if ((!egoLeftConflict) && foeLeftConflict) {
1788 #ifdef DEBUG_ENCOUNTER 1790 std::cout <<
" Updated classification: " << eInfo.
type <<
"\n";
1799 #ifdef DEBUG_ENCOUNTER 1801 std::cout <<
"classifyEncounter() called.\n";
1804 if (foeInfo ==
nullptr) {
1823 #ifdef DEBUG_ENCOUNTER 1825 std::cout <<
" Ongoing crossing conflict will be traced by passedEncounter().\n";
1841 double foeDistToConflictLane;
1844 #ifdef DEBUG_ENCOUNTER 1846 std::cout <<
" egoConflictLane='" << (egoConflictLane == 0 ?
"NULL" : egoConflictLane->
getID()) <<
"'\n" 1847 <<
" foeConflictLane='" << (foeConflictLane == 0 ?
"NULL" : foeConflictLane->
getID()) <<
"'\n" 1848 <<
" egoDistToConflictLane=" << egoDistToConflictLane
1849 <<
" foeDistToConflictLane=" << foeDistToConflictLane
1864 if (foeConflictLane ==
nullptr) {
1867 #ifdef DEBUG_ENCOUNTER 1869 std::cout <<
"-> Encounter type: No conflict.\n";
1872 }
else if (!egoConflictLane->isInternal()) {
1874 if (egoConflictLane == egoLane) {
1878 if (foeLane == egoLane) {
1880 if (!egoOpposite && !foeOpposite) {
1888 #ifdef DEBUG_ENCOUNTER 1890 std::cout <<
"-> Encounter type: Lead/follow-situation on non-internal lane '" << egoLane->
getID() <<
"'\n";
1893 }
else if (egoOpposite && foeOpposite) {
1901 #ifdef DEBUG_ENCOUNTER 1903 std::cout <<
"-> Encounter type: Lead/follow-situation while both are driving in the opposite direction on non-internal lane '" << egoLane->
getID() <<
"'\n";
1924 #ifdef DEBUG_ENCOUNTER 1926 std::cout <<
"-> Encounter type: oncoming on non-internal lane '" << egoLane->
getID() <<
"'\n";
1935 #ifdef DEBUG_ENCOUNTER 1937 std::cout <<
"-> Encounter type: " << type << std::endl;
1942 if (!egoOpposite && !foeOpposite) {
1945 assert(egoDistToConflictLane <= 0);
1947 if (foeConflictLane == egoLane) {
1951 #ifdef DEBUG_ENCOUNTER 1953 std::cout <<
"-> Encounter type: Ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' leads foe '" 1954 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'" 1960 #ifdef DEBUG_ENCOUNTER 1962 std::cout <<
"-> Encounter type: " << type << std::endl;
1967 }
else if (egoOpposite && foeOpposite) {
1979 #ifdef DEBUG_ENCOUNTER 1981 std::cout <<
"-> Encounter type: Lead/follow-situation while both are driving in the opposite direction on non-internal lane '" << egoLane->
getID() <<
"'\n";
2005 #ifdef DEBUG_ENCOUNTER 2007 std::cout <<
"-> Encounter type: oncoming on non-internal lane '" << egoLane->
getID() <<
"'\n";
2018 assert(&(foeLane->
getEdge()) == &(egoConflictLane->getEdge()));
2019 assert(foeDistToConflictLane <= 0);
2020 if (foeLane == egoConflictLane) {
2023 #ifdef DEBUG_ENCOUNTER 2025 std::cout <<
"-> Encounter type: Ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' follows foe '" 2026 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'" 2032 #ifdef DEBUG_ENCOUNTER 2034 std::cout <<
"-> Encounter type: " << type << std::endl;
2043 MSLink* egoEntryLink = egoConflictLane->getEntryLink();
2045 if (&(egoEntryLink->getViaLane()->getEdge()) == &(foeEntryLink->
getViaLane()->
getEdge())) {
2046 if (egoEntryLink != foeEntryLink) {
2049 #ifdef DEBUG_ENCOUNTER 2051 std::cout <<
"-> Encounter type: " << type << std::endl;
2056 if (egoLane == egoConflictLane && foeLane != foeConflictLane) {
2063 #ifdef DEBUG_ENCOUNTER 2065 std::cout <<
"-> Encounter type: Ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' leads foe '" 2066 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'" 2069 }
else if (egoLane != egoConflictLane && foeLane == foeConflictLane) {
2076 #ifdef DEBUG_ENCOUNTER 2078 std::cout <<
"-> Encounter type: Ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' follows foe '" 2079 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'" 2086 #ifdef DEBUG_ENCOUNTER 2088 std::cout <<
"-> Encounter type: Ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' merges with foe '" 2089 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'" 2097 assert(egoLane == egoConflictLane);
2098 assert(foeLane == foeConflictLane);
2099 if (egoLane == foeLane) {
2104 #ifdef DEBUG_ENCOUNTER 2106 std::cout <<
"-> Encounter type: Ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' leads foe '" 2107 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'" 2114 #ifdef DEBUG_ENCOUNTER 2116 std::cout <<
"-> Encounter type: Ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' follows foe '" 2117 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'" 2124 #ifdef DEBUG_ENCOUNTER 2126 std::cout <<
" Lead/follow situation on consecutive internal lanes." << std::endl;
2129 MSLane* lane = egoEntryLink->getViaLane();
2131 #pragma warning(push) 2132 #pragma warning(disable: 4127) // do not warn about constant conditional expression 2136 #pragma warning(pop) 2140 if (egoLane == lane) {
2145 while (lane != foeLane) {
2151 egoConflictLane = lane;
2152 #ifdef DEBUG_ENCOUNTER 2154 std::cout <<
"-> Encounter type: Ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' follows foe '" 2155 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'" 2160 }
else if (foeLane == lane) {
2165 while (lane != egoLane) {
2171 foeConflictLane = lane;
2172 #ifdef DEBUG_ENCOUNTER 2174 std::cout <<
"-> Encounter type: Ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' leads foe '" 2175 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'" 2185 #ifdef DEBUG_ENCOUNTER 2187 std::cout <<
"-> Encounter type: Lead/follow-situation on connection from '" << egoEntryLink->getLaneBefore()->getID()
2188 <<
"' to '" << egoEntryLink->getLane()->getID() <<
"'" << std::endl;
2195 const std::vector<MSLink*>& egoFoeLinks = egoEntryLink->getFoeLinks();
2196 const std::vector<MSLink*>& foeFoeLinks = foeEntryLink->
getFoeLinks();
2198 bool crossOrMerge = (find(egoFoeLinks.begin(), egoFoeLinks.end(), foeEntryLink) != egoFoeLinks.end()
2199 || std::find(foeFoeLinks.begin(), foeFoeLinks.end(), egoEntryLink) != foeFoeLinks.end());
2200 if (!crossOrMerge) {
2209 #ifdef DEBUG_ENCOUNTER 2211 std::cout <<
"-> Encounter type: No conflict.\n";
2215 }
else if (&(foeEntryLink->
getLane()->
getEdge()) == &(egoEntryLink->getLane()->getEdge())) {
2216 if (foeEntryLink->
getLane() == egoEntryLink->getLane()) {
2218 assert(egoConflictLane->isInternal());
2236 #ifdef DEBUG_ENCOUNTER 2238 std::cout <<
"-> Encounter type: Merging situation of ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' and foe '" 2239 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'" 2246 #ifdef DEBUG_ENCOUNTER 2248 std::cout <<
"-> Encounter type: No conflict: " << type << std::endl;
2255 assert(egoConflictLane->isInternal());
2256 assert(foeConflictLane->
getEdge().
getToJunction() == egoConflictLane->getEdge().getToJunction());
2263 egoConflictLane = egoConflictLane->getFirstInternalInConnection(offset);
2264 egoDistToConflictLane -= offset;
2266 foeDistToConflictLane -= offset;
2270 double egoDistToConflictFromJunctionEntry =
INVALID;
2271 double foeInternalLaneLengthsBeforeCrossing = 0.;
2272 while (foeConflictLane !=
nullptr && foeConflictLane->
isInternal()) {
2273 egoDistToConflictFromJunctionEntry = egoEntryLink->getLengthsBeforeCrossing(foeConflictLane);
2274 if (egoDistToConflictFromJunctionEntry !=
INVALID) {
2279 foeInternalLaneLengthsBeforeCrossing += foeConflictLane->
getLength();
2282 assert(foeConflictLane != 0 && foeConflictLane->
isInternal());
2284 assert(egoDistToConflictFromJunctionEntry !=
INVALID);
2287 double foeDistToConflictFromJunctionEntry =
INVALID;
2288 double egoInternalLaneLengthsBeforeCrossing = 0.;
2289 foeDistToConflictFromJunctionEntry =
INVALID;
2290 while (egoConflictLane !=
nullptr && egoConflictLane->isInternal()) {
2292 if (foeDistToConflictFromJunctionEntry !=
INVALID) {
2297 egoInternalLaneLengthsBeforeCrossing += egoConflictLane->getLength();
2299 egoConflictLane = egoConflictLane->getCanonicalSuccessorLane();
2300 assert(egoConflictLane != 0 && egoConflictLane->isInternal());
2302 assert(foeDistToConflictFromJunctionEntry !=
INVALID);
2314 Position egoEntryPos = egoEntryLink->getViaLane()->getShape().front();
2315 Position egoExitPos = egoEntryLink->getCorrespondingExitLink()->getInternalLaneBefore()->getShape().back();
2325 assert(angle <= 2 *
M_PI);
2329 assert(angle >= -
M_PI);
2330 assert(angle <=
M_PI);
2332 double crossingOrientation = (angle < 0) - (angle > 0);
2352 #ifdef DEBUG_ENCOUNTER 2354 std::cout <<
" Determined exact conflict distances for crossing conflict." 2355 <<
"\n crossingOrientation=" << crossingOrientation
2358 <<
", relativeAngle=" << angle
2359 <<
" (foe from " << (crossingOrientation > 0 ?
"right)" :
"left)")
2360 <<
"\n resulting offset for conflict entry distance:" 2363 <<
"\n distToConflictLane:" 2364 <<
"\n ego=" << egoDistToConflictLane
2365 <<
", foe=" << foeDistToConflictLane
2366 <<
"\n distToConflictFromJunctionEntry:" 2367 <<
"\n ego=" << egoDistToConflictFromJunctionEntry
2368 <<
", foe=" << foeDistToConflictFromJunctionEntry
2369 <<
"\n resulting entry distances:" 2372 <<
"\n resulting exit distances:" 2377 std::cout <<
"real egoConflictLane: '" << (egoConflictLane == 0 ?
"NULL" : egoConflictLane->getID()) <<
"'\n" 2378 <<
"real foeConflictLane: '" << (foeConflictLane == 0 ?
"NULL" : foeConflictLane->
getID()) <<
"'\n" 2379 <<
"-> Encounter type: Crossing situation of ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' and foe '" 2380 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'" 2397 std::cout <<
SIMTIME <<
" findFoeConflictLane() for foe '" 2399 <<
"' (with egoConflictLane=" << (egoConflictLane == 0 ?
"NULL" : egoConflictLane->
getID())
2409 #ifdef DEBUG_SSM_OPPOSITE 2425 return egoConflictLane;
2434 return egoConflictLane;
2444 assert(foeLane->
isInternal() || *laneIter == foeLane);
2451 if (conflictJunction != 0) {
2452 std::cout <<
"Potential conflict on junction '" << conflictJunction->
getID()
2458 if (egoConflictLane !=
nullptr && egoConflictLane->
isInternal() && egoConflictLane->
getLinkCont()[0]->getViaLane() == foeLane) {
2459 distToConflictLane += egoConflictLane->
getLength();
2467 if (*laneIter ==
nullptr) {
2468 while (foeLane !=
nullptr && foeLane->
isInternal()) {
2469 distToConflictLane += foeLane->
getLength();
2470 foeLane = foeLane->
getLinkCont()[0]->getViaLane();
2473 assert(laneIter == foeBestLanesEnd || *laneIter != 0);
2477 while (laneIter != foeBestLanesEnd && distToConflictLane <=
myRange) {
2479 assert(*laneIter == foeLane || foeLane == 0);
2480 foeLane = *laneIter;
2485 std::cout <<
"Found conflict lane for foe: '" << foeLane->
getID() <<
"'" << std::endl;
2492 distToConflictLane += foeLane->
getLength();
2496 if (laneIter == foeBestLanesEnd) {
2499 MSLane* nextNonInternalLane = *laneIter;
2503 assert(foeLane == 0 || foeLane->
isInternal());
2504 if (foeLane ==
nullptr) {
2505 foeLane = nextNonInternalLane;
2509 assert(foeLane != 0);
2512 std::cout <<
"Found conflict lane for foe: '" << foeLane->
getID() <<
"'" << std::endl;
2520 foeLane = nextNonInternalLane;
2553 std::cout <<
SIMTIME <<
" flushGlobalMeasures() of vehicle '" 2618 std::cout <<
SIMTIME <<
" writeOutConflict() of vehicles '" 2699 std::string res =
"";
2700 for (std::vector<double>::const_iterator i = v.begin(); i != v.end(); ++i) {
2701 res += (i == v.begin() ?
"" : sep) + (*i == NA ?
"NA" : ::
toString(*i));
2708 std::string res =
"";
2709 for (std::vector<double>::const_iterator i = v.begin(); i != v.end(); ++i) {
2710 res += (i == v.begin() ?
"" : sep) + (find(NAs.begin(), NAs.end(), *i) != NAs.end() ?
"NA" :
::toString(*i));
2720 bool trajectories,
double range,
double extraTime,
bool useGeoCoords) :
2729 myMinSGAP(std::make_pair(std::make_pair(-1,
Position(0., 0.)), std::numeric_limits<double>::max()),
""),
2730 myMinTGAP(std::make_pair(std::make_pair(-1,
Position(0., 0.)), std::numeric_limits<double>::max()),
"") {
2758 std::vector<std::string> measures;
2759 std::vector<double> threshVals;
2761 measures.push_back(i->first);
2762 threshVals.push_back(i->second);
2764 std::cout <<
"Initialized ssm device '" <<
id <<
"' with " 2787 #ifdef DEBUG_SSM_NOTIFICATIONS 2789 std::cout <<
"device '" <<
getID() <<
"' notifyEnter: reason=" << reason <<
" currentEdge=" << v->getLane()->getEdge().getID() <<
"\n";
2801 #ifdef DEBUG_SSM_NOTIFICATIONS 2803 std::cout <<
"device '" <<
getID() <<
"' notifyLeave: reason=" << reason <<
" currentEdge=" << v->getLane()->getEdge().getID() <<
"\n";
2813 double ,
double newSpeed) {
2814 #ifdef DEBUG_SSM_NOTIFICATIONS 2815 std::cout <<
"device '" <<
getID() <<
"' notifyMove: newSpeed=" << newSpeed <<
"\n";
2828 #ifdef DEBUG_SSM_SURROUNDING 2831 std::cout <<
SIMTIME <<
" Looking for surrounding vehicles for ego vehicle '" << veh.
getID()
2854 for (
int i = 0; i < (int)egoBestLanes.size(); i++) {
2855 if (egoBestLanes[i] !=
nullptr && egoBestLanes[i]->getEdge().getOppositeEdge() !=
nullptr) {
2856 egoBestLanes[i] = egoBestLanes[i]->getEdge().getOppositeEdge()->getLanes().back();
2860 std::vector<MSLane*>::const_iterator laneIter = egoBestLanes.begin();
2864 assert(lane->
isInternal() || lane == *laneIter);
2867 const MSLane* nextNonInternalLane =
nullptr;
2875 double remainingDownstreamRange = range;
2877 double distToConflictLane = isOpposite ? pos - veh.
getLane()->
getLength() : -pos;
2879 std::set<const MSJunction*> seenJunctions;
2882 std::vector<UpstreamScanStartInfo> upstreamScanStartPositions;
2891 #ifdef DEBUG_SSM_SURROUNDING 2893 std::cout <<
SIMTIME <<
" Vehicle '" << veh.
getID() <<
"' is on internal edge " << edge->
getID() <<
"'." << std::endl;
2908 for (ConstMSEdgeVector::const_iterator ei = incoming.begin(); ei != incoming.end(); ++ei) {
2909 if ((*ei)->isInternal()) {
2928 lane = *(++laneIter);
2942 while (remainingDownstreamRange > 0.) {
2943 #ifdef DEBUG_SSM_SURROUNDING 2945 std::cout <<
SIMTIME <<
" Scanning downstream for vehicle '" << veh.
getID() <<
"' on lane '" << veh.
getLane()->
getID() <<
"', position=" << pos <<
".\n" 2946 <<
"Considering edge '" << edge->
getID() <<
"' Remaining downstream range = " << remainingDownstreamRange
2947 <<
"\nbestLanes=" <<
::toString(egoBestLanes) <<
"\n" 2953 assert(pos == 0 || lane == veh.
getLane());
2954 if (pos + remainingDownstreamRange < lane->getLength()) {
2956 upstreamScanStartPositions.push_back(
UpstreamScanStartInfo(edge, pos + remainingDownstreamRange, remainingDownstreamRange, distToConflictLane, lane));
2964 remainingDownstreamRange -= lane->
getLength() - pos;
2965 distToConflictLane += lane->
getLength();
2970 assert(laneIter == egoBestLanes.end() || *laneIter != 0);
2973 if (laneIter != egoBestLanes.end()) {
2978 nextNonInternalLane = *laneIter;
2980 if (isOpposite && link ==
nullptr) {
2981 link = nextNonInternalLane->
getLinkTo(lane);
2982 if (link ==
nullptr) {
2986 assert(link != 0 || link->
getLength() == 0.);
2990 if (lane ==
nullptr) {
2992 lane = nextNonInternalLane;
2994 if (seenJunctions.count(junction) == 0) {
2995 seenJunctions.insert(junction);
3002 if (seenJunctions.count(junction) == 0) {
3005 seenJunctions.insert(junction);
3008 for (ConstMSEdgeVector::const_iterator ei = incoming.begin(); ei != incoming.end(); ++ei) {
3012 upstreamScanStartPositions.push_back(
UpstreamScanStartInfo(*ei, (*ei)->getLength(), range, distToConflictLane, lane));
3016 remainingDownstreamRange -= linkLength;
3017 distToConflictLane += linkLength;
3018 #ifdef DEBUG_SSM_SURROUNDING 3020 std::cout <<
" Downstream Scan for vehicle '" << veh.
getID() <<
"' proceeded over junction '" << junction->
getID()
3021 <<
"',\n linkLength=" << linkLength <<
", remainingDownstreamRange=" << remainingDownstreamRange
3027 lane = nextNonInternalLane;
3030 #ifdef DEBUG_SSM_SURROUNDING 3032 std::cout <<
" Downstream Scan for vehicle '" << veh.
getID() <<
"' stops at junction '" << junction->
getID()
3033 <<
"', which has already been scanned." 3051 foeCollector.erase(&veh);
3057 #ifdef DEBUG_SSM_SURROUNDING 3059 std::cout <<
SIMTIME <<
" getUpstreamVehicles() for edge '" << scanStart.
edge->
getID() <<
"'" 3060 <<
" pos = " << scanStart.
pos <<
" range = " << scanStart.
range 3061 <<
"\nFound vehicles:" 3065 if (scanStart.
range <= 0) {
3069 const std::vector<MSLane*>& lanes = scanStart.
edge->
getLanes();
3071 for (std::vector<MSLane*>::const_iterator li = lanes.begin(); li != lanes.end(); ++li) {
3074 for (MSLane::VehCont::const_iterator vi = vehicles.begin(); vi != vehicles.end(); ++vi) {
3076 if (foeCollector.find(veh) != foeCollector.end()) {
3081 #ifdef DEBUG_SSM_SURROUNDING 3083 std::cout << veh->
getID() <<
"\n";
3089 foeCollector[veh] = c;
3095 #ifdef DEBUG_SSM_SURROUNDING 3097 std::cout << std::endl;
3105 if (scanStart.
range <= scanStart.
pos) {
3110 double remainingRange = scanStart.
range - scanStart.
pos;
3114 if (seenJunctions.count(junction) == 0) {
3122 seenJunctions.insert(junction);
3126 for (ConstMSEdgeVector::const_iterator ei = incoming.begin(); ei != incoming.end(); ++ei) {
3127 if ((*ei)->isInternal()) {
3130 const MSEdge* inEdge = *ei;
3131 assert(inEdge != 0);
3133 if (distOnJunction >= remainingRange) {
3141 #ifdef DEBUG_SSM_SURROUNDING 3143 std::cout <<
" Downstream Scan for stops at junction '" << junction->
getID()
3144 <<
"', which has already been scanned." 3153 #ifdef DEBUG_SSM_SURROUNDING 3155 std::cout <<
SIMTIME <<
" getVehiclesOnJunction() for junction '" << junction->
getID() <<
"'" 3156 <<
"\nFound vehicles:" 3163 if (foeCollector.find(veh) != foeCollector.end()) {
3164 delete foeCollector[veh];
3169 foeCollector[veh] = c;
3170 #ifdef DEBUG_SSM_SURROUNDING 3173 std::cout << veh->getID() <<
"\n";
3182 for (std::vector<MSLane*>::const_iterator li = lanes.begin(); li != lanes.end(); ++li) {
3188 collectFoeInfos(vehicles);
3202 collectFoeInfos(vehicles2);
3207 #ifdef DEBUG_SSM_SURROUNDING 3209 std::cout << std::endl;
3229 std::string file = deviceID +
".xml";
3243 file = oc.
getString(
"device.ssm.file") ==
"" ? file : oc.
getString(
"device.ssm.file");
3245 std::cout <<
"vehicle '" << v.
getID() <<
"' does not supply vehicle parameter 'device.ssm.file'. Using default of '" << file <<
"'\n";
3255 bool useGeo =
false;
3269 useGeo = oc.
getBool(
"device.ssm.geo");
3271 std::cout <<
"vehicle '" << v.
getID() <<
"' does not supply vehicle parameter 'device.ssm.geo'. Using default of '" <<
::toString(useGeo) <<
"'\n";
3296 range = oc.
getFloat(
"device.ssm.range");
3298 std::cout <<
"vehicle '" << v.
getID() <<
"' does not supply vehicle parameter 'device.ssm.range'. Using default of '" << range <<
"'\n";
3323 extraTime = oc.
getFloat(
"device.ssm.extratime");
3325 std::cout <<
"vehicle '" << v.
getID() <<
"' does not supply vehicle parameter 'device.ssm.extratime'. Using default of '" << extraTime <<
"'\n";
3329 if (extraTime < 0.) {
3331 WRITE_WARNING(
"Negative (or no) value encountered for vehicle parameter 'device.ssm.extratime' in vehicle '" + v.
getID() +
"' using default value " +
::toString(extraTime) +
" instead");
3340 bool trajectories =
false;
3354 trajectories = oc.
getBool(
"device.ssm.trajectories");
3356 std::cout <<
"vehicle '" << v.
getID() <<
"' does not supply vehicle parameter 'device.ssm.trajectories'. Using default of '" <<
::toString(trajectories) <<
"'\n";
3360 return trajectories;
3369 std::string measures_str =
"";
3383 measures_str = oc.
getString(
"device.ssm.measures");
3385 std::cout <<
"vehicle '" << v.
getID() <<
"' does not supply vehicle parameter 'device.ssm.measures'. Using default of '" << measures_str <<
"'\n";
3391 if (measures_str ==
"") {
3392 WRITE_WARNING(
"No measures specified for ssm device of vehicle '" + v.
getID() +
"'. Registering all available SSMs.");
3396 std::vector<std::string> available = st.
getVector();
3398 std::vector<std::string> measures = st.
getVector();
3399 for (std::vector<std::string>::const_iterator i = measures.begin(); i != measures.end(); ++i) {
3400 if (std::find(available.begin(), available.end(), *i) == available.end()) {
3402 WRITE_ERROR(
"SSM identifier '" + *i +
"' is not supported. Aborting construction of SSM device '" + deviceID +
"'.");
3408 std::string thresholds_str =
"";
3422 thresholds_str = oc.
getString(
"device.ssm.thresholds");
3424 std::cout <<
"vehicle '" << v.
getID() <<
"' does not supply vehicle parameter 'device.ssm.thresholds'. Using default of '" << thresholds_str <<
"'\n";
3431 if (thresholds_str !=
"") {
3433 while (count < (
int)measures.size() && st.
hasNext()) {
3435 thresholds.insert(std::make_pair(measures[count], thresh));
3438 if (thresholds.size() < measures.size() || st.
hasNext()) {
3439 WRITE_ERROR(
"Given list of thresholds ('" + thresholds_str +
"') is not of the same size as the list of measures ('" + measures_str +
"').\nPlease specify exactly one threshold for each measure.");
3444 for (std::vector<std::string>::const_iterator i = measures.begin(); i != measures.end(); ++i) {
3447 }
else if (*i ==
"DRAC") {
3449 }
else if (*i ==
"PET") {
3451 }
else if (*i ==
"BR") {
3453 }
else if (*i ==
"SGAP") {
3455 }
else if (*i ==
"TGAP") {
3458 WRITE_ERROR(
"Unknown SSM identifier '" + (*i) +
"'. Aborting construction of ssm device.");
const std::vector< IncomingLaneInfo > & getIncomingLanes() const
#define DEFAULT_THRESHOLD_BR
double foeConflictAreaLength
bool myUseGeoCoords
Whether to use the original coordinate system for output.
void doRegister(const std::string &name, Option *v)
Adds an option under the given name.
OutputDevice & writeAttr(const SumoXMLAttr attr, const T &val)
writes a named attribute
static void determineConflictPoint(EncounterApproachInfo &eInfo)
Calculates the (x,y)-coordinate for the eventually predicted conflict point and stores the result in ...
std::vector< double > myBRspan
All values for brake rate.
ENCOUNTER_TYPE_MERGING_ADJACENT.
double getLength() const
Returns the vehicle's length.
MSEdge & getEdge() const
Returns the lane's edge.
static void getVehiclesOnJunction(const MSJunction *, double egoDistToConflictLane, const MSLane *const egoConflictLane, FoeInfoMap &foeCollector)
Collects all vehicles on the junction into foeCollector.
Representation of a vehicle in the micro simulation.
double getPreviousSpeed() const
Returns the vehicle's speed before the previous time step.
void createEncounters(FoeInfoMap &foes)
Makes new encounters for all given vehicles (these should be the ones entering the device's range in ...
std::string next()
returns the next substring when it exists. Otherwise the behaviour is undefined
virtual const MSVehicleType & getVehicleType() const =0
Returns the vehicle's type.
MSLane * getLane() const
Returns the connected lane.
Position getVelocityVector() const
Returns the vehicle's direction in radians.
ConflictPointInfo maxDRAC
virtual const std::string & getID() const =0
Get the vehicle's ID.
static void cleanup()
Clean up remaining devices instances.
virtual const MSRoute & getRoute() const =0
Returns the current route.
EncounterType
Different types of encounters corresponding to relative positions of the vehicles. The name describes the type from the ego perspective.
MSLane * getLane() const
Returns the lane the vehicle is on.
PositionVector conflictPointSpan
Predicted location of the conflict: In case of MERGING and CROSSING: entry point to conflict area for...
void updatePassedEncounter(Encounter *e, FoeInfo *foeInfo, EncounterApproachInfo &eInfo)
Updates an encounter, which was classified as ENCOUNTER_TYPE_NOCONFLICT_AHEAD this may be the case be...
static void insertOptions(OptionsCont &oc)
Inserts MSDevice_SSM-options.
int gPrecision
the precision for floating point outputs
double egoConflictEntryTime
Times when the ego vehicle entered/left the conflict area. Currently only applies for crossing situat...
void determinePET(EncounterApproachInfo &eInfo) const
Discriminates between different encounter types and correspondingly determines the PET for those case...
std::pair< const MSVehicle *const, double > getLeader(double dist=0) const
Returns the leader of the vehicle looking for a fixed distance.
A device which collects info on the vehicle trip (mainly on departure and arrival) ...
bool updateEncounter(Encounter *e, FoeInfo *foeInfo)
Updates the encounter (adds a new trajectory point).
void determineTTCandDRAC(EncounterApproachInfo &eInfo) const
Discriminates between different encounter types and correspondingly determines TTC and DRAC for those...
std::pair< std::pair< double, Position >, double > myMaxBR
Extremal values for the global measures (as <<<time, Position>, value>, [leaderID]>-pairs) ...
The base class for an intersection.
double getPositionOnLane() const
Get the vehicle's position along the lane.
EncounterVector myActiveEncounters
double foeEstimatedConflictExitTime
MSLink * getCorrespondingExitLink() const
returns the corresponding exit link for entryLinks to a junction.
virtual const MSEdge * getEdge() const =0
Returns the edge the vehicle is currently at.
virtual bool isVehicle() const =0
Get the vehicle's ID.
SUMOVehicle * getVehicle(const std::string &id) const
Returns the vehicle with the given id.
Notification
Definition of a vehicle state.
SUMOVehicle & myHolder
The vehicle that stores the device.
ENCOUNTER_TYPE_FOLLOWING_PASSED.
double computeTTC(double gap, double followerSpeed, double leaderSpeed) const
Computes the time to collision (in seconds) for two vehicles with a given initial gap under the assum...
EncounterType classifyEncounter(const FoeInfo *foeInfo, EncounterApproachInfo &eInfo) const
Classifies the current type of the encounter provided some information on the opponents.
const std::vector< MSLane * > & getLanes() const
Returns this edge's lanes.
std::vector< double > myGlobalMeasuresTimeSpan
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
double getLength() const
Returns the lane's length.
virtual const VehCont & getVehiclesSecure() const
Returns the vehicles container; locks it for microsimulation.
const PositionVector & getShape() const
Returns this lane's shape.
void closeEncounter(Encounter *e)
Finalizes the encounter and calculates SSM values.
const MSRoute & getRoute() const
Returns the current route.
MSLink * getLinkTo(const MSLane *) const
returns the link to the given lane or 0, if it is not connected
Position getPosition(const double offset=0) const
Return current position (x/y, cartesian)
double rotationAtOffset(double pos) const
Returns the rotation at the given length.
static double getDetectionRange(const SUMOVehicle &v)
ENCOUNTER_TYPE_COLLISION.
static void toGeo(Position &x)
convert SUMO-positions to geo coordinates (in place)
std::vector< double > TTCspan
All values for TTC.
ENCOUNTER_TYPE_EGO_ENTERED_CONFLICT_AREA.
Position pos
Predicted location of the conflict: In case of MERGING and CROSSING: entry point to conflict area for...
std::vector< const MSEdge * > ConstMSEdgeVector
bool getBool(const std::string &name) const
Returns the boolean-value of the named option (only for Option_Bool)
const std::string & getID() const
Returns the id.
bool hasNext()
returns the information whether further substrings exist
The base class for microscopic and mesoscopic vehicles.
#define DEFAULT_THRESHOLD_PET
double getInternalLengthsAfter() const
Returns the cumulative length of all internal lanes after this link.
double getLength() const
return the length of the edge
double time
time point of the conflict
double foeEstimatedConflictEntryTime
const MSJunction * getToJunction() const
void generateOutput() const
Finalizes output. Called on vehicle removal.
void resetExtraTime(double value)
resets remainingExtraTime to the given value
double egoConflictEntryDist
MSDevice_SSM(SUMOVehicle &holder, const std::string &id, std::string outputFilename, std::map< std::string, double > thresholds, bool trajectories, double range, double extraTime, bool useGeoCoords)
Constructor.
ENCOUNTER_TYPE_CROSSING_FOLLOWER.
double getWidth() const
Returns the lane's width.
#define UNUSED_PARAMETER(x)
double egoConflictExitTime
ENCOUNTER_TYPE_BOTH_ENTERED_CONFLICT_AREA.
bool isInternalJunctionLink() const
return whether the fromLane and the toLane of this link are internal lanes
Position getPositionAlongBestLanes(double offset) const
Return the (x,y)-position, which the vehicle would reach if it continued along its best continuation ...
#define WRITE_WARNING(msg)
MSLane * getCanonicalSuccessorLane() const
MSAbstractLaneChangeModel & getLaneChangeModel()
static OptionsCont & getOptions()
Retrieves the options.
std::vector< double > mySGAPspan
All values for space gap.
static bool requestsTrajectories(const SUMOVehicle &v)
Encounter(const MSVehicle *_ego, const MSVehicle *const _foe, double _begin, double extraTime)
Constructor.
double egoDistToConflictLane
ENCOUNTER_TYPE_CROSSING_LEADER.
double getMaxSpeedOnLane() const
Returns the maximal speed for the vehicle on its current lane (including speed factor and deviation...
static bool toBool(const std::string &sData)
converts a string into the bool value described by it by calling the char-type converter ...
EncounterType currentType
EncounterApproachInfo(Encounter *e)
std::vector< double > DRACspan
All values for DRAC.
ENCOUNTER_TYPE_FOLLOWING_LEADER.
void cartesian2geo(Position &cartesian) const
Converts the given cartesian (shifted) position to its geo (lat/long) representation.
double foeConflictExitTime
A road/street connecting two junctions.
bool isOnRoad() const
Returns the information whether the vehicle is on a road (is simulated)
bool isSet(const std::string &name, bool failOnNonExistant=true) const
Returns the information whether the named option is set.
ENCOUNTER_TYPE_MERGING_FOLLOWER.
const MSCFModel & getCarFollowModel() const
Returns the vehicle's car following model definition.
std::size_t size() const
Returns the number of trajectory points stored.
static bool getMeasuresAndThresholds(const SUMOVehicle &v, std::string deviceID, std::map< std::string, double > &thresholds)
ENCOUNTER_TYPE_BOTH_LEFT_CONFLICT_AREA.
double egoConflictExitDist
MSLink * getEntryLink() const
Returns the entry link if this is an internal lane, else 0.
bool knowsParameter(const std::string &key) const
Returns whether the parameter is known.
MSLane * getViaLane() const
Returns the following inner lane.
std::vector< double > myTGAPspan
All values for time gap.
#define DEFAULT_THRESHOLD_TGAP
Representation of a vehicle.
static std::string toString(EncounterType type)
static double computeDRAC(double gap, double followerSpeed, double leaderSpeed)
Computes the DRAC (deceleration to avoid a collision) for a lead/follow situation as defined...
static const std::set< MSDevice_SSM *, ComparatorNumericalIdLess > & getInstances()
returns all currently existing SSM devices
static double toDouble(const std::string &sData)
converts a string into the double value described by it by calling the char-type converter ...
void processEncounters(FoeInfoMap &foes, bool forceClose=false)
Finds encounters for which the foe vehicle has disappeared from range. remainingExtraTime is decrease...
bool writeXMLHeader(const std::string &rootElement, const std::string &schemaFile, std::map< SumoXMLAttr, std::string > attrs=std::map< SumoXMLAttr, std::string >())
Writes an XML header with optional configuration.
#define DEFAULT_THRESHOLD_DRAC
A point in 2D or 3D with translation and scaling methods.
bool myComputeTTC
Flags for switching on / off comutation of different SSMs, derived from myMeasures.
const MSEdge * getOppositeEdge() const
Returns the opposite direction edge if on exists else a nullptr.
OutputDevice * myOutputFile
Output device.
ENCOUNTER_TYPE_FOE_LEFT_CONFLICT_AREA.
void resetEncounters()
Closes all current Encounters and moves conflicts to myPastConflicts,.
const std::vector< MSLane * > & getBestLanesContinuation() const
Returns the best sequence of lanes to continue the route starting at myLane.
const MSEdge * getEdge() const
Returns the edge the vehicle is currently at.
MSVehicleControl & getVehicleControl()
Returns the vehicle control.
double getRemainingExtraTime() const
returns the remaining extra time
double myRange
Detection range. For vehicles closer than this distance from the ego vehicle, SSMs are traced...
std::vector< double > egoDistsToConflict
Evolution of the ego vehicle's distance to the conflict point.
static std::set< MSDevice_SSM *, ComparatorNumericalIdLess > * myInstances
All currently existing SSM devices.
const MSLane * egoConflictLane
static int issuedParameterWarnFlags
bitset storing info whether warning has already been issued about unset parameter (warn only once!) ...
std::string getString(const std::string &name) const
Returns the string-value of the named option (only for Option_String)
ENCOUNTER_TYPE_FOLLOWING_FOLLOWER.
ENCOUNTER_TYPE_NOCONFLICT_AHEAD.
#define DEFAULT_EXTRA_TIME
void computeSSMs(EncounterApproachInfo &e) const
Compute current values of the logged SSMs (myMeasures) for the given encounter 'e' and update 'e' acc...
double egoEstimatedConflictExitTime
const ConstMSEdgeVector & getIncoming() const
bool qualifiesAsConflict(Encounter *e)
Tests if the SSM values exceed the threshold for qualification as conflict.
const MSLane * getInternalLaneBefore() const
return myInternalLaneBefore (always 0 when compiled without internal lanes)
static void checkConflictEntryAndExit(EncounterApproachInfo &eInfo)
Checks whether ego or foe have entered or left the conflict area in the last step and eventually writ...
ENCOUNTER_TYPE_MERGING_LEADER.
double getLengthsBeforeCrossing(const MSLane *foeLane) const
Returns the sum of the lengths along internal lanes following this link to the crossing with the give...
std::ostream & operator<<(std::ostream &out, MSDevice_SSM::EncounterType type)
Nicer output for EncounterType enum.
std::pair< std::pair< std::pair< double, Position >, double >, std::string > myMinTGAP
ENCOUNTER_TYPE_FOE_ENTERED_CONFLICT_AREA.
void addOptionSubTopic(const std::string &topic)
Adds an option subtopic.
virtual bool isOnRoad() const =0
Returns the information whether the vehicle is on a road (is simulated)
void updateAndWriteOutput()
This is called once per time step in MSNet::writeOutput() and collects the surrounding vehicles...
static void insertDefaultAssignmentOptions(const std::string &deviceName, const std::string &optionsTopic, OptionsCont &oc, const bool isPerson=false)
Adds common command options that allow to assign devices to vehicles.
MSLane * getOpposite() const
return the opposite direction lane for lane changing or 0
void flushGlobalMeasures()
Write out all non-encounter specific measures as headways and braking rates.
const MSLane * findFoeConflictLane(const MSVehicle *foe, const MSLane *egoConflictLane, double &distToConflictLane) const
Computes the conflict lane for the foe.
EncounterType type
Type of the conflict.
double myOldestActiveEncounterBegin
begin time of the oldest active encounter
bool isInternal() const
return whether this edge is an internal edge
bool notifyLeave(SUMOTrafficObject &veh, double lastPos, MSMoveReminder::Notification reason, const MSLane *enteredLane=0)
Called whenever the holder leaves a lane.
virtual const std::vector< MSLane * > getInternalLanes() const
Returns all internal lanes on the junction.
ENCOUNTER_TYPE_FOLLOWING.
bool mySaveTrajectories
This determines whether the whole trajectories of the vehicles (position, speed, ssms) shall be saved...
const SUMOVTypeParameter & getParameter() const
static std::string getOutputFilename(const SUMOVehicle &v, std::string deviceID)
double getFloat(const std::string &name) const
Returns the double-value of the named option (only for Option_Float)
std::vector< MSVehicle * > VehCont
Container for vehicles.
double getLength() const
Returns the length of this link.
static double getExtraTime(const SUMOVehicle &v)
void flushConflicts(bool all=false)
Writes out all past conflicts that have begun earlier than the oldest active encounter.
static std::string makeStringWithNAs(std::vector< double > v, double NA, std::string sep=" ")
make a string of a double vector and treat a special value as invalid ("NA")
double getLateralPositionOnLane() const
Get the vehicle's lateral position on the lane.
std::vector< double > foeDistsToConflict
Evolution of the foe vehicle's distance to the conflict point.
bool closingRequested
this flag is set by updateEncounter() or directly in processEncounters(), where encounters are closed...
std::vector< std::string > getVector()
return vector of strings
#define DEFAULT_THRESHOLD_SGAP
ENCOUNTER_TYPE_FOLLOWING_PASSED.
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...
std::vector< Encounter * > EncounterVector
double getWidth() const
Get the width which vehicles of this class shall have when being drawn.
static bool useGeoCoords(const SUMOVehicle &v)
bool notifyMove(SUMOTrafficObject &veh, double oldPos, double newPos, double newSpeed)
Checks for waiting steps when the vehicle moves.
const std::vector< MSLink * > & getFoeLinks() const
void countDownExtraTime(double amount)
decreases myRemaingExtraTime by given amount in seconds
Representation of a vehicle or person.
double foeConflictEntryTime
Times when the foe vehicle entered/left the conflict area. Currently only applies for crossing situat...
const MSVehicleType & getVehicleType() const
Returns the vehicle's type definition.
static bool equippedByDefaultAssignmentOptions(const OptionsCont &oc, const std::string &deviceName, DEVICEHOLDER &v, bool outputOptionSet, const bool isPerson=false)
Determines whether a vehicle should get a certain device.
std::map< const MSVehicle *, FoeInfo * > FoeInfoMap
const MSLane * egoConflictLane
Trajectory egoTrajectory
Trajectory of the ego vehicle.
#define DEBUG_COND_ENCOUNTER(e)
const MSLane * getFirstInternalInConnection(double &offset) const
Returns 0 if the lane is not internal. Otherwise the first part of the connection (sequence of intern...
static OutputDevice & getDevice(const std::string &name)
Returns the described OutputDevice.
virtual const SUMOVehicleParameter & getParameter() const =0
Returns the vehicle's parameter (including departure definition)
~MSDevice_SSM()
Destructor.
const MSJunction * getFromJunction() const
bool notifyEnter(SUMOTrafficObject &veh, MSMoveReminder::Notification reason, const MSLane *enteredLane=0)
Called whenever the holder enteres a lane.
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...
void computeGlobalMeasures()
Stores measures, that are not associated to a specific encounter as headways and brake rates...
double getAcceleration() const
Returns the vehicle's acceleration in m/s (this is computed as the last step's mean acceleration in c...
Structure to collect some info on the encounter needed during ssm calculation by various functions...
std::vector< double > timeSpan
time points corresponding to the trajectories
A storage for options typed value containers)
const std::string getParameter(const std::string &key, const std::string &defaultValue="") const
Returns the value for a given key.
double getInternalFollowingLengthTo(const MSEdge *followerAfterInternal) const
returns the length of all internal edges on the junction until reaching the non-internal edge followe...
double egoDistToConflictLane
ENCOUNTER_TYPE_EGO_LEFT_CONFLICT_AREA.
Abstract in-vehicle device.
static const GeoConvHelper & getFinal()
the coordinate transformation for writing the location element and for tracking the original coordina...
std::pair< double, double > pet
double getLength() const
Get vehicle's length [m].
double egoEstimatedConflictEntryTime
std::map< std::string, double > myThresholds
double foeConflictExitDist
std::pair< std::pair< std::pair< double, Position >, double >, std::string > myMinSGAP
double getBackPositionOnLane(const MSLane *lane) const
Get the vehicle's position relative to the given lane.
double getLastStepDist() const
Get the distance the vehicle covered in the previous timestep.
Static storage of an output device and its base (abstract) implementation.
bool closeTag(const std::string &comment="")
Closes the most recently opened tag and optionally adds a comment.
An encounter is an episode involving two vehicles, which are closer to each other than some specified...
#define DEFAULT_THRESHOLD_TTC
Auxiliary structure used to handle upstream scanning start points Upstream scan has to be started aft...
double myExtraTime
Extra time in seconds to be logged after a conflict is over.
std::vector< int > typeSpan
Evolution of the encounter classification (.
double remainingExtraTime
Remaining extra time (decreases after an encounter ended)
ENCOUNTER_TYPE_ON_ADJACENT_LANES.
void addDescription(const std::string &name, const std::string &subtopic, const std::string &description)
Adds a description for an option.
const MSLinkCont & getLinkCont() const
returns the container with all links !!!
static void estimateConflictTimes(EncounterApproachInfo &eInfo)
Estimates the time until conflict for the vehicles based on the distance to the conflict entry points...
double getSpeed() const
Returns the vehicle's current speed.
double foeConflictEntryDist
void writeOutConflict(Encounter *e)
const MSRouteIterator & getCurrentRouteEdge() const
Returns an iterator pointing to the current edge in this vehicles route.
virtual const ConstMSEdgeVector::const_iterator & getCurrentRouteEdge() const =0
Returns an iterator pointing to the current edge in this vehicles route.
virtual void releaseVehicles() const
Allows to use the container for microsimulation again.
const std::string & getID() const
Returns the name of the vehicle.
static std::set< std::string > createdOutputFiles
remember which files were created already (don't duplicate xml root-elements)
Representation of a lane in the micro simulation.
MSRouteIterator end() const
Returns the end of the list of edges to pass.
std::string joinToString(const std::vector< T > &v, const T_BETWEEN &between, std::streamsize accuracy=gPrecision)
EncounterQueue myPastConflicts
Past encounters that where qualified as conflicts and are not yet flushed to the output file...
OutputDevice & openTag(const std::string &xmlElement)
Opens an XML tag.
double egoConflictAreaLength
double value
value of the corresponding SSM
static void getUpstreamVehicles(const UpstreamScanStartInfo &scanStart, FoeInfoMap &foeCollector, std::set< const MSJunction *> &seenJunctions)
Collects all vehicles within range 'range' upstream of the position 'pos' on the edge 'edge' into foe...
double getWidth() const
Returns the vehicle's width.
Trajectory foeTrajectory
Trajectory of the foe vehicle.
static void findSurroundingVehicles(const MSVehicle &veh, double range, FoeInfoMap &foeCollector)
Returns all vehicles, which are within the given range of the given vehicle.
static void buildVehicleDevices(SUMOVehicle &v, std::vector< MSVehicleDevice *> &into)
Build devices for the given vehicle, if needed.
std::priority_queue< Encounter *, std::vector< Encounter * >, Encounter::compare > EncounterQueue
void add(double time, EncounterType type, Position egoX, Position egoV, Position foeX, Position foeV, Position conflictPoint, double egoDistToConflict, double foeDistToConflict, double ttc, double drac, std::pair< double, double > pet)
add a new data point and update encounter type