58 #define DEBUG_COND_FIND(ego) (ego.getID() == DEBUG_EGO_ID)
59 #define DEBUG_EGO_ID "286"
60 #define DEBUG_FOE_ID "205"
62 #define DEBUG_COND(ego) ((ego)!=nullptr && (ego)->getID() == DEBUG_EGO_ID)
64 #define DEBUG_COND_ENCOUNTER(e) ((DEBUG_EGO_ID == std::string("") || e->egoID == DEBUG_EGO_ID) && (DEBUG_FOE_ID == std::string("") || e->foeID == DEBUG_FOE_ID))
71 #define DEFAULT_RANGE 50.0
77 #define AVAILABLE_SSMS "TTC DRAC PET BR SGAP TGAP"
79 #define DEFAULT_THRESHOLD_TTC 3.
80 #define DEFAULT_THRESHOLD_DRAC 3.
81 #define DEFAULT_THRESHOLD_PET 2.
83 #define DEFAULT_THRESHOLD_BR 0.0
84 #define DEFAULT_THRESHOLD_SGAP 0.2
85 #define DEFAULT_THRESHOLD_TGAP 0.5
87 #define DEFAULT_EXTRA_TIME 5.
99 out <<
"NOCONFLICT_AHEAD";
105 out <<
"FOLLOWING_FOLLOWER";
108 out <<
"FOLLOWING_LEADER";
111 out <<
"ON_ADJACENT_LANES";
117 out <<
"MERGING_LEADER";
120 out <<
"MERGING_FOLLOWER";
123 out <<
"MERGING_ADJACENT";
129 out <<
"CROSSING_LEADER";
132 out <<
"CROSSING_FOLLOWER";
135 out <<
"EGO_ENTERED_CONFLICT_AREA";
138 out <<
"FOE_ENTERED_CONFLICT_AREA";
141 out <<
"BOTH_ENTERED_CONFLICT_AREA";
144 out <<
"EGO_LEFT_CONFLICT_AREA";
147 out <<
"FOE_LEFT_CONFLICT_AREA";
150 out <<
"BOTH_LEFT_CONFLICT_AREA";
153 out <<
"FOLLOWING_PASSED";
156 out <<
"MERGING_PASSED";
166 out <<
"unknown type (" << int(type) <<
")";
177 std::set<MSDevice_SSM*, ComparatorNumericalIdLess>*
MSDevice_SSM::myInstances =
new std::set<MSDevice_SSM*, ComparatorNumericalIdLess>();
183 const std::set<MSDevice_SSM*, ComparatorNumericalIdLess>&
193 device->resetEncounters();
194 device->flushConflicts(
true);
195 device->flushGlobalMeasures();
211 oc.
doRegister(
"device.ssm.measures", Option::makeUnsetWithDefault<Option_String, std::string>(
""));
212 oc.
addDescription(
"device.ssm.measures",
"SSM Device",
"Specifies which measures will be logged (as a space separated sequence of IDs in ('TTC', 'DRAC', 'PET')).");
213 oc.
doRegister(
"device.ssm.thresholds", Option::makeUnsetWithDefault<Option_String, std::string>(
""));
214 oc.
addDescription(
"device.ssm.thresholds",
"SSM Device",
"Specifies thresholds corresponding to the specified measures (see documentation and watch the order!). Only events exceeding the thresholds will be logged.");
215 oc.
doRegister(
"device.ssm.trajectories", Option::makeUnsetWithDefault<Option_Bool, bool>(
false));
216 oc.
addDescription(
"device.ssm.trajectories",
"SSM Device",
"Specifies whether trajectories will be logged (if false, only the extremal values and times are reported, this is the default).");
218 oc.
addDescription(
"device.ssm.range",
"SSM Device",
"Specifies the detection range in meters (default is " + ::
toString(
DEFAULT_RANGE) +
"m.). For vehicles below this distance from the equipped vehicle, SSM values are traced.");
220 oc.
addDescription(
"device.ssm.extratime",
"SSM Device",
"Specifies the time in seconds to be logged after a conflict is over (default is " + ::
toString(
DEFAULT_EXTRA_TIME) +
"secs.). Required >0 if PET is to be calculated for crossing conflicts.");
221 oc.
doRegister(
"device.ssm.file", Option::makeUnsetWithDefault<Option_String, std::string>(
""));
222 oc.
addDescription(
"device.ssm.file",
"SSM Device",
"Give a global default filename for the SSM output.");
223 oc.
doRegister(
"device.ssm.geo", Option::makeUnsetWithDefault<Option_Bool, bool>(
false));
224 oc.
addDescription(
"device.ssm.geo",
"SSM Device",
"Whether to use coordinates of the original reference system in output (default is false).");
231 WRITE_WARNING(
"SSM Device for vehicle '" + v.
getID() +
"' will not be built. (SSMs not supported in MESO)");
235 std::string deviceID =
"ssm_" + v.
getID();
240 std::map<std::string, double> thresholds;
263 into.push_back(device);
271 egoID(_ego->getID()),
272 foeID(_foe->getID()),
275 currentType(ENCOUNTER_TYPE_NOCONFLICT_AHEAD),
276 remainingExtraTime(extraTime),
284 closingRequested(false) {
285 #ifdef DEBUG_ENCOUNTER
287 std::cout <<
"\n" <<
SIMTIME <<
" Constructing encounter of '" <<
ego->
getID() <<
"' and '" <<
foe->
getID() <<
"'" << std::endl;
293 #ifdef DEBUG_ENCOUNTER
295 std::cout <<
"\n" <<
SIMTIME <<
" Destroying encounter of '" << egoID <<
"' and '" << foeID <<
"' (begin was " << begin <<
")" << std::endl;
303 Position conflictPoint,
double egoDistToConflict,
double foeDistToConflict,
double ttc,
double drac, std::pair<double, double> pet) {
304 #ifdef DEBUG_ENCOUNTER
306 std::cout << time <<
" Adding data point for encounter of '" << egoID <<
"' and '" << foeID <<
"':\n"
307 <<
"type=" << type <<
", egoDistToConflict=" << (egoDistToConflict ==
INVALID_DOUBLE ?
"NA" :
::toString(egoDistToConflict))
316 timeSpan.push_back(time);
317 typeSpan.push_back(type);
318 egoTrajectory.x.push_back(egoX);
319 egoTrajectory.v.push_back(egoV);
320 foeTrajectory.x.push_back(foeX);
321 foeTrajectory.v.push_back(foeV);
322 conflictPointSpan.push_back(conflictPoint);
323 egoDistsToConflict.push_back(egoDistToConflict);
324 foeDistsToConflict.push_back(foeDistToConflict);
326 TTCspan.push_back(ttc);
330 minTTC.pos = conflictPoint;
334 DRACspan.push_back(drac);
336 maxDRAC.value = drac;
338 maxDRAC.pos = conflictPoint;
343 PET.value = pet.second;
344 PET.time = pet.first;
345 PET.pos = conflictPoint;
353 remainingExtraTime = value;
359 remainingExtraTime -= amount;
365 return remainingExtraTime;
383 egoLeftConflict(false),
384 foeLeftConflict(false),
400 std::cout <<
"\n" <<
SIMTIME <<
" Device '" <<
getID() <<
"' updateAndWriteOutput()\n"
401 <<
" Holder is off-road! Calling resetEncounters()."
414 std::cout <<
"\n" <<
SIMTIME <<
" Device '" <<
getID() <<
"' update()\n"
425 if (foes.size() > 0) {
426 std::cout <<
"Scanned surroundings: Found potential foes:\n";
427 for (FoeInfoMap::const_iterator i = foes.begin(); i != foes.end(); ++i) {
428 std::cout << i->first->getID() <<
" ";
430 std::cout << std::endl;
432 std::cout <<
"Scanned surroundings: No potential conflict could be identified." << std::endl;
462 double leaderSearchDist = 0;
463 std::pair<const MSVehicle*, double> leader(
nullptr, 0.);
471 if (leaderSearchDist > 0.) {
477 if (leader.first ==
nullptr || leader.second < 0) {
508 std::cout <<
"\n" <<
SIMTIME <<
" Device '" <<
getID() <<
"' createEncounters()" << std::endl;
509 std::cout <<
"New foes:\n";
510 for (FoeInfoMap::const_iterator vi = foes.begin(); vi != foes.end(); ++vi) {
511 std::cout << vi->first->getID() <<
"\n";
513 std::cout << std::endl;
517 for (FoeInfoMap::const_iterator foe = foes.begin(); foe != foes.end(); ++foe) {
524 assert(myOldestActiveEncounterBegin <= e->begin);
547 std::cout <<
"\n" <<
SIMTIME <<
" Device '" <<
getID() <<
"' processEncounters(forceClose = " << forceClose <<
")" << std::endl;
548 std::cout <<
"Currently present foes:\n";
549 for (FoeInfoMap::const_iterator vi = foes.begin(); vi != foes.end(); ++vi) {
550 std::cout << vi->first->getID() <<
"\n";
552 std::cout << std::endl;
568 if (foes.find(e->
foe) != foes.end()) {
580 std::cout <<
" Requesting encounter closure because both left conflict area of previous encounter but another encounter lies ahead." << std::endl;
594 std::cout <<
" Requesting encounter closure because..." << std::endl;
596 std::cout <<
" ... extra time elapsed." << std::endl;
597 }
else if (forceClose) {
598 std::cout <<
" ... closing was forced." << std::endl;
600 std::cout <<
" ... foe disappeared." << std::endl;
611 double eBegin = e->
begin;
636 std::cout <<
SIMTIME <<
" qualifiesAsConflict() for encounter of vehicles '"
656 assert(e->
size() > 0);
664 std::cout <<
SIMTIME <<
" closeEncounter() of vehicles '"
666 <<
"' (was ranked as " << (wasConflict ?
"conflict" :
"non-conflict") <<
")" << std::endl;
674 std::cout <<
"pastConflictsQueue of veh '" <<
myHolderMS->
getID() <<
"':\n";
682 std::cout <<
" Conflict with foe '" << c->foe <<
"' (time " << c->begin <<
"-" << c->end <<
")\n";
684 if (c->begin < lastBegin) {
685 std::cout <<
" Queue corrupt...\n";
688 lastBegin = c->begin;
690 std::cout << std::endl;
703 #ifdef DEBUG_ENCOUNTER
705 std::cout <<
SIMTIME <<
" updateEncounter() of vehicles '" << e->
egoID <<
"' and '" << e->
foeID <<
"'\n";
732 #ifdef DEBUG_ENCOUNTER
734 std::cout <<
SIMTIME <<
" Encounter of vehicles '" << e->
egoID <<
"' and '" << e->
foeID <<
"' does not imply any conflict.\n";
765 if (e->
size() == 0) {
766 #ifdef DEBUG_ENCOUNTER
768 std::cout <<
SIMTIME <<
" type when creating encounter: " << eInfo.
type <<
"\n";
810 std::cout <<
SIMTIME <<
" determineConflictPoint()" << std::endl;
820 assert(e->
size() > 0);
841 std::cout <<
"No conflict point associated with encounter type " << type << std::endl;
849 std::cout <<
" Conflict at " << eInfo.
conflictPoint << std::endl;
864 std::cout <<
SIMTIME <<
" estimateConflictTimes() for ego '" << e->
egoID <<
"' and foe '" << e->
foeID <<
"'\n"
865 <<
" encounter type: " << eInfo.
type <<
"\n"
877 std::cout <<
" encouter type " << type <<
" -> no exit times to be calculated."
889 std::cout <<
" encouter type " << type <<
" -> no entry/exit times to be calculated."
971 std::stringstream ss;
972 ss <<
"SSM device of vehicle '" << e->
egoID <<
"' detected collision with vehicle '" << e->
foeID <<
"' at time " <<
SIMTIME;
978 std::cout <<
" -> ego is estimated leader at conflict entry."
987 std::cout <<
" -> foe is estimated leader at conflict entry."
1003 std::cout <<
SIMTIME <<
" computeSSMs() for vehicles '"
1005 <<
"'" << std::endl;
1035 std::stringstream ss;
1036 ss <<
"'" << type <<
"'";
1037 WRITE_WARNING(
"Unknown or undetermined encounter type at computeSSMs(): " + ss.str());
1043 std::cout <<
"computeSSMs() for encounter of vehicles '" << e->
egoID <<
"' and '" << e->
foeID <<
"':\n"
1056 if (e->
size() == 0) {
1060 std::pair<double, double>& pet = eInfo.
pet;
1064 std::cout <<
SIMTIME <<
" determinePET() for encounter of vehicles '" << e->
egoID <<
"' and '" << e->
foeID <<
"'"
1079 std::cout <<
"PET for crossing encounter already calculated as " << e->
PET.
value
1122 std::cout <<
"determinePET: Both passed conflict area in the same step. Assume collision"
1137 std::cout <<
"Calculated PET = " << pet.second <<
" (at t=" << pet.first <<
")"
1144 std::cout <<
"PET unappropriate for merging and pre-crossing situations. No calculation performed."
1156 double& ttc = eInfo.
ttc;
1157 double& drac = eInfo.
drac;
1161 std::cout <<
SIMTIME <<
" determineTTCandDRAC() for encounter of vehicles '" << e->
egoID <<
"' and '" << e->
foeID <<
"' (type = " << eInfo.
type <<
")"
1204 std::cout <<
" Conflict times with constant speed extrapolation for merging situation:\n "
1219 std::cout <<
" No TTC and DRAC computed as one vehicle is stopped." << std::endl;
1224 double leaderEntryTime =
MIN2(egoEntryTime, foeEntryTime);
1225 double followerEntryTime =
MAX2(egoEntryTime, foeEntryTime);
1226 double leaderExitTime = leaderEntryTime == egoEntryTime ? egoExitTime : foeExitTime;
1233 if (leaderExitTime >= followerEntryTime) {
1236 ttc =
computeTTC(followerConflictDist, followerSpeed, 0.);
1241 drac =
computeDRAC(followerConflictDist, followerSpeed, 0.);
1247 std::cout <<
" Extrapolation predicts collision *at* merge point with TTC=" << ttc
1248 <<
", drac=" << drac << std::endl;
1255 double gapAfterMerge = followerConflictDist - leaderExitTime * followerSpeed;
1256 assert(gapAfterMerge >= 0);
1259 double ttcAfterMerge =
computeTTC(gapAfterMerge, followerSpeed, leaderSpeed);
1264 double g0 = followerConflictDist - leaderConflictDist - leaderLength;
1267 assert(leaderSpeed - followerSpeed > 0);
1272 drac =
computeDRAC(g0, followerSpeed, leaderSpeed);
1280 std::cout <<
" Extrapolation does not predict any collision." << std::endl;
1282 std::cout <<
" Extrapolation predicts collision *after* merge point with TTC="
1324 std::stringstream ss;
1325 ss <<
"'" << type <<
"'";
1326 WRITE_WARNING(
"Underspecified or unknown encounter type in MSDevice_SSM::determineTTCandDRAC(): " + ss.str());
1346 std::cout <<
"computeTTC() with gap=" << gap <<
", followerSpeed=" << followerSpeed <<
", leaderSpeed=" << leaderSpeed
1352 double dv = followerSpeed - leaderSpeed;
1371 double dv = followerSpeed - leaderSpeed;
1375 assert(followerSpeed > 0.);
1376 return 0.5 * dv * dv / gap;
1392 #ifdef DEBUG_SSM_DRAC
1394 std::cout <<
SIMTIME <<
"computeDRAC() with"
1395 <<
"\ndEntry1=" << dEntry1 <<
", dEntry2=" << dEntry2
1396 <<
", dExit1=" << dExit1 <<
", dExit2=" << dExit2
1397 <<
",\nv1=" << v1 <<
", v2=" << v2
1402 if (dExit1 <= 0. || dExit2 <= 0.) {
1404 #ifdef DEBUG_SSM_DRAC
1406 std::cout <<
"One already left conflict area -> drac == 0." << std::endl;
1411 if (dEntry1 <= 0. && dEntry2 <= 0.) {
1413 #ifdef DEBUG_SSM_DRAC
1415 std::cout <<
"Both entered conflict area but neither left. -> collision!" << std::endl;
1421 double drac = std::numeric_limits<double>::max();
1424 #ifdef DEBUG_SSM_DRAC
1426 std::cout <<
"Ego could break..." << std::endl;
1431 drac =
MIN2(drac, 2 * (v1 - dEntry1 / tExit2) / tExit2);
1432 #ifdef DEBUG_SSM_DRAC
1434 std::cout <<
" Foe expected to leave in " << tExit2 <<
"-> Ego needs drac=" << drac << std::endl;
1442 #ifdef DEBUG_SSM_DRAC
1444 std::cout <<
" Foe is expected stop on conflict area -> Ego needs drac=" << drac << std::endl;
1449 #ifdef DEBUG_SSM_DRAC
1451 std::cout <<
" Foe is expected stop before conflict area -> no drac computation for ego (will be done for foe if applicable)" << std::endl;
1460 #ifdef DEBUG_SSM_DRAC
1462 std::cout <<
"Foe could break..." << std::endl;
1467 #ifdef DEBUG_SSM_DRAC
1469 std::cout <<
" Ego expected to leave in " << tExit1 <<
"-> Foe needs drac=" << (2 * (v2 - dEntry2 / tExit1) / tExit1) << std::endl;
1472 drac =
MIN2(drac, 2 * (v2 - dEntry2 / tExit1) / tExit1);
1477 #ifdef DEBUG_SSM_DRAC
1479 std::cout <<
" Ego is expected stop on conflict area -> Foe needs drac=" <<
computeDRAC(dEntry2, v2, 0) << std::endl;
1485 #ifdef DEBUG_SSM_DRAC
1487 std::cout <<
" Ego is expected stop before conflict area -> no drac computation for foe (done for ego if applicable)" << std::endl;
1508 #ifdef DEBUG_ENCOUNTER
1510 std::cout <<
SIMTIME <<
" checkConflictEntryAndExit() for encounter of vehicles '" << e->
egoID <<
"' and '" << e->
foeID <<
"'"
1520 if (e->
size() == 0) {
1524 if (egoPastConflictExit) {
1525 if (foePastConflictExit) {
1527 }
else if (foePastConflictEntry) {
1532 }
else if (foePastConflictExit) {
1533 if (egoPastConflictEntry) {
1540 if (egoPastConflictEntry) {
1541 if (foePastConflictEntry) {
1546 }
else if (foePastConflictEntry) {
1574 #ifdef DEBUG_ENCOUNTER
1590 #ifdef DEBUG_ENCOUNTER
1608 #ifdef DEBUG_ENCOUNTER
1626 #ifdef DEBUG_ENCOUNTER
1643 #ifdef DEBUG_ENCOUNTER
1645 std::cout <<
SIMTIME <<
" updatePassedEncounter() for vehicles '" << e->
egoID <<
"' and '" << e->
foeID <<
"'\n";
1649 if (foeInfo ==
nullptr) {
1652 #ifdef DEBUG_ENCOUNTER
1668 #ifdef DEBUG_ENCOUNTER
1670 std::cout <<
" This encounter wasn't classified as a potential conflict lately.\n";
1673 if (foeInfo ==
nullptr) {
1679 std::cout <<
" Requesting encounter closure because foeInfo==nullptr" << std::endl;
1683 #ifdef DEBUG_ENCOUNTER
1685 std::cout <<
" Closing encounter.\n";
1695 #ifdef DEBUG_ENCOUNTER
1697 std::cout <<
" Encounter was previously classified as a follow/lead situation.\n";
1706 #ifdef DEBUG_ENCOUNTER
1708 std::cout <<
" Encounter was previously classified as a merging situation.\n";
1723 #ifdef DEBUG_ENCOUNTER
1725 std::cout <<
" Encounter was previously classified as a crossing situation of type " << lastPotentialConflictType <<
".\n";
1743 #ifdef DEBUG_ENCOUNTER
1758 if ((!egoEnteredConflict) && !foeEnteredConflict) {
1762 eInfo.
type = lastPotentialConflictType;
1763 }
else if (egoEnteredConflict && !foeEnteredConflict) {
1765 }
else if ((!egoEnteredConflict) && foeEnteredConflict) {
1771 if ((!egoLeftConflict) && !foeLeftConflict) {
1775 }
else if (egoLeftConflict && !foeLeftConflict) {
1779 }
else if ((!egoLeftConflict) && foeLeftConflict) {
1794 #ifdef DEBUG_ENCOUNTER
1796 std::cout <<
" Updated classification: " << eInfo.
type <<
"\n";
1805 #ifdef DEBUG_ENCOUNTER
1807 std::cout <<
"classifyEncounter() called.\n";
1810 if (foeInfo ==
nullptr) {
1829 #ifdef DEBUG_ENCOUNTER
1831 std::cout <<
" Ongoing crossing conflict will be traced by passedEncounter().\n";
1847 double foeDistToConflictLane;
1850 #ifdef DEBUG_ENCOUNTER
1852 std::cout <<
" egoConflictLane='" << (egoConflictLane == 0 ?
"NULL" : egoConflictLane->
getID()) <<
"'\n"
1853 <<
" foeConflictLane='" << (foeConflictLane == 0 ?
"NULL" : foeConflictLane->
getID()) <<
"'\n"
1854 <<
" egoDistToConflictLane=" << egoDistToConflictLane
1855 <<
" foeDistToConflictLane=" << foeDistToConflictLane
1870 if (foeConflictLane ==
nullptr) {
1873 #ifdef DEBUG_ENCOUNTER
1875 std::cout <<
"-> Encounter type: No conflict.\n";
1880 if (egoConflictLane == egoLane) {
1884 if (foeLane == egoLane) {
1886 if (!egoOpposite && !foeOpposite) {
1894 #ifdef DEBUG_ENCOUNTER
1896 std::cout <<
"-> Encounter type: Lead/follow-situation on non-internal lane '" << egoLane->
getID() <<
"'\n";
1899 }
else if (egoOpposite && foeOpposite) {
1907 #ifdef DEBUG_ENCOUNTER
1909 std::cout <<
"-> Encounter type: Lead/follow-situation while both are driving in the opposite direction on non-internal lane '" << egoLane->
getID() <<
"'\n";
1930 #ifdef DEBUG_ENCOUNTER
1932 std::cout <<
"-> Encounter type: oncoming on non-internal lane '" << egoLane->
getID() <<
"'\n";
1941 #ifdef DEBUG_ENCOUNTER
1943 std::cout <<
"-> Encounter type: " << type << std::endl;
1948 if (!egoOpposite && !foeOpposite) {
1951 assert(egoDistToConflictLane <= 0);
1953 if (foeConflictLane == egoLane) {
1957 #ifdef DEBUG_ENCOUNTER
1959 std::cout <<
"-> Encounter type: Ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' leads foe '"
1960 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'"
1966 #ifdef DEBUG_ENCOUNTER
1968 std::cout <<
"-> Encounter type: " << type << std::endl;
1973 }
else if (egoOpposite && foeOpposite) {
1985 #ifdef DEBUG_ENCOUNTER
1987 std::cout <<
"-> Encounter type: Lead/follow-situation while both are driving in the opposite direction on non-internal lane '" << egoLane->
getID() <<
"'\n";
2011 #ifdef DEBUG_ENCOUNTER
2013 std::cout <<
"-> Encounter type: oncoming on non-internal lane '" << egoLane->
getID() <<
"'\n";
2025 assert(foeDistToConflictLane <= 0);
2026 if (foeLane == egoConflictLane) {
2029 #ifdef DEBUG_ENCOUNTER
2031 std::cout <<
"-> Encounter type: Ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' follows foe '"
2032 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'"
2038 #ifdef DEBUG_ENCOUNTER
2040 std::cout <<
"-> Encounter type: " << type << std::endl;
2052 if (egoEntryLink != foeEntryLink) {
2055 #ifdef DEBUG_ENCOUNTER
2057 std::cout <<
"-> Encounter type: " << type << std::endl;
2062 if (egoLane == egoConflictLane && foeLane != foeConflictLane) {
2069 #ifdef DEBUG_ENCOUNTER
2071 std::cout <<
"-> Encounter type: Ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' leads foe '"
2072 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'"
2075 }
else if (egoLane != egoConflictLane && foeLane == foeConflictLane) {
2082 #ifdef DEBUG_ENCOUNTER
2084 std::cout <<
"-> Encounter type: Ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' follows foe '"
2085 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'"
2092 #ifdef DEBUG_ENCOUNTER
2094 std::cout <<
"-> Encounter type: Ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' merges with foe '"
2095 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'"
2103 if (egoLane != egoConflictLane || foeLane != foeConflictLane) {
2107 if (egoLane == foeLane) {
2112 #ifdef DEBUG_ENCOUNTER
2114 std::cout <<
"-> Encounter type: Ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' leads foe '"
2115 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'"
2122 #ifdef DEBUG_ENCOUNTER
2124 std::cout <<
"-> Encounter type: Ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' follows foe '"
2125 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'"
2132 #ifdef DEBUG_ENCOUNTER
2134 std::cout <<
" Lead/follow situation on consecutive internal lanes." << std::endl;
2139 #pragma warning(push)
2140 #pragma warning(disable: 4127)
2144 #pragma warning(pop)
2148 if (egoLane == lane) {
2153 while (lane != foeLane) {
2159 egoConflictLane = lane;
2160 #ifdef DEBUG_ENCOUNTER
2162 std::cout <<
"-> Encounter type: Ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' follows foe '"
2163 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'"
2168 }
else if (foeLane == lane) {
2173 while (lane != egoLane) {
2179 foeConflictLane = lane;
2180 #ifdef DEBUG_ENCOUNTER
2182 std::cout <<
"-> Encounter type: Ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' leads foe '"
2183 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'"
2193 #ifdef DEBUG_ENCOUNTER
2195 std::cout <<
"-> Encounter type: Lead/follow-situation on connection from '" << egoEntryLink->
getLaneBefore()->
getID()
2196 <<
"' to '" << egoEntryLink->
getLane()->
getID() <<
"'" << std::endl;
2203 const std::vector<MSLink*>& egoFoeLinks = egoEntryLink->
getFoeLinks();
2204 const std::vector<MSLink*>& foeFoeLinks = foeEntryLink->
getFoeLinks();
2206 bool crossOrMerge = (find(egoFoeLinks.begin(), egoFoeLinks.end(), foeEntryLink) != egoFoeLinks.end()
2207 || std::find(foeFoeLinks.begin(), foeFoeLinks.end(), egoEntryLink) != foeFoeLinks.end());
2208 if (!crossOrMerge) {
2217 #ifdef DEBUG_ENCOUNTER
2219 std::cout <<
"-> Encounter type: No conflict.\n";
2244 #ifdef DEBUG_ENCOUNTER
2246 std::cout <<
"-> Encounter type: Merging situation of ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' and foe '"
2247 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'"
2254 #ifdef DEBUG_ENCOUNTER
2256 std::cout <<
"-> Encounter type: No conflict: " << type << std::endl;
2272 egoDistToConflictLane -= offset;
2274 foeDistToConflictLane -= offset;
2279 double foeInternalLaneLengthsBeforeCrossing = 0.;
2280 while (foeConflictLane !=
nullptr && foeConflictLane->
isInternal()) {
2287 foeInternalLaneLengthsBeforeCrossing += foeConflictLane->
getLength();
2291 egoDistToConflictFromJunctionEntry = 0;
2292 WRITE_WARNINGF(
"Cannot compute SSM due to bad internal lane geometry at junction '%'. Crossing point between traffic from links % and % not found.",
2299 assert(foeConflictLane != 0 && foeConflictLane->
isInternal());
2305 double egoInternalLaneLengthsBeforeCrossing = 0.;
2307 while (egoConflictLane !=
nullptr && egoConflictLane->
isInternal()) {
2314 egoInternalLaneLengthsBeforeCrossing += egoConflictLane->
getLength();
2318 foeDistToConflictFromJunctionEntry = 0;
2319 WRITE_WARNINGF(
"Cannot compute SSM due to bad internal lane geometry at junction '%'. Crossing point between traffic from links % and % not found.",
2326 assert(egoConflictLane != 0 && egoConflictLane->
isInternal());
2351 assert(angle <= 2 *
M_PI);
2355 assert(angle >= -
M_PI);
2356 assert(angle <=
M_PI);
2358 double crossingOrientation = (angle < 0) - (angle > 0);
2378 #ifdef DEBUG_ENCOUNTER
2380 std::cout <<
" Determined exact conflict distances for crossing conflict."
2381 <<
"\n crossingOrientation=" << crossingOrientation
2384 <<
", relativeAngle=" << angle
2385 <<
" (foe from " << (crossingOrientation > 0 ?
"right)" :
"left)")
2386 <<
"\n resulting offset for conflict entry distance:"
2389 <<
"\n distToConflictLane:"
2390 <<
"\n ego=" << egoDistToConflictLane
2391 <<
", foe=" << foeDistToConflictLane
2392 <<
"\n distToConflictFromJunctionEntry:"
2393 <<
"\n ego=" << egoDistToConflictFromJunctionEntry
2394 <<
", foe=" << foeDistToConflictFromJunctionEntry
2395 <<
"\n resulting entry distances:"
2398 <<
"\n resulting exit distances:"
2403 std::cout <<
"real egoConflictLane: '" << (egoConflictLane == 0 ?
"NULL" : egoConflictLane->
getID()) <<
"'\n"
2404 <<
"real foeConflictLane: '" << (foeConflictLane == 0 ?
"NULL" : foeConflictLane->
getID()) <<
"'\n"
2405 <<
"-> Encounter type: Crossing situation of ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' and foe '"
2406 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'"
2424 std::cout <<
SIMTIME <<
" findFoeConflictLane() for foe '"
2426 <<
"' (with egoConflictLane=" << (egoConflictLane == 0 ?
"NULL" : egoConflictLane->
getID())
2436 #ifdef DEBUG_SSM_OPPOSITE
2452 return egoConflictLane;
2461 return egoConflictLane;
2471 assert(foeLane->
isInternal() || *laneIter == foeLane);
2478 if (conflictJunction != 0) {
2479 std::cout <<
"Potential conflict on junction '" << conflictJunction->
getID()
2485 if (egoConflictLane !=
nullptr && egoConflictLane->
isInternal() && egoConflictLane->
getLinkCont()[0]->getViaLane() == foeLane) {
2486 distToConflictLane += egoConflictLane->
getLength();
2494 if (*laneIter ==
nullptr) {
2495 while (foeLane !=
nullptr && foeLane->
isInternal()) {
2496 distToConflictLane += foeLane->
getLength();
2497 foeLane = foeLane->
getLinkCont()[0]->getViaLane();
2500 assert(laneIter == foeBestLanesEnd || *laneIter != 0);
2504 while (laneIter != foeBestLanesEnd && distToConflictLane <=
myRange) {
2506 assert(*laneIter == foeLane || foeLane == 0);
2507 foeLane = *laneIter;
2512 std::cout <<
"Found conflict lane for foe: '" << foeLane->
getID() <<
"'" << std::endl;
2519 distToConflictLane += foeLane->
getLength();
2523 if (laneIter == foeBestLanesEnd) {
2526 MSLane*
const nextNonInternalLane = *laneIter;
2530 assert(foeLane == 0 || foeLane->
isInternal());
2531 if (foeLane ==
nullptr) {
2532 foeLane = nextNonInternalLane;
2536 assert(foeLane != 0);
2539 std::cout <<
"Found conflict lane for foe: '" << foeLane->
getID() <<
"'" << std::endl;
2547 foeLane = nextNonInternalLane;
2580 std::cout <<
SIMTIME <<
" flushGlobalMeasures() of vehicle '"
2647 std::cout <<
SIMTIME <<
" writeOutConflict() of vehicles '"
2728 std::string res =
"";
2729 for (std::vector<double>::const_iterator i = v.begin(); i != v.end(); ++i) {
2730 res += (i == v.begin() ?
"" :
" ") + (*i == NA ?
"NA" : ::
toString(*i));
2737 std::string res =
"";
2738 for (std::vector<double>::const_iterator i = v.begin(); i != v.end(); ++i) {
2739 res += (i == v.begin() ?
"" :
" ") + (find(NAs.begin(), NAs.end(), *i) != NAs.end() ?
"NA" :
::toString(*i));
2746 std::string res =
"";
2747 for (PositionVector::const_iterator i = v.begin(); i != v.end(); ++i) {
2758 bool trajectories,
double range,
double extraTime,
bool useGeoCoords) :
2767 myMinSGAP(std::make_pair(std::make_pair(-1,
Position(0., 0.)), std::numeric_limits<double>::max()),
""),
2768 myMinTGAP(std::make_pair(std::make_pair(-1,
Position(0., 0.)), std::numeric_limits<double>::max()),
"") {
2796 std::vector<std::string> measures;
2797 std::vector<double> threshVals;
2799 measures.push_back(i->first);
2800 threshVals.push_back(i->second);
2802 std::cout <<
"Initialized ssm device '" <<
id <<
"' with "
2825 #ifdef DEBUG_SSM_NOTIFICATIONS
2841 #ifdef DEBUG_SSM_NOTIFICATIONS
2855 double ,
double newSpeed) {
2856 #ifdef DEBUG_SSM_NOTIFICATIONS
2859 std::cout <<
SIMTIME <<
"device '" <<
getID() <<
"' notifyMove: newSpeed=" << newSpeed <<
"\n";
2874 #ifdef DEBUG_SSM_SURROUNDING
2878 std::cout <<
SIMTIME <<
" Looking for surrounding vehicles for ego vehicle '" << veh.
getID()
2901 for (
int i = 0; i < (int)egoBestLanes.size(); i++) {
2902 if (egoBestLanes[i] !=
nullptr && egoBestLanes[i]->getEdge().getOppositeEdge() !=
nullptr) {
2903 egoBestLanes[i] = egoBestLanes[i]->getEdge().getOppositeEdge()->getLanes().back();
2907 std::vector<MSLane*>::const_iterator laneIter = egoBestLanes.begin();
2912 assert(lane->
isInternal() || lane == *laneIter);
2915 const MSLane* nextNonInternalLane =
nullptr;
2923 double remainingDownstreamRange = range;
2925 double distToConflictLane = isOpposite ? pos - veh.
getLane()->
getLength() : -pos;
2928 std::set<const MSLane*> seenLanes;
2929 std::set<const MSJunction*> routeJunctions;
2933 std::vector<UpstreamScanStartInfo> upstreamScanStartPositions;
2942 #ifdef DEBUG_SSM_SURROUNDING
2944 std::cout <<
SIMTIME <<
" Vehicle '" << veh.
getID() <<
"' is on internal edge " << edge->
getID() <<
"'." << std::endl;
2954 routeJunctions.insert(junction);
2960 for (ConstMSEdgeVector::const_iterator ei = incoming.begin(); ei != incoming.end(); ++ei) {
2961 if ((*ei)->isInternal()) {
2980 lane = *(++laneIter);
2986 double startScanPos = std::min(pos + remainingDownstreamRange, edgeLength);
2987 upstreamScanStartPositions.push_back(
UpstreamScanStartInfo(edge, startScanPos, std::max(0., startScanPos - pos + range + veh.
getLength()), distToConflictLane, lane));
2996 while (remainingDownstreamRange > 0.) {
2998 #ifdef DEBUG_SSM_SURROUNDING
3000 std::cout <<
SIMTIME <<
" Scanning downstream for vehicle '" << veh.
getID() <<
"' on lane '" << veh.
getLane()->
getID() <<
"', position=" << pos <<
".\n"
3001 <<
"Considering edge '" << edge->
getID() <<
"' Remaining downstream range = " << remainingDownstreamRange
3002 <<
"\nbestLanes=" <<
::toString(egoBestLanes) <<
"\n"
3008 assert(pos == 0 || lane == veh.
getLane());
3009 if (pos + remainingDownstreamRange < lane->getLength()) {
3012 upstreamScanStartPositions.push_back(
UpstreamScanStartInfo(edge, pos + remainingDownstreamRange, remainingDownstreamRange, distToConflictLane, lane));
3023 remainingDownstreamRange -= lane->
getLength() - pos;
3024 distToConflictLane += lane->
getLength();
3029 assert(laneIter == egoBestLanes.end() || *laneIter != 0);
3032 if (laneIter != egoBestLanes.end()) {
3043 nextNonInternalLane = *laneIter;
3045 if (isOpposite && link ==
nullptr) {
3046 link = nextNonInternalLane->
getLinkTo(lane);
3047 if (link ==
nullptr) {
3051 if (link ==
nullptr) {
3058 if (lane ==
nullptr) {
3060 lane = nextNonInternalLane;
3062 if (seenLanes.count(lane) == 0) {
3063 seenLanes.insert(lane);
3070 if (seenLanes.count(lane) == 0) {
3073 routeJunctions.insert(junction);
3078 for (ConstMSEdgeVector::const_iterator ei = outgoing.begin(); ei != outgoing.end(); ++ei) {
3082 upstreamScanStartPositions.push_back(
UpstreamScanStartInfo(*ei, (*ei)->getLength(), range, distToConflictLane, lane));
3086 for (ConstMSEdgeVector::const_iterator ei = incoming.begin(); ei != incoming.end(); ++ei) {
3090 upstreamScanStartPositions.push_back(
UpstreamScanStartInfo(*ei, (*ei)->getLength(), range, distToConflictLane, lane));
3096 remainingDownstreamRange -= linkLength;
3097 distToConflictLane += linkLength;
3098 #ifdef DEBUG_SSM_SURROUNDING
3100 std::cout <<
" Downstream Scan for vehicle '" << veh.
getID() <<
"' proceeded over junction '" << junction->
getID()
3101 <<
"',\n linkLength=" << linkLength <<
", remainingDownstreamRange=" << remainingDownstreamRange
3107 lane = nextNonInternalLane;
3110 #ifdef DEBUG_SSM_SURROUNDING
3112 std::cout <<
" Downstream Scan for vehicle '" << veh.
getID() <<
"' stops at lane '" << lane->
getID()
3113 <<
"', which has already been scanned."
3134 #ifdef DEBUG_SSM_SURROUNDING
3136 for (std::pair<const MSVehicle*, FoeInfo*> foeInfo : foeCollector) {
3137 std::cout <<
" foe " << foeInfo.first->getID() <<
" conflict at " << foeInfo.second->egoConflictLane->getID() <<
" egoDist " << foeInfo.second->egoDistToConflictLane << std::endl;
3143 foeCollector.erase(&veh);
3149 #ifdef DEBUG_SSM_SURROUNDING
3151 std::cout <<
SIMTIME <<
" getUpstreamVehicles() for edge '" << scanStart.
edge->
getID() <<
"'"
3153 <<
" pos = " << scanStart.
pos <<
" range = " << scanStart.
range
3157 if (scanStart.
range <= 0) {
3163 if (seenLanes.find(lane) != seenLanes.end()) {
3167 for (
MSVehicle*
const veh : lane->getVehiclesSecure()) {
3168 if (foeCollector.find(veh) != foeCollector.end()) {
3172 if (veh->getPositionOnLane() - veh->getLength() <= scanStart.
pos && veh->getPositionOnLane() >= scanStart.
pos - scanStart.
range) {
3173 #ifdef DEBUG_SSM_SURROUNDING
3175 std::cout <<
"\t" << veh->getID() <<
"\n";
3181 foeCollector[veh] = c;
3185 lane->releaseVehicles();
3187 #ifdef DEBUG_SSM_SURROUNDING
3189 std::cout <<
"\t" << lane->getID() <<
": Found " << foundCount <<
"\n";
3192 seenLanes.insert(lane);
3195 #ifdef DEBUG_SSM_SURROUNDING
3197 std::cout << std::endl;
3205 if (scanStart.
range <= scanStart.
pos) {
3210 double remainingRange = scanStart.
range - scanStart.
pos;
3216 if (routeJunctions.find(junction) != routeJunctions.end()) {
3221 int incomingEdgeCount = 0;
3229 if (internalLane->getEdge().getSuccessors()[0]->getID() == scanStart.
edge->
getID()) {
3231 incomingEdgeCount++;
3236 if (incomingEdgeCount > 0) {
3238 if (inEdge->isInternal() || inEdge->isCrossing()) {
3242 for (
MSLane*
const lane : inEdge->getLanes()) {
3243 if (seenLanes.find(lane) != seenLanes.end()) {
3249 #ifdef DEBUG_SSM_SURROUNDING
3255 double distOnJunction = scanStart.
edge->
isInternal() ? 0. : inEdge->getInternalFollowingLengthTo(scanStart.
edge);
3256 if (distOnJunction >= remainingRange) {
3257 #ifdef DEBUG_SSM_SURROUNDING
3272 #ifdef DEBUG_SSM_SURROUNDING
3274 std::cout <<
SIMTIME <<
" getVehiclesOnJunction() for junction '" << junction->
getID()
3276 <<
"\nFound vehicles:"
3283 if (foeCollector.find(veh) != foeCollector.end()) {
3284 delete foeCollector[veh];
3289 foeCollector[veh] = c;
3290 #ifdef DEBUG_SSM_SURROUNDING
3292 std::cout <<
"\t" << veh->getID() <<
" egoConflictLane=" <<
Named::getIDSecure(egoConflictLane) <<
"\n";
3299 if (seenLanes.find(egoJunctionLane) != seenLanes.end() || egoJunctionLane->
getEdge().
isCrossing()) {
3303 auto scanInternalLane = [&](
const MSLane * lane) {
3308 collectFoeInfos(vehicles);
3310 lane->releaseVehicles();
3314 if (lane->getCanonicalPredecessorLane()->isInternal()) {
3315 lane = lane->getCanonicalPredecessorLane();
3318 assert(!lane->getEntryLink()->fromInternalLane());
3323 collectFoeInfos(vehicles2);
3324 lane->releaseVehicles();
3329 if (lane->getLinkCont().size() > 1 && lane->getLinkCont()[0]->getViaLane() !=
nullptr) {
3331 lane = lane->getLinkCont()[0]->getViaLane();
3333 assert(lane->getLinkCont().size() == 0 || lane->getLinkCont()[0]->getViaLane() == 0);
3338 collectFoeInfos(vehicles2);
3339 lane->releaseVehicles();
3349 for (
MSLane* lane : foeLanes) {
3350 if (seenLanes.find(lane) != seenLanes.end()) {
3353 scanInternalLane(lane);
3354 seenLanes.insert(lane);
3357 scanInternalLane(egoJunctionLane);
3359 #ifdef DEBUG_SSM_SURROUNDING
3361 std::cout << std::endl;
3381 std::string file = deviceID +
".xml";
3395 file = oc.
getString(
"device.ssm.file") ==
"" ? file : oc.
getString(
"device.ssm.file");
3397 std::cout <<
"vehicle '" << v.
getID() <<
"' does not supply vehicle parameter 'device.ssm.file'. Using default of '" << file <<
"'\n";
3415 bool useGeo =
false;
3429 useGeo = oc.
getBool(
"device.ssm.geo");
3431 std::cout <<
"vehicle '" << v.
getID() <<
"' does not supply vehicle parameter 'device.ssm.geo'. Using default of '" <<
::toString(useGeo) <<
"'\n";
3456 range = oc.
getFloat(
"device.ssm.range");
3458 std::cout <<
"vehicle '" << v.
getID() <<
"' does not supply vehicle parameter 'device.ssm.range'. Using default of '" << range <<
"'\n";
3483 extraTime = oc.
getFloat(
"device.ssm.extratime");
3485 std::cout <<
"vehicle '" << v.
getID() <<
"' does not supply vehicle parameter 'device.ssm.extratime'. Using default of '" << extraTime <<
"'\n";
3489 if (extraTime < 0.) {
3491 WRITE_WARNING(
"Negative (or no) value encountered for vehicle parameter 'device.ssm.extratime' in vehicle '" + v.
getID() +
"' using default value " + ::
toString(extraTime) +
" instead");
3500 bool trajectories =
false;
3514 trajectories = oc.
getBool(
"device.ssm.trajectories");
3516 std::cout <<
"vehicle '" << v.
getID() <<
"' does not supply vehicle parameter 'device.ssm.trajectories'. Using default of '" <<
::toString(trajectories) <<
"'\n";
3520 return trajectories;
3529 std::string measures_str =
"";
3543 measures_str = oc.
getString(
"device.ssm.measures");
3545 std::cout <<
"vehicle '" << v.
getID() <<
"' does not supply vehicle parameter 'device.ssm.measures'. Using default of '" << measures_str <<
"'\n";
3551 if (measures_str ==
"") {
3552 WRITE_WARNING(
"No measures specified for ssm device of vehicle '" + v.
getID() +
"'. Registering all available SSMs.");
3556 std::vector<std::string> available = st.
getVector();
3558 std::vector<std::string> measures = st.
getVector();
3559 for (std::vector<std::string>::const_iterator i = measures.begin(); i != measures.end(); ++i) {
3560 if (std::find(available.begin(), available.end(), *i) == available.end()) {
3562 WRITE_ERROR(
"SSM identifier '" + *i +
"' is not supported. Aborting construction of SSM device '" + deviceID +
"'.");
3568 std::string thresholds_str =
"";
3582 thresholds_str = oc.
getString(
"device.ssm.thresholds");
3584 std::cout <<
"vehicle '" << v.
getID() <<
"' does not supply vehicle parameter 'device.ssm.thresholds'. Using default of '" << thresholds_str <<
"'\n";
3591 if (thresholds_str !=
"") {
3593 while (count < (
int)measures.size() && st.
hasNext()) {
3595 thresholds.insert(std::make_pair(measures[count], thresh));
3598 if (thresholds.size() < measures.size() || st.
hasNext()) {
3599 WRITE_ERROR(
"Given list of thresholds ('" + thresholds_str +
"') is not of the same size as the list of measures ('" + measures_str +
"').\nPlease specify exactly one threshold for each measure.");
3604 for (std::vector<std::string>::const_iterator i = measures.begin(); i != measures.end(); ++i) {
3607 }
else if (*i ==
"DRAC") {
3609 }
else if (*i ==
"PET") {
3611 }
else if (*i ==
"BR") {
3613 }
else if (*i ==
"SGAP") {
3615 }
else if (*i ==
"TGAP") {
3618 WRITE_ERROR(
"Unknown SSM identifier '" + (*i) +
"'. Aborting construction of ssm device.");
3638 if (key ==
"minTTC" ||
3646 minTTC =
MIN2(minTTC, e->minTTC.value);
3647 minPET =
MIN2(minPET, e->PET.value);
3648 maxDRAC =
MAX2(maxDRAC, e->maxDRAC.value);
3650 if (key ==
"minTTC") {
3652 }
else if (key ==
"maxDRAC") {
3654 }
else if (key ==
"minPET") {
3675 if (
false || key ==
"foo") {
#define DEFAULT_THRESHOLD_SGAP
#define DEFAULT_THRESHOLD_BR
#define DEFAULT_THRESHOLD_TGAP
#define DEFAULT_THRESHOLD_DRAC
#define DEFAULT_THRESHOLD_TTC
#define DEFAULT_EXTRA_TIME
#define DEFAULT_THRESHOLD_PET
#define DEBUG_COND_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
#define WRITE_WARNINGF(...)
#define WRITE_WARNING(msg)
int gPrecision
the precision for floating point outputs
const double INVALID_DOUBLE
#define UNUSED_PARAMETER(x)
std::string joinToString(const std::vector< T > &v, const T_BETWEEN &between, std::streamsize accuracy=gPrecision)
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
static std::string checkForRelativity(const std::string &filename, const std::string &basePath)
Returns the path from a configuration so that it is accessable from the current working directory.
void cartesian2geo(Position &cartesian) const
Converts the given cartesian (shifted) position to its geo (lat/long) representation.
static const GeoConvHelper & getFinal()
the coordinate transformation for writing the location element and for tracking the original coordina...
The base class for microscopic and mesoscopic vehicles.
double getLength() const
Returns the vehicle's length.
const MSEdge * getEdge() const
Returns the edge the vehicle is currently at.
double getWidth() const
Returns the vehicle's width.
const MSRouteIterator & getCurrentRouteEdge() const
Returns an iterator pointing to the current edge in this vehicles route.
const MSVehicleType & getVehicleType() const
Returns the vehicle's type definition.
const MSRoute & getRoute() const
Returns the current route.
static double passingTime(const double lastPos, const double passedPos, const double currentPos, const double lastSpeed, const double currentSpeed)
Calculates the time at which the position passedPosition has been passed In case of a ballistic updat...
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...
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.
void add(double time, EncounterType type, Position egoX, Position egoV, Position foeX, Position foeV, Position conflictPoint, double egoDistToConflict, double foeDistToConflict, double ttc, double drac, std::pair< double, double > pet)
add a new data point and update encounter type
std::size_t size() const
Returns the number of trajectory points stored.
void resetExtraTime(double value)
resets remainingExtraTime to the given value
PositionVector conflictPointSpan
Predicted location of the conflict: In case of MERGING and CROSSING: entry point to conflict area for...
ConflictPointInfo maxDRAC
double egoConflictExitTime
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 foeConflictExitTime
double getRemainingExtraTime() const
returns the remaining extra time
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.
static std::set< std::string > createdOutputFiles
remember which files were created already (don't duplicate xml root-elements)
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_ONCOMING
@ 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
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)
static int issuedParameterWarnFlags
bitset storing info whether warning has already been issued about unset parameter (warn only once!...
MSDevice_SSM(SUMOVehicle &holder, const std::string &id, std::string outputFilename, std::map< std::string, double > thresholds, bool trajectories, double range, double extraTime, bool useGeoCoords)
Constructor.
void setParameter(const std::string &key, const std::string &value)
try to set the given parameter for this device. Throw exception for unsupported key
static const std::set< MSDevice_SSM *, ComparatorNumericalIdLess > & getInstances()
returns all currently existing SSM devices
void closeEncounter(Encounter *e)
Finalizes the encounter and calculates SSM values.
static std::string makeStringWithNAs(const std::vector< double > &v, const double NA)
make a string of a double vector and treat a special value as invalid ("NA")
static double getDetectionRange(const SUMOVehicle &v)
static void cleanup()
Clean up remaining devices instances.
static void insertOptions(OptionsCont &oc)
Inserts MSDevice_SSM-options.
double myRange
Detection range. For vehicles closer than this distance from the ego vehicle, SSMs are traced.
const MSLane * findFoeConflictLane(const MSVehicle *foe, const MSLane *egoConflictLane, double &distToConflictLane) const
Computes the conflict lane for the foe.
bool notifyLeave(SUMOTrafficObject &veh, double lastPos, MSMoveReminder::Notification reason, const MSLane *enteredLane=0)
Called whenever the holder leaves a lane.
std::vector< Encounter * > EncounterVector
static void getUpstreamVehicles(const UpstreamScanStartInfo &scanStart, FoeInfoMap &foeCollector, std::set< const MSLane * > &seenLanes, const std::set< const MSJunction * > &routeJunctions)
Collects all vehicles within range 'range' upstream of the position 'pos' on the edge 'edge' into foe...
void createEncounters(FoeInfoMap &foes)
Makes new encounters for all given vehicles (these should be the ones entering the device's range in ...
bool qualifiesAsConflict(Encounter *e)
Tests if the SSM values exceed the threshold for qualification as conflict.
std::map< std::string, double > myThresholds
static std::string getOutputFilename(const SUMOVehicle &v, std::string deviceID)
void updateAndWriteOutput()
This is called once per time step in MSNet::writeOutput() and collects the surrounding vehicles,...
static std::set< MSDevice_SSM *, ComparatorNumericalIdLess > * myInstances
All currently existing SSM devices.
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 void toGeo(Position &x)
convert SUMO-positions to geo coordinates (in place)
static void findSurroundingVehicles(const MSVehicle &veh, double range, FoeInfoMap &foeCollector)
Returns all vehicles, which are within the given range of the given vehicle.
void resetEncounters()
Closes all current Encounters and moves conflicts to myPastConflicts,.
std::pair< std::pair< double, Position >, double > myMaxBR
Extremal values for the global measures (as <<<time, Position>, value>, [leaderID]>-pairs)
std::vector< double > myBRspan
All values for brake rate.
void determineTTCandDRAC(EncounterApproachInfo &eInfo) const
Discriminates between different encounter types and correspondingly determines TTC and DRAC for those...
bool myUseGeoCoords
Whether to use the original coordinate system for output.
~MSDevice_SSM()
Destructor.
const std::string deviceName() const
return the name for this type of device
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...
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.
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.
A road/street connecting two junctions.
bool isCrossing() const
return whether this edge is a pedestrian crossing
const MSEdge * getOppositeEdge() const
Returns the opposite direction edge if on exists else a nullptr.
const std::vector< MSLane * > & getLanes() const
Returns this edge's lanes.
const MSJunction * getFromJunction() const
double getLength() const
return the length of the edge
bool isInternal() const
return whether this edge is an internal edge
const MSJunction * getToJunction() const
The base class for an intersection.
const ConstMSEdgeVector & getIncoming() const
const ConstMSEdgeVector & getOutgoing() const
virtual const std::vector< MSLane * > getInternalLanes() const
Returns all internal lanes on the junction.
virtual const std::vector< MSLane * > & getFoeInternalLanes(const MSLink *const) const
Representation of a lane in the micro simulation.
const std::vector< MSLink * > & getLinkCont() const
returns the container with all links !!!
const MSLink * getEntryLink() const
Returns the entry link if this is an internal lane, else nullptr.
const MSLink * getLinkTo(const MSLane *const) const
returns the link to the given lane or nullptr, if it is not connected
std::vector< MSVehicle * > VehCont
Container for vehicles.
double getLength() const
Returns the lane's length.
const MSLane * getFirstInternalInConnection(double &offset) const
Returns 0 if the lane is not internal. Otherwise the first part of the connection (sequence of intern...
MSLane * getCanonicalSuccessorLane() const
const std::vector< IncomingLaneInfo > & getIncomingLanes() const
MSEdge & getEdge() const
Returns the lane's edge.
const PositionVector & getShape() const
Returns this lane's shape.
MSLane * getOpposite() const
return the opposite direction lane for lane changing or 0
double getWidth() const
Returns the lane's width.
MSLane * getViaLane() const
Returns the following inner lane.
MSLane * getLane() const
Returns the connected lane.
int getIndex() const
Returns the respond index (for visualization)
const MSLane * getLaneBefore() const
return the internalLaneBefore if it exists and the laneBefore otherwise
bool isInternalJunctionLink() const
return whether the fromLane and the toLane of this link are internal lanes
double getInternalLengthsAfter() const
Returns the cumulative length of all internal lanes after this link.
const std::vector< MSLink * > & getFoeLinks() const
const std::vector< const MSLane * > & getFoeLanes() const
double getLengthsBeforeCrossing(const MSLane *foeLane) const
Returns the sum of the lengths along internal lanes following this link to the crossing with the give...
MSJunction * getJunction() const
const MSLink * getCorrespondingExitLink() const
returns the corresponding exit link for entryLinks to a junction.
const MSLane * getInternalLaneBefore() const
return myInternalLaneBefore (always 0 when compiled without internal lanes)
Notification
Definition of a vehicle state.
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
MSVehicleControl & getVehicleControl()
Returns the vehicle control.
MSRouteIterator end() const
Returns the end of the list of edges to pass.
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.
bool isOnRoad() const
Returns the information whether the vehicle is on a road (is simulated)
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...
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.
std::pair< const MSVehicle *const, double > getLeader(double dist=0) const
Returns the leader of the vehicle looking for a fixed distance.
double getLastStepDist() const
Get the distance the vehicle covered in the previous timestep.
double getLateralPositionOnLane() const
Get the vehicle's lateral position on the lane.
double getSpeed() const
Returns the vehicle's current speed.
double getPositionOnLane() const
Get the vehicle's position along the lane.
const MSLane * getLane() const
Returns the lane the vehicle is on.
const MSCFModel & getCarFollowModel() const
Returns the vehicle's car following model definition.
double getPreviousSpeed() const
Returns the vehicle's speed before the previous time step.
Position getVelocityVector() const
Returns the vehicle's direction in radians.
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
const std::string & getID() const
Returns the id.
A storage for options typed value containers)
void addDescription(const std::string &name, const std::string &subtopic, const std::string &description)
Adds a description for an option.
void doRegister(const std::string &name, Option *v)
Adds an option under the given name.
bool isSet(const std::string &name, bool failOnNonExistant=true) const
Returns the information whether the named option is set.
double getFloat(const std::string &name) const
Returns the double-value of the named option (only for Option_Float)
std::string getString(const std::string &name) const
Returns the string-value of the named option (only for Option_String)
void addOptionSubTopic(const std::string &topic)
Adds an option subtopic.
bool getBool(const std::string &name) const
Returns the boolean-value of the named option (only for Option_Bool)
static OptionsCont & getOptions()
Retrieves the options.
Static storage of an output device and its base (abstract) implementation.
bool writeXMLHeader(const std::string &rootElement, const std::string &schemaFile, std::map< SumoXMLAttr, std::string > attrs=std::map< SumoXMLAttr, std::string >())
Writes an XML header with optional configuration.
OutputDevice & openTag(const std::string &xmlElement)
Opens an XML tag.
OutputDevice & writeAttr(const SumoXMLAttr attr, const T &val)
writes a named attribute
bool closeTag(const std::string &comment="")
Closes the most recently opened tag and optionally adds a comment.
static OutputDevice & getDevice(const std::string &name)
Returns the described OutputDevice.
const std::string getParameter(const std::string &key, const std::string defaultValue="") const
Returns the value for a given key.
bool knowsParameter(const std::string &key) const
Returns whether the parameter is known.
A point in 2D or 3D with translation and scaling methods.
static const Position INVALID
used to indicate that a position is valid
double rotationAtOffset(double pos) const
Returns the rotation at the given length.
Representation of a vehicle, person, or container.
virtual bool isVehicle() const
Whether it is a vehicle.
virtual const MSVehicleType & getVehicleType() const =0
Returns the object's "vehicle" type.
virtual const SUMOVehicleParameter & getParameter() const =0
Returns the vehicle's parameter (including departure definition)
virtual const MSLane * getLane() const =0
Returns the lane the object is currently at.
virtual const MSEdge * getEdge() const =0
Returns the edge the object is currently at.
Representation of a vehicle.
virtual const MSRoute & getRoute() const =0
Returns the current route.
virtual bool isOnRoad() const =0
Returns the information whether the vehicle is on a road (is simulated)
virtual const ConstMSEdgeVector::const_iterator & getCurrentRouteEdge() const =0
Returns an iterator pointing to the current edge in this vehicles route.
std::vector< std::string > getVector()
return vector of strings
bool hasNext()
returns the information whether further substrings exist
std::string next()
returns the next substring when it exists. Otherwise the behaviour is undefined
static std::string urlDecode(const std::string &encoded)
static double toDouble(const std::string &sData)
converts a string into the double value described by it by calling the char-type converter
static bool toBool(const std::string &sData)
converts a string into the bool value described by it by calling the char-type converter
EncounterType type
Type of the conflict.
double time
time point of the conflict
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
Structure to collect some info on the encounter needed during ssm calculation by various functions.
double egoConflictEntryDist
double egoConflictAreaLength
EncounterApproachInfo(Encounter *e)
double foeConflictAreaLength
double foeConflictExitDist
double egoConflictExitDist
double foeEstimatedConflictEntryTime
std::pair< double, double > pet
double foeEstimatedConflictExitTime
double egoEstimatedConflictExitTime
double foeConflictEntryDist
double egoEstimatedConflictEntryTime
const MSLane * egoConflictLane
double egoDistToConflictLane
Auxiliary structure used to handle upstream scanning start points Upstream scan has to be started aft...
double egoDistToConflictLane
const MSLane * egoConflictLane