52 #define DEBUG_SSM_SURROUNDING
56 #define DEBUG_COND(ego) (ego!=nullptr && ego->isSelected())
57 #define DEBUG_COND_FIND(ego) (ego.isSelected())
58 #define DEBUG_EGO_ID "EW.3"
59 #define DEBUG_FOE_ID "WE.0"
70 #define INVALID std::numeric_limits<double>::max()
73 #define DEFAULT_RANGE 50.0
79 #define AVAILABLE_SSMS "TTC DRAC PET BR SGAP TGAP"
81 #define DEFAULT_THRESHOLD_TTC 3. // in [s.], events get logged if time to collision is below threshold (1.5s. is an appropriate criticality threshold according to Van der Horst, A. R. A. (1991). Time-to-collision as a Cue for Decision-making in Braking [also see Guido et al. 2011])
82 #define DEFAULT_THRESHOLD_DRAC 3. // in [m/s^2], events get logged if "deceleration to avoid a crash" is above threshold (3.4s. is an appropriate criticality threshold according to American Association of State Highway and Transportation Officials (2004). A Policy on Geometric Design of Highways and Streets [also see Guido et al. 2011])
83 #define DEFAULT_THRESHOLD_PET 2. // in seconds, events get logged if post encroachment time is below threshold
85 #define DEFAULT_THRESHOLD_BR 0.0 // in [m/s^2], events get logged if brake rate is above threshold
86 #define DEFAULT_THRESHOLD_SGAP 0.2 // in [m.], events get logged if the space headway is below threshold.
87 #define DEFAULT_THRESHOLD_TGAP 0.5 // in [m.], events get logged if the time headway is below threshold.
89 #define DEFAULT_EXTRA_TIME 5. // in seconds, events get logged for extra time even if encounter is over
101 out <<
"NOCONFLICT_AHEAD";
107 out <<
"FOLLOWING_FOLLOWER";
110 out <<
"FOLLOWING_LEADER";
113 out <<
"ON_ADJACENT_LANES";
119 out <<
"MERGING_LEADER";
122 out <<
"MERGING_FOLLOWER";
125 out <<
"MERGING_ADJACENT";
131 out <<
"CROSSING_LEADER";
134 out <<
"CROSSING_FOLLOWER";
137 out <<
"EGO_ENTERED_CONFLICT_AREA";
140 out <<
"FOE_ENTERED_CONFLICT_AREA";
143 out <<
"BOTH_ENTERED_CONFLICT_AREA";
146 out <<
"EGO_LEFT_CONFLICT_AREA";
149 out <<
"FOE_LEFT_CONFLICT_AREA";
152 out <<
"BOTH_LEFT_CONFLICT_AREA";
155 out <<
"FOLLOWING_PASSED";
158 out <<
"MERGING_PASSED";
168 out <<
"unknown type (" << int(type) <<
")";
179 std::set<MSDevice_SSM*, ComparatorNumericalIdLess>*
MSDevice_SSM::myInstances =
new std::set<MSDevice_SSM*, ComparatorNumericalIdLess>();
185 const std::set<MSDevice_SSM*, ComparatorNumericalIdLess>&
195 device->resetEncounters();
196 device->flushConflicts(
true);
197 device->flushGlobalMeasures();
213 oc.
doRegister(
"device.ssm.measures", Option::makeUnsetWithDefault<Option_String, std::string>(
""));
214 oc.
addDescription(
"device.ssm.measures",
"SSM Device",
"Specifies which measures will be logged (as a space separated sequence of IDs in ('TTC', 'DRAC', 'PET')).");
215 oc.
doRegister(
"device.ssm.thresholds", Option::makeUnsetWithDefault<Option_String, std::string>(
""));
216 oc.
addDescription(
"device.ssm.thresholds",
"SSM Device",
"Specifies thresholds corresponding to the specified measures (see documentation and watch the order!). Only events exceeding the thresholds will be logged.");
217 oc.
doRegister(
"device.ssm.trajectories", Option::makeUnsetWithDefault<Option_Bool, bool>(
false));
218 oc.
addDescription(
"device.ssm.trajectories",
"SSM Device",
"Specifies whether trajectories will be logged (if false, only the extremal values and times are reported, this is the default).");
220 oc.
addDescription(
"device.ssm.range",
"SSM Device",
"Specifies the detection range in meters (default is " + ::
toString(
DEFAULT_RANGE) +
"m.). For vehicles below this distance from the equipped vehicle, SSM values are traced.");
222 oc.
addDescription(
"device.ssm.extratime",
"SSM Device",
"Specifies the time in seconds to be logged after a conflict is over (default is " + ::
toString(
DEFAULT_EXTRA_TIME) +
"secs.). Required >0 if PET is to be calculated for crossing conflicts.");
223 oc.
doRegister(
"device.ssm.file", Option::makeUnsetWithDefault<Option_String, std::string>(
""));
224 oc.
addDescription(
"device.ssm.file",
"SSM Device",
"Give a global default filename for the SSM output.");
225 oc.
doRegister(
"device.ssm.geo", Option::makeUnsetWithDefault<Option_Bool, bool>(
false));
226 oc.
addDescription(
"device.ssm.geo",
"SSM Device",
"Whether to use coordinates of the original reference system in output (default is false).");
233 WRITE_WARNING(
"SSM Device for vehicle '" + v.
getID() +
"' will not be built. (SSMs not supported in MESO)");
237 std::string deviceID =
"ssm_" + v.
getID();
242 std::map<std::string, double> thresholds;
265 into.push_back(device);
273 egoID(_ego->getID()),
274 foeID(_foe->getID()),
277 currentType(ENCOUNTER_TYPE_NOCONFLICT_AHEAD),
278 remainingExtraTime(extraTime),
286 closingRequested(false) {
287 #ifdef DEBUG_ENCOUNTER
288 if (DEBUG_COND_ENCOUNTER(
this)) {
289 std::cout <<
"\n" <<
SIMTIME <<
" Constructing encounter of '" <<
ego->
getID() <<
"' and '" <<
foe->
getID() <<
"'" << std::endl;
295 #ifdef DEBUG_ENCOUNTER
296 if (DEBUG_COND_ENCOUNTER(
this)) {
297 std::cout <<
"\n" <<
SIMTIME <<
" Destroying encounter of '" << egoID <<
"' and '" << foeID <<
"' (begin was " << begin <<
")" << std::endl;
305 Position conflictPoint,
double egoDistToConflict,
double foeDistToConflict,
double ttc,
double drac, std::pair<double, double> pet) {
306 #ifdef DEBUG_ENCOUNTER
307 if (DEBUG_COND_ENCOUNTER(
this))
308 std::cout << time <<
" Adding data point for encounter of '" << egoID <<
"' and '" << foeID <<
"':\n"
309 <<
"type=" << type <<
", egoDistToConflict=" << (egoDistToConflict ==
INVALID ?
"NA" :
::toString(egoDistToConflict))
310 <<
", foeDistToConflict=" << (foeDistToConflict ==
INVALID ?
"NA" : ::
toString(foeDistToConflict))
318 timeSpan.push_back(time);
319 typeSpan.push_back(type);
320 egoTrajectory.x.push_back(egoX);
321 egoTrajectory.v.push_back(egoV);
322 foeTrajectory.x.push_back(foeX);
323 foeTrajectory.v.push_back(foeV);
324 conflictPointSpan.push_back(conflictPoint);
325 egoDistsToConflict.push_back(egoDistToConflict);
326 foeDistsToConflict.push_back(foeDistToConflict);
328 TTCspan.push_back(ttc);
329 if (ttc !=
INVALID && (ttc < minTTC.value || minTTC.value ==
INVALID)) {
332 minTTC.pos = conflictPoint;
336 DRACspan.push_back(drac);
337 if (drac !=
INVALID && (drac > maxDRAC.value || maxDRAC.value ==
INVALID)) {
338 maxDRAC.value = drac;
340 maxDRAC.pos = conflictPoint;
344 if (pet.first !=
INVALID && (PET.value >= pet.second || PET.value ==
INVALID)) {
345 PET.value = pet.second;
346 PET.time = pet.first;
347 PET.pos = conflictPoint;
355 remainingExtraTime = value;
361 remainingExtraTime -= amount;
367 return remainingExtraTime;
374 conflictPoint(
Position::invalidPosition()),
379 egoEstimatedConflictEntryTime(
INVALID),
380 foeEstimatedConflictEntryTime(
INVALID),
381 egoEstimatedConflictExitTime(
INVALID),
382 foeEstimatedConflictExitTime(
INVALID),
383 egoConflictAreaLength(
INVALID),
384 foeConflictAreaLength(
INVALID),
385 egoLeftConflict(false),
386 foeLeftConflict(false),
402 std::cout <<
"\n" <<
SIMTIME <<
" Device '" <<
getID() <<
"' updateAndWriteOutput()\n"
403 <<
" Holder is off-road! Calling resetEncounters()."
416 std::cout <<
"\n" <<
SIMTIME <<
" Device '" <<
getID() <<
"' update()\n"
427 if (foes.size() > 0) {
428 std::cout <<
"Scanned surroundings: Found potential foes:\n";
429 for (FoeInfoMap::const_iterator i = foes.begin(); i != foes.end(); ++i) {
430 std::cout << i->first->getID() <<
" ";
432 std::cout << std::endl;
434 std::cout <<
"Scanned surroundings: No potential conflict could be identified." << std::endl;
464 double leaderSearchDist = 0;
465 std::pair<const MSVehicle*, double> leader(
nullptr, 0.);
473 if (leaderSearchDist > 0.) {
478 if (leader.first ==
nullptr) {
481 double sgap = leader.second + leader.first->getVehicleType().getMinGap();
493 const double tgap = (leader.second + leader.first->getVehicleType().getMinGap()) /
myHolderMS->
getSpeed();
509 std::cout <<
"\n" <<
SIMTIME <<
" Device '" <<
getID() <<
"' createEncounters()" << std::endl;
510 std::cout <<
"New foes:\n";
511 for (FoeInfoMap::const_iterator vi = foes.begin(); vi != foes.end(); ++vi) {
512 std::cout << vi->first->getID() <<
"\n";
514 std::cout << std::endl;
518 for (FoeInfoMap::const_iterator foe = foes.begin(); foe != foes.end(); ++foe) {
525 assert(myOldestActiveEncounterBegin <= e->begin);
548 std::cout <<
"\n" <<
SIMTIME <<
" Device '" <<
getID() <<
"' processEncounters(forceClose = " << forceClose <<
")" << std::endl;
549 std::cout <<
"Currently present foes:\n";
550 for (FoeInfoMap::const_iterator vi = foes.begin(); vi != foes.end(); ++vi) {
551 std::cout << vi->first->getID() <<
"\n";
553 std::cout << std::endl;
569 if (foes.find(e->
foe) != foes.end()) {
581 std::cout <<
" Requesting encounter closure because both left conflict area of previous encounter but another encounter lies ahead." << std::endl;
595 std::cout <<
" Requesting encounter closure because..." << std::endl;
597 std::cout <<
" ... extra time elapsed." << std::endl;
598 }
else if (forceClose) {
599 std::cout <<
" ... closing was forced." << std::endl;
601 std::cout <<
" ... foe disappeared." << std::endl;
612 double eBegin = e->
begin;
637 std::cout <<
SIMTIME <<
" qualifiesAsConflict() for encounter of vehicles '"
657 assert(e->
size() > 0);
665 std::cout <<
SIMTIME <<
" closeEncounter() of vehicles '"
667 <<
"' (was ranked as " << (wasConflict ?
"conflict" :
"non-conflict") <<
")" << std::endl;
675 std::cout <<
"pastConflictsQueue of veh '" <<
myHolderMS->
getID() <<
"':\n";
683 std::cout <<
" Conflict with foe '" << c->foe <<
"' (time " << c->begin <<
"-" << c->end <<
")\n";
685 if (c->begin < lastBegin) {
686 std::cout <<
" Queue corrupt...\n";
689 lastBegin = c->begin;
691 std::cout << std::endl;
704 #ifdef DEBUG_ENCOUNTER
705 if (DEBUG_COND_ENCOUNTER(e)) {
706 std::cout <<
SIMTIME <<
" updateEncounter() of vehicles '" << e->
egoID <<
"' and '" << e->
foeID <<
"'\n";
733 #ifdef DEBUG_ENCOUNTER
734 if (DEBUG_COND_ENCOUNTER(e)) {
735 std::cout <<
SIMTIME <<
" Encounter of vehicles '" << e->
egoID <<
"' and '" << e->
foeID <<
"' does not imply any conflict.\n";
766 if (e->
size() == 0) {
767 #ifdef DEBUG_ENCOUNTER
768 if (DEBUG_COND_ENCOUNTER(e)) {
769 std::cout <<
SIMTIME <<
" type when creating encounter: " << eInfo.
type <<
"\n";
811 std::cout <<
SIMTIME <<
" determineConflictPoint()" << std::endl;
821 assert(e->
size() > 0);
842 std::cout <<
"No conflict point associated with encounter type " << type << std::endl;
850 std::cout <<
" Conflict at " << eInfo.
conflictPoint << std::endl;
865 std::cout <<
SIMTIME <<
" estimateConflictTimes() for ego '" << e->
egoID <<
"' and foe '" << e->
foeID <<
"'\n"
866 <<
" encounter type: " << eInfo.
type <<
"\n"
880 std::cout <<
" encouter type " << type <<
" -> no entry/exit times to be calculated."
962 std::stringstream ss;
963 ss <<
"SSM device of vehicle '" << e->
egoID <<
"' detected collision with vehicle '" << e->
foeID <<
"' at time " <<
SIMTIME;
969 std::cout <<
" -> ego is estimated leader at conflict entry."
978 std::cout <<
" -> foe is estimated leader at conflict entry."
994 std::cout <<
SIMTIME <<
" computeSSMs() for vehicles '"
1026 std::stringstream ss;
1027 ss <<
"'" << type <<
"'";
1028 WRITE_WARNING(
"Unknown or undetermined encounter type at computeSSMs(): " + ss.str());
1034 std::cout <<
"computeSSMs() for encounter of vehicles '" << e->
egoID <<
"' and '" << e->
foeID <<
"':\n"
1047 if (e->
size() == 0) {
1051 std::pair<double, double>& pet = eInfo.
pet;
1055 std::cout <<
SIMTIME <<
" determinePET() for encounter of vehicles '" << e->
egoID <<
"' and '" << e->
foeID <<
"'"
1070 std::cout <<
"PET for crossing encounter already calculated as " << e->
PET.
value
1113 std::cout <<
"Unexpected branch in determinePET: Both passed conflict area in the same step."
1131 std::cout <<
"Calculated PET = " << pet.second <<
" (at t=" << pet.first <<
")"
1138 std::cout <<
"PET unappropriate for merging and pre-crossing situations. No calculation performed."
1150 double& ttc = eInfo.
ttc;
1151 double& drac = eInfo.
drac;
1155 std::cout <<
SIMTIME <<
" determineTTCandDRAC() for encounter of vehicles '" << e->
egoID <<
"' and '" << e->
foeID <<
"' (type = " << eInfo.
type <<
")"
1198 std::cout <<
" Conflict times with constant speed extrapolation for merging situation:\n "
1199 <<
" egoEntryTime=" << (egoEntryTime ==
INVALID ?
"NA" :
::toString(egoEntryTime))
1200 <<
", egoExitTime=" << (egoExitTime ==
INVALID ?
"NA" : ::
toString(egoExitTime))
1201 <<
", foeEntryTime=" << (foeEntryTime ==
INVALID ?
"NA" : ::
toString(foeEntryTime))
1202 <<
", foeExitTime=" << (foeExitTime ==
INVALID ?
"NA" : ::
toString(foeExitTime))
1213 std::cout <<
" No TTC and DRAC computed as one vehicle is stopped." << std::endl;
1218 double leaderEntryTime =
MIN2(egoEntryTime, foeEntryTime);
1219 double followerEntryTime =
MAX2(egoEntryTime, foeEntryTime);
1220 double leaderExitTime = leaderEntryTime == egoEntryTime ? egoExitTime : foeExitTime;
1227 if (leaderExitTime >= followerEntryTime) {
1230 ttc =
computeTTC(followerConflictDist, followerSpeed, 0.);
1235 drac =
computeDRAC(followerConflictDist, followerSpeed, 0.);
1241 std::cout <<
" Extrapolation predicts collision *at* merge point with TTC=" << ttc
1242 <<
", drac=" << drac << std::endl;
1249 double gapAfterMerge = followerConflictDist - leaderExitTime * followerSpeed;
1250 assert(gapAfterMerge >= 0);
1253 double ttcAfterMerge =
computeTTC(gapAfterMerge, followerSpeed, leaderSpeed);
1254 ttc = ttcAfterMerge ==
INVALID ?
INVALID : leaderExitTime + ttcAfterMerge;
1258 double g0 = followerConflictDist - leaderConflictDist - leaderLength;
1261 assert(leaderSpeed - followerSpeed > 0);
1266 drac =
computeDRAC(g0, followerSpeed, leaderSpeed);
1273 assert(drac ==
INVALID || drac == 0.0);
1274 std::cout <<
" Extrapolation does not predict any collision." << std::endl;
1276 std::cout <<
" Extrapolation predicts collision *after* merge point with TTC="
1278 <<
", drac=" << (drac ==
INVALID ?
"NA" : ::
toString(drac)) << std::endl;
1318 std::stringstream ss;
1319 ss <<
"'" << type <<
"'";
1320 WRITE_WARNING(
"Underspecified or unknown encounter type in MSDevice_SSM::determineTTCandDRAC(): " + ss.str());
1340 std::cout <<
"computeTTC() with gap=" << gap <<
", followerSpeed=" << followerSpeed <<
", leaderSpeed=" << leaderSpeed
1346 double dv = followerSpeed - leaderSpeed;
1365 double dv = followerSpeed - leaderSpeed;
1369 assert(followerSpeed > 0.);
1370 return 0.5 * dv * dv / gap;
1386 #ifdef DEBUG_SSM_DRAC
1388 std::cout <<
SIMTIME <<
"computeDRAC() with"
1389 <<
"\ndEntry1=" << dEntry1 <<
", dEntry2=" << dEntry2
1390 <<
", dExit1=" << dExit1 <<
", dExit2=" << dExit2
1391 <<
",\nv1=" << v1 <<
", v2=" << v2
1396 if (dExit1 <= 0. || dExit2 <= 0.) {
1398 #ifdef DEBUG_SSM_DRAC
1400 std::cout <<
"One already left conflict area -> drac == 0." << std::endl;
1405 if (dEntry1 <= 0. && dEntry2 <= 0.) {
1407 #ifdef DEBUG_SSM_DRAC
1409 std::cout <<
"Both entered conflict area but neither left. -> collision!" << std::endl;
1415 double drac = std::numeric_limits<double>::max();
1418 #ifdef DEBUG_SSM_DRAC
1420 std::cout <<
"Ego could break..." << std::endl;
1425 drac =
MIN2(drac, 2 * (v1 - dEntry1 / tExit2) / tExit2);
1426 #ifdef DEBUG_SSM_DRAC
1428 std::cout <<
" Foe expected to leave in " << tExit2 <<
"-> Ego needs drac=" << drac << std::endl;
1436 #ifdef DEBUG_SSM_DRAC
1438 std::cout <<
" Foe is expected stop on conflict area -> Ego needs drac=" << drac << std::endl;
1443 #ifdef DEBUG_SSM_DRAC
1445 std::cout <<
" Foe is expected stop before conflict area -> no drac computation for ego (will be done for foe if applicable)" << std::endl;
1454 #ifdef DEBUG_SSM_DRAC
1456 std::cout <<
"Foe could break..." << std::endl;
1461 #ifdef DEBUG_SSM_DRAC
1463 std::cout <<
" Ego expected to leave in " << tExit1 <<
"-> Foe needs drac=" << (2 * (v2 - dEntry2 / tExit1) / tExit1) << std::endl;
1466 drac =
MIN2(drac, 2 * (v2 - dEntry2 / tExit1) / tExit1);
1471 #ifdef DEBUG_SSM_DRAC
1473 std::cout <<
" Ego is expected stop on conflict area -> Foe needs drac=" <<
computeDRAC(dEntry2, v2, 0) << std::endl;
1479 #ifdef DEBUG_SSM_DRAC
1481 std::cout <<
" Ego is expected stop before conflict area -> no drac computation for foe (done for ego if applicable)" << std::endl;
1488 return drac > 0 ? drac :
INVALID;
1502 #ifdef DEBUG_ENCOUNTER
1503 if (DEBUG_COND_ENCOUNTER(e)) {
1504 std::cout <<
SIMTIME <<
" checkConflictEntryAndExit() for encounter of vehicles '" << e->
egoID <<
"' and '" << e->
foeID <<
"'"
1514 if (e->
size() == 0) {
1518 if (egoPastConflictExit) {
1519 if (foePastConflictExit) {
1521 }
else if (foePastConflictEntry) {
1526 }
else if (foePastConflictExit) {
1527 if (egoPastConflictEntry) {
1534 if (egoPastConflictEntry) {
1535 if (foePastConflictEntry) {
1540 }
else if (foePastConflictEntry) {
1568 #ifdef DEBUG_ENCOUNTER
1569 if (DEBUG_COND_ENCOUNTER(e)) {
1584 #ifdef DEBUG_ENCOUNTER
1585 if (DEBUG_COND_ENCOUNTER(e)) {
1602 #ifdef DEBUG_ENCOUNTER
1603 if (DEBUG_COND_ENCOUNTER(e)) {
1620 #ifdef DEBUG_ENCOUNTER
1621 if (DEBUG_COND_ENCOUNTER(e)) {
1637 #ifdef DEBUG_ENCOUNTER
1638 if (DEBUG_COND_ENCOUNTER(e)) {
1639 std::cout <<
SIMTIME <<
" updatePassedEncounter() for vehicles '" << e->
egoID <<
"' and '" << e->
foeID <<
"'\n";
1643 if (foeInfo ==
nullptr) {
1646 #ifdef DEBUG_ENCOUNTER
1647 if (DEBUG_COND_ENCOUNTER(e)) std::cout <<
" Foe is out of range. Counting down extra time."
1662 #ifdef DEBUG_ENCOUNTER
1663 if (DEBUG_COND_ENCOUNTER(e)) {
1664 std::cout <<
" This encounter wasn't classified as a potential conflict lately.\n";
1667 if (foeInfo ==
nullptr) {
1673 std::cout <<
" Requesting encounter closure because foeInfo==nullptr" << std::endl;
1677 #ifdef DEBUG_ENCOUNTER
1678 if (DEBUG_COND_ENCOUNTER(e)) {
1679 std::cout <<
" Closing encounter.\n";
1689 #ifdef DEBUG_ENCOUNTER
1690 if (DEBUG_COND_ENCOUNTER(e)) {
1691 std::cout <<
" Encounter was previously classified as a follow/lead situation.\n";
1700 #ifdef DEBUG_ENCOUNTER
1701 if (DEBUG_COND_ENCOUNTER(e)) {
1702 std::cout <<
" Encounter was previously classified as a merging situation.\n";
1717 #ifdef DEBUG_ENCOUNTER
1718 if (DEBUG_COND_ENCOUNTER(e)) {
1719 std::cout <<
" Encounter was previously classified as a crossing situation of type " << lastPotentialConflictType <<
".\n";
1737 #ifdef DEBUG_ENCOUNTER
1738 if (DEBUG_COND_ENCOUNTER(e))
1752 if ((!egoEnteredConflict) && !foeEnteredConflict) {
1756 eInfo.
type = lastPotentialConflictType;
1757 }
else if (egoEnteredConflict && !foeEnteredConflict) {
1759 }
else if ((!egoEnteredConflict) && foeEnteredConflict) {
1765 if ((!egoLeftConflict) && !foeLeftConflict) {
1769 }
else if (egoLeftConflict && !foeLeftConflict) {
1773 }
else if ((!egoLeftConflict) && foeLeftConflict) {
1788 #ifdef DEBUG_ENCOUNTER
1789 if (DEBUG_COND_ENCOUNTER(e)) {
1790 std::cout <<
" Updated classification: " << eInfo.
type <<
"\n";
1799 #ifdef DEBUG_ENCOUNTER
1800 if (DEBUG_COND_ENCOUNTER(eInfo.
encounter)) {
1801 std::cout <<
"classifyEncounter() called.\n";
1804 if (foeInfo ==
nullptr) {
1823 #ifdef DEBUG_ENCOUNTER
1824 if (DEBUG_COND_ENCOUNTER(eInfo.
encounter)) {
1825 std::cout <<
" Ongoing crossing conflict will be traced by passedEncounter().\n";
1841 double foeDistToConflictLane;
1844 #ifdef DEBUG_ENCOUNTER
1845 if (DEBUG_COND_ENCOUNTER(eInfo.
encounter))
1846 std::cout <<
" egoConflictLane='" << (egoConflictLane == 0 ?
"NULL" : egoConflictLane->
getID()) <<
"'\n"
1847 <<
" foeConflictLane='" << (foeConflictLane == 0 ?
"NULL" : foeConflictLane->
getID()) <<
"'\n"
1848 <<
" egoDistToConflictLane=" << egoDistToConflictLane
1849 <<
" foeDistToConflictLane=" << foeDistToConflictLane
1864 if (foeConflictLane ==
nullptr) {
1867 #ifdef DEBUG_ENCOUNTER
1868 if (DEBUG_COND_ENCOUNTER(eInfo.
encounter)) {
1869 std::cout <<
"-> Encounter type: No conflict.\n";
1872 }
else if (!egoConflictLane->isInternal()) {
1874 if (egoConflictLane == egoLane) {
1878 if (foeLane == egoLane) {
1880 if (!egoOpposite && !foeOpposite) {
1888 #ifdef DEBUG_ENCOUNTER
1889 if (DEBUG_COND_ENCOUNTER(eInfo.
encounter)) {
1890 std::cout <<
"-> Encounter type: Lead/follow-situation on non-internal lane '" << egoLane->
getID() <<
"'\n";
1893 }
else if (egoOpposite && foeOpposite) {
1901 #ifdef DEBUG_ENCOUNTER
1902 if (DEBUG_COND_ENCOUNTER(eInfo.
encounter)) {
1903 std::cout <<
"-> Encounter type: Lead/follow-situation while both are driving in the opposite direction on non-internal lane '" << egoLane->
getID() <<
"'\n";
1924 #ifdef DEBUG_ENCOUNTER
1925 if (DEBUG_COND_ENCOUNTER(eInfo.
encounter)) {
1926 std::cout <<
"-> Encounter type: oncoming on non-internal lane '" << egoLane->
getID() <<
"'\n";
1935 #ifdef DEBUG_ENCOUNTER
1936 if (DEBUG_COND_ENCOUNTER(eInfo.
encounter)) {
1937 std::cout <<
"-> Encounter type: " << type << std::endl;
1942 if (!egoOpposite && !foeOpposite) {
1945 assert(egoDistToConflictLane <= 0);
1947 if (foeConflictLane == egoLane) {
1951 #ifdef DEBUG_ENCOUNTER
1952 if (DEBUG_COND_ENCOUNTER(eInfo.
encounter))
1953 std::cout <<
"-> Encounter type: Ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' leads foe '"
1954 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'"
1960 #ifdef DEBUG_ENCOUNTER
1961 if (DEBUG_COND_ENCOUNTER(eInfo.
encounter)) {
1962 std::cout <<
"-> Encounter type: " << type << std::endl;
1967 }
else if (egoOpposite && foeOpposite) {
1979 #ifdef DEBUG_ENCOUNTER
1980 if (DEBUG_COND_ENCOUNTER(eInfo.
encounter)) {
1981 std::cout <<
"-> Encounter type: Lead/follow-situation while both are driving in the opposite direction on non-internal lane '" << egoLane->
getID() <<
"'\n";
2005 #ifdef DEBUG_ENCOUNTER
2006 if (DEBUG_COND_ENCOUNTER(eInfo.
encounter)) {
2007 std::cout <<
"-> Encounter type: oncoming on non-internal lane '" << egoLane->
getID() <<
"'\n";
2018 assert(&(foeLane->
getEdge()) == &(egoConflictLane->getEdge()));
2019 assert(foeDistToConflictLane <= 0);
2020 if (foeLane == egoConflictLane) {
2023 #ifdef DEBUG_ENCOUNTER
2024 if (DEBUG_COND_ENCOUNTER(eInfo.
encounter))
2025 std::cout <<
"-> Encounter type: Ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' follows foe '"
2026 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'"
2032 #ifdef DEBUG_ENCOUNTER
2033 if (DEBUG_COND_ENCOUNTER(eInfo.
encounter)) {
2034 std::cout <<
"-> Encounter type: " << type << std::endl;
2043 MSLink* egoEntryLink = egoConflictLane->getEntryLink();
2045 if (&(egoEntryLink->getViaLane()->getEdge()) == &(foeEntryLink->
getViaLane()->
getEdge())) {
2046 if (egoEntryLink != foeEntryLink) {
2049 #ifdef DEBUG_ENCOUNTER
2050 if (DEBUG_COND_ENCOUNTER(eInfo.
encounter)) {
2051 std::cout <<
"-> Encounter type: " << type << std::endl;
2056 if (egoLane == egoConflictLane && foeLane != foeConflictLane) {
2063 #ifdef DEBUG_ENCOUNTER
2064 if (DEBUG_COND_ENCOUNTER(eInfo.
encounter))
2065 std::cout <<
"-> Encounter type: Ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' leads foe '"
2066 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'"
2069 }
else if (egoLane != egoConflictLane && foeLane == foeConflictLane) {
2076 #ifdef DEBUG_ENCOUNTER
2077 if (DEBUG_COND_ENCOUNTER(eInfo.
encounter))
2078 std::cout <<
"-> Encounter type: Ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' follows foe '"
2079 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'"
2086 #ifdef DEBUG_ENCOUNTER
2087 if (DEBUG_COND_ENCOUNTER(eInfo.
encounter))
2088 std::cout <<
"-> Encounter type: Ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' merges with foe '"
2089 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'"
2097 assert(egoLane == egoConflictLane);
2098 assert(foeLane == foeConflictLane);
2099 if (egoLane == foeLane) {
2104 #ifdef DEBUG_ENCOUNTER
2105 if (DEBUG_COND_ENCOUNTER(eInfo.
encounter))
2106 std::cout <<
"-> Encounter type: Ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' leads foe '"
2107 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'"
2114 #ifdef DEBUG_ENCOUNTER
2115 if (DEBUG_COND_ENCOUNTER(eInfo.
encounter))
2116 std::cout <<
"-> Encounter type: Ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' follows foe '"
2117 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'"
2124 #ifdef DEBUG_ENCOUNTER
2125 if (DEBUG_COND_ENCOUNTER(eInfo.
encounter)) {
2126 std::cout <<
" Lead/follow situation on consecutive internal lanes." << std::endl;
2129 MSLane* lane = egoEntryLink->getViaLane();
2131 #pragma warning(push)
2132 #pragma warning(disable: 4127) // do not warn about constant conditional expression
2136 #pragma warning(pop)
2140 if (egoLane == lane) {
2145 while (lane != foeLane) {
2151 egoConflictLane = lane;
2152 #ifdef DEBUG_ENCOUNTER
2153 if (DEBUG_COND_ENCOUNTER(eInfo.
encounter))
2154 std::cout <<
"-> Encounter type: Ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' follows foe '"
2155 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'"
2160 }
else if (foeLane == lane) {
2165 while (lane != egoLane) {
2171 foeConflictLane = lane;
2172 #ifdef DEBUG_ENCOUNTER
2173 if (DEBUG_COND_ENCOUNTER(eInfo.
encounter))
2174 std::cout <<
"-> Encounter type: Ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' leads foe '"
2175 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'"
2185 #ifdef DEBUG_ENCOUNTER
2186 if (DEBUG_COND_ENCOUNTER(eInfo.
encounter))
2187 std::cout <<
"-> Encounter type: Lead/follow-situation on connection from '" << egoEntryLink->getLaneBefore()->getID()
2188 <<
"' to '" << egoEntryLink->getLane()->getID() <<
"'" << std::endl;
2195 const std::vector<MSLink*>& egoFoeLinks = egoEntryLink->getFoeLinks();
2196 const std::vector<MSLink*>& foeFoeLinks = foeEntryLink->
getFoeLinks();
2198 bool crossOrMerge = (find(egoFoeLinks.begin(), egoFoeLinks.end(), foeEntryLink) != egoFoeLinks.end()
2199 || std::find(foeFoeLinks.begin(), foeFoeLinks.end(), egoEntryLink) != foeFoeLinks.end());
2200 if (!crossOrMerge) {
2209 #ifdef DEBUG_ENCOUNTER
2210 if (DEBUG_COND_ENCOUNTER(eInfo.
encounter)) {
2211 std::cout <<
"-> Encounter type: No conflict.\n";
2215 }
else if (&(foeEntryLink->
getLane()->
getEdge()) == &(egoEntryLink->getLane()->getEdge())) {
2216 if (foeEntryLink->
getLane() == egoEntryLink->getLane()) {
2218 assert(egoConflictLane->isInternal());
2236 #ifdef DEBUG_ENCOUNTER
2237 if (DEBUG_COND_ENCOUNTER(eInfo.
encounter))
2238 std::cout <<
"-> Encounter type: Merging situation of ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' and foe '"
2239 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'"
2246 #ifdef DEBUG_ENCOUNTER
2247 if (DEBUG_COND_ENCOUNTER(eInfo.
encounter)) {
2248 std::cout <<
"-> Encounter type: No conflict: " << type << std::endl;
2255 assert(egoConflictLane->isInternal());
2256 assert(foeConflictLane->
getEdge().
getToJunction() == egoConflictLane->getEdge().getToJunction());
2263 egoConflictLane = egoConflictLane->getFirstInternalInConnection(offset);
2264 egoDistToConflictLane -= offset;
2266 foeDistToConflictLane -= offset;
2270 double egoDistToConflictFromJunctionEntry =
INVALID;
2271 double foeInternalLaneLengthsBeforeCrossing = 0.;
2272 while (foeConflictLane !=
nullptr && foeConflictLane->
isInternal()) {
2273 egoDistToConflictFromJunctionEntry = egoEntryLink->getLengthsBeforeCrossing(foeConflictLane);
2274 if (egoDistToConflictFromJunctionEntry !=
INVALID) {
2279 foeInternalLaneLengthsBeforeCrossing += foeConflictLane->
getLength();
2282 assert(foeConflictLane != 0 && foeConflictLane->
isInternal());
2284 assert(egoDistToConflictFromJunctionEntry !=
INVALID);
2287 double foeDistToConflictFromJunctionEntry =
INVALID;
2288 double egoInternalLaneLengthsBeforeCrossing = 0.;
2289 foeDistToConflictFromJunctionEntry =
INVALID;
2290 while (egoConflictLane !=
nullptr && egoConflictLane->isInternal()) {
2292 if (foeDistToConflictFromJunctionEntry !=
INVALID) {
2297 egoInternalLaneLengthsBeforeCrossing += egoConflictLane->getLength();
2299 egoConflictLane = egoConflictLane->getCanonicalSuccessorLane();
2300 assert(egoConflictLane != 0 && egoConflictLane->isInternal());
2302 assert(foeDistToConflictFromJunctionEntry !=
INVALID);
2314 Position egoEntryPos = egoEntryLink->getViaLane()->getShape().front();
2315 Position egoExitPos = egoEntryLink->getCorrespondingExitLink()->getInternalLaneBefore()->getShape().back();
2325 assert(angle <= 2 *
M_PI);
2329 assert(angle >= -
M_PI);
2330 assert(angle <=
M_PI);
2332 double crossingOrientation = (angle < 0) - (angle > 0);
2352 #ifdef DEBUG_ENCOUNTER
2353 if (DEBUG_COND_ENCOUNTER(eInfo.
encounter)) {
2354 std::cout <<
" Determined exact conflict distances for crossing conflict."
2355 <<
"\n crossingOrientation=" << crossingOrientation
2358 <<
", relativeAngle=" << angle
2359 <<
" (foe from " << (crossingOrientation > 0 ?
"right)" :
"left)")
2360 <<
"\n resulting offset for conflict entry distance:"
2363 <<
"\n distToConflictLane:"
2364 <<
"\n ego=" << egoDistToConflictLane
2365 <<
", foe=" << foeDistToConflictLane
2366 <<
"\n distToConflictFromJunctionEntry:"
2367 <<
"\n ego=" << egoDistToConflictFromJunctionEntry
2368 <<
", foe=" << foeDistToConflictFromJunctionEntry
2369 <<
"\n resulting entry distances:"
2372 <<
"\n resulting exit distances:"
2377 std::cout <<
"real egoConflictLane: '" << (egoConflictLane == 0 ?
"NULL" : egoConflictLane->getID()) <<
"'\n"
2378 <<
"real foeConflictLane: '" << (foeConflictLane == 0 ?
"NULL" : foeConflictLane->
getID()) <<
"'\n"
2379 <<
"-> Encounter type: Crossing situation of ego '" << e->
ego->
getID() <<
"' on lane '" << egoLane->
getID() <<
"' and foe '"
2380 << e->
foe->
getID() <<
"' on lane '" << foeLane->
getID() <<
"'"
2398 std::cout <<
SIMTIME <<
" findFoeConflictLane() for foe '"
2400 <<
"' (with egoConflictLane=" << (egoConflictLane == 0 ?
"NULL" : egoConflictLane->
getID())
2410 #ifdef DEBUG_SSM_OPPOSITE
2426 return egoConflictLane;
2435 return egoConflictLane;
2445 assert(foeLane->
isInternal() || *laneIter == foeLane);
2452 if (conflictJunction != 0) {
2453 std::cout <<
"Potential conflict on junction '" << conflictJunction->
getID()
2459 if (egoConflictLane !=
nullptr && egoConflictLane->
isInternal() && egoConflictLane->
getLinkCont()[0]->getViaLane() == foeLane) {
2460 distToConflictLane += egoConflictLane->
getLength();
2468 if (*laneIter ==
nullptr) {
2469 while (foeLane !=
nullptr && foeLane->
isInternal()) {
2470 distToConflictLane += foeLane->
getLength();
2471 foeLane = foeLane->
getLinkCont()[0]->getViaLane();
2474 assert(laneIter == foeBestLanesEnd || *laneIter != 0);
2478 while (laneIter != foeBestLanesEnd && distToConflictLane <=
myRange) {
2480 assert(*laneIter == foeLane || foeLane == 0);
2481 foeLane = *laneIter;
2486 std::cout <<
"Found conflict lane for foe: '" << foeLane->
getID() <<
"'" << std::endl;
2493 distToConflictLane += foeLane->
getLength();
2497 if (laneIter == foeBestLanesEnd) {
2500 MSLane* nextNonInternalLane = *laneIter;
2504 assert(foeLane == 0 || foeLane->
isInternal());
2505 if (foeLane ==
nullptr) {
2506 foeLane = nextNonInternalLane;
2510 assert(foeLane != 0);
2513 std::cout <<
"Found conflict lane for foe: '" << foeLane->
getID() <<
"'" << std::endl;
2521 foeLane = nextNonInternalLane;
2554 std::cout <<
SIMTIME <<
" flushGlobalMeasures() of vehicle '"
2619 std::cout <<
SIMTIME <<
" writeOutConflict() of vehicles '"
2700 std::string res =
"";
2701 for (std::vector<double>::const_iterator i = v.begin(); i != v.end(); ++i) {
2702 res += (i == v.begin() ?
"" : sep) + (*i == NA ?
"NA" : ::
toString(*i));
2709 std::string res =
"";
2710 for (std::vector<double>::const_iterator i = v.begin(); i != v.end(); ++i) {
2711 res += (i == v.begin() ?
"" : sep) + (find(NAs.begin(), NAs.end(), *i) != NAs.end() ?
"NA" :
::toString(*i));
2721 bool trajectories,
double range,
double extraTime,
bool useGeoCoords) :
2730 myMinSGAP(std::make_pair(std::make_pair(-1,
Position(0., 0.)), std::numeric_limits<double>::max()),
""),
2731 myMinTGAP(std::make_pair(std::make_pair(-1,
Position(0., 0.)), std::numeric_limits<double>::max()),
"") {
2733 myHolderMS = static_cast<MSVehicle*>(&holder);
2759 std::vector<std::string> measures;
2760 std::vector<double> threshVals;
2762 measures.push_back(i->first);
2763 threshVals.push_back(i->second);
2765 std::cout <<
"Initialized ssm device '" <<
id <<
"' with "
2788 #ifdef DEBUG_SSM_NOTIFICATIONS
2790 std::cout <<
"device '" <<
getID() <<
"' notifyEnter: reason=" << reason <<
" currentEdge=" << v->getLane()->getEdge().getID() <<
"\n";
2802 #ifdef DEBUG_SSM_NOTIFICATIONS
2804 std::cout <<
"device '" <<
getID() <<
"' notifyLeave: reason=" << reason <<
" currentEdge=" << v->getLane()->getEdge().getID() <<
"\n";
2814 double ,
double newSpeed) {
2815 #ifdef DEBUG_SSM_NOTIFICATIONS
2816 std::cout <<
"device '" <<
getID() <<
"' notifyMove: newSpeed=" << newSpeed <<
"\n";
2829 #ifdef DEBUG_SSM_SURROUNDING
2833 std::cout <<
SIMTIME <<
" Looking for surrounding vehicles for ego vehicle '" << veh.
getID()
2856 for (
int i = 0; i < (int)egoBestLanes.size(); i++) {
2857 if (egoBestLanes[i] !=
nullptr && egoBestLanes[i]->getEdge().getOppositeEdge() !=
nullptr) {
2858 egoBestLanes[i] = egoBestLanes[i]->getEdge().getOppositeEdge()->getLanes().back();
2862 std::vector<MSLane*>::const_iterator laneIter = egoBestLanes.begin();
2867 assert(lane->
isInternal() || lane == *laneIter);
2870 const MSLane* nextNonInternalLane =
nullptr;
2878 double remainingDownstreamRange = range;
2880 double distToConflictLane = isOpposite ? pos - veh.
getLane()->
getLength() : -pos;
2883 std::set<const MSLane*> seenLanes;
2884 std::set<const MSJunction*> routeJunctions;
2888 std::vector<UpstreamScanStartInfo> upstreamScanStartPositions;
2897 #ifdef DEBUG_SSM_SURROUNDING
2899 std::cout <<
SIMTIME <<
" Vehicle '" << veh.
getID() <<
"' is on internal edge " << edge->
getID() <<
"'." << std::endl;
2909 routeJunctions.insert(junction);
2915 for (ConstMSEdgeVector::const_iterator ei = incoming.begin(); ei != incoming.end(); ++ei) {
2916 if ((*ei)->isInternal()) {
2935 lane = *(++laneIter);
2941 double startScanPos = std::min(pos + remainingDownstreamRange, edgeLength);
2942 upstreamScanStartPositions.push_back(
UpstreamScanStartInfo(edge, startScanPos, std::max(0., startScanPos - pos + range + veh.
getLength()), distToConflictLane, lane));
2951 while (remainingDownstreamRange > 0.) {
2953 #ifdef DEBUG_SSM_SURROUNDING
2955 std::cout <<
SIMTIME <<
" Scanning downstream for vehicle '" << veh.
getID() <<
"' on lane '" << veh.
getLane()->
getID() <<
"', position=" << pos <<
".\n"
2956 <<
"Considering edge '" << edge->
getID() <<
"' Remaining downstream range = " << remainingDownstreamRange
2957 <<
"\nbestLanes=" <<
::toString(egoBestLanes) <<
"\n"
2963 assert(pos == 0 || lane == veh.
getLane());
2964 if (pos + remainingDownstreamRange < lane->getLength()) {
2967 upstreamScanStartPositions.push_back(
UpstreamScanStartInfo(edge, pos + remainingDownstreamRange, remainingDownstreamRange, distToConflictLane, lane));
2978 remainingDownstreamRange -= lane->
getLength() - pos;
2979 distToConflictLane += lane->
getLength();
2984 assert(laneIter == egoBestLanes.end() || *laneIter != 0);
2987 if (laneIter != egoBestLanes.end()) {
2998 nextNonInternalLane = *laneIter;
3000 if (isOpposite && link ==
nullptr) {
3001 link = nextNonInternalLane->
getLinkTo(lane);
3002 if (link ==
nullptr) {
3006 assert(link != 0 || link->
getLength() == 0.);
3010 if (lane ==
nullptr) {
3012 lane = nextNonInternalLane;
3014 if (seenLanes.count(lane) == 0) {
3015 seenLanes.insert(lane);
3022 if (seenLanes.count(lane) == 0) {
3025 routeJunctions.insert(junction);
3030 for (ConstMSEdgeVector::const_iterator ei = outgoing.begin(); ei != outgoing.end(); ++ei) {
3034 upstreamScanStartPositions.push_back(
UpstreamScanStartInfo(*ei, (*ei)->getLength(), range, distToConflictLane, lane));
3038 for (ConstMSEdgeVector::const_iterator ei = incoming.begin(); ei != incoming.end(); ++ei) {
3042 upstreamScanStartPositions.push_back(
UpstreamScanStartInfo(*ei, (*ei)->getLength(), range, distToConflictLane, lane));
3048 remainingDownstreamRange -= linkLength;
3049 distToConflictLane += linkLength;
3050 #ifdef DEBUG_SSM_SURROUNDING
3052 std::cout <<
" Downstream Scan for vehicle '" << veh.
getID() <<
"' proceeded over junction '" << junction->
getID()
3053 <<
"',\n linkLength=" << linkLength <<
", remainingDownstreamRange=" << remainingDownstreamRange
3059 lane = nextNonInternalLane;
3062 #ifdef DEBUG_SSM_SURROUNDING
3064 std::cout <<
" Downstream Scan for vehicle '" << veh.
getID() <<
"' stops at lane '" << lane->
getID()
3065 <<
"', which has already been scanned."
3086 #ifdef DEBUG_SSM_SURROUNDING
3088 for (std::pair<const MSVehicle*, FoeInfo*> foeInfo : foeCollector) {
3089 std::cout <<
" foe " << foeInfo.first->getID() <<
" conflict at " << foeInfo.second->egoConflictLane->getID() <<
" egoDist " << foeInfo.second->egoDistToConflictLane << std::endl;
3095 foeCollector.erase(&veh);
3101 #ifdef DEBUG_SSM_SURROUNDING
3103 std::cout <<
SIMTIME <<
" getUpstreamVehicles() for edge '" << scanStart.
edge->
getID() <<
"'"
3104 <<
" pos = " << scanStart.
pos <<
" range = " << scanStart.
range
3108 if (scanStart.
range <= 0) {
3112 const std::vector<MSLane*>& lanes = scanStart.
edge->
getLanes();
3114 for (
MSLane* lane : lanes) {
3115 if (seenLanes.find(lane) != seenLanes.end()) {
3121 for (MSLane::VehCont::const_iterator vi = vehicles.begin(); vi != vehicles.end(); ++vi) {
3124 if (foeCollector.find(veh) != foeCollector.end()) {
3129 #ifdef DEBUG_SSM_SURROUNDING
3131 std::cout <<
"\t" << veh->
getID() <<
"\n";
3137 foeCollector[veh] = c;
3141 lane->releaseVehicles();
3143 #ifdef DEBUG_SSM_SURROUNDING
3145 std::cout <<
"\t" << lane->getID() <<
": Found " << foundCount <<
"\n";
3148 seenLanes.insert(lane);
3151 #ifdef DEBUG_SSM_SURROUNDING
3153 std::cout << std::endl;
3161 if (scanStart.
range <= scanStart.
pos) {
3166 double remainingRange = scanStart.
range - scanStart.
pos;
3172 if (routeJunctions.find(junction) != routeJunctions.end()) {
3177 int incomingEdgeCount = 0;
3185 for (
MSLane* internalLane : internalLanes) {
3186 if (internalLane->getEdge().getSuccessors()[0]->getID() == scanStart.
edge->
getID()) {
3188 incomingEdgeCount++;
3193 if (incomingEdgeCount > 0) {
3195 for (ConstMSEdgeVector::const_iterator ei = incoming.begin(); ei != incoming.end(); ++ei) {
3196 if ((*ei)->isInternal() || (*ei)->isCrossing()) {
3199 const std::vector<MSLane*> lanes = (*ei)->getLanes();
3201 for (
MSLane* lane : lanes) {
3202 if (seenLanes.find(lane) != seenLanes.end()) {
3208 #ifdef DEBUG_SSM_SURROUNDING
3214 const MSEdge* inEdge = *ei;
3215 assert(inEdge != 0);
3217 if (distOnJunction >= remainingRange) {
3218 #ifdef DEBUG_SSM_SURROUNDING
3232 #ifdef DEBUG_SSM_SURROUNDING
3234 std::cout <<
SIMTIME <<
" getVehiclesOnJunction() for junction '" << junction->
getID() <<
"'"
3235 <<
"\nFound vehicles:"
3242 if (foeCollector.find(veh) != foeCollector.end()) {
3243 delete foeCollector[veh];
3248 foeCollector[veh] = c;
3249 #ifdef DEBUG_SSM_SURROUNDING
3252 std::cout <<
"\t" << veh->getID() <<
"\n";
3260 if (seenLanes.find(egoJunctionLane) != seenLanes.end() || egoJunctionLane->
getEdge().
isCrossing()) {
3264 auto scanInternalLane = [&](
const MSLane * lane) {
3269 collectFoeInfos(vehicles);
3271 lane->releaseVehicles();
3275 if (lane->getCanonicalPredecessorLane()->isInternal()) {
3276 lane = lane->getCanonicalPredecessorLane();
3279 assert(!lane->getEntryLink()->fromInternalLane());
3284 collectFoeInfos(vehicles2);
3285 lane->releaseVehicles();
3290 if (lane->getLinkCont().size() > 1 && lane->getLinkCont()[0]->getViaLane() !=
nullptr) {
3292 lane = lane->getLinkCont()[0]->getViaLane();
3294 assert(lane->getLinkCont().size() == 0 || lane->getLinkCont()[0]->getViaLane() == 0);
3299 collectFoeInfos(vehicles2);
3300 lane->releaseVehicles();
3310 for (
MSLane* lane : foeLanes) {
3311 if (seenLanes.find(lane) != seenLanes.end()) {
3314 scanInternalLane(lane);
3315 seenLanes.insert(lane);
3318 scanInternalLane(egoJunctionLane);
3320 #ifdef DEBUG_SSM_SURROUNDING
3322 std::cout << std::endl;
3342 std::string file = deviceID +
".xml";
3356 file = oc.
getString(
"device.ssm.file") ==
"" ? file : oc.
getString(
"device.ssm.file");
3358 std::cout <<
"vehicle '" << v.
getID() <<
"' does not supply vehicle parameter 'device.ssm.file'. Using default of '" << file <<
"'\n";
3368 bool useGeo =
false;
3382 useGeo = oc.
getBool(
"device.ssm.geo");
3384 std::cout <<
"vehicle '" << v.
getID() <<
"' does not supply vehicle parameter 'device.ssm.geo'. Using default of '" <<
::toString(useGeo) <<
"'\n";
3409 range = oc.
getFloat(
"device.ssm.range");
3411 std::cout <<
"vehicle '" << v.
getID() <<
"' does not supply vehicle parameter 'device.ssm.range'. Using default of '" << range <<
"'\n";
3436 extraTime = oc.
getFloat(
"device.ssm.extratime");
3438 std::cout <<
"vehicle '" << v.
getID() <<
"' does not supply vehicle parameter 'device.ssm.extratime'. Using default of '" << extraTime <<
"'\n";
3442 if (extraTime < 0.) {
3444 WRITE_WARNING(
"Negative (or no) value encountered for vehicle parameter 'device.ssm.extratime' in vehicle '" + v.
getID() +
"' using default value " +
::toString(extraTime) +
" instead");
3453 bool trajectories =
false;
3467 trajectories = oc.
getBool(
"device.ssm.trajectories");
3469 std::cout <<
"vehicle '" << v.
getID() <<
"' does not supply vehicle parameter 'device.ssm.trajectories'. Using default of '" <<
::toString(trajectories) <<
"'\n";
3473 return trajectories;
3482 std::string measures_str =
"";
3496 measures_str = oc.
getString(
"device.ssm.measures");
3498 std::cout <<
"vehicle '" << v.
getID() <<
"' does not supply vehicle parameter 'device.ssm.measures'. Using default of '" << measures_str <<
"'\n";
3504 if (measures_str ==
"") {
3505 WRITE_WARNING(
"No measures specified for ssm device of vehicle '" + v.
getID() +
"'. Registering all available SSMs.");
3509 std::vector<std::string> available = st.
getVector();
3511 std::vector<std::string> measures = st.
getVector();
3512 for (std::vector<std::string>::const_iterator i = measures.begin(); i != measures.end(); ++i) {
3513 if (std::find(available.begin(), available.end(), *i) == available.end()) {
3515 WRITE_ERROR(
"SSM identifier '" + *i +
"' is not supported. Aborting construction of SSM device '" + deviceID +
"'.");
3521 std::string thresholds_str =
"";
3535 thresholds_str = oc.
getString(
"device.ssm.thresholds");
3537 std::cout <<
"vehicle '" << v.
getID() <<
"' does not supply vehicle parameter 'device.ssm.thresholds'. Using default of '" << thresholds_str <<
"'\n";
3544 if (thresholds_str !=
"") {
3546 while (count < (
int)measures.size() && st.
hasNext()) {
3548 thresholds.insert(std::make_pair(measures[count], thresh));
3551 if (thresholds.size() < measures.size() || st.
hasNext()) {
3552 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.");
3557 for (std::vector<std::string>::const_iterator i = measures.begin(); i != measures.end(); ++i) {
3560 }
else if (*i ==
"DRAC") {
3562 }
else if (*i ==
"PET") {
3564 }
else if (*i ==
"BR") {
3566 }
else if (*i ==
"SGAP") {
3568 }
else if (*i ==
"TGAP") {
3571 WRITE_ERROR(
"Unknown SSM identifier '" + (*i) +
"'. Aborting construction of ssm device.");
3591 if (key ==
"minTTC" ||
3599 minTTC =
MIN2(minTTC, e->minTTC.value);
3600 minPET =
MIN2(minPET, e->PET.value);
3601 maxDRAC =
MAX2(maxDRAC, e->maxDRAC.value);
3603 if (key ==
"minTTC") {
3605 }
else if (key ==
"maxDRAC") {
3607 }
else if (key ==
"minPET") {
3628 if (
false || key ==
"foo") {