68#define DEBUG_COND(ego) (ego!=nullptr && ego->isSelected())
69#define DEBUG_COND_FIND(ego) (ego.isSelected())
70#define DEBUG_COND_ENCOUNTER(e) (e->ego != nullptr && e->ego->isSelected() && e->foe != nullptr && e->foe->isSelected())
79#define AVAILABLE_SSMS "TTC DRAC PET BR SGAP TGAP"
81#define DEFAULT_THRESHOLD_TTC 3.
82#define DEFAULT_THRESHOLD_DRAC 3.
83#define DEFAULT_THRESHOLD_PET 2.
85#define DEFAULT_THRESHOLD_BR 0.0
86#define DEFAULT_THRESHOLD_SGAP 0.2
87#define DEFAULT_THRESHOLD_TGAP 0.5
89#define DEFAULT_EXTRA_TIME 5.
106 out <<
"NOCONFLICT_AHEAD";
112 out <<
"FOLLOWING_FOLLOWER";
115 out <<
"FOLLOWING_LEADER";
118 out <<
"ON_ADJACENT_LANES";
124 out <<
"MERGING_LEADER";
127 out <<
"MERGING_FOLLOWER";
130 out <<
"MERGING_ADJACENT";
136 out <<
"CROSSING_LEADER";
139 out <<
"CROSSING_FOLLOWER";
142 out <<
"EGO_ENTERED_CONFLICT_AREA";
145 out <<
"FOE_ENTERED_CONFLICT_AREA";
148 out <<
"BOTH_ENTERED_CONFLICT_AREA";
151 out <<
"EGO_LEFT_CONFLICT_AREA";
154 out <<
"FOE_LEFT_CONFLICT_AREA";
157 out <<
"BOTH_LEFT_CONFLICT_AREA";
160 out <<
"FOLLOWING_PASSED";
163 out <<
"MERGING_PASSED";
173 out <<
"unknown type (" << int(type) <<
")";
184std::set<MSDevice_SSM*, ComparatorNumericalIdLess>*
MSDevice_SSM::myInstances =
new std::set<MSDevice_SSM*, ComparatorNumericalIdLess>();
190const std::set<MSDevice_SSM*, ComparatorNumericalIdLess>&
200 device->resetEncounters();
201 device->flushConflicts(
true);
202 device->flushGlobalMeasures();
220 oc.
addDescription(
"device.ssm.measures",
"SSM Device",
TL(
"Specifies which measures will be logged (as a space or comma-separated sequence of IDs in ('TTC', 'DRAC', 'PET'))"));
222 oc.
addDescription(
"device.ssm.thresholds",
"SSM Device",
TL(
"Specifies space or comma-separated thresholds corresponding to the specified measures (see documentation and watch the order!). Only events exceeding the thresholds will be logged."));
224 oc.
addDescription(
"device.ssm.trajectories",
"SSM Device",
TL(
"Specifies whether trajectories will be logged (if false, only the extremal values and times are reported)."));
226 oc.
addDescription(
"device.ssm.range",
"SSM Device",
TL(
"Specifies the detection range in meters. For vehicles below this distance from the equipped vehicle, SSM values are traced."));
228 oc.
addDescription(
"device.ssm.extratime",
"SSM Device",
TL(
"Specifies the time in seconds to be logged after a conflict is over. Required >0 if PET is to be calculated for crossing conflicts."));
230 oc.
addDescription(
"device.ssm.file",
"SSM Device",
TL(
"Give a global default filename for the SSM output"));
232 oc.
addDescription(
"device.ssm.geo",
"SSM Device",
TL(
"Whether to use coordinates of the original reference system in output"));
234 oc.
addDescription(
"device.ssm.write-positions",
"SSM Device",
TL(
"Whether to write positions (coordinates) for each timestep"));
236 oc.
addDescription(
"device.ssm.write-lane-positions",
"SSM Device",
TL(
"Whether to write lanes and their positions for each timestep"));
245 std::ifstream strm(file.c_str());
247 throw ProcessError(
TLF(
"Could not load names of edges for filtering SSM device output from '%'.", file));
250 while (strm.good()) {
255 std::string edgeID = line.substr(5);
257 if (edge !=
nullptr) {
260 WRITE_WARNING(
"Unknown edge ID '" + edgeID +
"' in SSM device edge filter (" + file +
"): " + line);
264 std::string junctionID = line.substr(9);
266 if (junction !=
nullptr) {
271 WRITE_WARNING(
"Unknown junction ID '" + junctionID +
"' in SSM device edge filter (" + file +
"): " + line);
273 }
else if (line ==
"") {
275 WRITE_WARNING(
"Cannot interpret line in SSM device edge filter (" + file +
"): " + line);
285 WRITE_WARNINGF(
"SSM Device for vehicle '%' will not be built. (SSMs not supported in MESO)", v.
getID());
289 std::string deviceID =
"ssm_" + v.
getID();
294 std::map<std::string, double> thresholds;
320 MSDevice_SSM* device =
new MSDevice_SSM(v, deviceID, file, thresholds, trajectories, range, extraTime, useGeo, writePos, writeLanesPos);
321 into.push_back(device);
334 egoID(_ego->getID()),
335 foeID(_foe->getID()),
338 currentType(ENCOUNTER_TYPE_NOCONFLICT_AHEAD),
339 remainingExtraTime(extraTime),
347 closingRequested(false) {
348#ifdef DEBUG_ENCOUNTER
350 std::cout <<
"\n" <<
SIMTIME <<
" Constructing encounter of '" <<
ego->
getID() <<
"' and '" <<
foe->
getID() <<
"'" << std::endl;
356#ifdef DEBUG_ENCOUNTER
358 std::cout <<
"\n" <<
SIMTIME <<
" Destroying encounter of '" << egoID <<
"' and '" << foeID <<
"' (begin was " << begin <<
")" << std::endl;
367 Position conflictPoint,
double egoDistToConflict,
double foeDistToConflict,
double ttc,
double drac, std::pair<double, double> pet) {
368#ifdef DEBUG_ENCOUNTER
370 std::cout << time <<
" Adding data point for encounter of '" << egoID <<
"' and '" << foeID <<
"':\n"
371 <<
"type=" << type <<
", egoDistToConflict=" << (egoDistToConflict ==
INVALID_DOUBLE ?
"NA" :
::toString(egoDistToConflict))
380 timeSpan.push_back(time);
381 typeSpan.push_back(type);
382 egoTrajectory.x.push_back(egoX);
383 egoTrajectory.lane.push_back(egoLane);
384 egoTrajectory.lanePos.push_back(egoLanePos);
385 egoTrajectory.v.push_back(egoV);
386 foeTrajectory.x.push_back(foeX);
387 foeTrajectory.lane.push_back(foeLane);
388 foeTrajectory.lanePos.push_back(foeLanePos);
389 foeTrajectory.v.push_back(foeV);
390 conflictPointSpan.push_back(conflictPoint);
391 egoDistsToConflict.push_back(egoDistToConflict);
392 foeDistsToConflict.push_back(foeDistToConflict);
394 TTCspan.push_back(ttc);
398 minTTC.pos = conflictPoint;
402 DRACspan.push_back(drac);
404 maxDRAC.value = drac;
406 maxDRAC.pos = conflictPoint;
411 PET.value = pet.second;
412 PET.time = pet.first;
413 PET.pos = conflictPoint;
421 remainingExtraTime = value;
427 remainingExtraTime -= amount;
433 return remainingExtraTime;
451 egoLeftConflict(false),
452 foeLeftConflict(false),
468 std::cout <<
"\n" <<
SIMTIME <<
" Device '" <<
getID() <<
"' updateAndWriteOutput()\n"
469 <<
" Holder is off-road! Calling resetEncounters()."
482 std::cout <<
"\n" <<
SIMTIME <<
" Device '" <<
getID() <<
"' update()\n"
492 const MSEdge* egoEdge = &((*myHolderMS).getLane()->getEdge());
501 if (foes.size() > 0) {
502 std::cout <<
"Scanned surroundings: Found potential foes:\n";
503 for (FoeInfoMap::const_iterator i = foes.begin(); i != foes.end(); ++i) {
504 std::cout << i->first->getID() <<
" ";
506 std::cout << std::endl;
508 std::cout <<
"Scanned surroundings: No potential conflict could be identified." << std::endl;
545 double leaderSearchDist = 0;
546 std::pair<const MSVehicle*, double> leader(
nullptr, 0.);
554 if (leaderSearchDist > 0.) {
560 if (leader.first ==
nullptr || leader.second < 0) {
591 std::cout <<
"\n" <<
SIMTIME <<
" Device '" <<
getID() <<
"' createEncounters()" << std::endl;
592 std::cout <<
"New foes:\n";
593 for (FoeInfoMap::const_iterator vi = foes.begin(); vi != foes.end(); ++vi) {
594 std::cout << vi->first->getID() <<
"\n";
596 std::cout << std::endl;
600 for (FoeInfoMap::const_iterator foe = foes.begin(); foe != foes.end(); ++foe) {
607 assert(myOldestActiveEncounterBegin <= e->begin);
632 std::cout <<
"\n" <<
SIMTIME <<
" Device '" <<
getID() <<
"' processEncounters(forceClose = " << forceClose <<
")" << std::endl;
633 std::cout <<
"Currently present foes:\n";
634 for (FoeInfoMap::const_iterator vi = foes.begin(); vi != foes.end(); ++vi) {
635 std::cout << vi->first->getID() <<
"\n";
637 std::cout << std::endl;
653 if (foes.find(e->
foe) != foes.end()) {
665 std::cout <<
" Requesting encounter closure because both left conflict area of previous encounter but another encounter lies ahead." << std::endl;
679 std::cout <<
" Requesting encounter closure because..." << std::endl;
681 std::cout <<
" ... extra time elapsed." << std::endl;
682 }
else if (forceClose) {
683 std::cout <<
" ... closing was forced." << std::endl;
685 std::cout <<
" ... foe disappeared." << std::endl;
696 double eBegin = e->
begin;
721 std::cout <<
SIMTIME <<
" qualifiesAsConflict() for encounter of vehicles '"
741 assert(e->
size() > 0);
749 std::cout <<
SIMTIME <<
" closeEncounter() of vehicles '"
751 <<
"' (was ranked as " << (wasConflict ?
"conflict" :
"non-conflict") <<
")" << std::endl;
759 std::cout <<
"pastConflictsQueue of veh '" <<
myHolderMS->
getID() <<
"':\n";
767 std::cout <<
" Conflict with foe '" << c->foe <<
"' (time=" << c->begin <<
"-" << c->end <<
")\n";
769 if (c->begin < lastBegin) {
770 std::cout <<
" Queue corrupt...\n";
773 lastBegin = c->begin;
775 std::cout << std::endl;
788#ifdef DEBUG_ENCOUNTER
790 std::cout <<
SIMTIME <<
" updateEncounter() of vehicles '" << e->
egoID <<
"' and '" << e->
foeID <<
"'\n";
817#ifdef DEBUG_ENCOUNTER
819 std::cout <<
SIMTIME <<
" Encounter of vehicles '" << e->
egoID <<
"' and '" << e->
foeID <<
"' does not imply any conflict.\n";
850 if (e->
size() == 0) {
851#ifdef DEBUG_ENCOUNTER
853 std::cout <<
SIMTIME <<
" type when creating encounter: " << eInfo.
type <<
"\n";
896 std::cout <<
SIMTIME <<
" determineConflictPoint()" << std::endl;
906 assert(e->
size() > 0);
927 std::cout <<
"No conflict point associated with encounter type " << type << std::endl;
935 std::cout <<
" Conflict at " << eInfo.
conflictPoint << std::endl;
950 std::cout <<
SIMTIME <<
" estimateConflictTimes() for ego '" << e->
egoID <<
"' and foe '" << e->
foeID <<
"'\n"
951 <<
" encounter type: " << eInfo.
type <<
"\n"
963 std::cout <<
" encouter type " << type <<
" -> no exit times to be calculated."
975 std::cout <<
" encouter type " << type <<
" -> no entry/exit times to be calculated."
1062 std::cout <<
" -> ego is estimated leader at conflict entry."
1071 std::cout <<
" -> foe is estimated leader at conflict entry."
1087 std::cout <<
SIMTIME <<
" computeSSMs() for vehicles '"
1089 <<
"'" << std::endl;
1119 std::stringstream ss;
1120 ss <<
"'" << type <<
"'";
1121 WRITE_WARNING(
"Unknown or undetermined encounter type at computeSSMs(): " + ss.str());
1127 std::cout <<
"computeSSMs() for encounter of vehicles '" << e->
egoID <<
"' and '" << e->
foeID <<
"':\n"
1140 if (e->
size() == 0) {
1144 std::pair<double, double>& pet = eInfo.
pet;
1148 std::cout <<
SIMTIME <<
" determinePET() for encounter of vehicles '" << e->
egoID <<
"' and '" << e->
foeID <<
"'"
1163 std::cout <<
"PET for crossing encounter already calculated as " << e->
PET.
value
1206 std::cout <<
"determinePET: Both passed conflict area in the same step. Assume collision"
1221 std::cout <<
"Calculated PET = " << pet.second <<
" (at t=" << pet.first <<
")"
1228 std::cout <<
"PET unappropriate for merging and pre-crossing situations. No calculation performed."
1240 double& ttc = eInfo.
ttc;
1241 double& drac = eInfo.
drac;
1245 std::cout <<
SIMTIME <<
" determineTTCandDRAC() for encounter of vehicles '" << e->
egoID <<
"' and '" << e->
foeID <<
"' (type = " << eInfo.
type <<
")"
1288 std::cout <<
" Conflict times with constant speed extrapolation for merging situation:\n "
1303 std::cout <<
" No TTC and DRAC computed as one vehicle is stopped." << std::endl;
1308 double leaderEntryTime =
MIN2(egoEntryTime, foeEntryTime);
1309 double followerEntryTime =
MAX2(egoEntryTime, foeEntryTime);
1310 double leaderExitTime = leaderEntryTime == egoEntryTime ? egoExitTime : foeExitTime;
1317 if (leaderExitTime >= followerEntryTime) {
1320 ttc =
computeTTC(followerConflictDist, followerSpeed, 0.);
1325 drac =
computeDRAC(followerConflictDist, followerSpeed, 0.);
1331 std::cout <<
" Extrapolation predicts collision *at* merge point with TTC=" << ttc
1332 <<
", drac=" << drac << std::endl;
1339 double gapAfterMerge = followerConflictDist - leaderExitTime * followerSpeed;
1340 assert(gapAfterMerge >= 0);
1343 double ttcAfterMerge =
computeTTC(gapAfterMerge, followerSpeed, leaderSpeed);
1348 double g0 = followerConflictDist - leaderConflictDist - leaderLength;
1351 assert(leaderSpeed - followerSpeed > 0);
1356 drac =
computeDRAC(g0, followerSpeed, leaderSpeed);
1364 std::cout <<
" Extrapolation does not predict any collision." << std::endl;
1366 std::cout <<
" Extrapolation predicts collision *after* merge point with TTC="
1408 std::stringstream ss;
1409 ss <<
"'" << type <<
"'";
1410 WRITE_WARNING(
"Underspecified or unknown encounter type in MSDevice_SSM::determineTTCandDRAC(): " + ss.str());
1430 std::cout <<
"computeTTC() with gap=" << gap <<
", followerSpeed=" << followerSpeed <<
", leaderSpeed=" << leaderSpeed
1436 double dv = followerSpeed - leaderSpeed;
1455 double dv = followerSpeed - leaderSpeed;
1459 assert(followerSpeed > 0.);
1460 return 0.5 * dv * dv / gap;
1476#ifdef DEBUG_SSM_DRAC
1478 std::cout <<
SIMTIME <<
"computeDRAC() with"
1479 <<
"\ndEntry1=" << dEntry1 <<
", dEntry2=" << dEntry2
1480 <<
", dExit1=" << dExit1 <<
", dExit2=" << dExit2
1481 <<
",\nv1=" << v1 <<
", v2=" << v2
1486 if (dExit1 <= 0. || dExit2 <= 0.) {
1488#ifdef DEBUG_SSM_DRAC
1490 std::cout <<
"One already left conflict area -> drac == 0." << std::endl;
1495 if (dEntry1 <= 0. && dEntry2 <= 0.) {
1497#ifdef DEBUG_SSM_DRAC
1499 std::cout <<
"Both entered conflict area but neither left. -> collision!" << std::endl;
1505 double drac = std::numeric_limits<double>::max();
1508#ifdef DEBUG_SSM_DRAC
1510 std::cout <<
"Ego could break..." << std::endl;
1515 drac =
MIN2(drac, 2 * (v1 - dEntry1 / tExit2) / tExit2);
1516#ifdef DEBUG_SSM_DRAC
1518 std::cout <<
" Foe expected to leave in " << tExit2 <<
"-> Ego needs drac=" << drac << std::endl;
1526#ifdef DEBUG_SSM_DRAC
1528 std::cout <<
" Foe is expected stop on conflict area -> Ego needs drac=" << drac << std::endl;
1533#ifdef DEBUG_SSM_DRAC
1535 std::cout <<
" Foe is expected stop before conflict area -> no drac computation for ego (will be done for foe if applicable)" << std::endl;
1544#ifdef DEBUG_SSM_DRAC
1546 std::cout <<
"Foe could break..." << std::endl;
1551#ifdef DEBUG_SSM_DRAC
1553 std::cout <<
" Ego expected to leave in " << tExit1 <<
"-> Foe needs drac=" << (2 * (v2 - dEntry2 / tExit1) / tExit1) << std::endl;
1556 drac =
MIN2(drac, 2 * (v2 - dEntry2 / tExit1) / tExit1);
1561#ifdef DEBUG_SSM_DRAC
1563 std::cout <<
" Ego is expected stop on conflict area -> Foe needs drac=" <<
computeDRAC(dEntry2, v2, 0) << std::endl;
1569#ifdef DEBUG_SSM_DRAC
1571 std::cout <<
" Ego is expected stop before conflict area -> no drac computation for foe (done for ego if applicable)" << std::endl;
1592#ifdef DEBUG_ENCOUNTER
1594 std::cout <<
SIMTIME <<
" checkConflictEntryAndExit() for encounter of vehicles '" << e->
egoID <<
"' and '" << e->
foeID <<
"'"
1604 if (e->
size() == 0) {
1608 if (egoPastConflictExit) {
1609 if (foePastConflictExit) {
1611 }
else if (foePastConflictEntry) {
1616 }
else if (foePastConflictExit) {
1617 if (egoPastConflictEntry) {
1624 if (egoPastConflictEntry) {
1625 if (foePastConflictEntry) {
1630 }
else if (foePastConflictEntry) {
1658#ifdef DEBUG_ENCOUNTER
1674#ifdef DEBUG_ENCOUNTER
1692#ifdef DEBUG_ENCOUNTER
1710#ifdef DEBUG_ENCOUNTER
1727#ifdef DEBUG_ENCOUNTER
1729 std::cout <<
SIMTIME <<
" updatePassedEncounter() for vehicles '" << e->
egoID <<
"' and '" << e->
foeID <<
"'\n";
1733 if (foeInfo ==
nullptr) {
1736#ifdef DEBUG_ENCOUNTER
1752#ifdef DEBUG_ENCOUNTER
1754 std::cout <<
" This encounter wasn't classified as a potential conflict lately.\n";
1757 if (foeInfo ==
nullptr) {
1763 std::cout <<
" Requesting encounter closure because foeInfo==nullptr" << std::endl;
1767#ifdef DEBUG_ENCOUNTER
1769 std::cout <<
" Closing encounter.\n";
1779#ifdef DEBUG_ENCOUNTER
1781 std::cout <<
" Encounter was previously classified as a follow/lead situation.\n";
1790#ifdef DEBUG_ENCOUNTER
1792 std::cout <<
" Encounter was previously classified as a merging situation.\n";
1807#ifdef DEBUG_ENCOUNTER
1809 std::cout <<
" Encounter was previously classified as a crossing situation of type " << lastPotentialConflictType <<
".\n";
1827#ifdef DEBUG_ENCOUNTER
1842 if ((!egoEnteredConflict) && !foeEnteredConflict) {
1846 eInfo.
type = lastPotentialConflictType;
1847 }
else if (egoEnteredConflict && !foeEnteredConflict) {
1849 }
else if ((!egoEnteredConflict) && foeEnteredConflict) {
1855 if ((!egoLeftConflict) && !foeLeftConflict) {
1859 }
else if (egoLeftConflict && !foeLeftConflict) {
1863 }
else if ((!egoLeftConflict) && foeLeftConflict) {
1878#ifdef DEBUG_ENCOUNTER
1880 std::cout <<
" Updated classification: " << eInfo.
type <<
"\n";
1889#ifdef DEBUG_ENCOUNTER
1891 std::cout <<
"classifyEncounter() called.\n";
1894 if (foeInfo ==
nullptr) {
1913#ifdef DEBUG_ENCOUNTER
1915 std::cout <<
" Ongoing crossing conflict will be traced by passedEncounter().\n";
1931 double foeDistToConflictLane;
1934#ifdef DEBUG_ENCOUNTER
1936 std::cout <<
" egoConflictLane='" << (egoConflictLane == 0 ?
"NULL" : egoConflictLane->
getID()) <<
"'\n"
1937 <<
" foeConflictLane='" << (foeConflictLane == 0 ?
"NULL" : foeConflictLane->
getID()) <<
"'\n"
1938 <<
" egoDistToConflictLane=" << egoDistToConflictLane
1939 <<
" foeDistToConflictLane=" << foeDistToConflictLane
1954 if (foeConflictLane ==
nullptr) {
1957#ifdef DEBUG_ENCOUNTER
1959 std::cout <<
"-> Encounter type: No conflict.\n";
1964 if (egoConflictLane == egoLane) {
1968 if (foeLane == egoLane) {
1970 if (!egoOpposite && !foeOpposite) {
1978#ifdef DEBUG_ENCOUNTER
1980 std::cout <<
"-> Encounter type: Lead/follow-situation on non-internal lane '" << egoLane->
getID() <<
"'\n";
1983 }
else if (egoOpposite && foeOpposite) {
1991#ifdef DEBUG_ENCOUNTER
1993 std::cout <<
"-> Encounter type: Lead/follow-situation while both are driving in the opposite direction on non-internal lane '" << egoLane->
getID() <<
"'\n";
2014#ifdef DEBUG_ENCOUNTER
2016 std::cout <<
"-> Encounter type: oncoming on non-internal lane '" << egoLane->
getID() <<
"'\n";
2025#ifdef DEBUG_ENCOUNTER
2027 std::cout <<
"-> Encounter type: " << type << std::endl;
2032 if (!egoOpposite && !foeOpposite) {
2035 assert(egoDistToConflictLane <= 0);
2037 if (foeConflictLane == egoLane) {
2041#ifdef DEBUG_ENCOUNTER
2043 std::cout <<
"-> Encounter type: Ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' leads foe '"
2044 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'"
2050#ifdef DEBUG_ENCOUNTER
2052 std::cout <<
"-> Encounter type: " << type << std::endl;
2057 }
else if (egoOpposite && foeOpposite) {
2069#ifdef DEBUG_ENCOUNTER
2071 std::cout <<
"-> Encounter type: Lead/follow-situation while both are driving in the opposite direction on non-internal lane '" << egoLane->
getID() <<
"'\n";
2095#ifdef DEBUG_ENCOUNTER
2097 std::cout <<
"-> Encounter type: oncoming on non-internal lane '" << egoLane->
getID() <<
"'\n";
2109 assert(foeDistToConflictLane <= 0);
2110 if (foeLane == egoConflictLane) {
2113#ifdef DEBUG_ENCOUNTER
2115 std::cout <<
"-> Encounter type: Ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' follows foe '"
2116 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'"
2122#ifdef DEBUG_ENCOUNTER
2124 std::cout <<
"-> Encounter type: " << type << std::endl;
2136 if (egoEntryLink != foeEntryLink) {
2139#ifdef DEBUG_ENCOUNTER
2141 std::cout <<
"-> Encounter type: " << type << std::endl;
2146 if (egoLane == egoConflictLane && foeLane != foeConflictLane) {
2153#ifdef DEBUG_ENCOUNTER
2155 std::cout <<
"-> Encounter type: Ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' leads foe '"
2156 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'"
2159 }
else if (egoLane != egoConflictLane && foeLane == foeConflictLane) {
2166#ifdef DEBUG_ENCOUNTER
2168 std::cout <<
"-> Encounter type: Ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' follows foe '"
2169 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'"
2176#ifdef DEBUG_ENCOUNTER
2178 std::cout <<
"-> Encounter type: Ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' merges with foe '"
2179 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'"
2187 if (egoLane != egoConflictLane || foeLane != foeConflictLane) {
2191 if (egoLane == foeLane) {
2196#ifdef DEBUG_ENCOUNTER
2198 std::cout <<
"-> Encounter type: Ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' leads foe '"
2199 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'"
2206#ifdef DEBUG_ENCOUNTER
2208 std::cout <<
"-> Encounter type: Ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' follows foe '"
2209 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'"
2216#ifdef DEBUG_ENCOUNTER
2218 std::cout <<
" Lead/follow situation on consecutive internal lanes." << std::endl;
2225 if (egoLane == lane) {
2230 while (lane != foeLane) {
2236 egoConflictLane = lane;
2237#ifdef DEBUG_ENCOUNTER
2239 std::cout <<
"-> Encounter type: Ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' follows foe '"
2240 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'"
2245 }
else if (foeLane == lane) {
2250 while (lane != egoLane) {
2256 foeConflictLane = lane;
2257#ifdef DEBUG_ENCOUNTER
2259 std::cout <<
"-> Encounter type: Ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' leads foe '"
2260 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'"
2270#ifdef DEBUG_ENCOUNTER
2272 std::cout <<
"-> Encounter type: Lead/follow-situation on connection from '" << egoEntryLink->
getLaneBefore()->
getID()
2273 <<
"' to '" << egoEntryLink->
getLane()->
getID() <<
"'" << std::endl;
2280 const std::vector<MSLink*>& egoFoeLinks = egoEntryLink->
getFoeLinks();
2281 const std::vector<MSLink*>& foeFoeLinks = foeEntryLink->
getFoeLinks();
2283 bool crossOrMerge = (find(egoFoeLinks.begin(), egoFoeLinks.end(), foeEntryLink) != egoFoeLinks.end()
2284 || std::find(foeFoeLinks.begin(), foeFoeLinks.end(), egoEntryLink) != foeFoeLinks.end());
2285 if (!crossOrMerge) {
2294#ifdef DEBUG_ENCOUNTER
2296 std::cout <<
"-> Encounter type: No conflict.\n";
2321#ifdef DEBUG_ENCOUNTER
2323 std::cout <<
"-> Encounter type: Merging situation of ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' and foe '"
2324 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'"
2331#ifdef DEBUG_ENCOUNTER
2333 std::cout <<
"-> Encounter type: No conflict: " << type << std::endl;
2349 egoDistToConflictLane -= offset;
2351 foeDistToConflictLane -= offset;
2356 while (foeConflictLane !=
nullptr && foeConflictLane->
isInternal()) {
2365 egoDistToConflictFromJunctionEntry = 0;
2366 WRITE_WARNINGF(
TL(
"Cannot compute SSM due to bad internal lane geometry at junction '%'. Crossing point between traffic from links % and % not found."),
2373 assert(foeConflictLane !=
nullptr && foeConflictLane->
isInternal());
2380 while (egoConflictLane !=
nullptr && egoConflictLane->
isInternal()) {
2389 foeDistToConflictFromJunctionEntry = 0;
2390 WRITE_WARNINGF(
TL(
"Cannot compute SSM due to bad internal lane geometry at junction '%'. Crossing point between traffic from links % and % not found."),
2397 assert(egoConflictLane !=
nullptr && egoConflictLane->
isInternal());
2422 assert(angle <= 2 *
M_PI);
2426 assert(angle >= -
M_PI);
2427 assert(angle <=
M_PI);
2429 double crossingOrientation = (angle < 0) - (angle > 0);
2449#ifdef DEBUG_ENCOUNTER
2451 std::cout <<
" Determined exact conflict distances for crossing conflict."
2452 <<
"\n crossingOrientation=" << crossingOrientation
2455 <<
", relativeAngle=" << angle
2456 <<
" (foe from " << (crossingOrientation > 0 ?
"right)" :
"left)")
2457 <<
"\n resulting offset for conflict entry distance:"
2460 <<
"\n distToConflictLane:"
2461 <<
"\n ego=" << egoDistToConflictLane
2462 <<
", foe=" << foeDistToConflictLane
2463 <<
"\n distToConflictFromJunctionEntry:"
2464 <<
"\n ego=" << egoDistToConflictFromJunctionEntry
2465 <<
", foe=" << foeDistToConflictFromJunctionEntry
2466 <<
"\n resulting entry distances:"
2469 <<
"\n resulting exit distances:"
2474 std::cout <<
"real egoConflictLane: '" << (egoConflictLane == 0 ?
"NULL" : egoConflictLane->
getID()) <<
"'\n"
2475 <<
"real foeConflictLane: '" << (foeConflictLane == 0 ?
"NULL" : foeConflictLane->
getID()) <<
"'\n"
2476 <<
"-> Encounter type: Crossing situation of ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' and foe '"
2477 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'"
2495 std::cout <<
SIMTIME <<
" findFoeConflictLane() for foe '"
2497 <<
"' (with egoConflictLane=" << (egoConflictLane == 0 ?
"NULL" : egoConflictLane->
getID())
2507#ifdef DEBUG_SSM_OPPOSITE
2527 return egoConflictLane;
2536 return egoConflictLane;
2546 assert(foeLane->
isInternal() || *laneIter == foeLane);
2553 if (conflictJunction != 0) {
2554 std::cout <<
"Potential conflict on junction '" << conflictJunction->
getID()
2560 if (egoConflictLane !=
nullptr && egoConflictLane->
isInternal() && egoConflictLane->
getLinkCont()[0]->getViaLane() == foeLane) {
2561 distToConflictLane += egoConflictLane->
getLength();
2569 if (*laneIter ==
nullptr) {
2570 while (foeLane !=
nullptr && foeLane->
isInternal()) {
2571 distToConflictLane += foeLane->
getLength();
2572 foeLane = foeLane->
getLinkCont()[0]->getViaLane();
2575 assert(laneIter == foeBestLanesEnd || *laneIter != 0);
2579 while (laneIter != foeBestLanesEnd && distToConflictLane <=
myRange) {
2581 assert(*laneIter == foeLane || foeLane == 0);
2582 foeLane = *laneIter;
2587 std::cout <<
"Found conflict lane for foe: '" << foeLane->
getID() <<
"'" << std::endl;
2594 distToConflictLane += foeLane->
getLength();
2598 if (laneIter == foeBestLanesEnd) {
2601 MSLane*
const nextNonInternalLane = *laneIter;
2605 assert(foeLane == 0 || foeLane->
isInternal());
2606 if (foeLane ==
nullptr) {
2607 foeLane = nextNonInternalLane;
2611 assert(foeLane != 0);
2614 std::cout <<
"Found conflict lane for foe: '" << foeLane->
getID() <<
"'" << std::endl;
2622 foeLane = nextNonInternalLane;
2655 std::cout <<
SIMTIME <<
" flushGlobalMeasures() of vehicle '"
2729 std::cout <<
SIMTIME <<
" writeOutConflict() of vehicles '"
2818 std::string res =
"";
2819 for (std::vector<double>::const_iterator i = v.begin(); i != v.end(); ++i) {
2820 res += (i == v.begin() ?
"" :
" ") + (*i == NA ?
"NA" :
::toString(*i));
2827 std::string res =
"";
2828 for (std::vector<double>::const_iterator i = v.begin(); i != v.end(); ++i) {
2829 res += (i == v.begin() ?
"" :
" ") + (find(NAs.begin(), NAs.end(), *i) != NAs.end() ?
"NA" :
::toString(*i));
2837 std::string res =
"";
2838 for (PositionVector::const_iterator i = v.begin(); i != v.end(); ++i) {
2895 std::vector<std::string> measures;
2896 std::vector<double> threshVals;
2898 measures.push_back(i->first);
2899 threshVals.push_back(i->second);
2901 std::cout <<
"Initialized ssm device '" <<
id <<
"' with "
2924#ifdef DEBUG_SSM_NOTIFICATIONS
2940#ifdef DEBUG_SSM_NOTIFICATIONS
2954 double ,
double newSpeed) {
2955#ifdef DEBUG_SSM_NOTIFICATIONS
2958 std::cout <<
SIMTIME <<
"device '" <<
getID() <<
"' notifyMove: newSpeed=" << newSpeed <<
"\n";
2973#ifdef DEBUG_SSM_SURROUNDING
2977 std::cout <<
SIMTIME <<
" Looking for surrounding vehicles for ego vehicle '" << veh.
getID()
3003 std::vector<MSLane*>::const_iterator laneIter = egoBestLanes.begin();
3004 assert(lane->
isInternal() || lane == *laneIter || isOpposite);
3006 if (lane->
isInternal() && egoBestLanes[0] !=
nullptr) {
3011 for (
int i = 0; i < (int)egoBestLanes.size(); i++) {
3012 if (egoBestLanes[i] !=
nullptr && egoBestLanes[i]->getEdge().getOppositeEdge() !=
nullptr) {
3013 egoBestLanes[i] = egoBestLanes[i]->getEdge().getOppositeEdge()->getLanes().back();
3019 const MSLane* nextNonInternalLane =
nullptr;
3027 double remainingDownstreamRange = range;
3029 double distToConflictLane = isOpposite ? pos - veh.
getLane()->
getLength() : -pos;
3032 std::set<const MSLane*> seenLanes;
3033 std::set<const MSJunction*> routeJunctions;
3037 std::vector<UpstreamScanStartInfo> upstreamScanStartPositions;
3046#ifdef DEBUG_SSM_SURROUNDING
3048 std::cout <<
SIMTIME <<
" Vehicle '" << veh.
getID() <<
"' is on internal edge " << edge->
getID() <<
"'." << std::endl;
3058 routeJunctions.insert(junction);
3064 for (ConstMSEdgeVector::const_iterator ei = incoming.begin(); ei != incoming.end(); ++ei) {
3065 if ((*ei)->isInternal()) {
3084 lane = *(++laneIter);
3090 double startScanPos = std::min(pos + remainingDownstreamRange, edgeLength);
3091 upstreamScanStartPositions.push_back(
UpstreamScanStartInfo(edge, startScanPos, std::max(0., startScanPos - pos + range + veh.
getLength()), distToConflictLane, lane));
3100 while (remainingDownstreamRange > 0.) {
3102#ifdef DEBUG_SSM_SURROUNDING
3104 std::cout <<
SIMTIME <<
" Scanning downstream for vehicle '" << veh.
getID() <<
"' on lane '" << veh.
getLane()->
getID() <<
"', position=" << pos <<
".\n"
3105 <<
"Considering edge '" << edge->
getID() <<
"' Remaining downstream range = " << remainingDownstreamRange
3106 <<
"\nbestLanes=" <<
::toString(egoBestLanes) <<
"\n"
3112 assert(pos == 0 || lane == veh.
getLane());
3113 if (pos + remainingDownstreamRange < lane->getLength()) {
3116 upstreamScanStartPositions.push_back(
UpstreamScanStartInfo(edge, pos + remainingDownstreamRange, remainingDownstreamRange, distToConflictLane, lane));
3127 remainingDownstreamRange -= lane->
getLength() - pos;
3128 distToConflictLane += lane->
getLength();
3133 assert(laneIter == egoBestLanes.end() || *laneIter != 0);
3136 if (laneIter != egoBestLanes.end()) {
3147 nextNonInternalLane = *laneIter;
3149 if (isOpposite && link ==
nullptr) {
3150 link = nextNonInternalLane->
getLinkTo(lane);
3151 if (link ==
nullptr) {
3155 if (link ==
nullptr) {
3162 if (lane ==
nullptr) {
3164 lane = nextNonInternalLane;
3166 if (seenLanes.count(lane) == 0) {
3167 seenLanes.insert(lane);
3174 if (seenLanes.count(lane) == 0) {
3177 routeJunctions.insert(junction);
3183 for (ConstMSEdgeVector::const_iterator ei = outgoing.begin(); ei != outgoing.end(); ++ei) {
3187 upstreamScanStartPositions.push_back(
UpstreamScanStartInfo(*ei, (*ei)->getLength(), range, distToConflictLane, lane));
3191 for (ConstMSEdgeVector::const_iterator ei = incoming.begin(); ei != incoming.end(); ++ei) {
3195 upstreamScanStartPositions.push_back(
UpstreamScanStartInfo(*ei, (*ei)->getLength(), range, distToConflictLane, lane));
3201 remainingDownstreamRange -= linkLength;
3202 distToConflictLane += linkLength;
3203#ifdef DEBUG_SSM_SURROUNDING
3205 std::cout <<
" Downstream Scan for vehicle '" << veh.
getID() <<
"' proceeded over junction '" << junction->
getID()
3206 <<
"',\n linkLength=" << linkLength <<
", remainingDownstreamRange=" << remainingDownstreamRange
3212 lane = nextNonInternalLane;
3215#ifdef DEBUG_SSM_SURROUNDING
3217 std::cout <<
" Downstream Scan for vehicle '" << veh.
getID() <<
"' stops at lane '" << lane->
getID()
3218 <<
"', which has already been scanned."
3239#ifdef DEBUG_SSM_SURROUNDING
3241 for (std::pair<const MSVehicle*, FoeInfo*> foeInfo : foeCollector) {
3242 std::cout <<
" foe " << foeInfo.first->getID() <<
" conflict at " << foeInfo.second->egoConflictLane->getID() <<
" egoDist " << foeInfo.second->egoDistToConflictLane << std::endl;
3248 const auto& it = foeCollector.find(&veh);
3249 if (it != foeCollector.end()) {
3251 foeCollector.erase(it);
3259#ifdef DEBUG_SSM_SURROUNDING
3261 std::cout <<
SIMTIME <<
" getUpstreamVehicles() for edge '" << scanStart.
edge->
getID() <<
"'"
3263 <<
" pos = " << scanStart.
pos <<
" range = " << scanStart.
range
3267 if (scanStart.
range <= 0) {
3273 if (seenLanes.find(lane) != seenLanes.end()) {
3277 for (
MSVehicle*
const veh : lane->getVehiclesSecure()) {
3278 if (foeCollector.find(veh) != foeCollector.end()) {
3282 if (veh->getPositionOnLane() - veh->getLength() <= scanStart.
pos && veh->getPositionOnLane() >= scanStart.
pos - scanStart.
range) {
3283#ifdef DEBUG_SSM_SURROUNDING
3285 std::cout <<
"\t" << veh->getID() <<
"\n";
3291 foeCollector[veh] = c;
3295 lane->releaseVehicles();
3297#ifdef DEBUG_SSM_SURROUNDING
3299 std::cout <<
"\t" << lane->getID() <<
": Found " << foundCount <<
"\n";
3302 seenLanes.insert(lane);
3305#ifdef DEBUG_SSM_SURROUNDING
3307 std::cout << std::endl;
3315 if (scanStart.
range <= scanStart.
pos) {
3320 double remainingRange = scanStart.
range - scanStart.
pos;
3326 if (routeJunctions.find(junction) != routeJunctions.end()) {
3331 int incomingEdgeCount = 0;
3339 if (internalLane->getEdge().getSuccessors()[0]->getID() == scanStart.
edge->
getID()) {
3341 incomingEdgeCount++;
3346 if (incomingEdgeCount > 0) {
3348 if (inEdge->isInternal() || inEdge->isCrossing()) {
3352 for (
MSLane*
const lane : inEdge->getLanes()) {
3353 if (seenLanes.find(lane) != seenLanes.end()) {
3359#ifdef DEBUG_SSM_SURROUNDING
3367 if (distOnJunction >= remainingRange) {
3368#ifdef DEBUG_SSM_SURROUNDING
3383#ifdef DEBUG_SSM_SURROUNDING
3385 std::cout <<
SIMTIME <<
" getVehiclesOnJunction() for junction '" << junction->
getID()
3387 <<
"\nFound vehicles:"
3394 if (foeCollector.find(veh) != foeCollector.end()) {
3395 delete foeCollector[veh];
3400 foeCollector[veh] = c;
3401#ifdef DEBUG_SSM_SURROUNDING
3403 std::cout <<
"\t" << veh->getID() <<
" egoConflictLane=" <<
Named::getIDSecure(egoConflictLane) <<
"\n";
3410 if (seenLanes.find(egoJunctionLane) != seenLanes.end() || egoJunctionLane->
getEdge().
isCrossing()) {
3414 auto scanInternalLane = [&](
const MSLane * lane) {
3419 collectFoeInfos(vehicles);
3421 lane->releaseVehicles();
3425 if (lane->getCanonicalPredecessorLane()->isInternal()) {
3426 lane = lane->getCanonicalPredecessorLane();
3429 assert(!lane->getEntryLink()->fromInternalLane());
3434 collectFoeInfos(vehicles2);
3435 lane->releaseVehicles();
3440 if (lane->getLinkCont().size() > 1 && lane->getLinkCont()[0]->getViaLane() !=
nullptr) {
3442 lane = lane->getLinkCont()[0]->getViaLane();
3444 assert(lane->getLinkCont().size() == 0 || lane->getLinkCont()[0]->getViaLane() == 0);
3449 collectFoeInfos(vehicles2);
3450 lane->releaseVehicles();
3460 for (
MSLane* lane : foeLanes) {
3461 if (seenLanes.find(lane) != seenLanes.end()) {
3464 scanInternalLane(lane);
3465 seenLanes.insert(lane);
3468 scanInternalLane(egoJunctionLane);
3470#ifdef DEBUG_SSM_SURROUNDING
3472 std::cout << std::endl;
3492 std::string file = deviceID +
".xml";
3506 file = oc.
getString(
"device.ssm.file") ==
"" ? file : oc.
getString(
"device.ssm.file");
3508 WRITE_MESSAGEF(
TL(
"Vehicle '%' does not supply vehicle parameter 'device.ssm.file'. Using default of '%'."), v.
getID(), file);
3526 bool useGeo =
false;
3540 useGeo = oc.
getBool(
"device.ssm.geo");
3553 bool writePos =
false;
3567 writePos = oc.
getBool(
"device.ssm.write-positions");
3569 WRITE_MESSAGEF(
TL(
"Vehicle '%' does not supply vehicle parameter 'device.ssm.write-positions'. Using default of '%'."), v.
getID(),
toString(writePos));
3580 bool writeLanesPos =
false;
3594 writeLanesPos = oc.
getBool(
"device.ssm.write-lane-positions");
3596 WRITE_MESSAGEF(
TL(
"Vehicle '%' does not supply vehicle parameter 'device.ssm.write-positions'. Using default of '%'."), v.
getID(),
toString(writeLanesPos));
3600 return writeLanesPos;
3621 range = oc.
getFloat(
"device.ssm.range");
3648 extraTime = oc.
getFloat(
"device.ssm.extratime");
3650 WRITE_MESSAGEF(
TL(
"Vehicle '%' does not supply vehicle parameter 'device.ssm.extratime'. Using default of '%'."), v.
getID(),
toString(extraTime));
3654 if (extraTime < 0.) {
3656 WRITE_WARNINGF(
TL(
"Negative (or no) value encountered for vehicle parameter 'device.ssm.extratime' in vehicle '%' using default value % instead."), v.
getID(),
::toString(extraTime));
3665 bool trajectories =
false;
3679 trajectories = oc.
getBool(
"device.ssm.trajectories");
3681 WRITE_MESSAGEF(
TL(
"Vehicle '%' does not supply vehicle parameter 'device.ssm.trajectories'. Using default of '%'."), v.
getID(),
toString(trajectories));
3685 return trajectories;
3694 std::string measures_str =
"";
3708 measures_str = oc.
getString(
"device.ssm.measures");
3710 WRITE_MESSAGEF(
TL(
"Vehicle '%' does not supply vehicle parameter 'device.ssm.measures'. Using default of '%'."), v.
getID(), measures_str);
3716 if (measures_str ==
"") {
3717 WRITE_WARNINGF(
"No measures specified for ssm device of vehicle '%'. Registering all available SSMs.", v.
getID());
3721 std::vector<std::string> available = st.
getVector();
3723 std::vector<std::string> measures = st.
getVector();
3724 for (std::vector<std::string>::const_iterator i = measures.begin(); i != measures.end(); ++i) {
3725 if (std::find(available.begin(), available.end(), *i) == available.end()) {
3727 WRITE_ERRORF(
TL(
"SSM identifier '%' is not supported. Aborting construction of SSM device '%'."), *i, deviceID);
3733 std::string thresholds_str =
"";
3747 thresholds_str = oc.
getString(
"device.ssm.thresholds");
3749 WRITE_MESSAGEF(
TL(
"Vehicle '%' does not supply vehicle parameter 'device.ssm.thresholds'. Using default of '%'."), v.
getID(), thresholds_str);
3756 if (thresholds_str !=
"") {
3758 while (count < (
int)measures.size() && st.
hasNext()) {
3760 thresholds.insert(std::make_pair(measures[count], thresh));
3763 if (thresholds.size() < measures.size() || st.
hasNext()) {
3764 WRITE_ERRORF(
TL(
"Given list of thresholds ('%') is not of the same size as the list of measures ('%').\nPlease specify exactly one threshold for each measure."), thresholds_str, measures_str);
3769 for (std::vector<std::string>::const_iterator i = measures.begin(); i != measures.end(); ++i) {
3772 }
else if (*i ==
"DRAC") {
3774 }
else if (*i ==
"PET") {
3776 }
else if (*i ==
"BR") {
3778 }
else if (*i ==
"SGAP") {
3780 }
else if (*i ==
"TGAP") {
3783 WRITE_ERROR(
"Unknown SSM identifier '" + (*i) +
"'. Aborting construction of ssm device.");
3803 if (key ==
"minTTC" ||
3811 minTTC =
MIN2(minTTC, e->minTTC.value);
3812 minPET =
MIN2(minPET, e->PET.value);
3813 maxDRAC =
MAX2(maxDRAC, e->maxDRAC.value);
3815 if (key ==
"minTTC") {
3817 }
else if (key ==
"maxDRAC") {
3819 }
else if (key ==
"minPET") {
3840 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_MESSAGEF(...)
#define WRITE_ERRORF(...)
#define WRITE_WARNING(msg)
std::string time2string(SUMOTime t)
convert SUMOTime to string
@ SVC_IGNORING
vehicles ignoring classes
int gPrecision
the precision for floating point outputs
const double INVALID_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.
static const GeoConvHelper & getFinal()
the coordinate transformation for writing the location element and for tracking the original coordina...
void cartesian2geo(Position &cartesian) const
Converts the given cartesian (shifted) position to its geo (lat/long) representation.
The base class for microscopic and mesoscopic vehicles.
const MSRouteIterator & getCurrentRouteEdge() const
Returns an iterator pointing to the current edge in this vehicles route.
double getLength() const
Returns the vehicle's length.
const MSEdge * getEdge() const
Returns the edge the vehicle is currently at.
double getWidth() const
Returns the vehicle's width.
const MSRoute & getRoute() const
Returns the current route.
const MSVehicleType & getVehicleType() const
Returns the vehicle's type definition.
static double passingTime(const double lastPos, const double passedPos, const double currentPos, const double lastSpeed, const double currentSpeed)
Calculates the time at which the position passedPosition has been passed In case of a ballistic updat...
static double estimateArrivalTime(double dist, double speed, double maxSpeed, double accel)
Computes the time needed to travel a distance dist given an initial speed and constant acceleration....
An encounter is an episode involving two vehicles, which are closer to each other than some specified...
EncounterType currentType
double foeConflictEntryTime
Times when the foe vehicle entered/left the conflict area. Currently only applies for crossing situat...
std::vector< double > foeDistsToConflict
Evolution of the foe vehicle's distance to the conflict point.
std::vector< double > timeSpan
time points corresponding to the trajectories
std::vector< int > typeSpan
Evolution of the encounter classification (.
bool closingRequested
this flag is set by updateEncounter() or directly in processEncounters(), where encounters are closed...
std::vector< double > TTCspan
All values for TTC.
std::size_t size() const
Returns the number of trajectory points stored.
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
void add(double time, EncounterType type, Position egoX, std::string egoLane, double egoLanePos, Position egoV, Position foeX, std::string foeLane, double foeLanePos, Position foeV, Position conflictPoint, double egoDistToConflict, double foeDistToConflict, double ttc, double drac, std::pair< double, double > pet)
add a new data point and update encounter type
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)
MSDevice_SSM(SUMOVehicle &holder, const std::string &id, std::string outputFilename, std::map< std::string, double > thresholds, bool trajectories, double range, double extraTime, bool useGeoCoords, bool writePositions, bool writeLanesPositions)
Constructor.
std::map< const MSVehicle *, FoeInfo * > FoeInfoMap
double myExtraTime
Extra time in seconds to be logged after a conflict is over.
void generateOutput(OutputDevice *tripinfoOut) const
Finalizes output. Called on vehicle removal.
std::pair< std::pair< std::pair< double, Position >, double >, std::string > myMinTGAP
bool myComputeTTC
Flags for switching on / off comutation of different SSMs, derived from myMeasures.
PositionVector myGlobalMeasuresPositions
All values for positions (coordinates)
static std::set< std::string > myCreatedOutputFiles
remember which files were created already (don't duplicate xml root-elements)
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
static void initEdgeFilter()
initialize edge filter (once)
std::vector< double > myGlobalMeasuresLanesPositions
All values for positions on the lanes.
bool notifyMove(SUMOTrafficObject &veh, double oldPos, double newPos, double newSpeed)
Checks for waiting steps when the vehicle moves.
static void determineConflictPoint(EncounterApproachInfo &eInfo)
Calculates the (x,y)-coordinate for the eventually predicted conflict point and stores the result in ...
static double computeDRAC(double gap, double followerSpeed, double leaderSpeed)
Computes the DRAC (deceleration to avoid a collision) for a lead/follow situation as defined,...
EncounterQueue myPastConflicts
Past encounters that where qualified as conflicts and are not yet flushed to the output file.
static bool useGeoCoords(const SUMOVehicle &v)
void setParameter(const std::string &key, const std::string &value)
try to set the given parameter for this device. Throw exception for unsupported key
static bool myEdgeFilterInitialized
static const std::set< MSDevice_SSM *, ComparatorNumericalIdLess > & getInstances()
returns all currently existing SSM devices
void closeEncounter(Encounter *e)
Finalizes the encounter and calculates SSM values.
static std::string makeStringWithNAs(const std::vector< double > &v, const double NA)
make a string of a double vector and treat a special value as invalid ("NA")
static bool writePositions(const SUMOVehicle &v)
static double getDetectionRange(const SUMOVehicle &v)
static void cleanup()
Clean up remaining devices instances.
static void insertOptions(OptionsCont &oc)
Inserts MSDevice_SSM-options.
double myRange
Detection range. For vehicles closer than this distance from the ego vehicle, SSMs are traced.
const MSLane * findFoeConflictLane(const MSVehicle *foe, const MSLane *egoConflictLane, double &distToConflictLane) const
Computes the conflict lane for the foe.
std::vector< std::string > myGlobalMeasuresLaneIDs
All values for lanes.
static int myIssuedParameterWarnFlags
bitset storing info whether warning has already been issued about unset parameter (warn only once!...
bool notifyLeave(SUMOTrafficObject &veh, double lastPos, MSMoveReminder::Notification reason, const MSLane *enteredLane=0)
Called whenever the holder leaves a lane.
std::vector< Encounter * > EncounterVector
static void getUpstreamVehicles(const UpstreamScanStartInfo &scanStart, FoeInfoMap &foeCollector, std::set< const MSLane * > &seenLanes, const std::set< const MSJunction * > &routeJunctions)
Collects all vehicles within range 'range' upstream of the position 'pos' on the edge 'edge' into foe...
void createEncounters(FoeInfoMap &foes)
Makes new encounters for all given vehicles (these should be the ones entering the device's range in ...
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 std::set< const MSEdge * > myEdgeFilter
spatial filter for SSM device output
static void toGeo(Position &x)
convert SUMO-positions to geo coordinates (in place)
static void findSurroundingVehicles(const MSVehicle &veh, double range, FoeInfoMap &foeCollector)
Returns all vehicles, which are within the given range of the given vehicle.
bool myWritePositions
Wether to print the positions for all timesteps.
void resetEncounters()
Closes all current Encounters and moves conflicts to myPastConflicts,.
std::pair< std::pair< double, Position >, double > myMaxBR
Extremal values for the global measures (as <<<time, Position>, value>, [leaderID]>-pairs)
std::vector< double > myBRspan
All values for brake rate.
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.
bool myWriteLanesPositions
Wether to print the lanes and positions for all timesteps and conflicts.
~MSDevice_SSM()
Destructor.
const std::string deviceName() const
return the name for this type of device
static bool myEdgeFilterActive
void flushGlobalMeasures()
Write out all non-encounter specific measures as headways and braking rates.
std::vector< double > myTGAPspan
All values for time gap.
static void getVehiclesOnJunction(const MSJunction *, const MSLane *egoJunctionLane, double egoDistToConflictLane, const MSLane *const egoConflictLane, FoeInfoMap &foeCollector, std::set< const MSLane * > &seenLanes)
Collects all vehicles on the junction into foeCollector.
static void estimateConflictTimes(EncounterApproachInfo &eInfo)
Estimates the time until conflict for the vehicles based on the distance to the conflict entry points...
bool notifyEnter(SUMOTrafficObject &veh, MSMoveReminder::Notification reason, const MSLane *enteredLane=0)
Called whenever the holder enteres a lane.
void updatePassedEncounter(Encounter *e, FoeInfo *foeInfo, EncounterApproachInfo &eInfo)
Updates an encounter, which was classified as ENCOUNTER_TYPE_NOCONFLICT_AHEAD this may be the case be...
void processEncounters(FoeInfoMap &foes, bool forceClose=false)
Finds encounters for which the foe vehicle has disappeared from range. remainingExtraTime is decrease...
static bool writeLanesPositions(const SUMOVehicle &v)
std::vector< double > mySGAPspan
All values for space gap.
static void insertDefaultAssignmentOptions(const std::string &deviceName, const std::string &optionsTopic, OptionsCont &oc, const bool isPerson=false)
Adds common command options that allow to assign devices to vehicles.
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 std::vector< MSLane * > & getLanes() const
Returns this edge's lanes.
const MSEdge * getOppositeEdge() const
Returns the opposite direction edge if on exists else a nullptr.
const MSJunction * getToJunction() const
double getLength() const
return the length of the edge
const MSJunction * getFromJunction() const
bool isInternal() const
return whether this edge is an internal edge
static bool dictionary(const std::string &id, MSEdge *edge)
Inserts edge into the static dictionary Returns true if the key id isn't already in the dictionary....
The base class for an intersection.
virtual const std::vector< MSLane * > & getFoeInternalLanes(const MSLink *const) const
virtual const std::vector< MSLane * > getInternalLanes() const
Returns all internal lanes on the junction.
const ConstMSEdgeVector & getOutgoing() const
const ConstMSEdgeVector & getIncoming() const
Representation of a lane in the micro simulation.
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.
const std::vector< IncomingLaneInfo > & getIncomingLanes() const
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
virtual const PositionVector & getShape(bool) const
MSLane * getParallelOpposite() const
return the opposite direction lane of this lanes edge or nullptr
MSEdge & getEdge() const
Returns the lane's edge.
double getWidth() const
Returns the lane's width.
const std::vector< MSLink * > & getLinkCont() const
returns the container with all links !!!
const std::vector< const MSLane * > & getFoeLanes() const
const MSLane * getInternalLaneBefore() const
return myInternalLaneBefore (always 0 when compiled without internal lanes)
MSJunction * getJunction() const
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
const std::vector< MSLink * > & getFoeLinks() const
bool isInternalJunctionLink() const
return whether the fromLane and the toLane of this link are internal lanes
MSLane * getViaLane() const
Returns the following inner lane.
double getInternalLengthsAfter() const
Returns the cumulative length of all internal lanes after this link.
double getLengthsBeforeCrossing(const MSLane *foeLane) const
Returns the sum of the lengths along internal lanes following this link to the crossing with the give...
const MSLink * getCorrespondingExitLink() const
returns the corresponding exit link for entryLinks to a junction.
Notification
Definition of a vehicle state.
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
MSJunctionControl & getJunctionControl()
Returns the junctions control.
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.
const MSLane * getLane() const
Returns the lane the vehicle is on.
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.
const MSCFModel & getCarFollowModel() const
Returns the vehicle's car following model definition.
double getPositionOnLane() const
Get the vehicle's position along the lane.
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.
T get(const std::string &id) const
Retrieves an item.
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.
double getFloat(const std::string &name) const
Returns the double-value of the named option (only for Option_Float)
std::string getString(const std::string &name) const
Returns the string-value of the named option (only for Option_String)
bool isDefault(const std::string &name) const
Returns the information whether the named option has still the default value.
void doRegister(const std::string &name, Option *o)
Adds an option under the given name.
void addOptionSubTopic(const std::string &topic)
Adds an option subtopic.
bool getBool(const std::string &name) const
Returns the boolean-value of the named option (only for Option_Bool)
static OptionsCont & getOptions()
Retrieves the options.
Static storage of an output device and its base (abstract) implementation.
OutputDevice & writeAttr(const SumoXMLAttr attr, const T &val)
writes a named attribute
OutputDevice & openTag(const std::string &xmlElement)
Opens an XML tag.
bool closeTag(const std::string &comment="")
Closes the most recently opened tag and optionally adds a comment.
static OutputDevice & getDevice(const std::string &name, bool usePrefix=true)
Returns the described OutputDevice.
bool writeXMLHeader(const std::string &rootElement, const std::string &schemaFile, std::map< SumoXMLAttr, std::string > attrs=std::map< SumoXMLAttr, std::string >(), bool includeConfig=true)
Writes an XML header with optional configuration.
virtual const std::string getParameter(const std::string &key, const std::string defaultValue="") const
Returns the value for a given key.
bool knowsParameter(const std::string &key) const
Returns whether the parameter is known.
A point in 2D or 3D with translation and scaling methods.
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 MSLane * getLane() const =0
Returns the lane the object is currently at.
virtual const SUMOVehicleParameter & getParameter() const =0
Returns the vehicle's parameter (including departure definition)
virtual const MSEdge * getEdge() const =0
Returns the edge the object is currently at.
Representation of a vehicle.
virtual bool isOnRoad() const =0
Returns the information whether the vehicle is on a road (is simulated)
virtual const ConstMSEdgeVector::const_iterator & getCurrentRouteEdge() const =0
Returns an iterator pointing to the current edge in this vehicles route.
virtual const MSRoute & getRoute() const =0
Returns the current route.
std::vector< std::string > getVector()
return vector of strings
bool hasNext()
returns the information whether further substrings exist
std::string next()
returns the next substring when it exists. Otherwise the behaviour is undefined
static std::string urlDecode(const std::string &encoded)
static double toDouble(const std::string &sData)
converts a string into the double value described by it by calling the char-type converter
static bool startsWith(const std::string &str, const std::string prefix)
Checks whether a given string starts with the prefix.
static bool toBool(const std::string &sData)
converts a string into the bool value described by it by calling the char-type converter
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
std::vector< std::string > lane
std::vector< double > lanePos
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