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