Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2013-2025 German Aerospace Center (DLR) and others.
4 : // This program and the accompanying materials are made available under the
5 : // terms of the Eclipse Public License 2.0 which is available at
6 : // https://www.eclipse.org/legal/epl-2.0/
7 : // This Source Code may also be made available under the following Secondary
8 : // Licenses when the conditions for such availability set forth in the Eclipse
9 : // Public License 2.0 are satisfied: GNU General Public License, version 2
10 : // or later which is available at
11 : // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12 : // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13 : /****************************************************************************/
14 : /// @file MSDevice_SSM.cpp
15 : /// @author Daniel Krajzewicz
16 : /// @author Michael Behrisch
17 : /// @author Jakob Erdmann
18 : /// @author Leonhard Luecken
19 : /// @author Mirko Barthauer
20 : /// @author Johannes Rummel
21 : /// @date 11.06.2013
22 : ///
23 : // An SSM-device logs encounters / conflicts of the carrying vehicle with other surrounding vehicles
24 : // XXX: Preliminary implementation. Use with care. Especially rerouting vehicles could be problematic.
25 : // TODO: implement SSM time-gap (estimated conflict entry and exit times are already calculated for PET calculation)
26 : /****************************************************************************/
27 : #include <config.h>
28 :
29 : #include <iostream>
30 : #include <algorithm>
31 : #include <utils/common/FileHelpers.h>
32 : #include <utils/common/StringTokenizer.h>
33 : #include <utils/common/StringUtils.h>
34 : #include <utils/geom/GeoConvHelper.h>
35 : #include <utils/geom/GeomHelper.h>
36 : #include <utils/geom/Position.h>
37 : #include <utils/options/OptionsCont.h>
38 : #include <utils/iodevices/OutputDevice.h>
39 : #include <utils/vehicle/SUMOVehicle.h>
40 : #include <microsim/MSNet.h>
41 : #include <microsim/MSJunction.h>
42 : #include <microsim/MSLane.h>
43 : #include <microsim/MSLink.h>
44 : #include <microsim/MSEdge.h>
45 : #include <microsim/MSVehicle.h>
46 : #include <microsim/MSVehicleControl.h>
47 : #include <microsim/MSJunctionControl.h>
48 : #include <microsim/lcmodels/MSAbstractLaneChangeModel.h>
49 : #include "MSDevice_SSM.h"
50 :
51 : // ===========================================================================
52 : // Debug constants
53 : // ===========================================================================
54 : //#define DEBUG_SSM
55 : //#define DEBUG_SSM_OPPOSITE
56 : //#define DEBUG_ENCOUNTER
57 : //#define DEBUG_SSM_SURROUNDING
58 : //#define DEBUG_SSM_DRAC
59 : //#define DEBUG_SSM_NOTIFICATIONS
60 : //#define DEBUG_COND(ego) MSNet::getInstance()->getCurrentTimeStep() > 308000
61 : //
62 : //#define DEBUG_EGO_ID ""
63 : //#define DEBUG_FOE_ID ""
64 : //#define DEBUG_COND_FIND(ego) (ego.getID() == DEBUG_EGO_ID)
65 : //#define DEBUG_COND(ego) ((ego)!=nullptr && (ego)->getID() == DEBUG_EGO_ID)
66 : //#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))
67 :
68 : //#define DEBUG_COND(ego) (ego!=nullptr && ego->isSelected())
69 : //#define DEBUG_COND_FIND(ego) (ego.isSelected())
70 : //#define DEBUG_COND_ENCOUNTER(e) (e->ego != nullptr && e->ego->isSelected() && e->foe != nullptr && e->foe->isSelected())
71 :
72 :
73 : // ===========================================================================
74 : // Constants
75 : // ===========================================================================
76 : // list of implemented SSMs (NOTE: To add more SSMs, identifiers are added to AVAILABLE_SSMS
77 : // and a default threshold must be defined. A corresponding
78 : // case should be added to the switch in buildVehicleDevices,
79 : // and in computeSSMs(), the SSM-value should be computed.)
80 : #define AVAILABLE_SSMS "TTC DRAC PET BR SGAP TGAP PPET MDRAC"
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_MDRAC 3.4 //in [m/s^2], events get logged if "deceleration to avoid a crash" is above threshold (MDRAC considers reaction time of follower)
84 :
85 : #define DEFAULT_THRESHOLD_PET 2. // in seconds, events get logged if post encroachment time is below threshold
86 : #define DEFAULT_THRESHOLD_PPET 2. // in seconds, events get logged if predicted post encroachment time is below threshold
87 :
88 : #define DEFAULT_THRESHOLD_BR 0.0 // in [m/s^2], events get logged if brake rate is above threshold
89 : #define DEFAULT_THRESHOLD_SGAP 0.2 // in [m.], events get logged if the space headway is below threshold.
90 : #define DEFAULT_THRESHOLD_TGAP 0.5 // in [m.], events get logged if the time headway is below threshold.
91 :
92 : #define DEFAULT_EXTRA_TIME 5. // in seconds, events get logged for extra time even if encounter is over
93 :
94 : // ===========================================================================
95 : // static members
96 : // ===========================================================================
97 : std::set<const MSEdge*> MSDevice_SSM::myEdgeFilter;
98 : bool MSDevice_SSM::myEdgeFilterInitialized(false);
99 : bool MSDevice_SSM::myEdgeFilterActive(false);
100 :
101 : // ===========================================================================
102 : // method definitions
103 : // ===========================================================================
104 :
105 : /// Nicer output for EncounterType enum
106 0 : std::ostream& operator<<(std::ostream& out, MSDevice_SSM::EncounterType type) {
107 0 : switch (type) {
108 0 : case MSDevice_SSM::ENCOUNTER_TYPE_NOCONFLICT_AHEAD:
109 0 : out << "NOCONFLICT_AHEAD";
110 0 : break;
111 0 : case MSDevice_SSM::ENCOUNTER_TYPE_FOLLOWING:
112 0 : out << "FOLLOWING";
113 0 : break;
114 0 : case MSDevice_SSM::ENCOUNTER_TYPE_FOLLOWING_FOLLOWER:
115 0 : out << "FOLLOWING_FOLLOWER";
116 0 : break;
117 0 : case MSDevice_SSM::ENCOUNTER_TYPE_FOLLOWING_LEADER:
118 0 : out << "FOLLOWING_LEADER";
119 0 : break;
120 0 : case MSDevice_SSM::ENCOUNTER_TYPE_ON_ADJACENT_LANES:
121 0 : out << "ON_ADJACENT_LANES";
122 0 : break;
123 0 : case MSDevice_SSM::ENCOUNTER_TYPE_MERGING:
124 0 : out << "MERGING";
125 0 : break;
126 0 : case MSDevice_SSM::ENCOUNTER_TYPE_MERGING_LEADER:
127 0 : out << "MERGING_LEADER";
128 0 : break;
129 0 : case MSDevice_SSM::ENCOUNTER_TYPE_MERGING_FOLLOWER:
130 0 : out << "MERGING_FOLLOWER";
131 0 : break;
132 0 : case MSDevice_SSM::ENCOUNTER_TYPE_MERGING_ADJACENT:
133 0 : out << "MERGING_ADJACENT";
134 0 : break;
135 0 : case MSDevice_SSM::ENCOUNTER_TYPE_CROSSING:
136 0 : out << "CROSSING";
137 0 : break;
138 0 : case MSDevice_SSM::ENCOUNTER_TYPE_CROSSING_LEADER:
139 0 : out << "CROSSING_LEADER";
140 0 : break;
141 0 : case MSDevice_SSM::ENCOUNTER_TYPE_CROSSING_FOLLOWER:
142 0 : out << "CROSSING_FOLLOWER";
143 0 : break;
144 0 : case MSDevice_SSM::ENCOUNTER_TYPE_EGO_ENTERED_CONFLICT_AREA:
145 0 : out << "EGO_ENTERED_CONFLICT_AREA";
146 0 : break;
147 0 : case MSDevice_SSM::ENCOUNTER_TYPE_FOE_ENTERED_CONFLICT_AREA:
148 0 : out << "FOE_ENTERED_CONFLICT_AREA";
149 0 : break;
150 0 : case MSDevice_SSM::ENCOUNTER_TYPE_BOTH_ENTERED_CONFLICT_AREA:
151 0 : out << "BOTH_ENTERED_CONFLICT_AREA";
152 0 : break;
153 0 : case MSDevice_SSM::ENCOUNTER_TYPE_EGO_LEFT_CONFLICT_AREA:
154 0 : out << "EGO_LEFT_CONFLICT_AREA";
155 0 : break;
156 0 : case MSDevice_SSM::ENCOUNTER_TYPE_FOE_LEFT_CONFLICT_AREA:
157 0 : out << "FOE_LEFT_CONFLICT_AREA";
158 0 : break;
159 0 : case MSDevice_SSM::ENCOUNTER_TYPE_BOTH_LEFT_CONFLICT_AREA:
160 0 : out << "BOTH_LEFT_CONFLICT_AREA";
161 0 : break;
162 0 : case MSDevice_SSM::ENCOUNTER_TYPE_FOLLOWING_PASSED:
163 0 : out << "FOLLOWING_PASSED";
164 0 : break;
165 0 : case MSDevice_SSM::ENCOUNTER_TYPE_MERGING_PASSED:
166 0 : out << "MERGING_PASSED";
167 0 : break;
168 : // Collision (currently unused, might be differentiated further)
169 0 : case MSDevice_SSM::ENCOUNTER_TYPE_COLLISION:
170 0 : out << "COLLISION";
171 0 : break;
172 0 : case MSDevice_SSM::ENCOUNTER_TYPE_ONCOMING:
173 0 : out << "ONCOMING";
174 0 : break;
175 : default:
176 0 : out << "unknown type (" << int(type) << ")";
177 0 : break;
178 : }
179 0 : return out;
180 : }
181 :
182 :
183 : // ---------------------------------------------------------------------------
184 : // static initialisation methods
185 : // ---------------------------------------------------------------------------
186 :
187 : std::set<MSDevice_SSM*, ComparatorNumericalIdLess>* MSDevice_SSM::myInstances = new std::set<MSDevice_SSM*, ComparatorNumericalIdLess>();
188 :
189 : std::set<std::string> MSDevice_SSM::myCreatedOutputFiles;
190 :
191 : int MSDevice_SSM::myIssuedParameterWarnFlags = 0;
192 :
193 : const std::set<int> MSDevice_SSM::FOE_ENCOUNTERTYPES({
194 : ENCOUNTER_TYPE_FOLLOWING_LEADER, ENCOUNTER_TYPE_MERGING_FOLLOWER,
195 : ENCOUNTER_TYPE_CROSSING_FOLLOWER, ENCOUNTER_TYPE_FOE_ENTERED_CONFLICT_AREA,
196 : ENCOUNTER_TYPE_FOE_LEFT_CONFLICT_AREA
197 : });
198 : const std::set<int> MSDevice_SSM::EGO_ENCOUNTERTYPES({
199 : ENCOUNTER_TYPE_FOLLOWING_FOLLOWER, ENCOUNTER_TYPE_MERGING_LEADER,
200 : ENCOUNTER_TYPE_CROSSING_LEADER, ENCOUNTER_TYPE_EGO_ENTERED_CONFLICT_AREA,
201 : ENCOUNTER_TYPE_EGO_LEFT_CONFLICT_AREA
202 : });
203 :
204 :
205 : const std::set<MSDevice_SSM*, ComparatorNumericalIdLess>&
206 71284419 : MSDevice_SSM::getInstances() {
207 71284419 : return *myInstances;
208 : }
209 :
210 : void
211 38777 : MSDevice_SSM::cleanup() {
212 : // Close current encounters and flush conflicts to file for all existing devices
213 38777 : if (myInstances != nullptr) {
214 38777 : for (MSDevice_SSM* device : *myInstances) {
215 0 : device->resetEncounters();
216 0 : device->flushConflicts(true);
217 0 : device->flushGlobalMeasures();
218 : }
219 38777 : myInstances->clear();
220 : }
221 39269 : for (const std::string& fn : myCreatedOutputFiles) {
222 984 : OutputDevice::getDevice(fn).closeTag();
223 : }
224 : myCreatedOutputFiles.clear();
225 : myEdgeFilter.clear();
226 38777 : myEdgeFilterInitialized = false;
227 38777 : myEdgeFilterActive = false;
228 38777 : }
229 :
230 :
231 : void
232 39900 : MSDevice_SSM::insertOptions(OptionsCont& oc) {
233 39900 : oc.addOptionSubTopic("SSM Device");
234 79800 : insertDefaultAssignmentOptions("ssm", "SSM Device", oc);
235 :
236 : // custom options
237 79800 : oc.doRegister("device.ssm.measures", new Option_String(""));
238 79800 : oc.addDescription("device.ssm.measures", "SSM Device", TL("Specifies which measures will be logged (as a space or comma-separated sequence of IDs in ('TTC', 'DRAC', 'PET', 'PPET', 'MDRAC'))"));
239 79800 : oc.doRegister("device.ssm.thresholds", new Option_String(""));
240 79800 : oc.addDescription("device.ssm.thresholds", "SSM Device", TL("Specifies space or comma-separated thresholds corresponding to the specified measures (see documentation and watch the order!). Only events exceeding the thresholds will be logged."));
241 39900 : oc.doRegister("device.ssm.trajectories", new Option_Bool(false));
242 79800 : oc.addDescription("device.ssm.trajectories", "SSM Device", TL("Specifies whether trajectories will be logged (if false, only the extremal values and times are reported)."));
243 39900 : oc.doRegister("device.ssm.range", new Option_Float(50.));
244 79800 : oc.addDescription("device.ssm.range", "SSM Device", TL("Specifies the detection range in meters. For vehicles below this distance from the equipped vehicle, SSM values are traced."));
245 39900 : oc.doRegister("device.ssm.extratime", new Option_Float(DEFAULT_EXTRA_TIME));
246 79800 : oc.addDescription("device.ssm.extratime", "SSM Device", TL("Specifies the time in seconds to be logged after a conflict is over. Required >0 if PET is to be calculated for crossing conflicts."));
247 39900 : oc.doRegister("device.ssm.mdrac.prt", new Option_Float(1.));
248 79800 : oc.addDescription("device.ssm.mdrac.prt", "SSM Device", TL("Specifies the perception reaction time for MDRAC computation."));
249 79800 : oc.doRegister("device.ssm.file", new Option_String(""));
250 79800 : oc.addDescription("device.ssm.file", "SSM Device", TL("Give a global default filename for the SSM output"));
251 39900 : oc.doRegister("device.ssm.geo", new Option_Bool(false));
252 79800 : oc.addDescription("device.ssm.geo", "SSM Device", TL("Whether to use coordinates of the original reference system in output"));
253 39900 : oc.doRegister("device.ssm.write-positions", new Option_Bool(false));
254 79800 : oc.addDescription("device.ssm.write-positions", "SSM Device", TL("Whether to write positions (coordinates) for each timestep"));
255 39900 : oc.doRegister("device.ssm.write-lane-positions", new Option_Bool(false));
256 79800 : oc.addDescription("device.ssm.write-lane-positions", "SSM Device", TL("Whether to write lanes and their positions for each timestep"));
257 39900 : oc.doRegister("device.ssm.write-na", new Option_Bool(true));
258 79800 : oc.addDescription("device.ssm.write-na", "SSM Device", TL("Whether to write conflict outputs with no data as NA values or skip it"));
259 79800 : oc.doRegister("device.ssm.exclude-conflict-types", new Option_String(""));
260 79800 : oc.addDescription("device.ssm.exclude-conflict-types", "SSM Device", TL("Which conflicts will be excluded from the log according to the conflict type they have been classified (combination of values in 'ego', 'foe' , '', any numerical valid conflict type code). An empty value will log all and 'ego'/'foe' refer to a certain conflict type subset."));
261 39900 : }
262 :
263 :
264 : void
265 480 : MSDevice_SSM::initEdgeFilter() {
266 480 : myEdgeFilterInitialized = true;
267 960 : if (OptionsCont::getOptions().isSet("device.ssm.filter-edges.input-file")) {
268 40 : const std::string file = OptionsCont::getOptions().getString("device.ssm.filter-edges.input-file");
269 20 : std::ifstream strm(file.c_str());
270 20 : if (!strm.good()) {
271 0 : throw ProcessError(TLF("Could not load names of edges for filtering SSM device output from '%'.", file));
272 : }
273 20 : myEdgeFilterActive = true;
274 48 : while (strm.good()) {
275 : std::string line;
276 28 : strm >> line;
277 : // maybe we're loading an edge-selection
278 56 : if (StringUtils::startsWith(line, "edge:")) {
279 16 : std::string edgeID = line.substr(5);
280 16 : MSEdge* edge = MSEdge::dictionary(edgeID);
281 16 : if (edge != nullptr) {
282 : myEdgeFilter.insert(edge);
283 : } else {
284 12 : WRITE_WARNING("Unknown edge ID '" + edgeID + "' in SSM device edge filter (" + file + "): " + line);
285 : }
286 24 : } else if (StringUtils::startsWith(line, "junction:")) {
287 : // get the internal edge(s) of a junction
288 8 : std::string junctionID = line.substr(9);
289 8 : MSJunction* junction = MSNet::getInstance()->getJunctionControl().get(junctionID);
290 4 : if (junction != nullptr) {
291 60 : for (MSLane* const internalLane : junction->getInternalLanes()) {
292 56 : myEdgeFilter.insert(&(internalLane->getEdge()));
293 4 : }
294 : } else {
295 12 : WRITE_WARNING("Unknown junction ID '" + junctionID + "' in SSM device edge filter (" + file + "): " + line);
296 : }
297 4 : } else if (line == "") { // ignore empty lines (mostly last line)
298 : } else {
299 12 : WRITE_WARNING("Cannot interpret line in SSM device edge filter (" + file + "): " + line);
300 : }
301 : }
302 20 : }
303 480 : }
304 :
305 : void
306 5382009 : MSDevice_SSM::buildVehicleDevices(SUMOVehicle& v, std::vector<MSVehicleDevice*>& into) {
307 10764018 : if (equippedByDefaultAssignmentOptions(OptionsCont::getOptions(), "ssm", v, false)) {
308 5200 : if (MSGlobals::gUseMesoSim) {
309 12 : WRITE_WARNINGF("SSM Device for vehicle '%' will not be built. (SSMs not supported in MESO)", v.getID());
310 20 : return;
311 : }
312 : // ID for the device
313 5196 : std::string deviceID = "ssm_" + v.getID();
314 :
315 : // Load parameters:
316 :
317 : // Measures and thresholds
318 : std::map<std::string, double> thresholds;
319 5196 : bool success = getMeasuresAndThresholds(v, deviceID, thresholds);
320 5196 : if (!success) {
321 : return;
322 : }
323 :
324 : // TODO: modify trajectory option: "all", "conflictPoints", ("position" && "speed" == "vehState"), "SSMs"!
325 : // Trajectories
326 5188 : bool trajectories = requestsTrajectories(v);
327 :
328 : // detection range
329 5188 : double range = getDetectionRange(v);
330 :
331 : // extra time
332 5188 : double extraTime = getExtraTime(v);
333 :
334 : // File
335 5188 : std::string file = getOutputFilename(v, deviceID);
336 :
337 5188 : const bool useGeo = useGeoCoords(v);
338 :
339 5188 : const bool writePos = writePositions(v);
340 :
341 5188 : const bool writeLanesPos = writeLanesPositions(v);
342 :
343 : std::vector<int> conflictTypeFilter;
344 5188 : success = filterByConflictType(v, deviceID, conflictTypeFilter);
345 5188 : if (!success) {
346 : return;
347 : }
348 :
349 : // Build the device (XXX: who deletes it?)
350 15540 : MSDevice_SSM* device = new MSDevice_SSM(v, deviceID, file, thresholds, trajectories, range, extraTime, useGeo, writePos, writeLanesPos, conflictTypeFilter);
351 5180 : into.push_back(device);
352 :
353 : // Init spatial filter (once)
354 5180 : if (!myEdgeFilterInitialized) {
355 480 : initEdgeFilter();
356 : }
357 5188 : }
358 : }
359 :
360 :
361 676808 : MSDevice_SSM::Encounter::Encounter(const MSVehicle* _ego, const MSVehicle* const _foe, double _begin, double extraTime) :
362 676808 : ego(_ego),
363 676808 : foe(_foe),
364 676808 : egoID(_ego->getID()),
365 676808 : foeID(_foe->getID()),
366 676808 : begin(_begin),
367 676808 : end(-INVALID_DOUBLE),
368 676808 : currentType(ENCOUNTER_TYPE_NOCONFLICT_AHEAD),
369 676808 : remainingExtraTime(extraTime),
370 676808 : egoConflictEntryTime(INVALID_DOUBLE),
371 676808 : egoConflictExitTime(INVALID_DOUBLE),
372 676808 : foeConflictEntryTime(INVALID_DOUBLE),
373 676808 : foeConflictExitTime(INVALID_DOUBLE),
374 : minTTC(INVALID_DOUBLE, Position::INVALID, ENCOUNTER_TYPE_NOCONFLICT_AHEAD, INVALID_DOUBLE, INVALID_DOUBLE),
375 : maxDRAC(INVALID_DOUBLE, Position::INVALID, ENCOUNTER_TYPE_NOCONFLICT_AHEAD, INVALID_DOUBLE, INVALID_DOUBLE),
376 : maxMDRAC(INVALID_DOUBLE, Position::INVALID, ENCOUNTER_TYPE_NOCONFLICT_AHEAD, INVALID_DOUBLE, INVALID_DOUBLE),
377 : PET(INVALID_DOUBLE, Position::INVALID, ENCOUNTER_TYPE_NOCONFLICT_AHEAD, INVALID_DOUBLE, INVALID_DOUBLE),
378 : minPPET(INVALID_DOUBLE, Position::INVALID, ENCOUNTER_TYPE_NOCONFLICT_AHEAD, INVALID_DOUBLE, INVALID_DOUBLE),
379 676808 : closingRequested(false) {
380 : #ifdef DEBUG_ENCOUNTER
381 : if (DEBUG_COND_ENCOUNTER(this)) {
382 : std::cout << "\n" << SIMTIME << " Constructing encounter of '" << ego->getID() << "' and '" << foe->getID() << "'" << std::endl;
383 : }
384 : #endif
385 676808 : }
386 :
387 676808 : MSDevice_SSM::Encounter::~Encounter() {
388 : #ifdef DEBUG_ENCOUNTER
389 : if (DEBUG_COND_ENCOUNTER(this)) {
390 : std::cout << "\n" << SIMTIME << " Destroying encounter of '" << egoID << "' and '" << foeID << "' (begin was " << begin << ")" << std::endl;
391 : }
392 : #endif
393 676808 : }
394 :
395 :
396 : void
397 6323511 : MSDevice_SSM::Encounter::add(double time, const EncounterType type, Position egoX, std::string egoLane, double egoLanePos, Position egoV,
398 : Position foeX, std::string foeLane, double foeLanePos, Position foeV,
399 : Position conflictPoint, double egoDistToConflict, double foeDistToConflict, double ttc, double drac, std::pair<double, double> pet, double ppet, double mdrac) {
400 : #ifdef DEBUG_ENCOUNTER
401 : if (DEBUG_COND_ENCOUNTER(this))
402 : std::cout << time << " Adding data point for encounter of '" << egoID << "' and '" << foeID << "':\n"
403 : << "type=" << type << ", egoDistToConflict=" << (egoDistToConflict == INVALID_DOUBLE ? "NA" : ::toString(egoDistToConflict))
404 : << ", foeDistToConflict=" << (foeDistToConflict == INVALID_DOUBLE ? "NA" : ::toString(foeDistToConflict))
405 : << ",\nttc=" << (ttc == INVALID_DOUBLE ? "NA" : ::toString(ttc))
406 : << ", drac=" << (drac == INVALID_DOUBLE ? "NA" : ::toString(drac))
407 : << ", pet=" << (pet.second == INVALID_DOUBLE ? "NA" : ::toString(pet.second))
408 : << std::endl;
409 : #endif
410 6323511 : currentType = type;
411 :
412 6323511 : timeSpan.push_back(time);
413 6323511 : typeSpan.push_back(type);
414 6323511 : egoTrajectory.x.push_back(egoX);
415 6323511 : egoTrajectory.lane.push_back(egoLane);
416 6323511 : egoTrajectory.lanePos.push_back(egoLanePos);
417 6323511 : egoTrajectory.v.push_back(egoV);
418 6323511 : foeTrajectory.x.push_back(foeX);
419 6323511 : foeTrajectory.lane.push_back(foeLane);
420 6323511 : foeTrajectory.lanePos.push_back(foeLanePos);
421 6323511 : foeTrajectory.v.push_back(foeV);
422 6323511 : conflictPointSpan.push_back(conflictPoint);
423 6323511 : egoDistsToConflict.push_back(egoDistToConflict);
424 6323511 : foeDistsToConflict.push_back(foeDistToConflict);
425 :
426 6323511 : TTCspan.push_back(ttc);
427 6323511 : if (ttc != INVALID_DOUBLE && (ttc < minTTC.value || minTTC.value == INVALID_DOUBLE)) {
428 298381 : minTTC.value = ttc;
429 298381 : minTTC.time = time;
430 298381 : minTTC.pos = conflictPoint;
431 596748 : minTTC.type = ttc <= 0 ? ENCOUNTER_TYPE_COLLISION : type;
432 298381 : minTTC.speed = egoV.distanceTo(Position(0, 0));
433 : }
434 :
435 6323511 : DRACspan.push_back(drac);
436 6323511 : if (drac != INVALID_DOUBLE && (drac > maxDRAC.value || maxDRAC.value == INVALID_DOUBLE)) {
437 98814 : maxDRAC.value = drac;
438 98814 : maxDRAC.time = time;
439 98814 : maxDRAC.pos = conflictPoint;
440 98814 : maxDRAC.type = type;
441 98814 : maxDRAC.speed = egoV.distanceTo(Position(0, 0));
442 : }
443 :
444 6323511 : if (pet.first != INVALID_DOUBLE && (PET.value >= pet.second || PET.value == INVALID_DOUBLE)) {
445 428 : PET.value = pet.second;
446 428 : PET.time = pet.first;
447 428 : PET.pos = conflictPoint;
448 840 : PET.type = PET.value <= 0 ? ENCOUNTER_TYPE_COLLISION : type;
449 428 : PET.speed = egoV.distanceTo(Position(0, 0));
450 : }
451 6323511 : PPETspan.push_back(ppet);
452 6323511 : if (ppet != INVALID_DOUBLE && (ppet < minPPET.value || minPPET.value == INVALID_DOUBLE)) {
453 1936 : minPPET.value = ppet;
454 1936 : minPPET.time = time;
455 1936 : minPPET.pos = conflictPoint;
456 3823 : minPPET.type = ppet <= 0 ? ENCOUNTER_TYPE_COLLISION : type;
457 1936 : minPPET.speed = egoV.distanceTo(Position(0, 0));
458 : }
459 6323511 : MDRACspan.push_back(mdrac);
460 6323511 : if (mdrac != INVALID_DOUBLE && (mdrac > maxMDRAC.value || maxMDRAC.value == INVALID_DOUBLE)) {
461 7817 : maxMDRAC.value = mdrac;
462 7817 : maxMDRAC.time = time;
463 7817 : maxMDRAC.pos = conflictPoint;
464 7817 : maxMDRAC.type = type;
465 7817 : maxMDRAC.speed = egoV.distanceTo(Position(0, 0));
466 : }
467 6323511 : }
468 :
469 :
470 : void
471 6488810 : MSDevice_SSM::Encounter::resetExtraTime(double value) {
472 6488810 : remainingExtraTime = value;
473 6488810 : }
474 :
475 :
476 : void
477 454861 : MSDevice_SSM::Encounter::countDownExtraTime(double amount) {
478 454861 : remainingExtraTime -= amount;
479 454861 : }
480 :
481 :
482 : double
483 508722 : MSDevice_SSM::Encounter::getRemainingExtraTime() const {
484 508722 : return remainingExtraTime;
485 : }
486 :
487 :
488 6943671 : MSDevice_SSM::EncounterApproachInfo::EncounterApproachInfo(Encounter* e) :
489 6943671 : encounter(e),
490 6943671 : type(ENCOUNTER_TYPE_NOCONFLICT_AHEAD),
491 6943671 : conflictPoint(Position::INVALID),
492 6943671 : egoConflictEntryDist(INVALID_DOUBLE),
493 6943671 : foeConflictEntryDist(INVALID_DOUBLE),
494 6943671 : egoConflictExitDist(INVALID_DOUBLE),
495 6943671 : foeConflictExitDist(INVALID_DOUBLE),
496 6943671 : egoEstimatedConflictEntryTime(INVALID_DOUBLE),
497 6943671 : foeEstimatedConflictEntryTime(INVALID_DOUBLE),
498 6943671 : egoEstimatedConflictExitTime(INVALID_DOUBLE),
499 6943671 : foeEstimatedConflictExitTime(INVALID_DOUBLE),
500 6943671 : egoConflictAreaLength(INVALID_DOUBLE),
501 6943671 : foeConflictAreaLength(INVALID_DOUBLE),
502 6943671 : ttc(INVALID_DOUBLE),
503 6943671 : drac(INVALID_DOUBLE),
504 6943671 : mdrac(INVALID_DOUBLE),
505 6943671 : pet(std::make_pair(INVALID_DOUBLE, INVALID_DOUBLE)),
506 6943671 : ppet(INVALID_DOUBLE) {
507 6943671 : }
508 :
509 :
510 : void
511 2533061 : MSDevice_SSM::updateAndWriteOutput() {
512 2533061 : if (myHolder.isOnRoad()) {
513 1834409 : update();
514 : // Write out past conflicts
515 1834409 : flushConflicts();
516 : } else {
517 : #ifdef DEBUG_SSM
518 : if (DEBUG_COND(myHolderMS))
519 : std::cout << "\n" << SIMTIME << " Device '" << getID() << "' updateAndWriteOutput()\n"
520 : << " Holder is off-road! Calling resetEncounters()."
521 : << std::endl;
522 : #endif
523 698652 : resetEncounters();
524 : // Write out past conflicts
525 698652 : flushConflicts(true);
526 : }
527 2533061 : }
528 :
529 : void
530 1834409 : MSDevice_SSM::update() {
531 : #ifdef DEBUG_SSM
532 : if (DEBUG_COND(myHolderMS))
533 : std::cout << "\n" << SIMTIME << " Device '" << getID() << "' update()\n"
534 : << "Size of myActiveEncounters: " << myActiveEncounters.size()
535 : << "\nSize of myPastConflicts: " << myPastConflicts.size()
536 : << std::endl;
537 : #endif
538 : // Scan surroundings for other vehicles
539 : FoeInfoMap foes;
540 : bool scan = true;
541 1834409 : if (myEdgeFilterActive) {
542 : // Is the ego vehicle inside the filtered edge subset?
543 2800 : const MSEdge* egoEdge = &((*myHolderMS).getLane()->getEdge());
544 : scan = myEdgeFilter.find(egoEdge) != myEdgeFilter.end();
545 : }
546 2800 : if (scan) {
547 1831957 : findSurroundingVehicles(*myHolderMS, myRange, foes);
548 : }
549 :
550 : #ifdef DEBUG_SSM
551 : if (DEBUG_COND(myHolderMS)) {
552 : if (foes.size() > 0) {
553 : std::cout << "Scanned surroundings: Found potential foes:\n";
554 : for (FoeInfoMap::const_iterator i = foes.begin(); i != foes.end(); ++i) {
555 : std::cout << i->first->getID() << " ";
556 : }
557 : std::cout << std::endl;
558 : } else {
559 : std::cout << "Scanned surroundings: No potential conflict could be identified." << std::endl;
560 : }
561 : }
562 : #endif
563 :
564 : // Update encounters and conflicts -> removes all foes (and deletes corresponding FoeInfos) for which already a corresponding encounter exists
565 1834409 : processEncounters(foes);
566 :
567 : // Make new encounters for all foes, which were not removed by processEncounters (and deletes corresponding FoeInfos)
568 1834409 : createEncounters(foes);
569 : foes.clear();
570 :
571 : // Compute "global SSMs" (only computed once per time-step)
572 1834409 : computeGlobalMeasures();
573 :
574 1834409 : }
575 :
576 :
577 : void
578 1834409 : MSDevice_SSM::computeGlobalMeasures() {
579 1834409 : if (myComputeBR || myComputeSGAP || myComputeTGAP) {
580 68872 : myGlobalMeasuresTimeSpan.push_back(SIMTIME);
581 68872 : if (myWritePositions) {
582 1120 : myGlobalMeasuresPositions.push_back(myHolderMS->getPosition());
583 : }
584 68872 : if (myWriteLanesPositions) {
585 1120 : myGlobalMeasuresLaneIDs.push_back(myHolderMS->getLane()->getID());
586 1120 : myGlobalMeasuresLanesPositions.push_back(myHolderMS->getPositionOnLane());
587 : }
588 68872 : if (myComputeBR) {
589 68872 : double br = MAX2(-myHolderMS->getAcceleration(), 0.0);
590 68872 : if (br > myMaxBR.second) {
591 2465 : myMaxBR = std::make_pair(std::make_pair(SIMTIME, myHolderMS->getPosition()), br);
592 : }
593 68872 : myBRspan.push_back(br);
594 : }
595 :
596 : double leaderSearchDist = 0;
597 : std::pair<const MSVehicle*, double> leader(nullptr, 0.);
598 68872 : if (myComputeSGAP) {
599 67024 : leaderSearchDist = myThresholds["SGAP"];
600 : }
601 68872 : if (myComputeTGAP) {
602 134048 : leaderSearchDist = MAX2(leaderSearchDist, myThresholds["TGAP"] * myHolderMS->getSpeed());
603 : }
604 :
605 68872 : if (leaderSearchDist > 0.) {
606 67024 : leader = myHolderMS->getLeader(leaderSearchDist);
607 : }
608 :
609 : // negative gap indicates theoretical car-following relationship for paths that cross at an intersection
610 68872 : if (myComputeSGAP) {
611 67024 : if (leader.first == nullptr || leader.second < 0) {
612 52770 : mySGAPspan.push_back(INVALID_DOUBLE);
613 : } else {
614 14254 : double sgap = leader.second + myHolder.getVehicleType().getMinGap();
615 14254 : mySGAPspan.push_back(sgap);
616 14254 : if (sgap < myMinSGAP.first.second) {
617 13618 : myMinSGAP = std::make_pair(std::make_pair(std::make_pair(SIMTIME, myHolderMS->getPosition()), sgap), leader.first->getID());
618 : }
619 : }
620 : }
621 :
622 68872 : if (myComputeTGAP) {
623 67024 : if (leader.first == nullptr || myHolderMS->getSpeed() == 0. || leader.second < 0) {
624 52975 : myTGAPspan.push_back(INVALID_DOUBLE);
625 : } else {
626 14049 : const double tgap = (leader.second + myHolder.getVehicleType().getMinGap()) / myHolderMS->getSpeed();
627 14049 : myTGAPspan.push_back(tgap);
628 14049 : if (tgap < myMinTGAP.first.second) {
629 14706 : myMinTGAP = std::make_pair(std::make_pair(std::make_pair(SIMTIME, myHolderMS->getPosition()), tgap), leader.first->getID());
630 : }
631 : }
632 : }
633 :
634 : }
635 1834409 : }
636 :
637 :
638 : void
639 1834409 : MSDevice_SSM::createEncounters(FoeInfoMap& foes) {
640 : #ifdef DEBUG_SSM
641 : if (DEBUG_COND(myHolderMS)) {
642 : std::cout << "\n" << SIMTIME << " Device '" << getID() << "' createEncounters()" << std::endl;
643 : std::cout << "New foes:\n";
644 : for (FoeInfoMap::const_iterator vi = foes.begin(); vi != foes.end(); ++vi) {
645 : std::cout << vi->first->getID() << "\n";
646 : }
647 : std::cout << std::endl;
648 : }
649 : #endif
650 :
651 2511217 : for (FoeInfoMap::const_iterator foe = foes.begin(); foe != foes.end(); ++foe) {
652 676808 : Encounter* e = new Encounter(myHolderMS, foe->first, SIMTIME, myExtraTime);
653 676808 : if (updateEncounter(e, foe->second)) {
654 56676 : if (myOldestActiveEncounterBegin == INVALID_DOUBLE) {
655 : assert(myActiveEncounters.empty());
656 6411 : myOldestActiveEncounterBegin = e->begin;
657 : }
658 : assert(myOldestActiveEncounterBegin <= e->begin);
659 56676 : myActiveEncounters.push_back(e);
660 : } else {
661 : // Discard encounters, where one vehicle already left the conflict area
662 620132 : delete e;
663 : }
664 : // free foeInfo
665 676808 : delete foe->second;
666 : }
667 1834409 : }
668 :
669 :
670 : void
671 703832 : MSDevice_SSM::resetEncounters() {
672 : // Call processEncounters() with empty vehicle set
673 : FoeInfoMap foes;
674 : // processEncounters with empty argument closes all encounters
675 703832 : processEncounters(foes, true);
676 703832 : }
677 :
678 :
679 : void
680 2538241 : MSDevice_SSM::processEncounters(FoeInfoMap& foes, bool forceClose) {
681 : #ifdef DEBUG_SSM
682 : if (DEBUG_COND(myHolderMS)) {
683 : std::cout << "\n" << SIMTIME << " Device '" << getID() << "' processEncounters(forceClose = " << forceClose << ")" << std::endl;
684 : std::cout << "Currently present foes:\n";
685 : for (FoeInfoMap::const_iterator vi = foes.begin(); vi != foes.end(); ++vi) {
686 : std::cout << vi->first->getID() << "\n";
687 : }
688 : std::cout << std::endl;
689 : }
690 : #endif
691 :
692 : // Run through active encounters. If corresponding foe is still present in foes update and
693 : // remove foe from foes. If the foe has disappeared close the encounter (check if it qualifies
694 : // as a conflict and in case transfer it to myPastConflicts).
695 : // Afterwards run through remaining elements in foes and create new encounters for them.
696 : EncounterVector::iterator ei = myActiveEncounters.begin();
697 8858965 : while (ei != myActiveEncounters.end()) {
698 6320724 : Encounter* e = *ei;
699 : // check whether foe is still on net
700 6320724 : bool foeExists = !(MSNet::getInstance()->getVehicleControl().getVehicle(e->foeID) == nullptr);
701 6320724 : if (!foeExists) {
702 5768 : e->foe = nullptr;
703 : }
704 6320724 : if (foes.find(e->foe) != foes.end()) {
705 5812002 : FoeInfo* foeInfo = foes[e->foe];
706 5812002 : EncounterType prevType = e->currentType;
707 : // Update encounter
708 5812002 : updateEncounter(e, foeInfo);
709 5812002 : if (prevType == ENCOUNTER_TYPE_BOTH_LEFT_CONFLICT_AREA
710 28 : && e->currentType != ENCOUNTER_TYPE_BOTH_LEFT_CONFLICT_AREA) {
711 : // The encounter classification switched from BOTH_LEFT to another
712 : // => Start new encounter (i.e. don't erase the foe, don't delete the foeInfo and request closing)
713 : // Note that updateEncounter did not add another trajectory point in this case.
714 : #ifdef DEBUG_SSM
715 : if (DEBUG_COND(myHolderMS)) {
716 : std::cout << " Requesting encounter closure because both left conflict area of previous encounter but another encounter lies ahead." << std::endl;
717 : }
718 : #endif
719 28 : e->closingRequested = true;
720 : } else {
721 : // Erase foes which were already encountered and should not be used to open a new conflict
722 5811974 : delete foeInfo;
723 : foes.erase(e->foe);
724 : }
725 : } else {
726 508722 : if (e->getRemainingExtraTime() <= 0. || forceClose || !foeExists) {
727 : // Close encounter, extra time has expired (deletes e if it does not qualify as conflict)
728 : #ifdef DEBUG_SSM
729 : if (DEBUG_COND(myHolderMS)) {
730 : std::cout << " Requesting encounter closure because..." << std::endl;
731 : if (e->getRemainingExtraTime() <= 0.) {
732 : std::cout << " ... extra time elapsed." << std::endl;
733 : } else if (forceClose) {
734 : std::cout << " ... closing was forced." << std::endl;
735 : } else {
736 : std::cout << " ... foe disappeared." << std::endl;
737 : }
738 : }
739 : #endif
740 53861 : e->closingRequested = true;
741 : } else {
742 454861 : updateEncounter(e, nullptr); // counts down extra time
743 : }
744 : }
745 :
746 6320724 : if (e->closingRequested) {
747 56676 : double eBegin = e->begin;
748 56676 : closeEncounter(e);
749 56676 : ei = myActiveEncounters.erase(ei);
750 56676 : if (myActiveEncounters.empty()) {
751 6411 : myOldestActiveEncounterBegin = INVALID_DOUBLE;
752 50265 : } else if (eBegin == myOldestActiveEncounterBegin) {
753 : // Erased the oldest encounter, update myOldestActiveEncounterBegin
754 : auto i = myActiveEncounters.begin();
755 15834 : myOldestActiveEncounterBegin = (*i++)->begin;
756 43617 : while (i != myActiveEncounters.end()) {
757 31494 : myOldestActiveEncounterBegin = MIN2(myOldestActiveEncounterBegin, (*i++)->begin);
758 : }
759 : }
760 : } else {
761 : ++ei;
762 : }
763 : }
764 2538241 : }
765 :
766 :
767 : bool
768 56676 : MSDevice_SSM::qualifiesAsConflict(Encounter* e) {
769 : // Check if conflict measure thresholds are exceeded (to decide whether to keep the encounter for writing out)
770 : #ifdef DEBUG_SSM
771 : if (DEBUG_COND(myHolderMS))
772 : std::cout << SIMTIME << " qualifiesAsConflict() for encounter of vehicles '"
773 : << e->egoID << "' and '" << e->foeID
774 : << "'" << std::endl;
775 : #endif
776 :
777 57156 : if (myComputePET && e->PET.value != INVALID_DOUBLE && e->PET.value <= myThresholds["PET"]) {
778 : return true;
779 : }
780 126936 : if (myComputeTTC && e->minTTC.value != INVALID_DOUBLE && e->minTTC.value <= myThresholds["TTC"]) {
781 : return true;
782 : }
783 117215 : if (myComputeDRAC && e->maxDRAC.value != INVALID_DOUBLE && e->maxDRAC.value >= myThresholds["DRAC"]) {
784 : return true;
785 : }
786 53326 : if (myComputePPET && e->minPPET.value != INVALID_DOUBLE && e->minPPET.value <= myThresholds["PPET"]) {
787 : return true;
788 : }
789 55364 : if (myComputeMDRAC && e->maxMDRAC.value != INVALID_DOUBLE && e->maxMDRAC.value >= myThresholds["MDRAC"]) {
790 : return true;
791 : }
792 : return false;
793 : }
794 :
795 :
796 : void
797 56676 : MSDevice_SSM::closeEncounter(Encounter* e) {
798 : assert(e->size() > 0);
799 : // erase pointers (encounter is stored before being destroyed and pointers could become invalid)
800 56676 : e->ego = nullptr;
801 56676 : e->foe = nullptr;
802 56676 : e->end = e->timeSpan.back();
803 56676 : bool wasConflict = qualifiesAsConflict(e);
804 : #ifdef DEBUG_SSM
805 : if (DEBUG_COND(myHolderMS)) {
806 : std::cout << SIMTIME << " closeEncounter() of vehicles '"
807 : << e->egoID << "' and '" << e->foeID
808 : << "' (was ranked as " << (wasConflict ? "conflict" : "non-conflict") << ")" << std::endl;
809 : }
810 : #endif
811 56676 : if (wasConflict) {
812 5508 : myPastConflicts.push(e);
813 : #ifdef DEBUG_SSM
814 : if (!myPastConflicts.empty()) {
815 : if (DEBUG_COND(myHolderMS)) {
816 : std::cout << "pastConflictsQueue of veh '" << myHolderMS->getID() << "':\n";
817 : }
818 : auto myPastConflicts_bak = myPastConflicts;
819 : double lastBegin = myPastConflicts.top()->begin;
820 : while (!myPastConflicts.empty()) {
821 : auto c = myPastConflicts.top();
822 : myPastConflicts.pop();
823 : if (DEBUG_COND(myHolderMS)) {
824 : std::cout << " Conflict with foe '" << c->foe << "' (time=" << c->begin << "-" << c->end << ")\n";
825 : }
826 : if (c->begin < lastBegin) {
827 : std::cout << " Queue corrupt...\n";
828 : assert(false);
829 : }
830 : lastBegin = c->begin;
831 : }
832 : std::cout << std::endl;
833 : myPastConflicts = myPastConflicts_bak;
834 : }
835 : #endif
836 : } else {
837 51168 : delete e;
838 : }
839 56676 : return;
840 : }
841 :
842 :
843 : bool
844 6943671 : MSDevice_SSM::updateEncounter(Encounter* e, FoeInfo* foeInfo) {
845 : #ifdef DEBUG_ENCOUNTER
846 : if (DEBUG_COND_ENCOUNTER(e)) {
847 : std::cout << SIMTIME << " updateEncounter() of vehicles '" << e->egoID << "' and '" << e->foeID << "'\n";
848 : }
849 : #endif
850 : assert(e->foe != 0);
851 :
852 : // Struct storing distances (determined in classifyEncounter()) and times to potential conflict entry / exit (in estimateConflictTimes())
853 6943671 : EncounterApproachInfo eInfo(e);
854 :
855 : // Classify encounter type based on the present information
856 : // More details on follower/lead relation are determined in a second step below, see estimateConflictTimes()
857 : // If a crossing situation is ongoing (i.e. one of the vehicles entered the conflict area already in the last step,
858 : // this is handled by passedEncounter by only tracing the vehicle's movements)
859 : // The further development of the encounter type is done in checkConflictEntryAndExit()
860 6943671 : eInfo.type = classifyEncounter(foeInfo, eInfo);
861 :
862 : // Discard new encounters, where one vehicle has already left the conflict area
863 6943671 : if (eInfo.encounter->size() == 0) {
864 676808 : if (eInfo.type == ENCOUNTER_TYPE_EGO_LEFT_CONFLICT_AREA
865 676808 : || eInfo.type == ENCOUNTER_TYPE_FOE_LEFT_CONFLICT_AREA) {
866 : // Signalize to discard
867 : return false;
868 : }
869 : }
870 :
871 6943671 : if (eInfo.type == ENCOUNTER_TYPE_NOCONFLICT_AHEAD) {
872 : // At this state, eInfo.type == ENCOUNTER_TYPE_NOCONFLICT_AHEAD implies that the foe
873 : // is either out of the device's range or its route does not interfere with the ego's route.
874 : #ifdef DEBUG_ENCOUNTER
875 : if (DEBUG_COND_ENCOUNTER(e)) {
876 : std::cout << SIMTIME << " Encounter of vehicles '" << e->egoID << "' and '" << e->foeID << "' does not imply any conflict.\n";
877 : }
878 : #endif
879 1073782 : updatePassedEncounter(e, foeInfo, eInfo);
880 : // return;
881 5869889 : } else if (eInfo.type == ENCOUNTER_TYPE_EGO_ENTERED_CONFLICT_AREA
882 : || eInfo.type == ENCOUNTER_TYPE_FOE_ENTERED_CONFLICT_AREA
883 : || eInfo.type == ENCOUNTER_TYPE_EGO_LEFT_CONFLICT_AREA
884 : || eInfo.type == ENCOUNTER_TYPE_FOE_LEFT_CONFLICT_AREA
885 : || eInfo.type == ENCOUNTER_TYPE_BOTH_ENTERED_CONFLICT_AREA
886 5869889 : || eInfo.type == ENCOUNTER_TYPE_BOTH_LEFT_CONFLICT_AREA) {
887 : // Ongoing encounter. Treat with update passed encounter (trace covered distances)
888 : // eInfo.type only holds the previous type
889 13520 : updatePassedEncounter(e, foeInfo, eInfo);
890 :
891 : // Estimate times until a possible conflict / collision
892 13520 : estimateConflictTimes(eInfo);
893 :
894 : } else {
895 : // Estimate times until a possible conflict / collision
896 : // Not all are used for all types of encounters:
897 : // Follow/lead situation doesn't need them at all, currently (might change if more SSMs are implemented).
898 : // Crossing / Merging calculates entry times to determine leader/follower and calculates the exit time for the leader.
899 5856369 : estimateConflictTimes(eInfo);
900 :
901 : // reset the remaining extra time (foe could have re-entered the device range after beginning extra time countdown already)
902 5856369 : e->resetExtraTime(myExtraTime);
903 : }
904 :
905 : // update entry/exit times for conflict area
906 6943671 : checkConflictEntryAndExit(eInfo);
907 6943671 : if (e->size() == 0) {
908 : #ifdef DEBUG_ENCOUNTER
909 : if (DEBUG_COND_ENCOUNTER(e)) {
910 : std::cout << SIMTIME << " type when creating encounter: " << eInfo.type << "\n";
911 : }
912 : #endif
913 676808 : if (eInfo.type == ENCOUNTER_TYPE_FOE_LEFT_CONFLICT_AREA
914 : || eInfo.type == ENCOUNTER_TYPE_EGO_LEFT_CONFLICT_AREA
915 676808 : || eInfo.type == ENCOUNTER_TYPE_BOTH_LEFT_CONFLICT_AREA
916 676724 : || eInfo.type == ENCOUNTER_TYPE_NOCONFLICT_AHEAD
917 56676 : || eInfo.type == ENCOUNTER_TYPE_BOTH_ENTERED_CONFLICT_AREA) {
918 : return false;
919 : }
920 : }
921 :
922 : // update (x,y)-coords of conflict point
923 6323539 : determineConflictPoint(eInfo);
924 :
925 : // Compute SSMs
926 6323539 : computeSSMs(eInfo);
927 :
928 6323539 : if (e->currentType == ENCOUNTER_TYPE_BOTH_LEFT_CONFLICT_AREA
929 6147 : && eInfo.type != ENCOUNTER_TYPE_BOTH_LEFT_CONFLICT_AREA) {
930 : // Don't add a point which switches back to a different encounter type from a passed encounter.
931 : // For this situation this encounter will be closed and a new encounter will be created,
932 : // @see correspondingly conditionalized code in processEncounters()
933 28 : e->currentType = eInfo.type;
934 : } else {
935 : // Add current states to trajectories and update type
936 18970533 : e->add(SIMTIME, eInfo.type, e->ego->getPosition(), e->ego->getLane()->getID(), e->ego->getPositionOnLane(), e->ego->getVelocityVector(),
937 12647022 : e->foe->getPosition(), e->foe->getLane()->getID(), e->foe->getPositionOnLane(), e->foe->getVelocityVector(),
938 : eInfo.conflictPoint, eInfo.egoConflictEntryDist, eInfo.foeConflictEntryDist, eInfo.ttc, eInfo.drac, eInfo.pet, eInfo.ppet, eInfo.mdrac);
939 : }
940 : // Keep encounter
941 : return true;
942 : }
943 :
944 :
945 : void
946 6323539 : MSDevice_SSM::determineConflictPoint(EncounterApproachInfo& eInfo) {
947 : /* Calculates the (x,y)-coordinate for the eventually predicted conflict point and stores the result in
948 : * eInfo.conflictPoint. In case of MERGING and CROSSING, this is the entry point to conflict area for follower
949 : * In case of FOLLOWING it is the position of leader's back. */
950 :
951 : #ifdef DEBUG_SSM
952 : if (DEBUG_COND(eInfo.encounter->ego)) {
953 : std::cout << SIMTIME << " determineConflictPoint()" << std::endl;
954 : }
955 : #endif
956 :
957 : const EncounterType& type = eInfo.type;
958 6323539 : const Encounter* e = eInfo.encounter;
959 : if (type == ENCOUNTER_TYPE_BOTH_LEFT_CONFLICT_AREA
960 : || type == ENCOUNTER_TYPE_BOTH_ENTERED_CONFLICT_AREA
961 6323539 : || type == ENCOUNTER_TYPE_COLLISION) {
962 : // Both vehicles have already past the conflict entry.
963 9303 : if (e->size() == 0) {
964 0 : eInfo.conflictPoint = e->ego->getPosition();
965 0 : WRITE_WARNINGF(TL("SSM device of vehicle '%' encountered an unexpected conflict with foe % at time=%. Please review your vehicle behavior settings."), e->egoID, e->foeID, time2string(SIMSTEP));
966 0 : return;
967 : }
968 : assert(e->size() > 0); // A new encounter should not be created if both vehicles already entered the conflict area
969 9303 : eInfo.conflictPoint = e->conflictPointSpan.back();
970 : } else if (type == ENCOUNTER_TYPE_CROSSING_FOLLOWER
971 : || type == ENCOUNTER_TYPE_MERGING_FOLLOWER
972 : || type == ENCOUNTER_TYPE_FOE_ENTERED_CONFLICT_AREA
973 : || type == ENCOUNTER_TYPE_FOE_LEFT_CONFLICT_AREA) {
974 46697 : eInfo.conflictPoint = e->ego->getPositionAlongBestLanes(eInfo.egoConflictEntryDist);
975 : } else if (type == ENCOUNTER_TYPE_CROSSING_LEADER
976 : || type == ENCOUNTER_TYPE_MERGING_LEADER
977 : || type == ENCOUNTER_TYPE_EGO_ENTERED_CONFLICT_AREA
978 : || type == ENCOUNTER_TYPE_EGO_LEFT_CONFLICT_AREA) {
979 56747 : eInfo.conflictPoint = e->foe->getPositionAlongBestLanes(eInfo.foeConflictEntryDist);
980 : } else if (type == ENCOUNTER_TYPE_FOLLOWING_FOLLOWER) {
981 2417052 : eInfo.conflictPoint = e->foe->getPosition(-e->foe->getLength());
982 : } else if (type == ENCOUNTER_TYPE_FOLLOWING_LEADER) {
983 2412446 : eInfo.conflictPoint = e->ego->getPosition(-e->ego->getLength());
984 : } else if (type == ENCOUNTER_TYPE_ONCOMING) {
985 1890 : eInfo.conflictPoint = (e->ego->getPosition() + e->foe->getPosition()) * 0.5;
986 : } else {
987 : #ifdef DEBUG_SSM
988 : if (DEBUG_COND(eInfo.encounter->ego)) {
989 : std::cout << "No conflict point associated with encounter type " << type << std::endl;
990 : }
991 : #endif
992 : return;
993 : }
994 :
995 : #ifdef DEBUG_SSM
996 : if (DEBUG_COND(eInfo.encounter->ego)) {
997 : std::cout << " Conflict at " << eInfo.conflictPoint << std::endl;
998 : }
999 : #endif
1000 : }
1001 :
1002 :
1003 : void
1004 5869889 : MSDevice_SSM::estimateConflictTimes(EncounterApproachInfo& eInfo) {
1005 :
1006 : EncounterType& type = eInfo.type;
1007 5869889 : Encounter* e = eInfo.encounter;
1008 :
1009 : assert(type != ENCOUNTER_TYPE_NOCONFLICT_AHEAD); // arrival times not defined, if no conflict is ahead.
1010 : #ifdef DEBUG_SSM
1011 : if (DEBUG_COND(e->ego))
1012 : std::cout << SIMTIME << " estimateConflictTimes() for ego '" << e->egoID << "' and foe '" << e->foeID << "'\n"
1013 : << " encounter type: " << eInfo.type << "\n"
1014 : << " egoConflictEntryDist=" << writeNA(eInfo.egoConflictEntryDist)
1015 : << " foeConflictEntryDist=" << writeNA(eInfo.foeConflictEntryDist)
1016 : << " egoConflictExitDist=" << writeNA(eInfo.egoConflictExitDist)
1017 : << " foeConflictExitDist=" << writeNA(eInfo.foeConflictExitDist)
1018 : << "\n ego speed=" << e->ego->getSpeed()
1019 : << ", foe speed=" << e->foe->getSpeed()
1020 : << std::endl;
1021 : #endif
1022 5869889 : if (type == ENCOUNTER_TYPE_COLLISION) {
1023 : #ifdef DEBUG_SSM
1024 : eInfo.egoEstimatedConflictEntryTime = 0;
1025 : eInfo.foeEstimatedConflictEntryTime = 0;
1026 : if (DEBUG_COND(e->ego))
1027 : std::cout << " encouter type " << type << " -> no exit times to be calculated."
1028 : << std::endl;
1029 : #endif
1030 : return;
1031 : }
1032 :
1033 5869873 : if (type == ENCOUNTER_TYPE_FOLLOWING_FOLLOWER || type == ENCOUNTER_TYPE_FOLLOWING_LEADER || type == ENCOUNTER_TYPE_MERGING_ADJACENT || type == ENCOUNTER_TYPE_ON_ADJACENT_LANES) {
1034 : // No need to know the times until ...ConflictDistEntry, currently. They would correspond to an estimated time headway or similar.
1035 : // TTC must take into account the movement of the leader, as would DRAC, PET doesn't need the time either, since it uses aposteriori
1036 : // values.
1037 : #ifdef DEBUG_SSM
1038 : if (DEBUG_COND(e->ego))
1039 : std::cout << " encouter type " << type << " -> no entry/exit times to be calculated."
1040 : << std::endl;
1041 : #endif
1042 : return;
1043 : }
1044 :
1045 : assert(type == ENCOUNTER_TYPE_MERGING || type == ENCOUNTER_TYPE_CROSSING
1046 : || type == ENCOUNTER_TYPE_EGO_ENTERED_CONFLICT_AREA
1047 : || type == ENCOUNTER_TYPE_FOE_ENTERED_CONFLICT_AREA
1048 : || type == ENCOUNTER_TYPE_EGO_LEFT_CONFLICT_AREA
1049 : || type == ENCOUNTER_TYPE_FOE_LEFT_CONFLICT_AREA
1050 : || type == ENCOUNTER_TYPE_BOTH_ENTERED_CONFLICT_AREA
1051 : || type == ENCOUNTER_TYPE_BOTH_LEFT_CONFLICT_AREA
1052 : || type == ENCOUNTER_TYPE_ONCOMING);
1053 :
1054 : // Determine exit distances
1055 81676 : if (type == ENCOUNTER_TYPE_MERGING || type == ENCOUNTER_TYPE_ONCOMING) {
1056 4023 : eInfo.egoConflictExitDist = eInfo.egoConflictEntryDist + e->ego->getVehicleType().getLength();
1057 4023 : eInfo.foeConflictExitDist = eInfo.foeConflictEntryDist + e->foe->getVehicleType().getLength();
1058 : } else {
1059 77653 : eInfo.egoConflictExitDist = eInfo.egoConflictEntryDist + eInfo.egoConflictAreaLength + e->ego->getVehicleType().getLength();
1060 77653 : eInfo.foeConflictExitDist = eInfo.foeConflictEntryDist + eInfo.foeConflictAreaLength + e->foe->getVehicleType().getLength();
1061 : }
1062 :
1063 : // Estimate entry times to stipulate a leader / follower relation for the encounter.
1064 81676 : if (eInfo.egoConflictEntryDist > NUMERICAL_EPS) {
1065 136084 : eInfo.egoEstimatedConflictEntryTime = e->ego->getCarFollowModel().estimateArrivalTime(eInfo.egoConflictEntryDist, e->ego->getSpeed(), e->ego->getMaxSpeedOnLane(), MIN2(0., e->ego->getAcceleration()));
1066 : assert(eInfo.egoEstimatedConflictEntryTime > 0.);
1067 : } else {
1068 : // ego already entered conflict area
1069 13634 : eInfo.egoEstimatedConflictEntryTime = 0.;
1070 : }
1071 81676 : if (eInfo.foeConflictEntryDist > NUMERICAL_EPS) {
1072 152942 : eInfo.foeEstimatedConflictEntryTime = e->foe->getCarFollowModel().estimateArrivalTime(eInfo.foeConflictEntryDist, e->foe->getSpeed(), e->foe->getMaxSpeedOnLane(), MIN2(0., e->foe->getAcceleration()));
1073 : assert(eInfo.foeEstimatedConflictEntryTime > 0.);
1074 : } else {
1075 : // foe already entered conflict area
1076 5205 : eInfo.foeEstimatedConflictEntryTime = 0.;
1077 : }
1078 :
1079 81676 : if (type == ENCOUNTER_TYPE_ONCOMING) {
1080 1890 : eInfo.egoEstimatedConflictEntryTime = eInfo.egoConflictEntryDist / (e->ego->getSpeed() + e->foe->getSpeed());
1081 1890 : eInfo.foeEstimatedConflictEntryTime = eInfo.egoEstimatedConflictEntryTime;
1082 : }
1083 :
1084 : #ifdef DEBUG_SSM
1085 : if (DEBUG_COND(e->ego))
1086 : std::cout << " Conflict type: " << encounterToString(type) << "\n"
1087 : << " egoConflictEntryTime=" << (eInfo.egoEstimatedConflictEntryTime == INVALID_DOUBLE ? "NA" : ::toString(eInfo.egoEstimatedConflictEntryTime))
1088 : << ", foeConflictEntryTime=" << (eInfo.foeEstimatedConflictEntryTime == INVALID_DOUBLE ? "NA" : ::toString(eInfo.foeEstimatedConflictEntryTime))
1089 : << std::endl;
1090 : #endif
1091 :
1092 : // Estimate exit times from conflict area for leader / follower.
1093 : // assume vehicles start to accelerate after entring the intersection
1094 81676 : if (eInfo.egoConflictExitDist >= 0.) {
1095 142850 : eInfo.egoEstimatedConflictExitTime = e->ego->getCarFollowModel().estimateArrivalTime(eInfo.egoConflictExitDist, e->ego->getSpeed(), e->ego->getMaxSpeedOnLane(), MIN2(0., e->ego->getAcceleration()));
1096 : //eInfo.egoEstimatedConflictExitTime = eInfo.egoEstimatedConflictEntryTime + e->ego->getCarFollowModel().estimateArrivalTime(
1097 : // eInfo.egoConflictExitDist - MAX2(0.0, eInfo.egoConflictEntryDist),
1098 : // e->ego->getSpeed(), e->ego->getMaxSpeedOnLane(),
1099 : // e->ego->getCarFollowModel().getMaxAccel());
1100 : } else {
1101 10251 : eInfo.egoEstimatedConflictExitTime = 0.;
1102 : }
1103 81676 : if (eInfo.foeConflictExitDist >= 0.) {
1104 160016 : eInfo.foeEstimatedConflictExitTime = e->foe->getCarFollowModel().estimateArrivalTime(eInfo.foeConflictExitDist, e->foe->getSpeed(), e->foe->getMaxSpeedOnLane(), MIN2(0., e->foe->getAcceleration()));
1105 : //eInfo.foeEstimatedConflictExitTime = eInfo.foeEstimatedConflictEntryTime + e->foe->getCarFollowModel().estimateArrivalTime(
1106 : // eInfo.foeConflictExitDist - MAX2(0.0, eInfo.foeConflictEntryDist),
1107 : // e->foe->getSpeed(), e->foe->getMaxSpeedOnLane(),
1108 : // e->foe->getCarFollowModel().getMaxAccel());
1109 : } else {
1110 1668 : eInfo.foeEstimatedConflictExitTime = 0.;
1111 : }
1112 :
1113 81676 : if (type == ENCOUNTER_TYPE_ONCOMING) {
1114 1890 : eInfo.egoEstimatedConflictExitTime = eInfo.egoEstimatedConflictEntryTime;
1115 1890 : eInfo.foeEstimatedConflictExitTime = eInfo.egoEstimatedConflictEntryTime;
1116 : }
1117 :
1118 81676 : if (type != ENCOUNTER_TYPE_MERGING && type != ENCOUNTER_TYPE_CROSSING) {
1119 : // this call is issued in context of an ongoing conflict, therefore complete type is already known for the encounter
1120 : // (One of EGO_ENTERED_CONFLICT_AREA, FOE_ENTERED_CONFLICT_AREA, EGO_LEFT_CONFLICT_AREA, FOE_LEFT_CONFLICT_AREA, BOTH_ENTERED_CONFLICT_AREA)
1121 : // --> no need to specify incomplete encounter type
1122 : return;
1123 : }
1124 :
1125 : // For merging and crossing situation, the leader/follower relation not determined by classifyEncounter()
1126 : // This is done below based on the estimated conflict entry times
1127 66282 : if (eInfo.egoEstimatedConflictEntryTime == 0. && eInfo.foeEstimatedConflictEntryTime == 0. &&
1128 40 : eInfo.egoConflictExitDist >= 0 && eInfo.foeConflictExitDist >= 0) {
1129 0 : type = ENCOUNTER_TYPE_COLLISION;
1130 0 : WRITE_WARNINGF(TL("SSM device of vehicle '%' detected collision with vehicle '%' at time=%."), e->egoID, e->foeID, time2string(SIMSTEP));
1131 66282 : } else if (eInfo.egoEstimatedConflictEntryTime == INVALID_DOUBLE && eInfo.foeEstimatedConflictEntryTime == INVALID_DOUBLE) {
1132 15562 : type = ENCOUNTER_TYPE_NOCONFLICT_AHEAD;
1133 50720 : } else if (eInfo.egoEstimatedConflictEntryTime < eInfo.foeEstimatedConflictEntryTime || eInfo.foeEstimatedConflictEntryTime == INVALID_DOUBLE) {
1134 : // ego is estimated first at conflict point
1135 : #ifdef DEBUG_SSM
1136 : if (DEBUG_COND(e->ego))
1137 : std::cout << " -> ego is estimated leader at conflict entry."
1138 : << " egoConflictExitTime=" << (eInfo.egoEstimatedConflictExitTime == INVALID_DOUBLE ? "NA" : ::toString(eInfo.egoEstimatedConflictExitTime))
1139 : << " egoEstimatedConflictEntryTime=" << (eInfo.egoEstimatedConflictEntryTime == INVALID_DOUBLE ? "NA" : ::toString(eInfo.egoEstimatedConflictEntryTime))
1140 : << " foeEstimatedConflictEntryTime=" << (eInfo.foeEstimatedConflictEntryTime == INVALID_DOUBLE ? "NA" : ::toString(eInfo.foeEstimatedConflictEntryTime))
1141 : << std::endl;
1142 : #endif
1143 26772 : type = type == ENCOUNTER_TYPE_CROSSING ? ENCOUNTER_TYPE_CROSSING_LEADER : ENCOUNTER_TYPE_MERGING_LEADER;
1144 : } else {
1145 : // ego is estimated second at conflict point
1146 : #ifdef DEBUG_SSM
1147 : if (DEBUG_COND(e->ego))
1148 : std::cout << " -> foe is estimated leader at conflict entry."
1149 : << " foeConflictExitTime=" << (eInfo.foeEstimatedConflictExitTime == INVALID_DOUBLE ? "NA" : ::toString(eInfo.foeEstimatedConflictExitTime))
1150 : << " egoEstimatedConflictEntryTime=" << (eInfo.egoEstimatedConflictEntryTime == INVALID_DOUBLE ? "NA" : ::toString(eInfo.egoEstimatedConflictEntryTime))
1151 : << " foeEstimatedConflictEntryTime=" << (eInfo.foeEstimatedConflictEntryTime == INVALID_DOUBLE ? "NA" : ::toString(eInfo.foeEstimatedConflictEntryTime))
1152 : << std::endl;
1153 : #endif
1154 25553 : type = type == ENCOUNTER_TYPE_CROSSING ? ENCOUNTER_TYPE_CROSSING_FOLLOWER : ENCOUNTER_TYPE_MERGING_FOLLOWER;
1155 : }
1156 :
1157 : }
1158 :
1159 :
1160 :
1161 : void
1162 6323539 : MSDevice_SSM::computeSSMs(EncounterApproachInfo& eInfo) const {
1163 : #ifdef DEBUG_SSM
1164 : if (DEBUG_COND(myHolderMS)) {
1165 : Encounter* e = eInfo.encounter;
1166 : std::cout << SIMTIME << " computeSSMs() for vehicles '"
1167 : << e->ego->getID() << "' and '" << e->foe->getID()
1168 : << "'" << std::endl;
1169 : }
1170 : #endif
1171 :
1172 : const EncounterType& type = eInfo.type;
1173 :
1174 : if (type == ENCOUNTER_TYPE_CROSSING_FOLLOWER || type == ENCOUNTER_TYPE_CROSSING_LEADER
1175 : || type == ENCOUNTER_TYPE_EGO_ENTERED_CONFLICT_AREA || type == ENCOUNTER_TYPE_FOE_ENTERED_CONFLICT_AREA
1176 6323539 : || type == ENCOUNTER_TYPE_MERGING_FOLLOWER || type == ENCOUNTER_TYPE_MERGING_LEADER
1177 : || type == ENCOUNTER_TYPE_FOLLOWING_FOLLOWER || type == ENCOUNTER_TYPE_FOLLOWING_LEADER
1178 : || type == ENCOUNTER_TYPE_ONCOMING) {
1179 4883848 : if (myComputeTTC || myComputeDRAC || myComputePPET || myComputeMDRAC) {
1180 4880940 : determineTTCandDRACandPPETandMDRAC(eInfo);
1181 : }
1182 4883848 : determinePET(eInfo);
1183 : } else if (type == ENCOUNTER_TYPE_BOTH_LEFT_CONFLICT_AREA) {
1184 6547 : determinePET(eInfo);
1185 : } else if (type == ENCOUNTER_TYPE_COLLISION) {
1186 : // TODO: handle collision
1187 : } else if (type == ENCOUNTER_TYPE_EGO_LEFT_CONFLICT_AREA || type == ENCOUNTER_TYPE_FOE_LEFT_CONFLICT_AREA
1188 : || type == ENCOUNTER_TYPE_BOTH_ENTERED_CONFLICT_AREA || type == ENCOUNTER_TYPE_NOCONFLICT_AHEAD) {
1189 : // No conflict measures apply for these states, which correspond to intermediate times between
1190 : // one vehicle leaving the conflict area and the arrival time for the other (difference corresponds to the PET)
1191 : } else if (type == ENCOUNTER_TYPE_ON_ADJACENT_LANES || type == ENCOUNTER_TYPE_MERGING_ADJACENT) {
1192 : // No conflict measures apply for this state
1193 : } else if (type == ENCOUNTER_TYPE_MERGING_PASSED || type == ENCOUNTER_TYPE_FOLLOWING_PASSED) {
1194 : // No conflict measures apply for this state
1195 : } else if (type == ENCOUNTER_TYPE_NOCONFLICT_AHEAD) {
1196 : // No conflict measures apply for this state
1197 : } else {
1198 0 : std::stringstream ss;
1199 0 : ss << "'" << type << "'";
1200 0 : WRITE_WARNING("Unknown or undetermined encounter type at computeSSMs(): " + ss.str());
1201 0 : }
1202 :
1203 : #ifdef DEBUG_SSM
1204 : if (DEBUG_COND(myHolderMS)) {
1205 : Encounter* e = eInfo.encounter;
1206 : std::cout << "computeSSMs() for encounter of vehicles '" << e->egoID << "' and '" << e->foeID << "':\n"
1207 : << " ttc=" << (eInfo.ttc == INVALID_DOUBLE ? "NA" : ::toString(eInfo.ttc))
1208 : << ", drac=" << (eInfo.drac == INVALID_DOUBLE ? "NA" : ::toString(eInfo.drac))
1209 : << ", pet=" << (eInfo.pet.second == INVALID_DOUBLE ? "NA" : ::toString(eInfo.pet.second))
1210 : << std::endl;
1211 : }
1212 : #endif
1213 6323539 : }
1214 :
1215 :
1216 : void
1217 4890395 : MSDevice_SSM::determinePET(EncounterApproachInfo& eInfo) const {
1218 4890395 : Encounter* e = eInfo.encounter;
1219 4890395 : if (e->size() == 0) {
1220 : return;
1221 : }
1222 : const EncounterType& type = eInfo.type;
1223 : std::pair<double, double>& pet = eInfo.pet;
1224 :
1225 : #ifdef DEBUG_SSM
1226 : if (DEBUG_COND(myHolderMS))
1227 : std::cout << SIMTIME << " determinePET() for encounter of vehicles '" << e->egoID << "' and '" << e->foeID << "'"
1228 : << "(type: " << encounterToString(static_cast<EncounterType>(e->typeSpan.back())) << ")" << std::endl;
1229 : #endif
1230 :
1231 4836042 : if (type == ENCOUNTER_TYPE_FOLLOWING_FOLLOWER || type == ENCOUNTER_TYPE_FOLLOWING_LEADER) {
1232 : // For a following situation, the corresponding PET-value is merely the time-headway.
1233 : // Determining these could be done by comparison of memorized gaps with memorized covered distances
1234 : // Implementation is postponed. Tracing the time gaps (in contrast to crossing PET) corresponds to
1235 : // a vector of values not a single value.
1236 : // pass
1237 56037 : } else if (type == ENCOUNTER_TYPE_BOTH_LEFT_CONFLICT_AREA) {
1238 6547 : EncounterType prevType = static_cast<EncounterType>(e->typeSpan.back());
1239 6547 : if (prevType == ENCOUNTER_TYPE_BOTH_LEFT_CONFLICT_AREA) {
1240 : #ifdef DEBUG_SSM
1241 : if (DEBUG_COND(myHolderMS))
1242 : std::cout << "PET for crossing encounter already calculated as " << e->PET.value
1243 : << std::endl;
1244 : #endif
1245 : // pet must have been calculated already
1246 : assert(e->PET.value != INVALID_DOUBLE);
1247 : return;
1248 : }
1249 :
1250 : // this situation should have emerged from one of the following
1251 : assert(prevType == ENCOUNTER_TYPE_CROSSING_FOLLOWER
1252 : || prevType == ENCOUNTER_TYPE_CROSSING_LEADER
1253 : || prevType == ENCOUNTER_TYPE_EGO_LEFT_CONFLICT_AREA
1254 : || prevType == ENCOUNTER_TYPE_FOE_LEFT_CONFLICT_AREA
1255 : || prevType == ENCOUNTER_TYPE_EGO_ENTERED_CONFLICT_AREA
1256 : || prevType == ENCOUNTER_TYPE_FOE_ENTERED_CONFLICT_AREA
1257 : || prevType == ENCOUNTER_TYPE_BOTH_ENTERED_CONFLICT_AREA);
1258 :
1259 :
1260 : #ifdef DEBUG_SSM
1261 : if (DEBUG_COND(myHolderMS))
1262 : std::cout << "e->egoDistsToConflict.back() = " << e->egoDistsToConflict.back()
1263 : << "\ne->egoConflictEntryTime = " << e->egoConflictEntryTime
1264 : << "\ne->egoConflictExitTime = " << e->egoConflictExitTime
1265 : << "\ne->foeDistsToConflict.back() = " << e->foeDistsToConflict.back()
1266 : << "\ne->foeConflictEntryTime = " << e->foeConflictEntryTime
1267 : << "\ne->foeConflictExitTime = " << e->foeConflictExitTime
1268 : << std::endl;
1269 : #endif
1270 :
1271 : // But both have passed the conflict area
1272 : assert(e->egoConflictEntryTime != INVALID_DOUBLE || e->foeConflictEntryTime != INVALID_DOUBLE);
1273 :
1274 : // Both have left the conflict region
1275 : // (Conflict may have started as one was already within the conflict area - thus the check for invalid entry times)
1276 428 : if (e->foeConflictEntryTime == INVALID_DOUBLE || (e->egoConflictEntryTime != INVALID_DOUBLE && e->egoConflictEntryTime > e->foeConflictExitTime)) {
1277 166 : pet.first = e->egoConflictEntryTime;
1278 166 : pet.second = e->egoConflictEntryTime - e->foeConflictExitTime;
1279 262 : } else if (e->egoConflictEntryTime == INVALID_DOUBLE || (e->egoConflictEntryTime != INVALID_DOUBLE && e->foeConflictEntryTime > e->egoConflictExitTime)) {
1280 246 : pet.first = e->foeConflictEntryTime;
1281 246 : pet.second = e->foeConflictEntryTime - e->egoConflictExitTime;
1282 : } else {
1283 : #ifdef DEBUG_SSM
1284 : if (DEBUG_COND(myHolderMS))
1285 : std::cout << "determinePET: Both passed conflict area in the same step. Assume collision"
1286 : << std::endl;
1287 : #endif
1288 16 : pet.first = e->egoConflictEntryTime;
1289 16 : pet.second = 0;
1290 : }
1291 :
1292 : // Reset entry and exit times two allow an eventual subsequent re-use
1293 428 : e->egoConflictEntryTime = INVALID_DOUBLE;
1294 428 : e->egoConflictExitTime = INVALID_DOUBLE;
1295 428 : e->foeConflictEntryTime = INVALID_DOUBLE;
1296 428 : e->foeConflictExitTime = INVALID_DOUBLE;
1297 :
1298 : #ifdef DEBUG_SSM
1299 : if (DEBUG_COND(myHolderMS))
1300 : std::cout << "Calculated PET = " << pet.second << " (at t=" << pet.first << ")"
1301 : << std::endl;
1302 : #endif
1303 : } else {
1304 : // other cases (merging and pre-crossing situations) do not correspond to a PET calculation.
1305 : #ifdef DEBUG_SSM
1306 : if (DEBUG_COND(myHolderMS))
1307 : std::cout << "PET unappropriate for merging and pre-crossing situations. No calculation performed."
1308 : << std::endl;
1309 : #endif
1310 : return;
1311 : }
1312 : }
1313 :
1314 :
1315 : void
1316 4880940 : MSDevice_SSM::determineTTCandDRACandPPETandMDRAC(EncounterApproachInfo& eInfo) const {
1317 4880940 : Encounter* e = eInfo.encounter;
1318 : const EncounterType& type = eInfo.type;
1319 : double& ttc = eInfo.ttc;
1320 : double& drac = eInfo.drac;
1321 : double& ppet = eInfo.ppet;
1322 : double& mdrac = eInfo.mdrac;
1323 :
1324 : #ifdef DEBUG_SSM
1325 : if (DEBUG_COND(myHolderMS))
1326 : std::cout << SIMTIME << " determineTTCandDRAC() for encounter of vehicles '" << e->egoID << "' and '" << e->foeID << "' (type = " << eInfo.type << ")"
1327 : << std::endl;
1328 : #endif
1329 :
1330 : // Dependent on the actual encounter situation (eInfo.type) calculate the TTC.
1331 : // For merging and crossing, different cases occur when a collision during the merging / crossing process is predicted.
1332 4880940 : if (type == ENCOUNTER_TYPE_FOLLOWING_FOLLOWER) {
1333 2415552 : double gap = eInfo.egoConflictEntryDist;
1334 2415552 : if (myComputeTTC) {
1335 2411832 : ttc = computeTTC(gap, e->ego->getSpeed(), e->foe->getSpeed());
1336 : }
1337 2415552 : if (myComputeDRAC) {
1338 713216 : drac = computeDRAC(gap, e->ego->getSpeed(), e->foe->getSpeed());
1339 : }
1340 2415552 : if (myComputeMDRAC) {
1341 123025 : mdrac = computeMDRAC(gap, e->ego->getSpeed(), e->foe->getSpeed(), myMDRACPRT);
1342 : }
1343 : } else if (type == ENCOUNTER_TYPE_FOLLOWING_LEADER) {
1344 2412446 : double gap = eInfo.foeConflictEntryDist;
1345 2412446 : if (myComputeTTC) {
1346 2412446 : ttc = computeTTC(gap, e->foe->getSpeed(), e->ego->getSpeed());
1347 : }
1348 2412446 : if (myComputeDRAC) {
1349 712427 : drac = computeDRAC(gap, e->foe->getSpeed(), e->ego->getSpeed());
1350 : }
1351 2412446 : if (myComputeMDRAC) {
1352 120938 : mdrac = computeMDRAC(gap, e->foe->getSpeed(), e->ego->getSpeed(), myMDRACPRT);
1353 : }
1354 : } else if (type == ENCOUNTER_TYPE_ONCOMING) {
1355 1890 : if (myComputeTTC) {
1356 1890 : const double dv = e->ego->getSpeed() + e->foe->getSpeed();
1357 1890 : if (dv > 0) {
1358 1890 : ttc = eInfo.egoConflictEntryDist / dv;
1359 : }
1360 : }
1361 : } else if (type == ENCOUNTER_TYPE_MERGING_FOLLOWER || type == ENCOUNTER_TYPE_MERGING_LEADER) {
1362 : // TODO: calculate more specifically whether a following situation in the merge conflict area
1363 : // is predicted when assuming constant speeds or whether a side collision is predicted.
1364 : // Currently, we ignore any conflict area before the actual merging point of the lanes.
1365 :
1366 : // linearly extrapolated arrival times at the conflict
1367 : // NOTE: These differ from the estimated times stored in eInfo
1368 1605 : double egoEntryTime = e->ego->getSpeed() > 0 ? eInfo.egoConflictEntryDist / e->ego->getSpeed() : INVALID_DOUBLE;
1369 1605 : double egoExitTime = e->ego->getSpeed() > 0 ? eInfo.egoConflictExitDist / e->ego->getSpeed() : INVALID_DOUBLE;
1370 1605 : double foeEntryTime = e->foe->getSpeed() > 0 ? eInfo.foeConflictEntryDist / e->foe->getSpeed() : INVALID_DOUBLE;
1371 1605 : double foeExitTime = e->foe->getSpeed() > 0 ? eInfo.foeConflictExitDist / e->foe->getSpeed() : INVALID_DOUBLE;
1372 :
1373 : #ifdef DEBUG_SSM
1374 : if (DEBUG_COND(myHolderMS))
1375 : std::cout << " Conflict times with constant speed extrapolation for merging situation:\n "
1376 : << " egoEntryTime=" << (egoEntryTime == INVALID_DOUBLE ? "NA" : ::toString(egoEntryTime))
1377 : << ", egoExitTime=" << (egoExitTime == INVALID_DOUBLE ? "NA" : ::toString(egoExitTime))
1378 : << ", foeEntryTime=" << (foeEntryTime == INVALID_DOUBLE ? "NA" : ::toString(foeEntryTime))
1379 : << ", foeExitTime=" << (foeExitTime == INVALID_DOUBLE ? "NA" : ::toString(foeExitTime))
1380 : << std::endl;
1381 : #endif
1382 :
1383 : // based on that, we obtain
1384 1605 : if (egoEntryTime == INVALID_DOUBLE || foeEntryTime == INVALID_DOUBLE) {
1385 : // at least one vehicle is stopped
1386 415 : ttc = INVALID_DOUBLE;
1387 415 : drac = INVALID_DOUBLE;
1388 415 : mdrac = INVALID_DOUBLE;
1389 : #ifdef DEBUG_SSM
1390 : if (DEBUG_COND(myHolderMS)) {
1391 : std::cout << " No TTC and DRAC computed as one vehicle is stopped." << std::endl;
1392 : }
1393 : #endif
1394 415 : return;
1395 : }
1396 : double leaderEntryTime = MIN2(egoEntryTime, foeEntryTime);
1397 : double followerEntryTime = MAX2(egoEntryTime, foeEntryTime);
1398 1190 : double leaderExitTime = leaderEntryTime == egoEntryTime ? egoExitTime : foeExitTime;
1399 : //double followerExitTime = leaderEntryTime==egoEntryTime?foeExitTime:egoExitTime;
1400 1190 : double leaderSpeed = leaderEntryTime == egoEntryTime ? e->ego->getSpeed() : e->foe->getSpeed();
1401 1190 : double followerSpeed = leaderEntryTime == egoEntryTime ? e->foe->getSpeed() : e->ego->getSpeed();
1402 1190 : double leaderConflictDist = leaderEntryTime == egoEntryTime ? eInfo.egoConflictEntryDist : eInfo.foeConflictEntryDist;
1403 1190 : double followerConflictDist = leaderEntryTime == egoEntryTime ? eInfo.foeConflictEntryDist : eInfo.egoConflictEntryDist;
1404 1190 : double leaderLength = leaderEntryTime == egoEntryTime ? e->ego->getLength() : e->foe->getLength();
1405 1190 : if (myComputePPET && followerEntryTime != INVALID_DOUBLE && leaderEntryTime != INVALID_DOUBLE) {
1406 0 : ppet = followerEntryTime - leaderExitTime;
1407 : //std::cout << " debug1 ppet=" << ppet << "\n";
1408 : }
1409 1190 : if (leaderExitTime >= followerEntryTime) {
1410 : // collision would occur at merge area
1411 266 : if (myComputeTTC) {
1412 266 : ttc = computeTTC(followerConflictDist, followerSpeed, 0.);
1413 : }
1414 : // TODO: Calculate more specific drac for merging case here (complete stop is not always necessary -> see calculation for crossing case)
1415 : // Rather the
1416 266 : if (myComputeDRAC) {
1417 266 : drac = computeDRAC(followerConflictDist, followerSpeed, 0.);
1418 : }
1419 266 : if (myComputeMDRAC) {
1420 0 : mdrac = computeMDRAC(followerConflictDist, followerSpeed, 0., myMDRACPRT);
1421 : }
1422 :
1423 : #ifdef DEBUG_SSM
1424 : if (DEBUG_COND(myHolderMS))
1425 : std::cout << " Extrapolation predicts collision *at* merge point with TTC=" << ttc
1426 : << ", drac=" << drac << std::endl;
1427 : #endif
1428 :
1429 : } else {
1430 : // -> No collision at the merge area
1431 924 : if (myComputeTTC) {
1432 : // Check if after merge a collision would occur if speeds are hold constant.
1433 924 : double gapAfterMerge = followerConflictDist - leaderExitTime * followerSpeed;
1434 : assert(gapAfterMerge >= 0);
1435 :
1436 : // ttc as for following situation (assumes no collision until leader merged)
1437 924 : double ttcAfterMerge = computeTTC(gapAfterMerge, followerSpeed, leaderSpeed);
1438 924 : ttc = ttcAfterMerge == INVALID_DOUBLE ? INVALID_DOUBLE : leaderExitTime + ttcAfterMerge;
1439 : }
1440 924 : if (myComputeDRAC) {
1441 : // Intitial gap. (May be negative only if the leader speed is higher than the follower speed, i.e., dv < 0)
1442 924 : double g0 = followerConflictDist - leaderConflictDist - leaderLength;
1443 924 : if (g0 < 0) {
1444 : // Speed difference must be positive if g0<0.
1445 : assert(leaderSpeed - followerSpeed > 0);
1446 : // no deceleration needed for dv>0 and gap after merge >= 0
1447 740 : drac = INVALID_DOUBLE;
1448 : } else {
1449 : // compute drac as for a following situation
1450 184 : drac = computeDRAC(g0, followerSpeed, leaderSpeed);
1451 : }
1452 : }
1453 924 : if (myComputeMDRAC) {
1454 : // Intitial gap. (May be negative only if the leader speed is higher than the follower speed, i.e., dv < 0)
1455 96 : double g0 = followerConflictDist - leaderConflictDist - leaderLength;
1456 96 : if (g0 < 0) {
1457 : // Speed difference must be positive if g0<0.
1458 : assert(leaderSpeed - followerSpeed > 0);
1459 : // no deceleration needed for dv>0 and gap after merge >= 0
1460 48 : mdrac = INVALID_DOUBLE;
1461 : } else {
1462 : // compute drac as for a following situation
1463 48 : mdrac = computeMDRAC(g0, followerSpeed, leaderSpeed, myMDRACPRT);
1464 : }
1465 : }
1466 : #ifdef DEBUG_SSM
1467 : if (DEBUG_COND(myHolderMS)) {
1468 : if (ttc == INVALID_DOUBLE) {
1469 : // assert(dv >= 0);
1470 : assert(drac == INVALID_DOUBLE || drac == 0.0);
1471 : std::cout << " Extrapolation does not predict any collision." << std::endl;
1472 : } else {
1473 : std::cout << " Extrapolation predicts collision *after* merge point with TTC="
1474 : << (ttc == INVALID_DOUBLE ? "NA" : ::toString(ttc))
1475 : << ", drac=" << (drac == INVALID_DOUBLE ? "NA" : ::toString(drac)) << std::endl;
1476 : }
1477 : }
1478 : #endif
1479 :
1480 : }
1481 :
1482 : } else if (type == ENCOUNTER_TYPE_CROSSING_FOLLOWER
1483 : || type == ENCOUNTER_TYPE_FOE_ENTERED_CONFLICT_AREA) {
1484 23551 : if (myComputeDRAC) {
1485 23487 : drac = computeDRAC(eInfo);
1486 : }
1487 23551 : if (eInfo.egoEstimatedConflictEntryTime <= eInfo.foeEstimatedConflictExitTime && eInfo.egoEstimatedConflictEntryTime != INVALID_DOUBLE) {
1488 : // follower's predicted arrival at the crossing area is earlier than the leader's predicted exit -> collision predicted
1489 302 : double gap = eInfo.egoConflictEntryDist;
1490 302 : if (myComputeTTC) {
1491 302 : ttc = computeTTC(gap, e->ego->getSpeed(), 0.);
1492 : }
1493 302 : if (myComputeMDRAC) {
1494 78 : mdrac = computeMDRAC(gap, e->ego->getSpeed(), 0., myMDRACPRT);
1495 : }
1496 : } else {
1497 : // encounter is expected to happen without collision
1498 23249 : ttc = INVALID_DOUBLE;
1499 23249 : mdrac = INVALID_DOUBLE;
1500 : }
1501 23551 : if (myComputePPET) {
1502 21656 : const double entryTime = eInfo.egoEstimatedConflictEntryTime;
1503 21656 : const double exitTime = (e->foeConflictExitTime == INVALID_DOUBLE
1504 21656 : ? eInfo.foeEstimatedConflictExitTime : e->foeConflictExitTime);
1505 21656 : if (entryTime != INVALID_DOUBLE && exitTime != INVALID_DOUBLE) {
1506 1165 : ppet = entryTime - exitTime;
1507 : }
1508 : //std::cout << " debug2 ppet=" << ppet << "\n";
1509 : }
1510 :
1511 : } else if (type == ENCOUNTER_TYPE_CROSSING_LEADER
1512 : || type == ENCOUNTER_TYPE_EGO_ENTERED_CONFLICT_AREA) {
1513 25896 : if (myComputeDRAC) {
1514 25312 : drac = computeDRAC(eInfo);
1515 : }
1516 25896 : if (eInfo.foeEstimatedConflictEntryTime <= eInfo.egoEstimatedConflictExitTime && eInfo.foeEstimatedConflictEntryTime != INVALID_DOUBLE) {
1517 : // follower's predicted arrival at the crossing area is earlier than the leader's predicted exit -> collision predicted
1518 448 : double gap = eInfo.foeConflictEntryDist;
1519 448 : if (myComputeTTC) {
1520 376 : ttc = computeTTC(gap, e->foe->getSpeed(), 0.);
1521 : }
1522 448 : if (myComputeMDRAC) {
1523 208 : mdrac = computeMDRAC(gap, e->foe->getSpeed(), 0., myMDRACPRT);
1524 : }
1525 : } else {
1526 : // encounter is expected to happen without collision
1527 25448 : ttc = INVALID_DOUBLE;
1528 25448 : mdrac = INVALID_DOUBLE;
1529 : }
1530 25896 : if (myComputePPET) {
1531 22688 : const double entryTime = eInfo.foeEstimatedConflictEntryTime;
1532 22688 : const double exitTime = (e->egoConflictExitTime == INVALID_DOUBLE
1533 22688 : ? eInfo.egoEstimatedConflictExitTime : e->egoConflictExitTime);
1534 22688 : if (entryTime != INVALID_DOUBLE && exitTime != INVALID_DOUBLE) {
1535 1488 : ppet = entryTime - exitTime;
1536 : }
1537 : //std::cout << SIMTIME << " debug3 ppet=" << writeNA(ppet)
1538 : // << " fecet=" << writeNA(eInfo.foeEstimatedConflictEntryTime)
1539 : // << " ecet=" << writeNA(e->egoConflictExitTime)
1540 : // << " eecec=" << writeNA(eInfo.egoEstimatedConflictExitTime)
1541 : // << "\n";
1542 : }
1543 : } else {
1544 : #ifdef DEBUG_SSM
1545 : if (DEBUG_COND(myHolderMS)) {
1546 : std::stringstream ss;
1547 : ss << "'" << type << "'";
1548 : WRITE_WARNING("Underspecified or unknown encounter type in MSDevice_SSM::determineTTCandDRAC(): " + ss.str());
1549 : }
1550 : #endif
1551 : }
1552 :
1553 : #ifdef DEBUG_SSM
1554 : if (DEBUG_COND(myHolderMS))
1555 : std::cout << "ttc=" << (ttc == INVALID_DOUBLE ? "NA" : ::toString(ttc)) << ", drac=" << (drac == INVALID_DOUBLE ? "NA" : ::toString(drac))
1556 : << std::endl;
1557 : #endif
1558 : }
1559 :
1560 :
1561 : double
1562 4826146 : MSDevice_SSM::computeTTC(double gap, double followerSpeed, double leaderSpeed) const {
1563 : // TODO: in merging situations, the TTC may be lower than the one computed here for following situations
1564 : // (currently only the cross section corresponding to the target lane's begin is considered)
1565 : // More specifically, the minimum has to be taken from the two if a collision at merge was predicted.
1566 : #ifdef DEBUG_SSM
1567 : if (DEBUG_COND(myHolderMS))
1568 : std::cout << "computeTTC() with gap=" << gap << ", followerSpeed=" << followerSpeed << ", leaderSpeed=" << leaderSpeed
1569 : << std::endl;
1570 : #endif
1571 4826146 : if (gap <= 0.) {
1572 : return 0.; // collision already happend
1573 : }
1574 4825282 : double dv = followerSpeed - leaderSpeed;
1575 4825282 : if (dv <= 0.) {
1576 : return INVALID_DOUBLE; // no collision
1577 : }
1578 :
1579 2697423 : return gap / dv;
1580 : }
1581 :
1582 :
1583 : double
1584 1427333 : MSDevice_SSM::computeDRAC(double gap, double followerSpeed, double leaderSpeed) {
1585 : //#ifdef DEBUG_SSM_DRAC
1586 : // if (DEBUG_COND)
1587 : // std::cout << "computeDRAC() with gap=" << gap << ", followerSpeed=" << followerSpeed << ", leaderSpeed=" << leaderSpeed
1588 : // << std::endl;
1589 : //#endif
1590 1427333 : if (gap <= 0.) {
1591 : return INVALID_DOUBLE; // collision!
1592 : }
1593 1426477 : double dv = followerSpeed - leaderSpeed;
1594 1426477 : if (dv <= 0.) {
1595 : return 0.0; // no need to break
1596 : }
1597 : assert(followerSpeed > 0.);
1598 720480 : return 0.5 * dv * dv / gap; // following Guido et al. (2011)
1599 : }
1600 :
1601 : double
1602 244297 : MSDevice_SSM::computeMDRAC(double gap, double followerSpeed, double leaderSpeed, double prt) {
1603 : //#ifdef DEBUG_SSM_DRAC
1604 : // if (DEBUG_COND)
1605 : // std::cout << "computeMDRAC() with gap=" << gap << ", followerSpeed=" << followerSpeed << ", leaderSpeed=" << leaderSpeed
1606 : // << std::endl;
1607 : //#endif
1608 244297 : if (gap <= 0.) {
1609 : return INVALID_DOUBLE; // collision!
1610 : }
1611 243441 : double dv = followerSpeed - leaderSpeed;
1612 243441 : if (dv <= 0.) {
1613 : return 0.0; // no need to brake
1614 : }
1615 106855 : if (gap / dv == prt) {
1616 : return INVALID_DOUBLE;
1617 : }
1618 : assert(followerSpeed > 0.);
1619 106855 : return 0.5 * dv / (gap / dv - prt);
1620 : }
1621 :
1622 :
1623 : double
1624 48799 : MSDevice_SSM::computeDRAC(const EncounterApproachInfo& eInfo) {
1625 : // Introduce concise variable names
1626 48799 : double dEntry1 = eInfo.egoConflictEntryDist;
1627 48799 : double dEntry2 = eInfo.foeConflictEntryDist;
1628 48799 : double dExit1 = eInfo.egoConflictExitDist;
1629 48799 : double dExit2 = eInfo.foeConflictExitDist;
1630 48799 : double v1 = eInfo.encounter->ego->getSpeed();
1631 48799 : double v2 = eInfo.encounter->foe->getSpeed();
1632 48799 : double tEntry1 = eInfo.egoEstimatedConflictEntryTime;
1633 48799 : double tEntry2 = eInfo.foeEstimatedConflictEntryTime;
1634 48799 : double tExit1 = eInfo.egoEstimatedConflictExitTime;
1635 48799 : double tExit2 = eInfo.foeEstimatedConflictExitTime;
1636 : #ifdef DEBUG_SSM_DRAC
1637 : if (DEBUG_COND(eInfo.encounter->ego))
1638 : std::cout << SIMTIME << "computeDRAC() with"
1639 : << "\ndEntry1=" << dEntry1 << ", dEntry2=" << dEntry2
1640 : << ", dExit1=" << dExit1 << ", dExit2=" << dExit2
1641 : << ",\nv1=" << v1 << ", v2=" << v2
1642 : << "\ntEntry1=" << (tEntry1 == INVALID_DOUBLE ? "NA" : ::toString(tEntry1)) << ", tEntry2=" << (tEntry2 == INVALID_DOUBLE ? "NA" : ::toString(tEntry2))
1643 : << ", tExit1=" << (tExit1 == INVALID_DOUBLE ? "NA" : ::toString(tExit1)) << ", tExit2=" << (tExit2 == INVALID_DOUBLE ? "NA" : ::toString(tExit2))
1644 : << std::endl;
1645 : #endif
1646 48799 : if (dExit1 <= 0. || dExit2 <= 0.) {
1647 : // At least one vehicle already left or is not about to enter conflict area at all => no breaking needed.
1648 : #ifdef DEBUG_SSM_DRAC
1649 : if (DEBUG_COND(eInfo.encounter->ego)) {
1650 : std::cout << "One already left conflict area -> drac == 0." << std::endl;
1651 : }
1652 : #endif
1653 : return 0.;
1654 : }
1655 48799 : if (dEntry1 <= 0. && dEntry2 <= 0.) {
1656 : // collision... (both already entered conflict area but none left)
1657 : #ifdef DEBUG_SSM_DRAC
1658 : if (DEBUG_COND(eInfo.encounter->ego)) {
1659 : std::cout << "Both entered conflict area but neither left. -> collision!" << std::endl;
1660 : }
1661 : #endif
1662 : return INVALID_DOUBLE;
1663 : }
1664 :
1665 : double drac = std::numeric_limits<double>::max();
1666 48799 : if (dEntry1 > 0.) {
1667 : // vehicle 1 could break
1668 : #ifdef DEBUG_SSM_DRAC
1669 : if (DEBUG_COND(eInfo.encounter->ego)) {
1670 : std::cout << "Ego could break..." << std::endl;
1671 : }
1672 : #endif
1673 45684 : if (tExit2 != INVALID_DOUBLE) {
1674 : // Vehicle 2 is expected to leave conflict area at t2
1675 25179 : drac = MIN2(drac, 2 * (v1 - dEntry1 / tExit2) / tExit2);
1676 : #ifdef DEBUG_SSM_DRAC
1677 : if (DEBUG_COND(eInfo.encounter->ego)) {
1678 : std::cout << " Foe expected to leave in " << tExit2 << "-> Ego needs drac=" << drac << std::endl;
1679 : }
1680 : #endif
1681 : } else {
1682 : // Vehicle 2 is expected to stop on conflict area or earlier
1683 20505 : if (tEntry2 != INVALID_DOUBLE) {
1684 : // ... on conflict area => veh1 has to stop before entry
1685 233 : drac = MIN2(drac, computeDRAC(dEntry1, v1, 0));
1686 : #ifdef DEBUG_SSM_DRAC
1687 : if (DEBUG_COND(eInfo.encounter->ego)) {
1688 : std::cout << " Foe is expected stop on conflict area -> Ego needs drac=" << drac << std::endl;
1689 : }
1690 : #endif
1691 : } else {
1692 : // ... before conflict area
1693 : #ifdef DEBUG_SSM_DRAC
1694 : if (DEBUG_COND(eInfo.encounter->ego)) {
1695 : std::cout << " Foe is expected stop before conflict area -> no drac computation for ego (will be done for foe if applicable)" << std::endl;
1696 : }
1697 : #endif
1698 : }
1699 : }
1700 : }
1701 :
1702 48799 : if (dEntry2 > 0.) {
1703 : // vehicle 2 could break
1704 : #ifdef DEBUG_SSM_DRAC
1705 : if (DEBUG_COND(eInfo.encounter->ego)) {
1706 : std::cout << "Foe could break..." << std::endl;
1707 : }
1708 : #endif
1709 46255 : if (tExit1 != INVALID_DOUBLE) {
1710 : // Vehicle 1 is expected to leave conflict area at t1
1711 : #ifdef DEBUG_SSM_DRAC
1712 : if (DEBUG_COND(eInfo.encounter->ego)) {
1713 : std::cout << " Ego expected to leave in " << tExit1 << "-> Foe needs drac=" << (2 * (v2 - dEntry2 / tExit1) / tExit1) << std::endl;
1714 : }
1715 : #endif
1716 25753 : drac = MIN2(drac, 2 * (v2 - dEntry2 / tExit1) / tExit1);
1717 : } else {
1718 : // Vehicle 1 is expected to stop on conflict area or earlier
1719 20502 : if (tEntry1 != INVALID_DOUBLE) {
1720 : // ... on conflict area => veh2 has to stop before entry
1721 : #ifdef DEBUG_SSM_DRAC
1722 : if (DEBUG_COND(eInfo.encounter->ego)) {
1723 : std::cout << " Ego is expected stop on conflict area -> Foe needs drac=" << computeDRAC(dEntry2, v2, 0) << std::endl;
1724 : }
1725 : #endif
1726 1007 : drac = MIN2(drac, computeDRAC(dEntry2, v2, 0));
1727 : } else {
1728 : // ... before conflict area
1729 : #ifdef DEBUG_SSM_DRAC
1730 : if (DEBUG_COND(eInfo.encounter->ego)) {
1731 : std::cout << " Ego is expected stop before conflict area -> no drac computation for foe (done for ego if applicable)" << std::endl;
1732 : }
1733 : #endif
1734 : }
1735 : }
1736 : }
1737 :
1738 48799 : return drac > 0 ? drac : INVALID_DOUBLE;
1739 : }
1740 :
1741 : void
1742 6943671 : MSDevice_SSM::checkConflictEntryAndExit(EncounterApproachInfo& eInfo) {
1743 : // determine exact entry and exit times
1744 6943671 : Encounter* e = eInfo.encounter;
1745 :
1746 6943671 : const bool foePastConflictEntry = eInfo.foeConflictEntryDist < 0.0 && eInfo.foeConflictEntryDist != INVALID_DOUBLE;
1747 6943671 : const bool egoPastConflictEntry = eInfo.egoConflictEntryDist < 0.0 && eInfo.egoConflictEntryDist != INVALID_DOUBLE;
1748 6943671 : const bool foePastConflictExit = eInfo.foeConflictExitDist < 0.0 && eInfo.foeConflictExitDist != INVALID_DOUBLE;
1749 6943671 : const bool egoPastConflictExit = eInfo.egoConflictExitDist < 0.0 && eInfo.egoConflictExitDist != INVALID_DOUBLE;
1750 :
1751 : #ifdef DEBUG_ENCOUNTER
1752 : if (DEBUG_COND_ENCOUNTER(e)) {
1753 : std::cout << SIMTIME << " checkConflictEntryAndExit() for encounter of vehicles '" << e->egoID << "' and '" << e->foeID << "'"
1754 : << " foeEntryDist=" << eInfo.foeConflictEntryDist
1755 : << " egoEntryDist=" << eInfo.egoConflictEntryDist
1756 : << " foeExitDist=" << eInfo.foeConflictExitDist
1757 : << " egoExitDist=" << eInfo.egoConflictExitDist
1758 : << "\n";
1759 : }
1760 : #endif
1761 :
1762 :
1763 6943671 : if (e->size() == 0) {
1764 : // This is a new conflict (or a conflict that was considered earlier
1765 : // but disregarded due to being 'over')
1766 :
1767 676808 : if (egoPastConflictExit) {
1768 56 : if (foePastConflictExit) {
1769 32 : eInfo.type = ENCOUNTER_TYPE_BOTH_LEFT_CONFLICT_AREA;
1770 24 : } else if (foePastConflictEntry) {
1771 0 : eInfo.type = ENCOUNTER_TYPE_BOTH_ENTERED_CONFLICT_AREA;
1772 : } else {
1773 24 : eInfo.type = ENCOUNTER_TYPE_EGO_LEFT_CONFLICT_AREA;
1774 : }
1775 676752 : } else if (foePastConflictExit) {
1776 28 : if (egoPastConflictEntry) {
1777 0 : eInfo.type = ENCOUNTER_TYPE_BOTH_ENTERED_CONFLICT_AREA;
1778 : } else {
1779 28 : eInfo.type = ENCOUNTER_TYPE_FOE_LEFT_CONFLICT_AREA;
1780 : }
1781 : } else {
1782 : // No one left conflict area
1783 676724 : if (egoPastConflictEntry) {
1784 44 : if (foePastConflictEntry) {
1785 0 : eInfo.type = ENCOUNTER_TYPE_COLLISION;
1786 : } else {
1787 44 : eInfo.type = ENCOUNTER_TYPE_EGO_ENTERED_CONFLICT_AREA;
1788 : }
1789 676680 : } else if (foePastConflictEntry) {
1790 28 : eInfo.type = ENCOUNTER_TYPE_FOE_ENTERED_CONFLICT_AREA;
1791 : }
1792 : // else: both before conflict, keep current type
1793 : }
1794 676808 : return;
1795 : }
1796 :
1797 : // Distances to conflict area boundaries in previous step
1798 6266863 : double prevEgoConflictEntryDist = eInfo.egoConflictEntryDist + e->ego->getLastStepDist();
1799 6266863 : double prevFoeConflictEntryDist = eInfo.foeConflictEntryDist + e->foe->getLastStepDist();
1800 6266863 : double prevEgoConflictExitDist = prevEgoConflictEntryDist + eInfo.egoConflictAreaLength + e->ego->getLength();
1801 6266863 : double prevFoeConflictExitDist = prevFoeConflictEntryDist + eInfo.foeConflictAreaLength + e->foe->getLength();
1802 6266863 : EncounterType prevType = e->currentType;
1803 :
1804 : //#ifdef DEBUG_ENCOUNTER
1805 : // if (DEBUG_COND_ENCOUNTER(eInfo.encounter))
1806 : // std::cout << "\nEgo's prev distance to conflict entry: " << prevEgoConflictEntryDist
1807 : // << "\nEgo's prev distance to conflict exit: " << prevEgoConflictExitDist
1808 : // << "\nFoe's prev distance to conflict entry: " << prevFoeConflictEntryDist
1809 : // << "\nFoe's prev distance to conflict exit: " << prevFoeConflictExitDist
1810 : // << std::endl;
1811 : //#endif
1812 :
1813 : // Check if ego entered in last step
1814 6266863 : if (e->egoConflictEntryTime == INVALID_DOUBLE && egoPastConflictEntry && prevEgoConflictEntryDist >= 0) {
1815 : // ego must have entered the conflict in the last step. Determine exact entry time
1816 2316 : e->egoConflictEntryTime = SIMTIME - TS + MSCFModel::passingTime(-prevEgoConflictEntryDist, 0., -eInfo.egoConflictEntryDist, e->ego->getPreviousSpeed(), e->ego->getSpeed());
1817 : #ifdef DEBUG_ENCOUNTER
1818 : if (DEBUG_COND_ENCOUNTER(e)) {
1819 : std::cout << " ego entered conflict area at t=" << e->egoConflictEntryTime << std::endl;
1820 : }
1821 : #endif
1822 : // Update encounter type (only done here for entering, the other transitions are done in updatePassedEncounter)
1823 2316 : if (prevType == ENCOUNTER_TYPE_CROSSING_FOLLOWER
1824 2316 : || prevType == ENCOUNTER_TYPE_CROSSING_LEADER) {
1825 2131 : eInfo.type = ENCOUNTER_TYPE_EGO_ENTERED_CONFLICT_AREA;
1826 : }
1827 : }
1828 :
1829 : // Check if foe entered in last step
1830 6266863 : if (e->foeConflictEntryTime == INVALID_DOUBLE && foePastConflictEntry && prevFoeConflictEntryDist >= 0) {
1831 : // foe must have entered the conflict in the last step. Determine exact entry time
1832 2297 : e->foeConflictEntryTime = SIMTIME - TS + MSCFModel::passingTime(-prevFoeConflictEntryDist, 0., -eInfo.foeConflictEntryDist, e->foe->getPreviousSpeed(), e->foe->getSpeed());
1833 : #ifdef DEBUG_ENCOUNTER
1834 : if (DEBUG_COND_ENCOUNTER(e)) {
1835 : std::cout << " foe entered conflict area at t=" << e->foeConflictEntryTime << std::endl;
1836 : }
1837 : #endif
1838 : // Update encounter type (only done here for entering, the other transitions are done in updatePassedEncounter)
1839 2297 : if (prevType == ENCOUNTER_TYPE_CROSSING_FOLLOWER
1840 2297 : || prevType == ENCOUNTER_TYPE_CROSSING_LEADER) {
1841 2032 : eInfo.type = ENCOUNTER_TYPE_FOE_ENTERED_CONFLICT_AREA;
1842 : }
1843 : }
1844 :
1845 : // Check if ego left conflict area
1846 6266863 : if (e->egoConflictExitTime == INVALID_DOUBLE && eInfo.egoConflictExitDist < 0 && prevEgoConflictExitDist >= 0) {
1847 : // ego must have left the conflict area in the last step. Determine exact exit time
1848 2349 : e->egoConflictExitTime = SIMTIME - TS + MSCFModel::passingTime(-prevEgoConflictExitDist, 0., -eInfo.egoConflictExitDist, e->ego->getPreviousSpeed(), e->ego->getSpeed());
1849 : // Add cross section to calculate PET for foe
1850 : // e->foePETCrossSections.push_back(std::make_pair(eInfo.foeConflictEntryCrossSection, e->egoConflictExitTime));
1851 : #ifdef DEBUG_ENCOUNTER
1852 : if (DEBUG_COND_ENCOUNTER(e)) {
1853 : std::cout << " ego left conflict area at t=" << e->egoConflictExitTime << std::endl;
1854 : }
1855 : #endif
1856 : // Update encounter type (only done here for entering, the other transitions are done in updatePassedEncounter)
1857 2349 : if (prevType == ENCOUNTER_TYPE_CROSSING_FOLLOWER
1858 2349 : || prevType == ENCOUNTER_TYPE_CROSSING_LEADER) {
1859 82 : eInfo.type = ENCOUNTER_TYPE_EGO_LEFT_CONFLICT_AREA;
1860 : }
1861 : }
1862 :
1863 : // Check if foe left conflict area
1864 6266863 : if (e->foeConflictExitTime == INVALID_DOUBLE && eInfo.foeConflictExitDist < 0 && prevFoeConflictExitDist >= 0) {
1865 : // foe must have left the conflict area in the last step. Determine exact exit time
1866 2310 : e->foeConflictExitTime = SIMTIME - TS + MSCFModel::passingTime(-prevFoeConflictExitDist, 0., -eInfo.foeConflictExitDist, e->foe->getPreviousSpeed(), e->foe->getSpeed());
1867 : // Add cross section to calculate PET for ego
1868 : // e->egoPETCrossSections.push_back(std::make_pair(eInfo.egoConflictEntryCrossSection, e->foeConflictExitTime));
1869 : #ifdef DEBUG_ENCOUNTER
1870 : if (DEBUG_COND_ENCOUNTER(e)) {
1871 : std::cout << " foe left conflict area at t=" << e->foeConflictExitTime << std::endl;
1872 : }
1873 : #endif
1874 : // Update encounter type (only done here for entering, the other transitions are done in updatePassedEncounter)
1875 2310 : if (prevType == ENCOUNTER_TYPE_CROSSING_FOLLOWER
1876 2310 : || prevType == ENCOUNTER_TYPE_CROSSING_LEADER) {
1877 94 : eInfo.type = ENCOUNTER_TYPE_FOE_LEFT_CONFLICT_AREA;
1878 : }
1879 : }
1880 : }
1881 :
1882 :
1883 : void
1884 1087302 : MSDevice_SSM::updatePassedEncounter(Encounter* e, FoeInfo* foeInfo, EncounterApproachInfo& eInfo) {
1885 :
1886 : #ifdef DEBUG_ENCOUNTER
1887 : if (DEBUG_COND_ENCOUNTER(e)) {
1888 : std::cout << SIMTIME << " updatePassedEncounter() for vehicles '" << e->egoID << "' and '" << e->foeID << "'\n";
1889 : }
1890 : #endif
1891 :
1892 1087302 : if (foeInfo == nullptr) {
1893 : // the foe is out of the device's range, proceed counting down the remaining extra time to trace
1894 454861 : e->countDownExtraTime(TS);
1895 : #ifdef DEBUG_ENCOUNTER
1896 : if (DEBUG_COND_ENCOUNTER(e)) std::cout << " Foe is out of range. Counting down extra time."
1897 : << " Remaining seconds before closing encounter: " << e->getRemainingExtraTime() << std::endl;
1898 : #endif
1899 :
1900 : } else {
1901 : // reset the remaining extra time (foe could have re-entered the device range after beginning extra time countdown already)
1902 632441 : e->resetExtraTime(myExtraTime);
1903 : }
1904 :
1905 : // Check, whether this was really a potential conflict at some time:
1906 : // Search through typeSpan for a type other than no conflict
1907 1087302 : EncounterType lastPotentialConflictType = e->typeSpan.size() > 0 ? static_cast<EncounterType>(e->typeSpan.back()) : ENCOUNTER_TYPE_NOCONFLICT_AHEAD;
1908 :
1909 : if (lastPotentialConflictType == ENCOUNTER_TYPE_NOCONFLICT_AHEAD) {
1910 : // This encounter was no conflict in the last step -> remains so
1911 : #ifdef DEBUG_ENCOUNTER
1912 : if (DEBUG_COND_ENCOUNTER(e)) {
1913 : std::cout << " This encounter wasn't classified as a potential conflict lately.\n";
1914 : }
1915 : #endif
1916 618818 : if (foeInfo == nullptr) {
1917 : // Encounter was either never a potential conflict and foe is out of range
1918 : // or the foe has left the network
1919 : // -> no use in further tracing this encounter
1920 : #ifdef DEBUG_SSM
1921 : if (DEBUG_COND(myHolderMS)) {
1922 : std::cout << " Requesting encounter closure because foeInfo==nullptr" << std::endl;
1923 : }
1924 : #endif
1925 2787 : e->closingRequested = true;
1926 : #ifdef DEBUG_ENCOUNTER
1927 : if (DEBUG_COND_ENCOUNTER(e)) {
1928 : std::cout << " Closing encounter.\n";
1929 : }
1930 : #endif
1931 2787 : eInfo.type = ENCOUNTER_TYPE_NOCONFLICT_AHEAD;
1932 : }
1933 : } else if (lastPotentialConflictType == ENCOUNTER_TYPE_FOLLOWING_FOLLOWER
1934 : || lastPotentialConflictType == ENCOUNTER_TYPE_FOLLOWING_LEADER
1935 : || lastPotentialConflictType == ENCOUNTER_TYPE_FOLLOWING_PASSED) {
1936 : // if a following situation leads to a no-conflict situation this encounter switches no-conflict, since no further computations (PET) are needed.
1937 403501 : eInfo.type = ENCOUNTER_TYPE_FOLLOWING_PASSED;
1938 : #ifdef DEBUG_ENCOUNTER
1939 : if (DEBUG_COND_ENCOUNTER(e)) {
1940 : std::cout << " Encounter was previously classified as a follow/lead situation.\n";
1941 : }
1942 : #endif
1943 : } else if (lastPotentialConflictType == ENCOUNTER_TYPE_MERGING_FOLLOWER
1944 : || lastPotentialConflictType == ENCOUNTER_TYPE_MERGING_LEADER
1945 : || lastPotentialConflictType == ENCOUNTER_TYPE_MERGING_PASSED) {
1946 : // if a merging situation leads to a no-conflict situation the leader was either removed from the net (we disregard special treatment)
1947 : // or route- or lane-changes removed the conflict.
1948 0 : eInfo.type = ENCOUNTER_TYPE_MERGING_PASSED;
1949 : #ifdef DEBUG_ENCOUNTER
1950 : if (DEBUG_COND_ENCOUNTER(e)) {
1951 : std::cout << " Encounter was previously classified as a merging situation.\n";
1952 : }
1953 : #endif
1954 : }
1955 1087302 : if (lastPotentialConflictType == ENCOUNTER_TYPE_CROSSING_FOLLOWER
1956 : || lastPotentialConflictType == ENCOUNTER_TYPE_CROSSING_LEADER
1957 : || lastPotentialConflictType == ENCOUNTER_TYPE_EGO_ENTERED_CONFLICT_AREA
1958 1087302 : || lastPotentialConflictType == ENCOUNTER_TYPE_FOE_ENTERED_CONFLICT_AREA
1959 1087302 : || lastPotentialConflictType == ENCOUNTER_TYPE_BOTH_ENTERED_CONFLICT_AREA
1960 : || lastPotentialConflictType == ENCOUNTER_TYPE_EGO_LEFT_CONFLICT_AREA
1961 1078515 : || lastPotentialConflictType == ENCOUNTER_TYPE_FOE_LEFT_CONFLICT_AREA
1962 1031326 : || lastPotentialConflictType == ENCOUNTER_TYPE_BOTH_LEFT_CONFLICT_AREA
1963 1031326 : || lastPotentialConflictType == ENCOUNTER_TYPE_COLLISION) {
1964 : // Encounter has been a crossing situation.
1965 :
1966 : #ifdef DEBUG_ENCOUNTER
1967 : if (DEBUG_COND_ENCOUNTER(e)) {
1968 : std::cout << " Encounter was previously classified as a crossing situation of type " << lastPotentialConflictType << ".\n";
1969 : }
1970 : #endif
1971 : // For passed encounters, the xxxConflictAreaLength variables are not determined before -> we use the stored values.
1972 :
1973 : // TODO: This could also more precisely be calculated wrt the angle of the crossing *at the conflict point*
1974 62111 : if (eInfo.egoConflictAreaLength == INVALID_DOUBLE) {
1975 62111 : eInfo.egoConflictAreaLength = e->foe->getWidth();
1976 : }
1977 62111 : if (eInfo.foeConflictAreaLength == INVALID_DOUBLE) {
1978 62111 : eInfo.foeConflictAreaLength = e->ego->getWidth();
1979 : }
1980 :
1981 62111 : eInfo.egoConflictEntryDist = e->egoDistsToConflict.back() - e->ego->getLastStepDist();
1982 62111 : eInfo.egoConflictExitDist = eInfo.egoConflictEntryDist + eInfo.egoConflictAreaLength + e->ego->getLength();
1983 62111 : eInfo.foeConflictEntryDist = e->foeDistsToConflict.back() - e->foe->getLastStepDist();
1984 62111 : eInfo.foeConflictExitDist = eInfo.foeConflictEntryDist + eInfo.foeConflictAreaLength + e->foe->getLength();
1985 :
1986 : #ifdef DEBUG_ENCOUNTER
1987 : if (DEBUG_COND_ENCOUNTER(e))
1988 : std::cout << " egoConflictEntryDist = " << eInfo.egoConflictEntryDist
1989 : << ", egoConflictExitDist = " << eInfo.egoConflictExitDist
1990 : << "\n foeConflictEntryDist = " << eInfo.foeConflictEntryDist
1991 : << ", foeConflictExitDist = " << eInfo.foeConflictExitDist
1992 : << std::endl;
1993 : #endif
1994 :
1995 : // Determine actual encounter type
1996 62111 : bool egoEnteredConflict = eInfo.egoConflictEntryDist < 0.;
1997 62111 : bool foeEnteredConflict = eInfo.foeConflictEntryDist < 0.;
1998 62111 : bool egoLeftConflict = eInfo.egoConflictExitDist < 0.;
1999 62111 : bool foeLeftConflict = eInfo.foeConflictExitDist < 0.;
2000 62111 : if ((!egoEnteredConflict) && !foeEnteredConflict) {
2001 : // XXX: do we need to recompute the follow/lead order, here?
2002 : assert(lastPotentialConflictType == ENCOUNTER_TYPE_CROSSING_FOLLOWER
2003 : || lastPotentialConflictType == ENCOUNTER_TYPE_CROSSING_LEADER);
2004 24 : eInfo.type = lastPotentialConflictType;
2005 62087 : } else if (egoEnteredConflict && !foeEnteredConflict) {
2006 30575 : eInfo.type = ENCOUNTER_TYPE_EGO_ENTERED_CONFLICT_AREA;
2007 31512 : } else if ((!egoEnteredConflict) && foeEnteredConflict) {
2008 22209 : eInfo.type = ENCOUNTER_TYPE_FOE_ENTERED_CONFLICT_AREA;
2009 : } else { // (egoEnteredConflict && foeEnteredConflict) {
2010 9303 : eInfo.type = ENCOUNTER_TYPE_BOTH_ENTERED_CONFLICT_AREA;
2011 : }
2012 :
2013 62111 : if ((!egoLeftConflict) && !foeLeftConflict) {
2014 2012 : if (eInfo.type == ENCOUNTER_TYPE_BOTH_ENTERED_CONFLICT_AREA) {
2015 16 : eInfo.type = ENCOUNTER_TYPE_COLLISION;
2016 : }
2017 60099 : } else if (egoLeftConflict && !foeLeftConflict) {
2018 30989 : if (eInfo.type != ENCOUNTER_TYPE_BOTH_ENTERED_CONFLICT_AREA) {
2019 29289 : eInfo.type = ENCOUNTER_TYPE_EGO_LEFT_CONFLICT_AREA;
2020 : }
2021 29110 : } else if ((!egoLeftConflict) && foeLeftConflict) {
2022 22563 : if (eInfo.type != ENCOUNTER_TYPE_BOTH_ENTERED_CONFLICT_AREA) {
2023 21523 : eInfo.type = ENCOUNTER_TYPE_FOE_LEFT_CONFLICT_AREA;
2024 : }
2025 : } else {
2026 6547 : eInfo.type = ENCOUNTER_TYPE_BOTH_LEFT_CONFLICT_AREA;
2027 : // It should not occur that both leave the conflict at the same step
2028 : assert(lastPotentialConflictType == ENCOUNTER_TYPE_FOE_LEFT_CONFLICT_AREA
2029 : || lastPotentialConflictType == ENCOUNTER_TYPE_EGO_LEFT_CONFLICT_AREA
2030 : || lastPotentialConflictType == ENCOUNTER_TYPE_BOTH_ENTERED_CONFLICT_AREA
2031 : || lastPotentialConflictType == ENCOUNTER_TYPE_BOTH_LEFT_CONFLICT_AREA);
2032 : }
2033 :
2034 : // TODO: adjust the conflict distances according to lateral movement for single ENTERED-cases
2035 :
2036 : #ifdef DEBUG_ENCOUNTER
2037 : if (DEBUG_COND_ENCOUNTER(e)) {
2038 : std::cout << " Updated classification: " << eInfo.type << "\n";
2039 : }
2040 : #endif
2041 : }
2042 1087302 : }
2043 :
2044 :
2045 : MSDevice_SSM::EncounterType
2046 6943671 : MSDevice_SSM::classifyEncounter(const FoeInfo* foeInfo, EncounterApproachInfo& eInfo) const {
2047 : #ifdef DEBUG_ENCOUNTER
2048 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
2049 : std::cout << "classifyEncounter() called.\n";
2050 : }
2051 : #endif
2052 6943671 : if (foeInfo == nullptr) {
2053 : // foeInfo == 0 signalizes, that no corresponding foe info was returned by findSurroundingVehicles(),
2054 : // i.e. the foe is actually out of range (This may also mean that it has left the network)
2055 : return ENCOUNTER_TYPE_NOCONFLICT_AHEAD;
2056 : }
2057 6488810 : const Encounter* e = eInfo.encounter;
2058 :
2059 : // previous classification (if encounter was not just created)
2060 6488810 : EncounterType prevType = e->typeSpan.size() > 0 ? static_cast<EncounterType>(e->typeSpan.back()) : ENCOUNTER_TYPE_NOCONFLICT_AHEAD;
2061 : if (e->typeSpan.size() > 0
2062 6488810 : && (prevType == ENCOUNTER_TYPE_EGO_ENTERED_CONFLICT_AREA
2063 : || prevType == ENCOUNTER_TYPE_FOE_ENTERED_CONFLICT_AREA
2064 : || prevType == ENCOUNTER_TYPE_EGO_LEFT_CONFLICT_AREA
2065 : || prevType == ENCOUNTER_TYPE_FOE_LEFT_CONFLICT_AREA
2066 5812002 : || prevType == ENCOUNTER_TYPE_BOTH_ENTERED_CONFLICT_AREA)) {
2067 : // This is an ongoing crossing situation with at least one of the vehicles not
2068 : // having passed the conflict area.
2069 : // -> Merely trace the change of distances to the conflict entry / exit
2070 : // -> Derefer this to updatePassedEncounter, where this is done anyhow.
2071 : #ifdef DEBUG_ENCOUNTER
2072 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
2073 : std::cout << " Ongoing crossing conflict will be traced by passedEncounter().\n";
2074 : }
2075 : #endif
2076 : return prevType;
2077 : }
2078 :
2079 :
2080 : // Ego's current Lane
2081 6475290 : const MSLane* egoLane = e->ego->getLane();
2082 : // Foe's current Lane
2083 6475290 : const MSLane* foeLane = e->foe->getLane();
2084 :
2085 : // Ego's conflict lane is memorized in foeInfo
2086 6475290 : const MSLane* egoConflictLane = foeInfo->egoConflictLane;
2087 6475290 : double egoDistToConflictLane = foeInfo->egoDistToConflictLane;
2088 : // Find conflicting lane and the distance to its entry link for the foe
2089 : double foeDistToConflictLane;
2090 6475290 : const MSLane* foeConflictLane = findFoeConflictLane(e->foe, foeInfo->egoConflictLane, foeDistToConflictLane);
2091 :
2092 : #ifdef DEBUG_ENCOUNTER
2093 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter))
2094 : std::cout << " egoConflictLane='" << (egoConflictLane == 0 ? "NULL" : egoConflictLane->getID()) << "'\n"
2095 : << " foeConflictLane='" << (foeConflictLane == 0 ? "NULL" : foeConflictLane->getID()) << "'\n"
2096 : << " egoDistToConflictLane=" << egoDistToConflictLane
2097 : << " foeDistToConflictLane=" << foeDistToConflictLane
2098 : << std::endl;
2099 : #endif
2100 :
2101 : // Treat different cases for foeConflictLane and egoConflictLane (internal or non-internal / equal to egoLane or to foeLane),
2102 : // and thereby determine encounterType and the ego/foeEncounterDistance.
2103 : // The encounter distance has a different meaning for different types of encounters:
2104 : // 1) For rear-end conflicts (lead/follow situations) the follower's encounter distance is the distance to the actual back position of the leader. The leaders's distance is undefined.
2105 : // 2) For merging encounters the encounter distance is the distance until the begin of the common target edge/lane.
2106 : // (XXX: Perhaps this should be adjusted to include the entry point to the region where a simultaneous occupancy of
2107 : // both merging lanes could imply a collision)
2108 : // 3) For crossing encounters the encounter distances is the distance until the entry point to the conflicting lane.
2109 :
2110 : EncounterType type;
2111 :
2112 6475290 : if (foeConflictLane == nullptr) {
2113 : // foe vehicle is not on course towards the ego's route (see findFoeConflictLane)
2114 : type = ENCOUNTER_TYPE_NOCONFLICT_AHEAD;
2115 : #ifdef DEBUG_ENCOUNTER
2116 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
2117 : std::cout << "-> Encounter type: No conflict.\n";
2118 : }
2119 : #endif
2120 6270797 : } else if (!egoConflictLane->isInternal()) {
2121 : // The conflict lane is non-internal, therefore we either have no potential conflict or a lead/follow situation (i.e., no crossing or merging)
2122 5619497 : if (egoConflictLane == egoLane) {
2123 5353438 : const bool egoOpposite = e->ego->getLaneChangeModel().isOpposite();
2124 5353438 : const bool foeOpposite = e->foe->getLaneChangeModel().isOpposite();
2125 : #ifdef DEBUG_ENCOUNTER
2126 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
2127 : std::cout << "-> ego " << e->ego->getID() << " isOpposite " << egoOpposite << " foe " << e->foe->getID() << " isOpposite " << foeOpposite << " .\n";
2128 : }
2129 : #endif
2130 : // The conflict point is on the ego's current lane.
2131 5353438 : if (foeLane == egoLane) {
2132 : // Foe is on the same non-internal lane
2133 4204387 : if (!egoOpposite && !foeOpposite) {
2134 4179847 : if (e->ego->getPositionOnLane() > e->foe->getPositionOnLane()) {
2135 : type = ENCOUNTER_TYPE_FOLLOWING_LEADER;
2136 2086943 : eInfo.foeConflictEntryDist = e->ego->getBackPositionOnLane() - e->foe->getPositionOnLane();
2137 : } else {
2138 : type = ENCOUNTER_TYPE_FOLLOWING_FOLLOWER;
2139 2092904 : eInfo.egoConflictEntryDist = e->foe->getBackPositionOnLane() - e->ego->getPositionOnLane();
2140 : }
2141 : #ifdef DEBUG_ENCOUNTER
2142 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
2143 : std::cout << "-> Encounter type: Lead/follow-situation on non-internal lane '" << egoLane->getID() << "'\n";
2144 : }
2145 : #endif
2146 24540 : } else if (egoOpposite && foeOpposite) {
2147 15128 : if (e->ego->getPositionOnLane() < e->foe->getPositionOnLane()) {
2148 : type = ENCOUNTER_TYPE_FOLLOWING_LEADER;
2149 7564 : eInfo.foeConflictEntryDist = -(e->ego->getBackPositionOnLane() - e->foe->getPositionOnLane());
2150 : } else {
2151 : type = ENCOUNTER_TYPE_FOLLOWING_FOLLOWER;
2152 7564 : eInfo.egoConflictEntryDist = -(e->foe->getBackPositionOnLane() - e->ego->getPositionOnLane());
2153 : }
2154 : #ifdef DEBUG_ENCOUNTER
2155 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
2156 : std::cout << "-> Encounter type: Lead/follow-situation while both are driving in the opposite direction on non-internal lane '" << egoLane->getID() << "'\n";
2157 : }
2158 : #endif
2159 : } else {
2160 : type = ENCOUNTER_TYPE_ONCOMING;
2161 9412 : const double gap = e->ego->getPositionOnLane() - e->foe->getPositionOnLane();
2162 9412 : if (egoOpposite) {
2163 4706 : if (e->ego->getPositionOnLane() > e->foe->getPositionOnLane()) {
2164 931 : eInfo.egoConflictEntryDist = gap;
2165 931 : eInfo.foeConflictEntryDist = gap;
2166 : } else {
2167 : type = ENCOUNTER_TYPE_NOCONFLICT_AHEAD;
2168 : }
2169 : } else {
2170 4706 : if (e->ego->getPositionOnLane() < e->foe->getPositionOnLane()) {
2171 931 : eInfo.egoConflictEntryDist = -gap;
2172 931 : eInfo.foeConflictEntryDist = -gap;
2173 : } else {
2174 : type = ENCOUNTER_TYPE_NOCONFLICT_AHEAD;
2175 : }
2176 : }
2177 : #ifdef DEBUG_ENCOUNTER
2178 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
2179 : std::cout << "-> Encounter type: oncoming on non-internal lane '" << egoLane->getID() << "'\n";
2180 : }
2181 : #endif
2182 :
2183 : }
2184 1149051 : } else if (&(foeLane->getEdge()) == &(egoLane->getEdge())) {
2185 : // Foe is on the same non-internal edge but not on the same lane. Treat this as no conflict for now
2186 : // XXX: this disregards conflicts for vehicles on adjacent lanes
2187 883716 : if (foeOpposite != egoOpposite) {
2188 : type = ENCOUNTER_TYPE_NOCONFLICT_AHEAD;
2189 : } else {
2190 : type = ENCOUNTER_TYPE_ON_ADJACENT_LANES;
2191 : }
2192 : #ifdef DEBUG_ENCOUNTER
2193 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
2194 : std::cout << "-> Encounter type: " << type << std::endl;
2195 : }
2196 : #endif
2197 : } else {
2198 :
2199 265335 : if (!egoOpposite && !foeOpposite) {
2200 :
2201 : assert(&(egoLane->getEdge()) == &(foeConflictLane->getEdge()));
2202 : assert(egoDistToConflictLane <= 0);
2203 : // Foe must be on a route leading into the ego's edge
2204 265328 : if (foeConflictLane == egoLane) {
2205 : type = ENCOUNTER_TYPE_FOLLOWING_LEADER;
2206 246568 : eInfo.foeConflictEntryDist = foeDistToConflictLane + e->ego->getBackPositionOnLane();
2207 :
2208 : #ifdef DEBUG_ENCOUNTER
2209 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter))
2210 : std::cout << "-> Encounter type: Ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' leads foe '"
2211 : << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
2212 : << " (gap = " << eInfo.foeConflictEntryDist << ")\n";
2213 : #endif
2214 : } else {
2215 : // Foe's route leads to an adjacent lane of the current lane of the ego
2216 : type = ENCOUNTER_TYPE_ON_ADJACENT_LANES;
2217 : #ifdef DEBUG_ENCOUNTER
2218 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
2219 : std::cout << "-> Encounter type: " << type << std::endl;
2220 : }
2221 : #endif
2222 : }
2223 :
2224 7 : } else if (egoOpposite && foeOpposite) {
2225 : // XXX determine follower relationship by searching for the foe lane in the opposites of ego bestlanes
2226 : type = ENCOUNTER_TYPE_FOLLOWING_LEADER;
2227 : /*
2228 : if (e->ego->getPositionOnLane() < e->foe->getPositionOnLane()) {
2229 : type = ENCOUNTER_TYPE_FOLLOWING_LEADER;
2230 : eInfo.foeConflictEntryDist = -(e->ego->getBackPositionOnLane() - e->foe->getPositionOnLane());
2231 : } else {
2232 : type = ENCOUNTER_TYPE_FOLLOWING_FOLLOWER;
2233 : eInfo.egoConflictEntryDist = -(e->foe->getBackPositionOnLane() - e->ego->getPositionOnLane());
2234 : }
2235 : */
2236 : #ifdef DEBUG_ENCOUNTER
2237 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
2238 : std::cout << "-> Encounter type: Lead/follow-situation while both are driving in the opposite direction on non-internal lane '" << egoLane->getID() << "'\n";
2239 : }
2240 : #endif
2241 : } else {
2242 : type = ENCOUNTER_TYPE_ONCOMING;
2243 : // XXX determine distance by searching for the foe lane in the opposites of ego bestlanes
2244 : /*
2245 : const double gap = e->ego->getPositionOnLane() - e->foe->getPositionOnLane();
2246 : if (egoOpposite) {
2247 : if (e->ego->getPositionOnLane() > e->foe->getPositionOnLane()) {
2248 : eInfo.egoConflictEntryDist = gap;
2249 : eInfo.foeConflictEntryDist = gap;
2250 : } else {
2251 : type = ENCOUNTER_TYPE_NOCONFLICT_AHEAD;
2252 : }
2253 : } else {
2254 : if (e->ego->getPositionOnLane() < e->foe->getPositionOnLane()) {
2255 : eInfo.egoConflictEntryDist = -gap;
2256 : eInfo.foeConflictEntryDist = -gap;
2257 : } else {
2258 : type = ENCOUNTER_TYPE_NOCONFLICT_AHEAD;
2259 : }
2260 : }
2261 : */
2262 : #ifdef DEBUG_ENCOUNTER
2263 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
2264 : std::cout << "-> Encounter type: oncoming on non-internal lane '" << egoLane->getID() << "'\n";
2265 : }
2266 : #endif
2267 :
2268 : }
2269 : }
2270 : } else {
2271 : // The egoConflictLane is a non-internal lane which is not the ego's current lane. Thus it must lie ahead of the ego vehicle and
2272 : // is located on the foe's current edge see findSurroundingVehicles()
2273 : // (otherwise the foe would have had to enter the ego's route along a junction and the corresponding
2274 : // conflict lane would be internal)
2275 : assert(&(foeLane->getEdge()) == &(egoConflictLane->getEdge()));
2276 : assert(foeDistToConflictLane <= 0);
2277 266059 : if (foeLane == egoConflictLane) {
2278 : type = ENCOUNTER_TYPE_FOLLOWING_FOLLOWER;
2279 247307 : eInfo.egoConflictEntryDist = egoDistToConflictLane + e->foe->getBackPositionOnLane();
2280 : #ifdef DEBUG_ENCOUNTER
2281 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter))
2282 : std::cout << "-> Encounter type: Ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' follows foe '"
2283 : << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
2284 : << " (gap = " << eInfo.egoConflictEntryDist << ", case1)\n";
2285 : #endif
2286 : } else {
2287 : // Ego's route leads to an adjacent lane of the current lane of the foe
2288 : type = ENCOUNTER_TYPE_ON_ADJACENT_LANES;
2289 : #ifdef DEBUG_ENCOUNTER
2290 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
2291 : std::cout << "-> Encounter type: " << type << std::endl;
2292 : }
2293 : #endif
2294 : }
2295 : }
2296 : } else {
2297 : // egoConflictLane is internal, i.e., lies on a junction. Besides the lead/follow situation (which may stretch over different lanes of a connection),
2298 : // merging or crossing of the conflict lanes is possible.
2299 : assert(foeConflictLane->isInternal());
2300 651300 : const MSLink* egoEntryLink = egoConflictLane->getEntryLink();
2301 651300 : const MSLink* foeEntryLink = foeConflictLane->getEntryLink();
2302 651300 : const bool egoOpposite = e->ego->getLaneChangeModel().isOpposite();
2303 651300 : const bool foeOpposite = e->foe->getLaneChangeModel().isOpposite();
2304 :
2305 651300 : if (((!egoOpposite && foeOpposite) || (egoOpposite && !foeOpposite)) && egoConflictLane == foeConflictLane) {
2306 : // oncoming on junction
2307 : #ifdef DEBUG_ENCOUNTER
2308 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
2309 :
2310 : std::cout << "-> egoConflictLane: " << egoConflictLane->getID() << " foeConflictLane: " << foeConflictLane->getID() << " egoOpposite " << egoOpposite << " foeOpposite " << foeOpposite << std::endl;
2311 : }
2312 : #endif
2313 : // The conflict point is on the ego's current lane.
2314 28 : if (foeLane == egoLane) {
2315 : // Foe is on the same non-internal lane
2316 0 : if (!egoOpposite && !foeOpposite) {
2317 0 : if (e->ego->getPositionOnLane() > e->foe->getPositionOnLane()) {
2318 : type = ENCOUNTER_TYPE_FOLLOWING_LEADER;
2319 0 : eInfo.foeConflictEntryDist = e->ego->getBackPositionOnLane() - e->foe->getPositionOnLane();
2320 : } else {
2321 : type = ENCOUNTER_TYPE_FOLLOWING_FOLLOWER;
2322 0 : eInfo.egoConflictEntryDist = e->foe->getBackPositionOnLane() - e->ego->getPositionOnLane();
2323 : }
2324 : #ifdef DEBUG_ENCOUNTER
2325 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
2326 : std::cout << "-> Encounter type: Lead/follow-situation on non-internal lane '" << egoLane->getID() << "'\n";
2327 : }
2328 : #endif
2329 0 : } else if (egoOpposite && foeOpposite) {
2330 0 : if (e->ego->getPositionOnLane() < e->foe->getPositionOnLane()) {
2331 : type = ENCOUNTER_TYPE_FOLLOWING_LEADER;
2332 0 : eInfo.foeConflictEntryDist = -(e->ego->getBackPositionOnLane() - e->foe->getPositionOnLane());
2333 : } else {
2334 : type = ENCOUNTER_TYPE_FOLLOWING_FOLLOWER;
2335 0 : eInfo.egoConflictEntryDist = -(e->foe->getBackPositionOnLane() - e->ego->getPositionOnLane());
2336 : }
2337 : #ifdef DEBUG_ENCOUNTER
2338 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
2339 : std::cout << "-> Encounter type: Lead/follow-situation while both are driving in the opposite direction on non-internal lane '" << egoLane->getID() << "'\n";
2340 : }
2341 : #endif
2342 : } else {
2343 : type = ENCOUNTER_TYPE_ONCOMING;
2344 0 : const double gap = e->ego->getPositionOnLane() - e->foe->getPositionOnLane();
2345 0 : if (egoOpposite) {
2346 0 : if (e->ego->getPositionOnLane() > e->foe->getPositionOnLane()) {
2347 0 : eInfo.egoConflictEntryDist = gap;
2348 0 : eInfo.foeConflictEntryDist = gap;
2349 : } else {
2350 : type = ENCOUNTER_TYPE_NOCONFLICT_AHEAD;
2351 : }
2352 : } else {
2353 0 : if (e->ego->getPositionOnLane() < e->foe->getPositionOnLane()) {
2354 0 : eInfo.egoConflictEntryDist = -gap;
2355 0 : eInfo.foeConflictEntryDist = -gap;
2356 : } else {
2357 : type = ENCOUNTER_TYPE_NOCONFLICT_AHEAD;
2358 : }
2359 : }
2360 : #ifdef DEBUG_ENCOUNTER
2361 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
2362 : std::cout << "-> Encounter type: oncoming on non-internal lane '" << egoLane->getID() << "'\n";
2363 : }
2364 : #endif
2365 :
2366 : }
2367 28 : } else if (&(foeLane->getEdge()) == &(egoLane->getEdge())) {
2368 : // Foe is on the same non-internal edge but not on the same lane. Treat this as no conflict for now
2369 : // XXX: this disregards conflicts for vehicles on adjacent lanes
2370 : if (foeOpposite != egoOpposite) {
2371 : type = ENCOUNTER_TYPE_NOCONFLICT_AHEAD;
2372 : } else {
2373 : type = ENCOUNTER_TYPE_ON_ADJACENT_LANES;
2374 : }
2375 : #ifdef DEBUG_ENCOUNTER
2376 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
2377 : std::cout << "-> Encounter type: " << type << std::endl;
2378 : }
2379 : #endif
2380 : } else {
2381 28 : if (!egoOpposite && !foeOpposite) {
2382 : assert(&(egoLane->getEdge()) == &(foeConflictLane->getEdge()));
2383 : assert(egoDistToConflictLane <= 0);
2384 : // Foe must be on a route leading into the ego's edge
2385 0 : if (foeConflictLane == egoLane) {
2386 : type = ENCOUNTER_TYPE_FOLLOWING_LEADER;
2387 0 : eInfo.foeConflictEntryDist = foeDistToConflictLane + e->ego->getBackPositionOnLane();
2388 :
2389 : #ifdef DEBUG_ENCOUNTER
2390 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter))
2391 : std::cout << "-> Encounter type: Ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' leads foe '"
2392 : << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
2393 : << " (gap = " << eInfo.foeConflictEntryDist << ")\n";
2394 : #endif
2395 : } else {
2396 : // Foe's route leads to an adjacent lane of the current lane of the ego
2397 : type = ENCOUNTER_TYPE_ON_ADJACENT_LANES;
2398 : #ifdef DEBUG_ENCOUNTER
2399 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
2400 : std::cout << "-> Encounter type: " << type << std::endl;
2401 : }
2402 : #endif
2403 : }
2404 28 : } else if (egoOpposite && foeOpposite) {
2405 : // XXX determine follower relationship by searching for the foe lane in the opposites of ego bestlanes
2406 : type = ENCOUNTER_TYPE_FOLLOWING_LEADER;
2407 : /*
2408 : if (e->ego->getPositionOnLane() < e->foe->getPositionOnLane()) {
2409 : type = ENCOUNTER_TYPE_FOLLOWING_LEADER;
2410 : eInfo.foeConflictEntryDist = -(e->ego->getBackPositionOnLane() - e->foe->getPositionOnLane());
2411 : } else {
2412 : type = ENCOUNTER_TYPE_FOLLOWING_FOLLOWER;
2413 : eInfo.egoConflictEntryDist = -(e->foe->getBackPositionOnLane() - e->ego->getPositionOnLane());
2414 : }
2415 : */
2416 : #ifdef DEBUG_ENCOUNTER
2417 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
2418 : std::cout << "-> Encounter type: Lead/follow-situation while both are driving in the opposite direction on non-internal lane '" << egoLane->getID() << "'\n";
2419 : }
2420 : #endif
2421 : } else {
2422 : type = ENCOUNTER_TYPE_ONCOMING;
2423 : // XXX determine distance by searching for the foe lane in the opposites of ego bestlanes
2424 : /*
2425 : const double gap = e->ego->getPositionOnLane() - e->foe->getPositionOnLane();
2426 : if (egoOpposite) {
2427 : if (e->ego->getPositionOnLane() > e->foe->getPositionOnLane()) {
2428 : eInfo.egoConflictEntryDist = gap;
2429 : eInfo.foeConflictEntryDist = gap;
2430 : } else {
2431 : type = ENCOUNTER_TYPE_NOCONFLICT_AHEAD;
2432 : }
2433 : } else {
2434 : if (e->ego->getPositionOnLane() < e->foe->getPositionOnLane()) {
2435 : eInfo.egoConflictEntryDist = -gap;
2436 : eInfo.foeConflictEntryDist = -gap;
2437 : } else {
2438 : type = ENCOUNTER_TYPE_NOCONFLICT_AHEAD;
2439 : }
2440 : }
2441 : */
2442 : #ifdef DEBUG_ENCOUNTER
2443 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
2444 : std::cout << "-> Encounter type: oncoming on non-internal lane '" << egoLane->getID() << "'\n";
2445 : }
2446 : #endif
2447 : }
2448 : }
2449 651272 : } else if (&(egoEntryLink->getViaLane()->getEdge()) == &(foeEntryLink->getViaLane()->getEdge())) {
2450 179584 : if (egoEntryLink != foeEntryLink) {
2451 : // XXX: this disregards conflicts for vehicles on adjacent internal lanes
2452 : type = ENCOUNTER_TYPE_ON_ADJACENT_LANES;
2453 : #ifdef DEBUG_ENCOUNTER
2454 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
2455 : std::cout << "-> Encounter type: " << type << std::endl;
2456 : }
2457 : #endif
2458 : } else {
2459 : // Lead / follow situation on connection
2460 140641 : if (egoLane == egoConflictLane && foeLane != foeConflictLane) {
2461 : // ego on junction, foe not yet
2462 : type = ENCOUNTER_TYPE_FOLLOWING_LEADER;
2463 70657 : eInfo.foeConflictEntryDist = foeDistToConflictLane + e->ego->getBackPositionOnLane();
2464 70657 : if (e->ego->getLane()->getIncomingLanes()[0].lane->isInternal()) {
2465 24 : eInfo.foeConflictEntryDist += e->ego->getLane()->getIncomingLanes()[0].lane->getLength();
2466 : }
2467 : #ifdef DEBUG_ENCOUNTER
2468 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter))
2469 : std::cout << "-> Encounter type: Ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' leads foe '"
2470 : << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
2471 : << " (gap = " << eInfo.foeConflictEntryDist << ")\n";
2472 : #endif
2473 69984 : } else if (egoLane != egoConflictLane && foeLane == foeConflictLane) {
2474 : // foe on junction, ego not yet
2475 : type = ENCOUNTER_TYPE_FOLLOWING_FOLLOWER;
2476 68570 : eInfo.egoConflictEntryDist = egoDistToConflictLane + e->foe->getBackPositionOnLane();
2477 68570 : if (e->foe->getLane()->getIncomingLanes()[0].lane->isInternal()) {
2478 0 : eInfo.egoConflictEntryDist += e->foe->getLane()->getIncomingLanes()[0].lane->getLength();
2479 : }
2480 : #ifdef DEBUG_ENCOUNTER
2481 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter))
2482 : std::cout << "-> Encounter type: Ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' follows foe '"
2483 : << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
2484 : << " (gap = " << eInfo.egoConflictEntryDist << ", case2)\n";
2485 : #endif
2486 1414 : } else if (e->ego->getLaneChangeModel().isOpposite() || e->foe->getLaneChangeModel().isOpposite()) {
2487 : type = ENCOUNTER_TYPE_MERGING;
2488 0 : eInfo.foeConflictEntryDist = foeDistToConflictLane;
2489 0 : eInfo.egoConflictEntryDist = egoDistToConflictLane;
2490 : #ifdef DEBUG_ENCOUNTER
2491 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter))
2492 : std::cout << "-> Encounter type: Ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' merges with foe '"
2493 : << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
2494 : << " (gap = " << eInfo.egoConflictEntryDist << ", case5)\n";
2495 : #endif
2496 :
2497 : } else {
2498 : // Both must be already on the junction in a lead / follow situation on a connection
2499 : // (since they approach via the same link, findSurroundingVehicles() would have determined a
2500 : // different conflictLane if both are not on the junction)
2501 1414 : if (egoLane != egoConflictLane || foeLane != foeConflictLane) {
2502 0 : WRITE_WARNINGF(TL("Cannot classify SSM encounter between ego vehicle % and foe vehicle % at time=%\n"), e->ego->getID(), e->foe->getID(), SIMTIME);
2503 0 : return ENCOUNTER_TYPE_NOCONFLICT_AHEAD;
2504 : }
2505 1414 : if (egoLane == foeLane) {
2506 : // both on the same internal lane
2507 1414 : if (e->ego->getPositionOnLane() > e->foe->getPositionOnLane()) {
2508 : type = ENCOUNTER_TYPE_FOLLOWING_LEADER;
2509 707 : eInfo.foeConflictEntryDist = foeDistToConflictLane + e->ego->getBackPositionOnLane();
2510 : #ifdef DEBUG_ENCOUNTER
2511 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter))
2512 : std::cout << "-> Encounter type: Ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' leads foe '"
2513 : << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
2514 : << " (gap = " << eInfo.foeConflictEntryDist << ")"
2515 : << std::endl;
2516 : #endif
2517 : } else {
2518 : type = ENCOUNTER_TYPE_FOLLOWING_FOLLOWER;
2519 707 : eInfo.egoConflictEntryDist = egoDistToConflictLane + e->foe->getBackPositionOnLane();
2520 : #ifdef DEBUG_ENCOUNTER
2521 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter))
2522 : std::cout << "-> Encounter type: Ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' follows foe '"
2523 : << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
2524 : << " (gap = " << eInfo.egoConflictEntryDist << ", case3)"
2525 : << std::endl;
2526 : #endif
2527 : }
2528 : } else {
2529 : // ego and foe on distinct, consecutive internal lanes
2530 : #ifdef DEBUG_ENCOUNTER
2531 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
2532 : std::cout << " Lead/follow situation on consecutive internal lanes." << std::endl;
2533 : }
2534 : #endif
2535 : MSLane* lane = egoEntryLink->getViaLane();
2536 : while (true) {
2537 : // Find first of egoLane and foeLane while crossing the junction (this dertermines who's the follower)
2538 : // Then set the conflict lane to the lane of the leader and adapt the follower's distance to conflict
2539 0 : if (egoLane == lane) {
2540 : // ego is follower
2541 : type = ENCOUNTER_TYPE_FOLLOWING_FOLLOWER;
2542 : // adapt conflict dist
2543 0 : eInfo.egoConflictEntryDist = egoDistToConflictLane;
2544 0 : while (lane != foeLane) {
2545 0 : eInfo.egoConflictEntryDist += lane->getLength();
2546 0 : lane = lane->getLinkCont()[0]->getViaLane();
2547 : assert(lane != 0);
2548 : }
2549 0 : eInfo.egoConflictEntryDist += e->foe->getBackPositionOnLane();
2550 : egoConflictLane = lane;
2551 : #ifdef DEBUG_ENCOUNTER
2552 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter))
2553 : std::cout << "-> Encounter type: Ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' follows foe '"
2554 : << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
2555 : << " (gap = " << eInfo.egoConflictEntryDist << ", case4)"
2556 : << std::endl;
2557 : #endif
2558 0 : break;
2559 0 : } else if (foeLane == lane) {
2560 : // ego is leader
2561 : type = ENCOUNTER_TYPE_FOLLOWING_LEADER;
2562 : // adapt conflict dist
2563 0 : eInfo.foeConflictEntryDist = foeDistToConflictLane;
2564 0 : while (lane != egoLane) {
2565 0 : eInfo.foeConflictEntryDist += lane->getLength();
2566 0 : lane = lane->getLinkCont()[0]->getViaLane();
2567 : assert(lane != 0);
2568 : }
2569 0 : eInfo.foeConflictEntryDist += e->ego->getBackPositionOnLane();
2570 : foeConflictLane = lane;
2571 : #ifdef DEBUG_ENCOUNTER
2572 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter))
2573 : std::cout << "-> Encounter type: Ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' leads foe '"
2574 : << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
2575 : << " (gap = " << eInfo.foeConflictEntryDist << ")"
2576 : << std::endl;
2577 : #endif
2578 0 : break;
2579 : }
2580 0 : lane = lane->getLinkCont()[0]->getViaLane();
2581 : assert(lane != 0);
2582 : }
2583 : }
2584 : #ifdef DEBUG_ENCOUNTER
2585 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter))
2586 : std::cout << "-> Encounter type: Lead/follow-situation on connection from '" << egoEntryLink->getLaneBefore()->getID()
2587 : << "' to '" << egoEntryLink->getLane()->getID() << "'" << std::endl;
2588 : #endif
2589 : }
2590 : }
2591 : } else {
2592 : // Entry links to junctions lead to different internal edges.
2593 : // There are three possibilities, either the edges cross, merge or have no conflict
2594 : const std::vector<MSLink*>& egoFoeLinks = egoEntryLink->getFoeLinks();
2595 : const std::vector<MSLink*>& foeFoeLinks = foeEntryLink->getFoeLinks();
2596 : // Determine whether ego and foe links are foes
2597 471688 : bool crossOrMerge = (find(egoFoeLinks.begin(), egoFoeLinks.end(), foeEntryLink) != egoFoeLinks.end()
2598 471688 : || std::find(foeFoeLinks.begin(), foeFoeLinks.end(), egoEntryLink) != foeFoeLinks.end());
2599 : if (!crossOrMerge) {
2600 : // if (&(foeEntryLink->getLane()->getEdge()) == &(egoEntryLink->getLane()->getEdge())) {
2601 : // // XXX: the situation of merging into adjacent lanes is disregarded for now <- the alleged situation appears to imply crossOrMerge!!!
2602 : // type = ENCOUNTER_TYPE_MERGING_ADJACENT;
2603 : //#ifdef DEBUG_SSM
2604 : // std::cout << "-> Encounter type: No conflict (adjacent lanes)." << std::endl;
2605 : //#endif
2606 : // } else {
2607 : type = ENCOUNTER_TYPE_NOCONFLICT_AHEAD;
2608 : #ifdef DEBUG_ENCOUNTER
2609 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
2610 : std::cout << "-> Encounter type: No conflict.\n";
2611 : }
2612 : #endif
2613 : // }
2614 66766 : } else if (&(foeEntryLink->getLane()->getEdge()) == &(egoEntryLink->getLane()->getEdge())) {
2615 2617 : if (foeEntryLink->getLane() == egoEntryLink->getLane()) {
2616 : type = ENCOUNTER_TYPE_MERGING;
2617 : assert(egoConflictLane->isInternal());
2618 : assert(foeConflictLane->isInternal());
2619 2133 : eInfo.egoConflictEntryDist = egoDistToConflictLane + egoEntryLink->getInternalLengthsAfter();
2620 2133 : eInfo.foeConflictEntryDist = foeDistToConflictLane + foeEntryLink->getInternalLengthsAfter();
2621 :
2622 2133 : MSLink* egoEntryLinkSucc = egoEntryLink->getViaLane()->getLinkCont().front();
2623 2133 : if (egoEntryLinkSucc->isInternalJunctionLink() && e->ego->getLane() == egoEntryLinkSucc->getViaLane()) {
2624 : // ego is already past the internal junction
2625 48 : eInfo.egoConflictEntryDist -= egoEntryLink->getViaLane()->getLength();
2626 48 : eInfo.egoConflictExitDist -= egoEntryLink->getViaLane()->getLength();
2627 : }
2628 2133 : MSLink* foeEntryLinkSucc = foeEntryLink->getViaLane()->getLinkCont().front();
2629 2133 : if (foeEntryLinkSucc->isInternalJunctionLink() && e->foe->getLane() == foeEntryLinkSucc->getViaLane()) {
2630 : // foe is already past the internal junction
2631 48 : eInfo.foeConflictEntryDist -= foeEntryLink->getViaLane()->getLength();
2632 48 : eInfo.foeConflictExitDist -= foeEntryLink->getViaLane()->getLength();
2633 : }
2634 :
2635 : #ifdef DEBUG_ENCOUNTER
2636 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter))
2637 : std::cout << "-> Encounter type: Merging situation of ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' and foe '"
2638 : << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
2639 : << "\nDistances to merge-point: ego: " << eInfo.egoConflictEntryDist << ", foe: " << eInfo.foeConflictEntryDist
2640 : << std::endl;
2641 : #endif
2642 : } else {
2643 : // Links leading to the same edge but different lanes. XXX: Disregards conflicts on adjacent lanes
2644 : type = ENCOUNTER_TYPE_MERGING_ADJACENT;
2645 : #ifdef DEBUG_ENCOUNTER
2646 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
2647 : std::cout << "-> Encounter type: No conflict: " << type << std::endl;
2648 : }
2649 : #endif
2650 : }
2651 : } else {
2652 : type = ENCOUNTER_TYPE_CROSSING;
2653 :
2654 : assert(egoConflictLane->isInternal());
2655 : assert(foeConflictLane->getEdge().getToJunction() == egoConflictLane->getEdge().getToJunction());
2656 :
2657 : // If the conflict lanes are internal, they may not correspond to the
2658 : // actually crossing parts of the corresponding connections.
2659 : // Adjust the conflict lanes accordingly.
2660 : // set back both to the first parts of the corresponding connections
2661 64149 : double offset = 0.;
2662 64149 : egoConflictLane = egoConflictLane->getFirstInternalInConnection(offset);
2663 64149 : egoDistToConflictLane -= offset;
2664 64149 : foeConflictLane = foeConflictLane->getFirstInternalInConnection(offset);
2665 64149 : foeDistToConflictLane -= offset;
2666 : // find the distances to the conflict from the junction entry for both vehicles
2667 : // Here we also determine the real crossing lanes (before, the conflict lane is the first lane of the connection)
2668 : // for the ego
2669 : double egoDistToConflictFromJunctionEntry = INVALID_DOUBLE;
2670 64651 : while (foeConflictLane != nullptr && foeConflictLane->isInternal()) {
2671 64651 : egoDistToConflictFromJunctionEntry = egoEntryLink->getLengthsBeforeCrossing(foeConflictLane);
2672 64651 : if (egoDistToConflictFromJunctionEntry != INVALID_DOUBLE) {
2673 : // found correct foeConflictLane
2674 64149 : egoDistToConflictFromJunctionEntry += 0.5 * (foeConflictLane->getWidth() - e->foe->getVehicleType().getWidth());
2675 64149 : break;
2676 : }
2677 502 : if (!foeConflictLane->getCanonicalSuccessorLane()->isInternal()) {
2678 : // intersection has wierd geometry and the intersection was found
2679 : egoDistToConflictFromJunctionEntry = 0;
2680 0 : WRITE_WARNINGF(TL("Cannot compute SSM due to bad internal lane geometry at junction '%'. Crossing point between traffic from links % and % not found."),
2681 : egoEntryLink->getJunction()->getID(),
2682 : egoEntryLink->getIndex(),
2683 : foeEntryLink->getIndex());
2684 0 : break;
2685 : }
2686 502 : foeConflictLane = foeConflictLane->getCanonicalSuccessorLane();
2687 : assert(foeConflictLane != nullptr && foeConflictLane->isInternal()); // this loop should be ended by the break! Otherwise the lanes do not cross, which should be the case here.
2688 : }
2689 : assert(egoDistToConflictFromJunctionEntry != INVALID_DOUBLE);
2690 :
2691 : // for the foe
2692 : double foeDistToConflictFromJunctionEntry = INVALID_DOUBLE;
2693 : foeDistToConflictFromJunctionEntry = INVALID_DOUBLE;
2694 65017 : while (egoConflictLane != nullptr && egoConflictLane->isInternal()) {
2695 65017 : foeDistToConflictFromJunctionEntry = foeEntryLink->getLengthsBeforeCrossing(egoConflictLane);
2696 65017 : if (foeDistToConflictFromJunctionEntry != INVALID_DOUBLE) {
2697 : // found correct egoConflictLane
2698 64149 : foeDistToConflictFromJunctionEntry += 0.5 * (egoConflictLane->getWidth() - e->ego->getVehicleType().getWidth());
2699 64149 : break;
2700 : }
2701 868 : if (!egoConflictLane->getCanonicalSuccessorLane()->isInternal()) {
2702 : // intersection has wierd geometry and the intersection was found
2703 : foeDistToConflictFromJunctionEntry = 0;
2704 0 : WRITE_WARNINGF(TL("Cannot compute SSM due to bad internal lane geometry at junction '%'. Crossing point between traffic from links % and % not found."),
2705 : foeEntryLink->getJunction()->getID(),
2706 : foeEntryLink->getIndex(),
2707 : egoEntryLink->getIndex());
2708 0 : break;
2709 : }
2710 868 : egoConflictLane = egoConflictLane->getCanonicalSuccessorLane();
2711 : assert(egoConflictLane != nullptr && egoConflictLane->isInternal()); // this loop should be ended by the break! Otherwise the lanes do not cross, which should be the case here.
2712 : }
2713 : assert(foeDistToConflictFromJunctionEntry != INVALID_DOUBLE);
2714 :
2715 : // store conflict entry information in eInfo
2716 :
2717 : // // TO-DO: equip these with exit times to store relevant PET sections in encounter
2718 : // eInfo.egoConflictEntryCrossSection = std::make_pair(egoConflictLane, egoDistToConflictFromJunctionEntry - egoInternalLaneLengthsBeforeCrossing);
2719 : // eInfo.foeConflictEntryCrossSection = std::make_pair(foeConflictLane, foeDistToConflictFromJunctionEntry - foeInternalLaneLengthsBeforeCrossing);
2720 :
2721 : // Take into account the lateral position for the exact determination of the conflict point
2722 : // whether lateral position increases or decreases conflict distance depends on lane angles at conflict
2723 : // -> conflictLaneOrientation in {-1,+1}
2724 : // First, measure the angle between the two connection lines (straight lines from junction entry point to junction exit point)
2725 64149 : Position egoEntryPos = egoEntryLink->getViaLane()->getShape().front();
2726 64149 : Position egoExitPos = egoEntryLink->getCorrespondingExitLink()->getInternalLaneBefore()->getShape().back();
2727 64149 : PositionVector egoConnectionLine(egoEntryPos, egoExitPos);
2728 64149 : Position foeEntryPos = foeEntryLink->getViaLane()->getShape().front();
2729 64149 : Position foeExitPos = foeEntryLink->getCorrespondingExitLink()->getInternalLaneBefore()->getShape().back();
2730 64149 : PositionVector foeConnectionLine(foeEntryPos, foeExitPos);
2731 64149 : double angle = std::fmod(egoConnectionLine.rotationAtOffset(0.) - foeConnectionLine.rotationAtOffset(0.), (2 * M_PI));
2732 64149 : if (angle < 0) {
2733 33121 : angle += 2 * M_PI;
2734 : }
2735 : assert(angle >= 0);
2736 : assert(angle <= 2 * M_PI);
2737 64149 : if (angle > M_PI) {
2738 33121 : angle -= 2 * M_PI;
2739 : }
2740 : assert(angle >= -M_PI);
2741 : assert(angle <= M_PI);
2742 : // Determine orientation of the connection lines. (Positive values mean that the ego vehicle approaches from the foe's left side.)
2743 64149 : double crossingOrientation = (angle < 0) - (angle > 0);
2744 :
2745 : // Adjust conflict dist to lateral positions
2746 : // TODO: This could more precisely be calculated wrt the angle of the crossing *at the conflict point*
2747 64149 : egoDistToConflictFromJunctionEntry -= crossingOrientation * e->foe->getLateralPositionOnLane();
2748 64149 : foeDistToConflictFromJunctionEntry += crossingOrientation * e->ego->getLateralPositionOnLane();
2749 :
2750 : // Complete entry distances
2751 64149 : eInfo.egoConflictEntryDist = egoDistToConflictLane + egoDistToConflictFromJunctionEntry;
2752 64149 : eInfo.foeConflictEntryDist = foeDistToConflictLane + foeDistToConflictFromJunctionEntry;
2753 :
2754 :
2755 : // TODO: This could also more precisely be calculated wrt the angle of the crossing *at the conflict point*
2756 64149 : eInfo.egoConflictAreaLength = e->foe->getWidth();
2757 64149 : eInfo.foeConflictAreaLength = e->ego->getWidth();
2758 :
2759 : // resulting exit distances
2760 64149 : eInfo.egoConflictExitDist = eInfo.egoConflictEntryDist + eInfo.egoConflictAreaLength + e->ego->getLength();
2761 64149 : eInfo.foeConflictExitDist = eInfo.foeConflictEntryDist + eInfo.foeConflictAreaLength + e->foe->getLength();
2762 :
2763 : #ifdef DEBUG_ENCOUNTER
2764 : if (DEBUG_COND_ENCOUNTER(eInfo.encounter)) {
2765 : std::cout << " Determined exact conflict distances for crossing conflict."
2766 : << "\n crossingOrientation=" << crossingOrientation
2767 : << ", egoCrossingAngle=" << egoConnectionLine.rotationAtOffset(0.)
2768 : << ", foeCrossingAngle=" << foeConnectionLine.rotationAtOffset(0.)
2769 : << ", relativeAngle=" << angle
2770 : << " (foe from " << (crossingOrientation > 0 ? "right)" : "left)")
2771 : << "\n resulting offset for conflict entry distance:"
2772 : << "\n ego=" << crossingOrientation* e->foe->getLateralPositionOnLane()
2773 : << ", foe=" << crossingOrientation* e->ego->getLateralPositionOnLane()
2774 : << "\n distToConflictLane:"
2775 : << "\n ego=" << egoDistToConflictLane
2776 : << ", foe=" << foeDistToConflictLane
2777 : << "\n distToConflictFromJunctionEntry:"
2778 : << "\n ego=" << egoDistToConflictFromJunctionEntry
2779 : << ", foe=" << foeDistToConflictFromJunctionEntry
2780 : << "\n resulting entry distances:"
2781 : << "\n ego=" << eInfo.egoConflictEntryDist
2782 : << ", foe=" << eInfo.foeConflictEntryDist
2783 : << "\n resulting exit distances:"
2784 : << "\n ego=" << eInfo.egoConflictExitDist
2785 : << ", foe=" << eInfo.foeConflictExitDist
2786 : << std::endl;
2787 :
2788 : std::cout << "real egoConflictLane: '" << (egoConflictLane == 0 ? "NULL" : egoConflictLane->getID()) << "'\n"
2789 : << "real foeConflictLane: '" << (foeConflictLane == 0 ? "NULL" : foeConflictLane->getID()) << "'\n"
2790 : << "-> Encounter type: Crossing situation of ego '" << e->ego->getID() << "' on lane '" << egoLane->getID() << "' and foe '"
2791 : << e->foe->getID() << "' on lane '" << foeLane->getID() << "'"
2792 : << "\nDistances to crossing-point: ego: " << eInfo.egoConflictEntryDist << ", foe: " << eInfo.foeConflictEntryDist
2793 : << std::endl;
2794 : }
2795 : #endif
2796 64149 : }
2797 : }
2798 : }
2799 : return type;
2800 : }
2801 :
2802 :
2803 :
2804 : const MSLane*
2805 6475290 : MSDevice_SSM::findFoeConflictLane(const MSVehicle* foe, const MSLane* egoConflictLane, double& distToConflictLane) const {
2806 :
2807 : #ifdef DEBUG_SSM
2808 : if (DEBUG_COND(myHolderMS))
2809 : std::cout << SIMTIME << " findFoeConflictLane() for foe '"
2810 : << foe->getID() << "' on lane '" << foe->getLane()->getID()
2811 : << "' (with egoConflictLane=" << (egoConflictLane == 0 ? "NULL" : egoConflictLane->getID())
2812 : << ")\nfoeBestLanes: " << ::toString(foe->getBestLanesContinuation())
2813 : << std::endl;
2814 : #endif
2815 6475290 : if (foe->getLaneChangeModel().isOpposite()) {
2816 : // distinguish three cases
2817 : // 1) foe is driving in the same direction as ego and ego is driving in lane direction -> ENCOUNTER_TYPE_ON_ADJACENT_LANES
2818 : // 2) foe is driving in the same direction as ego and ego is also driving in the opposite direction -> ENCOUNTER_TYPE_FOLLOWING
2819 : // 3) foe is driving in the opposite direction as ego and both are driving way from each other -> ENCOUNTER_TYPE_NOCONFLICT_AHEAD
2820 : // 3) foe is driving in the opposite direction as ego and both are driving towards each other -> ENCOUNTER_TYPE_ONCOMING
2821 : #ifdef DEBUG_SSM_OPPOSITE
2822 : #endif
2823 24755 : auto egoIt = std::find(myHolder.getCurrentRouteEdge(), myHolder.getRoute().end(), foe->getEdge());
2824 24755 : if (egoIt != myHolder.getRoute().end()) {
2825 : // same direction, foe is leader
2826 19037 : if (myHolderMS->getLaneChangeModel().isOpposite()) {
2827 15135 : if (egoConflictLane->isInternal() && !foe->getLane()->isInternal()) {
2828 : // lead/follow situation resolved elsewhere
2829 : return nullptr;
2830 : }
2831 15135 : return foe->getLane();
2832 : } else {
2833 : // adjacent
2834 : return nullptr;
2835 : }
2836 : }
2837 5718 : auto foeIt = std::find(foe->getCurrentRouteEdge(), foe->getRoute().end(), myHolder.getEdge());
2838 5718 : if (foeIt != foe->getRoute().end()) {
2839 : // same direction, ego is leader
2840 0 : if (myHolderMS->getLaneChangeModel().isOpposite()) {
2841 : return egoConflictLane;
2842 : } else {
2843 : // adjacent
2844 : return nullptr;
2845 : }
2846 : }
2847 5718 : auto egoIt2 = std::find(myHolder.getCurrentRouteEdge(), myHolder.getRoute().end(), foe->getEdge()->getOppositeEdge());
2848 5718 : if (egoIt2 != myHolder.getRoute().end()) {
2849 : // opposite direction, driving towards each other
2850 : return egoConflictLane;
2851 : } else {
2852 : // opposite direction, driving away from each other
2853 : return nullptr;
2854 : }
2855 : }
2856 :
2857 6450535 : const MSLane* foeLane = foe->getLane();
2858 6450535 : std::vector<MSLane*>::const_iterator laneIter = foe->getBestLanesContinuation().begin();
2859 6450535 : std::vector<MSLane*>::const_iterator foeBestLanesEnd = foe->getBestLanesContinuation().end();
2860 : assert(foeLane->isInternal() || *laneIter == foeLane);
2861 6450535 : distToConflictLane = -foe->getPositionOnLane();
2862 :
2863 : // Potential conflict lies on junction if egoConflictLane is internal
2864 6450535 : const MSJunction* conflictJunction = egoConflictLane->isInternal() ? egoConflictLane->getEdge().getToJunction() : nullptr;
2865 : #ifdef DEBUG_SSM
2866 : if (DEBUG_COND(myHolderMS))
2867 : if (conflictJunction != 0) {
2868 : std::cout << "Potential conflict on junction '" << conflictJunction->getID()
2869 : << std::endl;
2870 : }
2871 : #endif
2872 6450535 : if (foeLane->isInternal() && foeLane->getEdge().getToJunction() == conflictJunction) {
2873 : // foe is already on the conflict junction
2874 97766 : if (egoConflictLane != nullptr && egoConflictLane->isInternal() && egoConflictLane->getLinkCont()[0]->getViaLane() == foeLane) {
2875 0 : distToConflictLane += egoConflictLane->getLength();
2876 : }
2877 97766 : return foeLane;
2878 : }
2879 :
2880 : // Foe is not on the conflict junction
2881 :
2882 : // Leading internal lanes in bestlanes are resembled as a single NULL-pointer skip them
2883 6352769 : if (*laneIter == nullptr) {
2884 175566 : while (foeLane != nullptr && foeLane->isInternal()) {
2885 87863 : distToConflictLane += foeLane->getLength();
2886 87863 : foeLane = foeLane->getLinkCont()[0]->getViaLane();
2887 : }
2888 : ++laneIter;
2889 : assert(laneIter == foeBestLanesEnd || *laneIter != 0);
2890 : }
2891 :
2892 : // Look for the junction downstream along foeBestLanes
2893 6885903 : while (laneIter != foeBestLanesEnd && distToConflictLane <= myRange) {
2894 : // Eventual internal lanes were skipped
2895 : assert(*laneIter == foeLane || foeLane == 0);
2896 6717234 : foeLane = *laneIter;
2897 : assert(!foeLane->isInternal());
2898 6717234 : if (&foeLane->getEdge() == &egoConflictLane->getEdge()) {
2899 : #ifdef DEBUG_SSM
2900 : if (DEBUG_COND(myHolderMS)) {
2901 : std::cout << "Found conflict lane for foe: '" << foeLane->getID() << "'" << std::endl;
2902 : }
2903 : #endif
2904 : // found the potential conflict edge along foeBestLanes
2905 5598678 : return foeLane;
2906 : }
2907 : // No conflict on foeLane
2908 1118556 : distToConflictLane += foeLane->getLength();
2909 :
2910 : // set laneIter to next non internal lane along foeBestLanes
2911 : ++laneIter;
2912 1118556 : if (laneIter == foeBestLanesEnd) {
2913 : return nullptr;
2914 : }
2915 1086641 : MSLane* const nextNonInternalLane = *laneIter;
2916 1086641 : const MSLink* const link = foeLane->getLinkTo(nextNonInternalLane);
2917 1086641 : if (link == nullptr) {
2918 : // encountered incomplete route
2919 : return nullptr;
2920 : }
2921 : // Set foeLane to first internal lane on the next junction
2922 : foeLane = link->getViaLane();
2923 : assert(foeLane == 0 || foeLane->isInternal());
2924 1086641 : if (foeLane == nullptr) {
2925 : foeLane = nextNonInternalLane;
2926 0 : continue;
2927 : }
2928 1086641 : if (foeLane->getEdge().getToJunction() == conflictJunction) {
2929 : assert(foeLane != 0);
2930 : #ifdef DEBUG_SSM
2931 : if (DEBUG_COND(myHolderMS)) {
2932 : std::cout << "Found conflict lane for foe: '" << foeLane->getID() << "'" << std::endl;
2933 : }
2934 : #endif
2935 : // found egoConflictLane, resp. the conflict junction, along foeBestLanes
2936 553507 : return foeLane;
2937 : }
2938 : // No conflict on junction
2939 533134 : distToConflictLane += link->getInternalLengthsAfter();
2940 : foeLane = nextNonInternalLane;
2941 : }
2942 : // Didn't find conflicting lane on foeBestLanes within range.
2943 : return nullptr;
2944 : }
2945 :
2946 : void
2947 2538241 : MSDevice_SSM::flushConflicts(bool flushAll) {
2948 : #ifdef DEBUG_SSM
2949 : if (DEBUG_COND(myHolderMS)) {
2950 : std::cout << "\n" << SIMTIME << " Device '" << getID() << "' flushConflicts past=" << myPastConflicts.size()
2951 : << " oldestActive=" << (myOldestActiveEncounterBegin == INVALID_DOUBLE ? -1 : myOldestActiveEncounterBegin)
2952 : << " topBegin=" << (myPastConflicts.size() > 0 ? myPastConflicts.top()->begin : -1)
2953 : << "\n";
2954 : }
2955 : #endif
2956 5081990 : while (!myPastConflicts.empty()) {
2957 61283 : Encounter* top = myPastConflicts.top();
2958 61283 : if (flushAll || top->begin <= myOldestActiveEncounterBegin) {
2959 : bool write = true;
2960 5508 : if (myFilterConflictTypes) {
2961 : std::vector<int> foundTypes;
2962 16 : std::set<int> encounterTypes(top->typeSpan.begin(), top->typeSpan.end());
2963 16 : std::set_intersection(
2964 : myDroppedConflictTypes.begin(), myDroppedConflictTypes.end(),
2965 : encounterTypes.begin(), encounterTypes.end(),
2966 : std::back_inserter(foundTypes));
2967 : write = foundTypes.size() == 0;
2968 16 : }
2969 16 : if (write) {
2970 5500 : writeOutConflict(top);
2971 : }
2972 : myPastConflicts.pop();
2973 5508 : delete top;
2974 : } else {
2975 : break;
2976 : }
2977 : }
2978 2538241 : }
2979 :
2980 : void
2981 5180 : MSDevice_SSM::flushGlobalMeasures() {
2982 5180 : std::string egoID = myHolderMS->getID();
2983 : #ifdef DEBUG_SSM
2984 : if (DEBUG_COND(myHolderMS))
2985 : std::cout << SIMTIME << " flushGlobalMeasures() of vehicle '"
2986 : << egoID << "'"
2987 : << "'\ntoGeo=" << myUseGeoCoords << std::endl;
2988 : #endif
2989 5180 : if (myComputeBR || myComputeSGAP || myComputeTGAP) {
2990 460 : myOutputFile->openTag("globalMeasures");
2991 460 : myOutputFile->writeAttr("ego", egoID);
2992 920 : myOutputFile->openTag("timeSpan").writeAttr("values", myGlobalMeasuresTimeSpan).closeTag();
2993 460 : if (myWritePositions) {
2994 8 : myOutputFile->openTag("positions").writeAttr("values", myGlobalMeasuresPositions).closeTag();
2995 : }
2996 460 : if (myWriteLanesPositions) {
2997 16 : myOutputFile->openTag("lane").writeAttr("values", myGlobalMeasuresLaneIDs).closeTag();
2998 16 : myOutputFile->openTag("lanePosition").writeAttr("values", myGlobalMeasuresLanesPositions).closeTag();
2999 : }
3000 460 : if (myComputeBR) {
3001 920 : myOutputFile->openTag("BRSpan").writeAttr("values", myBRspan).closeTag();
3002 :
3003 460 : if (myMaxBR.second != 0.0) {
3004 384 : if (myUseGeoCoords) {
3005 4 : toGeo(myMaxBR.first.second);
3006 : }
3007 768 : myOutputFile->openTag("maxBR").writeAttr("time", myMaxBR.first.first).writeAttr("position", makeStringWithNAs(myMaxBR.first.second)).writeAttr("value", myMaxBR.second).closeTag();
3008 : }
3009 : }
3010 :
3011 460 : if (myComputeSGAP) {
3012 896 : myOutputFile->openTag("SGAPSpan").writeAttr("values", makeStringWithNAs(mySGAPspan, INVALID_DOUBLE)).closeTag();
3013 448 : if (myMinSGAP.second != "") {
3014 176 : if (myUseGeoCoords) {
3015 4 : toGeo(myMinSGAP.first.first.second);
3016 : }
3017 352 : myOutputFile->openTag("minSGAP").writeAttr("time", myMinSGAP.first.first.first)
3018 352 : .writeAttr("position", makeStringWithNAs(myMinSGAP.first.first.second))
3019 352 : .writeAttr("value", myMinSGAP.first.second)
3020 352 : .writeAttr("leader", myMinSGAP.second).closeTag();
3021 : }
3022 : }
3023 :
3024 460 : if (myComputeTGAP) {
3025 896 : myOutputFile->openTag("TGAPSpan").writeAttr("values", makeStringWithNAs(myTGAPspan, INVALID_DOUBLE)).closeTag();
3026 448 : if (myMinTGAP.second != "") {
3027 168 : if (myUseGeoCoords) {
3028 4 : toGeo(myMinTGAP.first.first.second);
3029 : }
3030 336 : myOutputFile->openTag("minTGAP").writeAttr("time", myMinTGAP.first.first.first)
3031 336 : .writeAttr("position", makeStringWithNAs(myMinTGAP.first.first.second))
3032 336 : .writeAttr("value", myMinTGAP.first.second)
3033 336 : .writeAttr("leader", myMinTGAP.second).closeTag();
3034 : }
3035 : }
3036 : // close globalMeasures
3037 920 : myOutputFile->closeTag();
3038 : }
3039 5180 : }
3040 :
3041 : void
3042 2902 : MSDevice_SSM::toGeo(Position& x) {
3043 2902 : GeoConvHelper::getFinal().cartesian2geo(x);
3044 2902 : }
3045 :
3046 : void
3047 12 : MSDevice_SSM::toGeo(PositionVector& xv) {
3048 828 : for (Position& x : xv) {
3049 : if (x != Position::INVALID) {
3050 688 : toGeo(x);
3051 : }
3052 : }
3053 12 : }
3054 :
3055 : void
3056 5500 : MSDevice_SSM::writeOutConflict(Encounter* e) {
3057 : #ifdef DEBUG_SSM
3058 : if (DEBUG_COND(myHolderMS))
3059 : std::cout << SIMTIME << " writeOutConflict() of vehicles '"
3060 : << e->egoID << "' and '" << e->foeID
3061 : << "'\ntoGeo=" << myUseGeoCoords << std::endl;
3062 : #endif
3063 5500 : myOutputFile->openTag("conflict");
3064 11000 : myOutputFile->writeAttr("begin", e->begin).writeAttr("end", e->end);
3065 11000 : myOutputFile->writeAttr("ego", e->egoID).writeAttr("foe", e->foeID);
3066 :
3067 5500 : if (mySaveTrajectories) {
3068 280 : myOutputFile->openTag("timeSpan").writeAttr("values", e->timeSpan).closeTag();
3069 280 : myOutputFile->openTag("typeSpan").writeAttr("values", e->typeSpan).closeTag();
3070 :
3071 : // Some useful snippets for that (from MSFCDExport.cpp):
3072 140 : if (myUseGeoCoords) {
3073 4 : toGeo(e->egoTrajectory.x);
3074 4 : toGeo(e->foeTrajectory.x);
3075 4 : toGeo(e->conflictPointSpan);
3076 : }
3077 :
3078 280 : myOutputFile->openTag("egoPosition").writeAttr("values", ::toString(e->egoTrajectory.x, myUseGeoCoords ? gPrecisionGeo : gPrecision)).closeTag();
3079 140 : if (myWriteLanesPositions) {
3080 8 : myOutputFile->openTag("egoLane").writeAttr("values", ::toString(e->egoTrajectory.lane)).closeTag();
3081 8 : myOutputFile->openTag("egoLanePosition").writeAttr("values", ::toString(e->egoTrajectory.lanePos)).closeTag();
3082 : }
3083 280 : myOutputFile->openTag("egoVelocity").writeAttr("values", ::toString(e->egoTrajectory.v)).closeTag();
3084 :
3085 280 : myOutputFile->openTag("foePosition").writeAttr("values", ::toString(e->foeTrajectory.x, myUseGeoCoords ? gPrecisionGeo : gPrecision)).closeTag();
3086 140 : if (myWriteLanesPositions) {
3087 8 : myOutputFile->openTag("foeLane").writeAttr("values", ::toString(e->foeTrajectory.lane)).closeTag();
3088 8 : myOutputFile->openTag("foeLanePosition").writeAttr("values", ::toString(e->foeTrajectory.lanePos)).closeTag();
3089 : }
3090 280 : myOutputFile->openTag("foeVelocity").writeAttr("values", ::toString(e->foeTrajectory.v)).closeTag();
3091 :
3092 280 : myOutputFile->openTag("conflictPoint").writeAttr("values", makeStringWithNAs(e->conflictPointSpan)).closeTag();
3093 : }
3094 :
3095 5500 : if (myComputeTTC) {
3096 5464 : if (mySaveTrajectories) {
3097 272 : myOutputFile->openTag("TTCSpan").writeAttr("values", makeStringWithNAs(e->TTCspan, INVALID_DOUBLE)).closeTag();
3098 : }
3099 5464 : if (e->minTTC.time == INVALID_DOUBLE) {
3100 259 : if (myWriteNA) {
3101 510 : myOutputFile->openTag("minTTC").writeAttr("time", "NA").writeAttr("position", "NA").writeAttr("type", "NA").writeAttr("value", "NA").writeAttr("speed", "NA").closeTag();
3102 : }
3103 : } else {
3104 5205 : std::string time = ::toString(e->minTTC.time);
3105 5205 : std::string type = ::toString(int(e->minTTC.type));
3106 5205 : std::string value = ::toString(e->minTTC.value);
3107 5205 : std::string speed = ::toString(e->minTTC.speed);
3108 5205 : if (myUseGeoCoords) {
3109 1110 : toGeo(e->minTTC.pos);
3110 : }
3111 5205 : std::string position = makeStringWithNAs(e->minTTC.pos);
3112 10410 : myOutputFile->openTag("minTTC").writeAttr("time", time).writeAttr("position", position).writeAttr("type", type).writeAttr("value", value).writeAttr("speed", speed).closeTag();
3113 : }
3114 : }
3115 5500 : if (myComputeDRAC) {
3116 5424 : if (mySaveTrajectories) {
3117 280 : myOutputFile->openTag("DRACSpan").writeAttr("values", makeStringWithNAs(e->DRACspan, {0.0, INVALID_DOUBLE})).closeTag();
3118 : }
3119 5424 : if (e->maxDRAC.time == INVALID_DOUBLE) {
3120 238 : if (myWriteNA) {
3121 476 : myOutputFile->openTag("maxDRAC").writeAttr("time", "NA").writeAttr("position", "NA").writeAttr("type", "NA").writeAttr("value", "NA").writeAttr("speed", "NA").closeTag();
3122 : }
3123 : } else {
3124 5186 : std::string time = ::toString(e->maxDRAC.time);
3125 5186 : std::string type = ::toString(int(e->maxDRAC.type));
3126 5186 : std::string value = ::toString(e->maxDRAC.value);
3127 5186 : std::string speed = ::toString(e->maxDRAC.speed);
3128 5186 : if (myUseGeoCoords) {
3129 1092 : toGeo(e->maxDRAC.pos);
3130 : }
3131 5186 : std::string position = makeStringWithNAs(e->maxDRAC.pos);
3132 10372 : myOutputFile->openTag("maxDRAC").writeAttr("time", time).writeAttr("position", position).writeAttr("type", type).writeAttr("value", value).writeAttr("speed", speed).closeTag();
3133 : }
3134 : }
3135 5500 : if (myComputePET) {
3136 3266 : if (e->PET.time == INVALID_DOUBLE) {
3137 2946 : if (myWriteNA) {
3138 5892 : myOutputFile->openTag("PET").writeAttr("time", "NA").writeAttr("position", "NA").writeAttr("type", "NA").writeAttr("value", "NA").writeAttr("speed", "NA").closeTag();
3139 : }
3140 : } else {
3141 320 : std::string time = ::toString(e->PET.time);
3142 320 : std::string type = ::toString(int(e->PET.type));
3143 320 : std::string value = ::toString(e->PET.value);
3144 320 : std::string speed = ::toString(e->PET.speed);
3145 320 : if (myUseGeoCoords) {
3146 0 : toGeo(e->PET.pos);
3147 : }
3148 320 : std::string position = ::toString(e->PET.pos, myUseGeoCoords ? gPrecisionGeo : gPrecision);
3149 640 : myOutputFile->openTag("PET").writeAttr("time", time).writeAttr("position", position).writeAttr("type", type).writeAttr("value", value).writeAttr("speed", speed).closeTag();
3150 : }
3151 : }
3152 5500 : if (myComputePPET) {
3153 1780 : if (mySaveTrajectories) {
3154 8 : myOutputFile->openTag("PPETSpan").writeAttr("values", makeStringWithNAs(e->PPETspan, INVALID_DOUBLE)).closeTag();
3155 : }
3156 1780 : if (e->minPPET.time == INVALID_DOUBLE) {
3157 1619 : if (myWriteNA) {
3158 3238 : myOutputFile->openTag("minPPET").writeAttr("time", "NA").writeAttr("position", "NA").writeAttr("type", "NA").writeAttr("value", "NA").writeAttr("speed", "NA").closeTag();
3159 : }
3160 : } else {
3161 161 : std::string time = ::toString(e->minPPET.time);
3162 161 : std::string type = ::toString(int(e->minPPET.type));
3163 161 : std::string value = ::toString(e->minPPET.value);
3164 161 : std::string speed = ::toString(e->minPPET.speed);
3165 161 : if (myUseGeoCoords) {
3166 0 : toGeo(e->minPPET.pos);
3167 : }
3168 161 : std::string position = makeStringWithNAs(e->minPPET.pos);
3169 322 : myOutputFile->openTag("minPPET").writeAttr("time", time).writeAttr("position", position).writeAttr("type", type).writeAttr("value", value).writeAttr("speed", speed).closeTag();
3170 : }
3171 : }
3172 5500 : if (myComputeMDRAC) {
3173 1824 : if (mySaveTrajectories) {
3174 24 : myOutputFile->openTag("MDRACSpan").writeAttr("values", makeStringWithNAs(e->MDRACspan, {0.0, INVALID_DOUBLE})).closeTag();
3175 : }
3176 1824 : if (e->maxMDRAC.time == INVALID_DOUBLE) {
3177 92 : if (myWriteNA) {
3178 184 : myOutputFile->openTag("maxMDRAC").writeAttr("time", "NA").writeAttr("position", "NA").writeAttr("type", "NA").writeAttr("value", "NA").writeAttr("speed", "NA").closeTag();
3179 : }
3180 : } else {
3181 1732 : std::string time = ::toString(e->maxMDRAC.time);
3182 1732 : std::string type = ::toString(int(e->maxMDRAC.type));
3183 1732 : std::string value = ::toString(e->maxMDRAC.value);
3184 1732 : std::string speed = ::toString(e->maxMDRAC.speed);
3185 1732 : if (myUseGeoCoords) {
3186 0 : toGeo(e->maxMDRAC.pos);
3187 : }
3188 1732 : std::string position = makeStringWithNAs(e->maxMDRAC.pos);
3189 3464 : myOutputFile->openTag("maxMDRAC").writeAttr("time", time).writeAttr("position", position).writeAttr("type", type).writeAttr("value", value).writeAttr("speed", speed).closeTag();
3190 : }
3191 : }
3192 5500 : myOutputFile->closeTag();
3193 5500 : }
3194 :
3195 : std::string
3196 1036 : MSDevice_SSM::makeStringWithNAs(const std::vector<double>& v, double NA) {
3197 1036 : std::string res = "";
3198 144700 : for (std::vector<double>::const_iterator i = v.begin(); i != v.end(); ++i) {
3199 287328 : res += (i == v.begin() ? "" : " ") + (*i == NA ? "NA" : ::toString(*i));
3200 : }
3201 1036 : return res;
3202 : }
3203 :
3204 : std::string
3205 152 : MSDevice_SSM::makeStringWithNAs(const std::vector<double>& v, const std::vector<double>& NAs) {
3206 152 : std::string res = "";
3207 10948 : for (std::vector<double>::const_iterator i = v.begin(); i != v.end(); ++i) {
3208 21592 : res += (i == v.begin() ? "" : " ") + (find(NAs.begin(), NAs.end(), *i) != NAs.end() ? "NA" : ::toString(*i));
3209 : }
3210 152 : return res;
3211 : }
3212 :
3213 : std::string
3214 140 : MSDevice_SSM::makeStringWithNAs(const PositionVector& v) {
3215 140 : const int precision = myUseGeoCoords ? gPrecisionGeo : gPrecision;
3216 140 : std::string res = "";
3217 9756 : for (PositionVector::const_iterator i = v.begin(); i != v.end(); ++i) {
3218 19232 : res += (i == v.begin() ? "" : " ") + (*i == Position::INVALID ? "NA" : ::toString(*i, precision));
3219 : }
3220 140 : return res;
3221 : }
3222 :
3223 : std::string
3224 13012 : MSDevice_SSM::makeStringWithNAs(const Position& p) {
3225 13012 : const int precision = myUseGeoCoords ? gPrecisionGeo : gPrecision;
3226 13012 : return p == Position::INVALID ? "NA" : toString(p, precision);
3227 : }
3228 :
3229 :
3230 : std::string
3231 0 : MSDevice_SSM::writeNA(double v, double NA) {
3232 0 : return v == NA ? "NA" : toString(v);
3233 : }
3234 :
3235 : // ---------------------------------------------------------------------------
3236 : // MSDevice_SSM-methods
3237 : // ---------------------------------------------------------------------------
3238 5180 : MSDevice_SSM::MSDevice_SSM(SUMOVehicle& holder, const std::string& id, std::string outputFilename, std::map<std::string, double> thresholds,
3239 : bool trajectories, double range, double extraTime, bool useGeoCoords, bool writePositions, bool writeLanesPositions,
3240 5180 : std::vector<int> conflictTypeFilter) :
3241 : MSVehicleDevice(holder, id),
3242 : myThresholds(thresholds),
3243 5180 : mySaveTrajectories(trajectories),
3244 5180 : myRange(range),
3245 5180 : myMDRACPRT(getMDRAC_PRT(holder)),
3246 5180 : myExtraTime(extraTime),
3247 5180 : myUseGeoCoords(useGeoCoords),
3248 5180 : myWritePositions(writePositions),
3249 5180 : myWriteLanesPositions(writeLanesPositions),
3250 5180 : myWriteNA(holder.getBoolParam("device.ssm.write-na", true, true)),
3251 5180 : myOldestActiveEncounterBegin(INVALID_DOUBLE),
3252 : myMaxBR(std::make_pair(-1, Position(0., 0.)), 0.0),
3253 : myMinSGAP(std::make_pair(std::make_pair(-1, Position(0., 0.)), std::numeric_limits<double>::max()), ""),
3254 10360 : myMinTGAP(std::make_pair(std::make_pair(-1, Position(0., 0.)), std::numeric_limits<double>::max()), "") {
3255 : // Take care! Holder is currently being constructed. Cast occurs before completion.
3256 5180 : myHolderMS = static_cast<MSVehicle*>(&holder);
3257 :
3258 5180 : myComputeTTC = myThresholds.find("TTC") != myThresholds.end();
3259 5180 : myComputeDRAC = myThresholds.find("DRAC") != myThresholds.end();
3260 5180 : myComputeMDRAC = myThresholds.find("MDRAC") != myThresholds.end();
3261 5180 : myComputePET = myThresholds.find("PET") != myThresholds.end();
3262 5180 : myComputePPET = myThresholds.find("PPET") != myThresholds.end();
3263 :
3264 5180 : myComputeBR = myThresholds.find("BR") != myThresholds.end();
3265 5180 : myComputeSGAP = myThresholds.find("SGAP") != myThresholds.end();
3266 5180 : myComputeTGAP = myThresholds.find("TGAP") != myThresholds.end();
3267 :
3268 5180 : myDroppedConflictTypes = conflictTypeFilter;
3269 5180 : myFilterConflictTypes = myDroppedConflictTypes.size() > 0;
3270 :
3271 5180 : myActiveEncounters = EncounterVector();
3272 5180 : myPastConflicts = EncounterQueue();
3273 :
3274 : // XXX: Who deletes the OutputDevice?
3275 5180 : myOutputFile = &OutputDevice::getDevice(outputFilename);
3276 : // TODO: make xsd, include header
3277 : // myOutputFile.writeXMLHeader("SSMLog", "SSMLog.xsd");
3278 : if (myCreatedOutputFiles.count(outputFilename) == 0) {
3279 984 : myOutputFile->writeXMLHeader("SSMLog", "");
3280 : myCreatedOutputFiles.insert(outputFilename);
3281 : }
3282 : // register at static instance container
3283 5180 : myInstances->insert(this);
3284 :
3285 : #ifdef DEBUG_SSM
3286 : if (DEBUG_COND(myHolderMS)) {
3287 : std::vector<std::string> measures;
3288 : std::vector<double> threshVals;
3289 : for (std::map<std::string, double>::const_iterator i = myThresholds.begin(); i != myThresholds.end(); ++i) {
3290 : measures.push_back(i->first);
3291 : threshVals.push_back(i->second);
3292 : }
3293 : std::cout << "Initialized ssm device '" << id << "' with "
3294 : << "myMeasures=" << joinToString(measures, " ")
3295 : << ", myThresholds=" << joinToString(threshVals, " ")
3296 : << ", mySaveTrajectories=" << mySaveTrajectories
3297 : << ", myRange=" << myRange << ", output file=" << outputFilename << ", extra time=" << myExtraTime << ", useGeo=" << myUseGeoCoords << "\n";
3298 : }
3299 : #endif
3300 5180 : }
3301 :
3302 : /// @brief Destructor.
3303 10360 : MSDevice_SSM::~MSDevice_SSM() {
3304 : // Deleted in ~BaseVehicle()
3305 : // unregister from static instance container
3306 5180 : myInstances->erase(this);
3307 5180 : resetEncounters();
3308 5180 : flushConflicts(true);
3309 5180 : flushGlobalMeasures();
3310 15540 : }
3311 :
3312 :
3313 : bool
3314 36715 : MSDevice_SSM::notifyEnter(SUMOTrafficObject& veh, MSMoveReminder::Notification reason, const MSLane* /* enteredLane */) {
3315 : assert(veh.isVehicle());
3316 : #ifdef DEBUG_SSM_NOTIFICATIONS
3317 : MSBaseVehicle* v = (MSBaseVehicle*) &veh;
3318 : if (DEBUG_COND(v)) {
3319 : std::cout << SIMTIME << "device '" << getID() << "' notifyEnter: reason=" << reason << " currentEdge=" << v->getLane()->getEdge().getID() << "\n";
3320 : }
3321 : #else
3322 : UNUSED_PARAMETER(veh);
3323 : UNUSED_PARAMETER(reason);
3324 : #endif
3325 36715 : return true; // keep the device
3326 : }
3327 :
3328 : bool
3329 36554 : MSDevice_SSM::notifyLeave(SUMOTrafficObject& veh, double /*lastPos*/,
3330 : MSMoveReminder::Notification reason, const MSLane* /* enteredLane */) {
3331 : assert(veh.isVehicle());
3332 : #ifdef DEBUG_SSM_NOTIFICATIONS
3333 : MSBaseVehicle* v = (MSBaseVehicle*) &veh;
3334 : if (DEBUG_COND(v)) {
3335 : std::cout << SIMTIME << "device '" << getID() << "' notifyLeave: reason=" << reason << " currentEdge=" << v->getLane()->getEdge().getID() << "\n";
3336 : }
3337 : #else
3338 : UNUSED_PARAMETER(veh);
3339 : UNUSED_PARAMETER(reason);
3340 : #endif
3341 36554 : return true; // keep the device
3342 : }
3343 :
3344 : bool
3345 1835048 : MSDevice_SSM::notifyMove(SUMOTrafficObject& veh, double /* oldPos */,
3346 : double /* newPos */, double newSpeed) {
3347 : #ifdef DEBUG_SSM_NOTIFICATIONS
3348 : MSBaseVehicle* v = (MSBaseVehicle*) &veh;
3349 : if (DEBUG_COND(v)) {
3350 : std::cout << SIMTIME << "device '" << getID() << "' notifyMove: newSpeed=" << newSpeed << "\n";
3351 : }
3352 : #else
3353 : UNUSED_PARAMETER(veh);
3354 : UNUSED_PARAMETER(newSpeed);
3355 : #endif
3356 1835048 : return true; // keep the device
3357 : }
3358 :
3359 :
3360 : void
3361 1831957 : MSDevice_SSM::findSurroundingVehicles(const MSVehicle& veh, double range, FoeInfoMap& foeCollector) {
3362 1831957 : if (!veh.isOnRoad()) {
3363 0 : return;
3364 : }
3365 : #ifdef DEBUG_SSM_SURROUNDING
3366 :
3367 : gDebugFlag3 = DEBUG_COND_FIND(veh);
3368 : if (gDebugFlag3) {
3369 : std::cout << SIMTIME << " Looking for surrounding vehicles for ego vehicle '" << veh.getID()
3370 : << "' on edge '" << veh.getLane()->getEdge().getID()
3371 : << "'."
3372 : << "\nVehicle's best lanes = " << ::toString(veh.getBestLanesContinuation())
3373 : << std::endl;
3374 : }
3375 : #endif
3376 :
3377 :
3378 : // The requesting vehicle's current route
3379 : // XXX: Restriction to route scanning may have to be generalized to scanning of possible continuations when
3380 : // considering situations involving sudden route changes. See also the definition of the EncounterTypes.
3381 : // A second problem is that following situations on deviating routes may result in closing encounters
3382 : // too early if a leading foe is not traced on its new lane. (see test 'foe_leader_deviating_routes')
3383 :
3384 : // If veh is on an internal edge, the edgeIter points towards the last edge before the junction
3385 : //ConstMSEdgeVector::const_iterator edgeIter = veh.getCurrentRouteEdge();
3386 : //assert(*edgeIter != 0);
3387 :
3388 : // Best continuation lanes for the ego vehicle
3389 1831957 : std::vector<MSLane*> egoBestLanes = veh.getBestLanesContinuation();
3390 :
3391 : // current lane in loop below
3392 1831957 : const MSLane* lane = veh.getLane();
3393 : const MSEdge* egoEdge = &(lane->getEdge());
3394 1831957 : const bool isOpposite = veh.getLaneChangeModel().isOpposite();
3395 : std::vector<MSLane*>::const_iterator laneIter = egoBestLanes.begin();
3396 : assert(lane->isInternal() || lane == *laneIter || isOpposite);
3397 : assert(lane != 0);
3398 1831957 : if (lane->isInternal() && egoBestLanes[0] != nullptr) { // outdated BestLanes, see #11336
3399 : return;
3400 : }
3401 :
3402 1831957 : if (isOpposite) {
3403 59550 : for (int i = 0; i < (int)egoBestLanes.size(); i++) {
3404 41008 : if (egoBestLanes[i] != nullptr && egoBestLanes[i]->getEdge().getOppositeEdge() != nullptr) {
3405 30108 : egoBestLanes[i] = egoBestLanes[i]->getEdge().getOppositeEdge()->getLanes().back();
3406 : }
3407 : }
3408 : }
3409 :
3410 : // next non-internal lane on the route
3411 : const MSLane* nextNonInternalLane = nullptr;
3412 :
3413 : const MSEdge* edge; // current edge in loop below
3414 :
3415 : // Init pos with vehicle's current position. Below pos is set to zero to denote
3416 : // the beginning position of the currently considered edge
3417 1831957 : double pos = veh.getPositionOnLane();
3418 : // remainingDownstreamRange is the range minus the distance that is already scanned downstream along the vehicles route
3419 : double remainingDownstreamRange = range;
3420 : // distToConflictLane is the distance of the ego vehicle to the start of the currently considered potential conflict lane (can be negative for its current lane)
3421 1831957 : double distToConflictLane = isOpposite ? pos - veh.getLane()->getLength() : -pos;
3422 :
3423 : // remember already visited lanes (no matter whether internal or not) and junctions downstream along the route
3424 : std::set<const MSLane*> seenLanes;
3425 : std::set<const MSJunction*> routeJunctions;
3426 :
3427 : // Starting points for upstream scans to be executed after downstream scan is complete.
3428 : // Holds pairs (starting edge, starting position on edge)
3429 : std::vector<UpstreamScanStartInfo> upstreamScanStartPositions;
3430 :
3431 :
3432 : // if the current edge is internal, collect all vehicles from the junction and within upstream range (except on the vehicles own edge),
3433 : // this is analogous to the code treating junctions in the loop below. Note that the distance on the junction itself is not included into
3434 : // range, so vehicles farther away than range can be collected, too.
3435 1831957 : if (lane->isInternal()) {
3436 : edge = &(lane->getEdge());
3437 :
3438 : #ifdef DEBUG_SSM_SURROUNDING
3439 : if (gDebugFlag3) {
3440 : std::cout << SIMTIME << " Vehicle '" << veh.getID() << "' is on internal edge " << edge->getID() << "'." << std::endl;
3441 : // << "Previous edge of its route: '" << (*edgeIter)->getID() << "'" << std::endl;
3442 : }
3443 : #endif
3444 :
3445 : assert(edge->getToJunction() == edge->getFromJunction());
3446 :
3447 48697 : const MSJunction* junction = edge->getToJunction();
3448 : // Collect vehicles on the junction
3449 48697 : getVehiclesOnJunction(junction, lane, distToConflictLane, lane, foeCollector, seenLanes);
3450 : routeJunctions.insert(junction);
3451 :
3452 : // Collect vehicles on incoming edges.
3453 : // Note that this includes the previous edge on the ego vehicle's route.
3454 : // (The distance on the current internal edge is ignored)
3455 : const ConstMSEdgeVector& incoming = junction->getIncoming();
3456 447403 : for (ConstMSEdgeVector::const_iterator ei = incoming.begin(); ei != incoming.end(); ++ei) {
3457 398706 : if ((*ei)->isInternal()) {
3458 263210 : continue;
3459 : }
3460 : // Upstream range is taken from the vehicle's back
3461 135496 : upstreamScanStartPositions.push_back(UpstreamScanStartInfo(*ei, (*ei)->getLength(), range + veh.getLength(), distToConflictLane, lane));
3462 : }
3463 :
3464 : // // Take into account internal distance covered on the current lane
3465 : // (commented out, because upstream scanning disregards internal lanes on the last scanned junction
3466 : // -- this makes the scanning symmetric between leader and follower)
3467 : // remainingDownstreamRange -= lane->getLength() - pos;
3468 :
3469 : // Take into account non-internal lengths until next non-internal lane
3470 48697 : MSLink* link = lane->getLinkCont()[0];
3471 48697 : remainingDownstreamRange -= link->getInternalLengthsAfter();
3472 48697 : distToConflictLane += lane->getLength() + link->getInternalLengthsAfter();
3473 :
3474 : // The next non-internal lane
3475 : pos = 0.;
3476 48697 : lane = *(++laneIter);
3477 : edge = &lane->getEdge();
3478 : } else {
3479 : // Collect all vehicles in range behind ego vehicle
3480 : edge = &(lane->getEdge());
3481 1783260 : double edgeLength = edge->getLength();
3482 1783260 : double startScanPos = std::min(pos + remainingDownstreamRange, edgeLength);
3483 3566520 : upstreamScanStartPositions.push_back(UpstreamScanStartInfo(edge, startScanPos, std::max(0., startScanPos - pos + range + veh.getLength()), distToConflictLane, lane));
3484 : }
3485 :
3486 : assert(lane != 0);
3487 : assert(!lane->isInternal());
3488 :
3489 : // Advance downstream the ego vehicle's route for distance 'range'.
3490 : // Collect all vehicles on the traversed Edges and on incoming edges at junctions
3491 : // and starting points for upstream vehicle collection strated below after downstream scan.
3492 2117533 : while (remainingDownstreamRange > 0.) {
3493 :
3494 : #ifdef DEBUG_SSM_SURROUNDING
3495 : if (gDebugFlag3) {
3496 : std::cout << SIMTIME << " Scanning downstream for vehicle '" << veh.getID() << "' on lane '" << veh.getLane()->getID() << "', position=" << pos << ".\n"
3497 : << "Considering edge '" << edge->getID() << "' Remaining downstream range = " << remainingDownstreamRange
3498 : << "\nbestLanes=" << ::toString(egoBestLanes) << "\n"
3499 : << std::endl;
3500 : }
3501 : #endif
3502 : assert(!edge->isInternal());
3503 : assert(!lane->isInternal());
3504 : assert(pos == 0 || lane == veh.getLane());
3505 2069816 : if (pos + remainingDownstreamRange < lane->getLength()) {
3506 : // scan range ends on this lane
3507 1728737 : if (edge->getID() != egoEdge->getID()) {
3508 277926 : upstreamScanStartPositions.push_back(UpstreamScanStartInfo(edge, pos + remainingDownstreamRange, remainingDownstreamRange, distToConflictLane, lane));
3509 : }
3510 : // scanned required downstream range
3511 : break;
3512 : } else {
3513 : // Also need to scan area that reaches beyond the lane
3514 : // Collecting vehicles on non-internal edge ahead
3515 341079 : if (edge->getID() != egoEdge->getID()) {
3516 8630 : upstreamScanStartPositions.push_back(UpstreamScanStartInfo(edge, edge->getLength(), edge->getLength() - pos, distToConflictLane, lane));
3517 : }
3518 : // account for scanned distance on lane
3519 341079 : remainingDownstreamRange -= lane->getLength() - pos;
3520 341079 : distToConflictLane += lane->getLength();
3521 : pos = 0.;
3522 :
3523 : // proceed to next non-internal lane
3524 : ++laneIter;
3525 : assert(laneIter == egoBestLanes.end() || *laneIter != 0);
3526 :
3527 : // If the vehicle's best lanes go on, collect vehicles on the upcoming junction
3528 341079 : if (laneIter != egoBestLanes.end()) {
3529 : // Upcoming junction
3530 : const MSJunction* junction;
3531 286796 : if (isOpposite) {
3532 1461 : junction = lane->getParallelOpposite()->getEdge().getToJunction();
3533 : } else {
3534 285335 : junction = lane->getEdge().getToJunction();
3535 : }
3536 :
3537 :
3538 : // Find connection for ego on the junction
3539 286796 : nextNonInternalLane = *laneIter;
3540 286796 : const MSLink* link = lane->getLinkTo(nextNonInternalLane);
3541 286796 : if (isOpposite && link == nullptr) {
3542 1461 : link = nextNonInternalLane->getLinkTo(lane);
3543 1461 : if (link == nullptr) {
3544 535 : link = lane->getParallelOpposite()->getLinkTo(nextNonInternalLane);
3545 : }
3546 : }
3547 285870 : if (link == nullptr) {
3548 : // disconnected route
3549 : break;
3550 : }
3551 :
3552 : // First lane of the connection
3553 285576 : lane = link->getViaLane();
3554 285576 : if (lane == nullptr) {
3555 : // link without internal lane
3556 80 : lane = nextNonInternalLane;
3557 : edge = &(lane->getEdge());
3558 80 : if (seenLanes.count(lane) == 0) {
3559 : seenLanes.insert(lane);
3560 80 : continue;
3561 : } else {
3562 : break;
3563 : }
3564 : }
3565 :
3566 : if (seenLanes.count(lane) == 0) {
3567 : // Collect vehicles on the junction, if it wasn't considered already
3568 285496 : getVehiclesOnJunction(junction, lane, distToConflictLane, lane, foeCollector, seenLanes);
3569 : routeJunctions.insert(junction);
3570 :
3571 : // Collect vehicles on incoming edges (except the last edge, where we already collected). Use full range.
3572 285496 : if (isOpposite) {
3573 : // look for vehicles that are also driving on the opposite side behind ego
3574 : const ConstMSEdgeVector& outgoing = junction->getOutgoing();
3575 9445 : for (ConstMSEdgeVector::const_iterator ei = outgoing.begin(); ei != outgoing.end(); ++ei) {
3576 7984 : if (*ei == edge || (*ei)->isInternal()) {
3577 6523 : continue;
3578 : }
3579 1461 : upstreamScanStartPositions.push_back(UpstreamScanStartInfo(*ei, (*ei)->getLength(), range, distToConflictLane, lane));
3580 : }
3581 : } else {
3582 : const ConstMSEdgeVector& incoming = junction->getIncoming();
3583 2046772 : for (ConstMSEdgeVector::const_iterator ei = incoming.begin(); ei != incoming.end(); ++ei) {
3584 1762737 : if (*ei == edge || (*ei)->isInternal()) {
3585 1393046 : continue;
3586 : }
3587 369691 : upstreamScanStartPositions.push_back(UpstreamScanStartInfo(*ei, (*ei)->getLength(), range, distToConflictLane, lane));
3588 : }
3589 : }
3590 :
3591 : // account for scanned distance on junction
3592 285496 : double linkLength = link->getInternalLengthsAfter();
3593 285496 : remainingDownstreamRange -= linkLength;
3594 285496 : distToConflictLane += linkLength;
3595 : #ifdef DEBUG_SSM_SURROUNDING
3596 : if (gDebugFlag3) {
3597 : std::cout << " Downstream Scan for vehicle '" << veh.getID() << "' proceeded over junction '" << junction->getID()
3598 : << "',\n linkLength=" << linkLength << ", remainingDownstreamRange=" << remainingDownstreamRange
3599 : << std::endl;
3600 : }
3601 : #endif
3602 :
3603 : // update ego's lane to next non internal edge
3604 285496 : lane = nextNonInternalLane;
3605 : edge = &(lane->getEdge());
3606 : } else {
3607 : #ifdef DEBUG_SSM_SURROUNDING
3608 : if (gDebugFlag3) {
3609 : std::cout << " Downstream Scan for vehicle '" << veh.getID() << "' stops at lane '" << lane->getID()
3610 : << "', which has already been scanned."
3611 : << std::endl;
3612 : }
3613 : #endif
3614 : break;
3615 : }
3616 : } else {
3617 : // Further vehicle path unknown, break search
3618 : break;
3619 : }
3620 : }
3621 : }
3622 : // add junction from the end of the route
3623 1831957 : routeJunctions.insert(lane->getEdge().getToJunction());
3624 :
3625 :
3626 : // Scan upstream branches from collected starting points
3627 4408421 : for (UpstreamScanStartInfo& i : upstreamScanStartPositions) {
3628 2576464 : getUpstreamVehicles(i, foeCollector, seenLanes, routeJunctions);
3629 : }
3630 :
3631 : #ifdef DEBUG_SSM_SURROUNDING
3632 : if (gDebugFlag3) {
3633 : for (std::pair<const MSVehicle*, FoeInfo*> foeInfo : foeCollector) {
3634 : std::cout << " foe " << foeInfo.first->getID() << " conflict at " << foeInfo.second->egoConflictLane->getID() << " egoDist " << foeInfo.second->egoDistToConflictLane << std::endl;
3635 : }
3636 : }
3637 : #endif
3638 :
3639 : // remove ego vehicle
3640 : const auto& it = foeCollector.find(&veh);
3641 1831957 : if (it != foeCollector.end()) {
3642 1831957 : delete it->second;
3643 : foeCollector.erase(it);
3644 : }
3645 1831957 : gDebugFlag3 = false;
3646 3663914 : }
3647 :
3648 :
3649 : void
3650 3157066 : MSDevice_SSM::getUpstreamVehicles(const UpstreamScanStartInfo& scanStart, FoeInfoMap& foeCollector, std::set<const MSLane*>& seenLanes, const std::set<const MSJunction*>& routeJunctions) {
3651 : #ifdef DEBUG_SSM_SURROUNDING
3652 : if (gDebugFlag3) {
3653 : std::cout << SIMTIME << " getUpstreamVehicles() for edge '" << scanStart.edge->getID() << "'"
3654 : << " egoConflictLane=" << scanStart.egoConflictLane->getID()
3655 : << " pos = " << scanStart.pos << " range = " << scanStart.range
3656 : << std::endl;
3657 : }
3658 : #endif
3659 3157066 : if (scanStart.range <= 0) {
3660 : return;
3661 : }
3662 :
3663 : // Collect vehicles on the given edge with position in [pos-range,pos]
3664 7659595 : for (MSLane* lane : scanStart.edge->getLanes()) {
3665 4511663 : if (seenLanes.find(lane) != seenLanes.end()) {
3666 9134 : return;
3667 : }
3668 : #ifdef DEBUG_SSM_SURROUNDING
3669 : int foundCount = 0;
3670 : #endif
3671 39226510 : for (MSVehicle* const veh : lane->getVehiclesSecure()) {
3672 34723981 : if (foeCollector.find(veh) != foeCollector.end()) {
3673 : // vehicle already recognized, earlier recognized conflict has priority
3674 0 : continue;
3675 : }
3676 34723981 : if (veh->getPositionOnLane() - veh->getLength() <= scanStart.pos && veh->getPositionOnLane() >= scanStart.pos - scanStart.range) {
3677 : #ifdef DEBUG_SSM_SURROUNDING
3678 : if (gDebugFlag3) {
3679 : std::cout << "\t" << veh->getID() << "\n";
3680 : }
3681 : foundCount++;
3682 : #endif
3683 8082308 : FoeInfo* c = new FoeInfo(); // c is deleted in updateEncounter()
3684 8082308 : c->egoDistToConflictLane = scanStart.egoDistToConflictLane;
3685 8082308 : c->egoConflictLane = scanStart.egoConflictLane;
3686 8082308 : foeCollector[veh] = c;
3687 : }
3688 : }
3689 4502529 : lane->releaseVehicles();
3690 :
3691 : #ifdef DEBUG_SSM_SURROUNDING
3692 : if (gDebugFlag3 && foundCount > 0) {
3693 : std::cout << "\t" << lane->getID() << ": Found " << foundCount << "\n";
3694 : }
3695 : #endif
3696 : seenLanes.insert(lane);
3697 : }
3698 :
3699 : #ifdef DEBUG_SSM_SURROUNDING
3700 : if (gDebugFlag3) {
3701 : std::cout << std::endl;
3702 : }
3703 : #endif
3704 :
3705 : // TODO: Gather vehicles from opposite direction. This should happen in any case, where opposite direction overtaking is possible.
3706 : // If it isn't it might still be nicer to trace oncoming vehicles for the resulting trajectories in the encounters
3707 : // if (edge->hasOpposite...)
3708 :
3709 3147932 : if (scanStart.range <= scanStart.pos) {
3710 : return;
3711 : }
3712 :
3713 : // Here we have: range > pos, i.e. we proceed collecting vehicles on preceding edges
3714 417525 : double remainingRange = scanStart.range - scanStart.pos;
3715 :
3716 : // Junction representing the origin of 'edge'
3717 417525 : const MSJunction* junction = scanStart.edge->getFromJunction();
3718 :
3719 : // stop if upstream search reaches the ego route
3720 417525 : if (routeJunctions.find(junction) != routeJunctions.end()) {
3721 : return;
3722 : }
3723 :
3724 : // Collect vehicles from incoming edges of the junction
3725 : int incomingEdgeCount = 0;
3726 369280 : if (!scanStart.edge->isInternal()) {
3727 : // collect vehicles on preceding junction (for internal edges this is already done in caller,
3728 : // i.e. findSurroundingVehicles() or the recursive call from getUpstreamVehicles())
3729 :
3730 : // Collect vehicles on the junction, if it wasn't considered already
3731 : // run vehicle collection for all incoming connections
3732 1830635 : for (MSLane* const internalLane : junction->getInternalLanes()) {
3733 1461355 : if (internalLane->getEdge().getSuccessors()[0]->getID() == scanStart.edge->getID()) {
3734 613511 : getVehiclesOnJunction(junction, internalLane, scanStart.egoDistToConflictLane, scanStart.egoConflictLane, foeCollector, seenLanes);
3735 613511 : incomingEdgeCount++;
3736 : }
3737 369280 : }
3738 : }
3739 : // Collect vehicles from incoming edges from the junction representing the origin of 'edge'
3740 369280 : if (incomingEdgeCount > 0) {
3741 1949310 : for (const MSEdge* inEdge : junction->getIncoming()) {
3742 1640237 : if (inEdge->isInternal() || inEdge->isCrossing()) {
3743 1059635 : continue;
3744 : }
3745 : bool skip = false;
3746 1575837 : for (MSLane* const lane : inEdge->getLanes()) {
3747 944578 : if (seenLanes.find(lane) != seenLanes.end()) {
3748 : skip = true;
3749 : break;
3750 : }
3751 : }
3752 659639 : if (skip) {
3753 : #ifdef DEBUG_SSM_SURROUNDING
3754 : //if (gDebugFlag3) std::cout << "Scan skips already seen edge " << (*ei)->getID() << "\n";
3755 : #endif
3756 28380 : continue;
3757 : }
3758 :
3759 : // XXX the length may be wrong if there are parallel internal edges for different vClasses
3760 631259 : double distOnJunction = scanStart.edge->isInternal() ? 0. : inEdge->getInternalFollowingLengthTo(scanStart.edge, SVC_IGNORING);
3761 631259 : if (distOnJunction >= remainingRange) {
3762 : #ifdef DEBUG_SSM_SURROUNDING
3763 : //if (gDebugFlag3) std::cout << "Scan stops on junction (between " << inEdge->getID() << " and " << scanStart.edge->getID() << ") at rel. dist " << distOnJunction << "\n";
3764 : #endif
3765 50657 : continue;
3766 : }
3767 : // account for vehicles on the predecessor edge
3768 580602 : UpstreamScanStartInfo nextInfo(inEdge, inEdge->getLength(), remainingRange - distOnJunction, scanStart.egoDistToConflictLane, scanStart.egoConflictLane);
3769 580602 : getUpstreamVehicles(nextInfo, foeCollector, seenLanes, routeJunctions);
3770 : }
3771 : }
3772 : }
3773 :
3774 :
3775 : void
3776 947704 : MSDevice_SSM::getVehiclesOnJunction(const MSJunction* junction, const MSLane* const egoJunctionLane, double egoDistToConflictLane, const MSLane* const egoConflictLane, FoeInfoMap& foeCollector, std::set<const MSLane*>& seenLanes) {
3777 : #ifdef DEBUG_SSM_SURROUNDING
3778 : if (gDebugFlag3) {
3779 : std::cout << SIMTIME << " getVehiclesOnJunction() for junction '" << junction->getID()
3780 : << "' egoJunctionLane=" << Named::getIDSecure(egoJunctionLane)
3781 : << "\nFound vehicles:"
3782 : << std::endl;
3783 : }
3784 : #endif
3785 : // FoeInfo creation
3786 1534777 : auto collectFoeInfos = [&](const MSLane::VehCont & vehicles) {
3787 1773896 : for (MSVehicle* const veh : vehicles) {
3788 478238 : if (foeCollector.find(veh) != foeCollector.end()) {
3789 688 : delete foeCollector[veh];
3790 : }
3791 239119 : FoeInfo* c = new FoeInfo();
3792 239119 : c->egoConflictLane = egoConflictLane;
3793 239119 : c->egoDistToConflictLane = egoDistToConflictLane;
3794 239119 : foeCollector[veh] = c;
3795 : #ifdef DEBUG_SSM_SURROUNDING
3796 : if (gDebugFlag3) {
3797 : std::cout << "\t" << veh->getID() << " egoConflictLane=" << Named::getIDSecure(egoConflictLane) << "\n";
3798 : }
3799 : #endif
3800 : }
3801 947704 : };
3802 :
3803 : // stop condition
3804 947704 : if (seenLanes.find(egoJunctionLane) != seenLanes.end() || egoJunctionLane->isCrossing()) {
3805 153965 : return;
3806 : }
3807 :
3808 1475546 : auto scanInternalLane = [&](const MSLane * lane) {
3809 1475546 : const MSLane::VehCont& vehicles = lane->getVehiclesSecure();
3810 :
3811 : // Add FoeInfos (XXX: for some situations, a vehicle may be collected twice. Then the later finding overwrites the earlier in foeCollector.
3812 : // This could lead to neglecting a conflict when determining foeConflictLane later.) -> TODO: test with twice intersecting routes
3813 1475546 : collectFoeInfos(vehicles);
3814 :
3815 1475546 : lane->releaseVehicles();
3816 :
3817 : // check additional internal link upstream in the same junction
3818 : // TODO: getEntryLink returns nullptr
3819 1475546 : if (lane->getCanonicalPredecessorLane()->isInternal()) {
3820 59231 : lane = lane->getCanonicalPredecessorLane();
3821 :
3822 : // This code must be modified, if more than two-piece internal lanes are allowed. Thus, assert:
3823 : assert(!lane->getEntryLink()->fromInternalLane());
3824 :
3825 : // collect vehicles
3826 59231 : const MSLane::VehCont& vehicles2 = lane->getVehiclesSecure();
3827 : // Add FoeInfos for the first internal lane
3828 59231 : collectFoeInfos(vehicles2);
3829 59231 : lane->releaseVehicles();
3830 : }
3831 :
3832 :
3833 : // If there is an internal continuation lane, also collect vehicles on that lane
3834 1475546 : if (lane->getLinkCont().size() > 1 && lane->getLinkCont()[0]->getViaLane() != nullptr) {
3835 : // There's a second internal lane of the connection
3836 : lane = lane->getLinkCont()[0]->getViaLane();
3837 : // This code must be modified, if more than two-piece internal lanes are allowed. Thus, assert:
3838 : assert(lane->getLinkCont().size() == 0 || lane->getLinkCont()[0]->getViaLane() == 0);
3839 :
3840 : // collect vehicles
3841 0 : const MSLane::VehCont& vehicles2 = lane->getVehiclesSecure();
3842 : // Add FoeInfos for the first internal lane
3843 0 : collectFoeInfos(vehicles2);
3844 0 : lane->releaseVehicles();
3845 : }
3846 :
3847 2269285 : };
3848 :
3849 : // Collect vehicles on conflicting lanes
3850 793739 : const MSLink* entryLink = egoJunctionLane->getEntryLink();
3851 793739 : if (entryLink->getFoeLanes().size() > 0) {
3852 :
3853 310984 : const std::vector<MSLane*> foeLanes = junction->getFoeInternalLanes(entryLink);
3854 1065590 : for (MSLane* lane : foeLanes) {
3855 754606 : if (seenLanes.find(lane) != seenLanes.end()) {
3856 72799 : continue;
3857 : }
3858 681807 : scanInternalLane(lane);
3859 : seenLanes.insert(lane);
3860 : }
3861 310984 : }
3862 793739 : scanInternalLane(egoJunctionLane);
3863 :
3864 : #ifdef DEBUG_SSM_SURROUNDING
3865 : if (gDebugFlag3) {
3866 : std::cout << std::endl;
3867 : }
3868 : #endif
3869 : }
3870 :
3871 :
3872 :
3873 : void
3874 4407 : MSDevice_SSM::generateOutput(OutputDevice* /*tripinfoOut*/) const {
3875 : // This is called once at vehicle removal.
3876 : // Also: flush myOutputFile? Or is this done automatically?
3877 : // myOutputFile->closeTag();
3878 4407 : }
3879 :
3880 : // ---------------------------------------------------------------------------
3881 : // Static parameter load helpers
3882 : // ---------------------------------------------------------------------------
3883 : std::string
3884 5188 : MSDevice_SSM::getOutputFilename(const SUMOVehicle& v, std::string deviceID) {
3885 5188 : OptionsCont& oc = OptionsCont::getOptions();
3886 5188 : std::string file = deviceID + ".xml";
3887 5188 : std::string basePath = "";
3888 10376 : if (v.getParameter().hasParameter("device.ssm.file")) {
3889 : try {
3890 1020 : file = v.getParameter().getParameter("device.ssm.file", file);
3891 0 : } catch (...) {
3892 0 : WRITE_WARNINGF(TL("Invalid value '%' for vehicle parameter 'ssm.measures'."), v.getParameter().getParameter("device.ssm.file", file));
3893 0 : }
3894 9696 : } else if (v.getVehicleType().getParameter().hasParameter("device.ssm.file")) {
3895 : try {
3896 6408 : file = v.getVehicleType().getParameter().getParameter("device.ssm.file", file);
3897 0 : } catch (...) {
3898 0 : WRITE_WARNINGF(TL("Invalid value '%' for vType parameter 'ssm.measures'."), v.getVehicleType().getParameter().getParameter("device.ssm.file", file));
3899 0 : }
3900 : } else {
3901 8136 : file = oc.getString("device.ssm.file") == "" ? file : oc.getString("device.ssm.file");
3902 5424 : if (oc.isDefault("device.ssm.file") && (myIssuedParameterWarnFlags & SSM_WARN_FILE) == 0) {
3903 324 : WRITE_MESSAGEF(TL("Vehicle '%' does not supply vehicle parameter 'device.ssm.file'. Using default of '%'."), v.getID(), file);
3904 108 : myIssuedParameterWarnFlags |= SSM_WARN_FILE;
3905 : }
3906 2768 : if (OptionsCont::getOptions().isSet("configuration-file") && !oc.isDefault("device.ssm.file")) {
3907 80 : basePath = OptionsCont::getOptions().getString("configuration-file");
3908 : }
3909 : }
3910 5188 : if (basePath != "") {
3911 40 : file = FileHelpers::checkForRelativity(file, basePath);
3912 : try {
3913 80 : file = StringUtils::urlDecode(file);
3914 0 : } catch (NumberFormatException& e) {
3915 0 : WRITE_WARNING(toString(e.what()) + " when trying to decode filename '" + file + "'.");
3916 0 : }
3917 : }
3918 5188 : return file;
3919 : }
3920 :
3921 : bool
3922 5188 : MSDevice_SSM::useGeoCoords(const SUMOVehicle& v) {
3923 5188 : OptionsCont& oc = OptionsCont::getOptions();
3924 5188 : bool useGeo = false;
3925 10376 : if (v.getParameter().hasParameter("device.ssm.geo")) {
3926 : try {
3927 16 : useGeo = StringUtils::toBool(v.getParameter().getParameter("device.ssm.geo", "no"));
3928 0 : } catch (...) {
3929 0 : WRITE_WARNINGF(TL("Invalid value '%' for vehicle parameter 'ssm.geo'."), v.getParameter().getParameter("device.ssm.geo", "no"));
3930 0 : }
3931 10360 : } else if (v.getVehicleType().getParameter().hasParameter("device.ssm.geo")) {
3932 : try {
3933 4208 : useGeo = StringUtils::toBool(v.getVehicleType().getParameter().getParameter("device.ssm.geo", "no"));
3934 0 : } catch (...) {
3935 0 : WRITE_WARNINGF(TL("Invalid value '%' for vType parameter 'ssm.geo'."), v.getVehicleType().getParameter().getParameter("device.ssm.geo", "no"));
3936 0 : }
3937 : } else {
3938 3076 : useGeo = oc.getBool("device.ssm.geo");
3939 6152 : if (oc.isDefault("device.ssm.geo") && (myIssuedParameterWarnFlags & SSM_WARN_GEO) == 0) {
3940 1413 : WRITE_MESSAGEF(TL("Vehicle '%' does not supply vehicle parameter 'device.ssm.geo'. Using default of '%'."), v.getID(), toString(useGeo));
3941 471 : myIssuedParameterWarnFlags |= SSM_WARN_GEO;
3942 : }
3943 : }
3944 5188 : return useGeo;
3945 : }
3946 :
3947 :
3948 : bool
3949 5188 : MSDevice_SSM::writePositions(const SUMOVehicle& v) {
3950 5188 : OptionsCont& oc = OptionsCont::getOptions();
3951 5188 : bool writePos = false;
3952 10376 : if (v.getParameter().hasParameter("device.ssm.write-positions")) {
3953 : try {
3954 8 : writePos = StringUtils::toBool(v.getParameter().getParameter("device.ssm.write-positions", "no"));
3955 0 : } catch (...) {
3956 0 : WRITE_WARNINGF(TL("Invalid value '%' for vehicle parameter 'ssm.write-positions'."), v.getParameter().getParameter("device.ssm.write-positions", "no"));
3957 0 : }
3958 10368 : } else if (v.getVehicleType().getParameter().hasParameter("device.ssm.write-positions")) {
3959 : try {
3960 0 : writePos = StringUtils::toBool(v.getVehicleType().getParameter().getParameter("device.ssm.write-positions", "no"));
3961 0 : } catch (...) {
3962 0 : WRITE_WARNINGF(TL("Invalid value '%' for vType parameter 'ssm.write-positions'."), v.getVehicleType().getParameter().getParameter("device.ssm.write-positions", "no"));
3963 0 : }
3964 : } else {
3965 5184 : writePos = oc.getBool("device.ssm.write-positions");
3966 10368 : if (oc.isDefault("device.ssm.write-positions") && (myIssuedParameterWarnFlags & SSM_WARN_POS) == 0) {
3967 1437 : WRITE_MESSAGEF(TL("Vehicle '%' does not supply vehicle parameter 'device.ssm.write-positions'. Using default of '%'."), v.getID(), toString(writePos));
3968 479 : myIssuedParameterWarnFlags |= SSM_WARN_POS;
3969 : }
3970 : }
3971 5188 : return writePos;
3972 : }
3973 :
3974 :
3975 : bool
3976 5188 : MSDevice_SSM::writeLanesPositions(const SUMOVehicle& v) {
3977 5188 : OptionsCont& oc = OptionsCont::getOptions();
3978 5188 : bool writeLanesPos = false;
3979 10376 : if (v.getParameter().hasParameter("device.ssm.write-lane-positions")) {
3980 : try {
3981 16 : writeLanesPos = StringUtils::toBool(v.getParameter().getParameter("device.ssm.write-lane-positions", "no"));
3982 0 : } catch (...) {
3983 0 : WRITE_WARNINGF(TL("Invalid value '%' for vehicle parameter 'ssm.write-lane-positions'."), v.getParameter().getParameter("device.ssm.write-lane-positions", "no"));
3984 0 : }
3985 10360 : } else if (v.getVehicleType().getParameter().hasParameter("device.ssm.write-lane-positions")) {
3986 : try {
3987 0 : writeLanesPos = StringUtils::toBool(v.getVehicleType().getParameter().getParameter("device.ssm.write-lane-positions", "no"));
3988 0 : } catch (...) {
3989 0 : WRITE_WARNINGF(TL("Invalid value '%' for vType parameter 'ssm.write-lane-positions'."), v.getVehicleType().getParameter().getParameter("device.ssm.write-lane-positions", "no"));
3990 0 : }
3991 : } else {
3992 5180 : writeLanesPos = oc.getBool("device.ssm.write-lane-positions");
3993 10360 : if (oc.isDefault("device.ssm.write-lane-positions") && (myIssuedParameterWarnFlags & SSM_WARN_LANEPOS) == 0) {
3994 1425 : WRITE_MESSAGEF(TL("Vehicle '%' does not supply vehicle parameter 'device.ssm.write-lane-positions'. Using default of '%'."), v.getID(), toString(writeLanesPos));
3995 475 : myIssuedParameterWarnFlags |= SSM_WARN_LANEPOS;
3996 : }
3997 : }
3998 5188 : return writeLanesPos;
3999 : }
4000 :
4001 :
4002 : bool
4003 5188 : MSDevice_SSM::filterByConflictType(const SUMOVehicle& v, std::string deviceID, std::vector<int>& conflictTypes) {
4004 5188 : OptionsCont& oc = OptionsCont::getOptions();
4005 5188 : std::string typeString = "";
4006 10376 : if (v.getParameter().hasParameter("device.ssm.exclude-conflict-types")) {
4007 : try {
4008 0 : typeString = v.getParameter().getParameter("device.ssm.exclude-conflict-types", "");
4009 0 : } catch (...) {
4010 0 : WRITE_WARNINGF(TL("Invalid value '%' for vehicle parameter 'ssm.conflict-order'."), v.getParameter().getParameter("device.ssm.exclude-conflict-types", ""));
4011 0 : }
4012 10376 : } else if (v.getVehicleType().getParameter().hasParameter("device.ssm.exclude-conflict-types")) {
4013 : try {
4014 64 : typeString = v.getVehicleType().getParameter().getParameter("device.ssm.exclude-conflict-types", "");
4015 0 : } catch (...) {
4016 0 : WRITE_WARNINGF(TL("Invalid value '%' for vType parameter 'ssm.conflict-order'."), v.getVehicleType().getParameter().getParameter("device.ssm.exclude-conflict-types", ""));
4017 0 : }
4018 : } else {
4019 5156 : typeString = oc.getString("device.ssm.exclude-conflict-types");
4020 10312 : if (oc.isDefault("device.ssm.exclude-conflict-types") && (myIssuedParameterWarnFlags & SSM_WARN_CONFLICTFILTER) == 0) {
4021 1401 : WRITE_MESSAGEF(TL("Vehicle '%' does not supply vehicle parameter 'device.ssm.exclude-conflict-types'. Using default of '%'."), v.getID(), typeString);
4022 467 : myIssuedParameterWarnFlags |= SSM_WARN_CONFLICTFILTER;
4023 : }
4024 : }
4025 : // Check retrieved conflict keys
4026 10376 : StringTokenizer st = StringTokenizer("");
4027 15564 : st = (typeString.find(",") != std::string::npos) ? StringTokenizer(typeString, ",") : StringTokenizer(typeString);
4028 5188 : std::vector<std::string> found = st.getVector();
4029 : std::set<int> confirmed;
4030 5204 : for (std::vector<std::string>::const_iterator i = found.begin(); i != found.end(); ++i) {
4031 32 : if (*i == "foe") {
4032 8 : confirmed.insert(FOE_ENCOUNTERTYPES.begin(), FOE_ENCOUNTERTYPES.end());
4033 24 : } else if (*i == "ego") {
4034 8 : confirmed.insert(EGO_ENCOUNTERTYPES.begin(), EGO_ENCOUNTERTYPES.end());
4035 16 : } else if (*i == "none") {
4036 : return true;
4037 8 : } else if (StringUtils::isInt(*i) && encounterToString(static_cast<EncounterType>(StringUtils::toInt(*i))) != "UNKNOWN") {
4038 0 : confirmed.insert(std::stoi(*i));
4039 : } else {
4040 : // Given identifier is unknown
4041 24 : WRITE_ERRORF(TL("SSM exclude-conflict-type '%' is not supported. Aborting construction of SSM device '%'."), *i, deviceID);
4042 8 : return false;
4043 : }
4044 : }
4045 5172 : conflictTypes.insert(conflictTypes.end(), confirmed.begin(), confirmed.end());
4046 5172 : return true;
4047 5188 : }
4048 :
4049 :
4050 : double
4051 5188 : MSDevice_SSM::getDetectionRange(const SUMOVehicle& v) {
4052 5188 : OptionsCont& oc = OptionsCont::getOptions();
4053 5188 : double range = -INVALID_DOUBLE;
4054 10376 : if (v.getParameter().hasParameter("device.ssm.range")) {
4055 : try {
4056 696 : range = StringUtils::toDouble(v.getParameter().getParameter("device.ssm.range", ""));
4057 0 : } catch (...) {
4058 0 : WRITE_WARNINGF(TL("Invalid value '%' for vehicle parameter 'ssm.range'."), v.getParameter().getParameter("device.ssm.range", ""));
4059 0 : }
4060 9680 : } else if (v.getVehicleType().getParameter().hasParameter("device.ssm.range")) {
4061 : try {
4062 4208 : range = StringUtils::toDouble(v.getVehicleType().getParameter().getParameter("device.ssm.range", ""));
4063 0 : } catch (...) {
4064 0 : WRITE_WARNINGF(TL("Invalid value '%' for vType parameter 'ssm.range'."), v.getVehicleType().getParameter().getParameter("device.ssm.range", ""));
4065 0 : }
4066 : } else {
4067 2736 : range = oc.getFloat("device.ssm.range");
4068 5472 : if (oc.isDefault("device.ssm.range") && (myIssuedParameterWarnFlags & SSM_WARN_RANGE) == 0) {
4069 465 : WRITE_MESSAGEF(TL("Vehicle '%' does not supply vehicle parameter 'device.ssm.range'. Using default of '%'."), v.getID(), toString(range));
4070 155 : myIssuedParameterWarnFlags |= SSM_WARN_RANGE;
4071 : }
4072 : }
4073 5188 : return range;
4074 : }
4075 :
4076 :
4077 : double
4078 5180 : MSDevice_SSM::getMDRAC_PRT(const SUMOVehicle& v) {
4079 5180 : OptionsCont& oc = OptionsCont::getOptions();
4080 5180 : double prt = 1;
4081 10360 : if (v.getParameter().hasParameter("device.ssm.mdrac.prt")) {
4082 : try {
4083 8 : prt = StringUtils::toDouble(v.getParameter().getParameter("device.ssm.mdrac.prt", ""));
4084 0 : } catch (...) {
4085 0 : WRITE_WARNINGF(TL("Invalid value '%' for vehicle parameter 'ssm.mdrac.prt'."), v.getParameter().getParameter("device.ssm.mdrac.prt", ""));
4086 0 : }
4087 10352 : } else if (v.getVehicleType().getParameter().hasParameter("device.ssm.mdrac.prt")) {
4088 : try {
4089 0 : prt = StringUtils::toDouble(v.getVehicleType().getParameter().getParameter("device.ssm.mdrac.prt", ""));
4090 0 : } catch (...) {
4091 0 : WRITE_WARNINGF(TL("Invalid value '%' for vType parameter 'ssm.mdrac.prt'."), v.getVehicleType().getParameter().getParameter("device.ssm.mdrac.prt", ""));
4092 0 : }
4093 : } else {
4094 5176 : prt = oc.getFloat("device.ssm.mdrac.prt");
4095 10352 : if (oc.isDefault("device.ssm.mdrac.prt") && (myIssuedParameterWarnFlags & SSM_WARN_MDRAC_PRT) == 0) {
4096 984 : WRITE_MESSAGEF(TL("Vehicle '%' does not supply vehicle parameter 'device.ssm.mdrac.prt'. Using default of '%'."), v.getID(), toString(prt));
4097 328 : myIssuedParameterWarnFlags |= SSM_WARN_MDRAC_PRT;
4098 : }
4099 : }
4100 5180 : return prt;
4101 : }
4102 :
4103 :
4104 :
4105 :
4106 : double
4107 5188 : MSDevice_SSM::getExtraTime(const SUMOVehicle& v) {
4108 5188 : OptionsCont& oc = OptionsCont::getOptions();
4109 5188 : double extraTime = INVALID_DOUBLE;
4110 10376 : if (v.getParameter().hasParameter("device.ssm.extratime")) {
4111 : try {
4112 16 : extraTime = StringUtils::toDouble(v.getParameter().getParameter("device.ssm.extratime", ""));
4113 0 : } catch (...) {
4114 0 : WRITE_WARNINGF(TL("Invalid value '%' for vehicle parameter 'ssm.extratime'."), v.getParameter().getParameter("device.ssm.extratime", ""));
4115 0 : }
4116 10360 : } else if (v.getVehicleType().getParameter().hasParameter("device.ssm.extratime")) {
4117 : try {
4118 4208 : extraTime = StringUtils::toDouble(v.getVehicleType().getParameter().getParameter("device.ssm.extratime", ""));
4119 0 : } catch (...) {
4120 0 : WRITE_WARNINGF(TL("Invalid value '%' for vType parameter 'ssm.extratime'."), v.getVehicleType().getParameter().getParameter("device.ssm.extratime", ""));
4121 0 : }
4122 : } else {
4123 3076 : extraTime = oc.getFloat("device.ssm.extratime");
4124 6152 : if (oc.isDefault("device.ssm.extratime") && (myIssuedParameterWarnFlags & SSM_WARN_EXTRATIME) == 0) {
4125 1413 : WRITE_MESSAGEF(TL("Vehicle '%' does not supply vehicle parameter 'device.ssm.extratime'. Using default of '%'."), v.getID(), toString(extraTime));
4126 471 : myIssuedParameterWarnFlags |= SSM_WARN_EXTRATIME;
4127 : }
4128 : }
4129 5188 : if (extraTime < 0.) {
4130 0 : extraTime = DEFAULT_EXTRA_TIME;
4131 0 : WRITE_WARNINGF(TL("Negative (or no) value encountered for vehicle parameter 'device.ssm.extratime' in vehicle '%' using default value % instead."), v.getID(), ::toString(extraTime));
4132 : }
4133 5188 : return extraTime;
4134 : }
4135 :
4136 :
4137 : bool
4138 5188 : MSDevice_SSM::requestsTrajectories(const SUMOVehicle& v) {
4139 5188 : OptionsCont& oc = OptionsCont::getOptions();
4140 5188 : bool trajectories = false;
4141 10376 : if (v.getParameter().hasParameter("device.ssm.trajectories")) {
4142 : try {
4143 704 : trajectories = StringUtils::toBool(v.getParameter().getParameter("device.ssm.trajectories", "no"));
4144 0 : } catch (...) {
4145 0 : WRITE_WARNINGF(TL("Invalid value '%' for vehicle parameter 'ssm.trajectories'."), v.getParameter().getParameter("device.ssm.trajectories", "no"));
4146 0 : }
4147 9672 : } else if (v.getVehicleType().getParameter().hasParameter("device.ssm.trajectories")) {
4148 : try {
4149 4208 : trajectories = StringUtils::toBool(v.getVehicleType().getParameter().getParameter("device.ssm.trajectories", "no"));
4150 0 : } catch (...) {
4151 0 : WRITE_WARNINGF(TL("Invalid value '%' for vType parameter 'ssm.trajectories'."), v.getVehicleType().getParameter().getParameter("device.ssm.trajectories", "no"));
4152 0 : }
4153 : } else {
4154 2732 : trajectories = oc.getBool("device.ssm.trajectories");
4155 5464 : if (oc.isDefault("device.ssm.trajectories") && (myIssuedParameterWarnFlags & SSM_WARN_TRAJECTORIES) == 0) {
4156 489 : WRITE_MESSAGEF(TL("Vehicle '%' does not supply vehicle parameter 'device.ssm.trajectories'. Using default of '%'."), v.getID(), toString(trajectories));
4157 163 : myIssuedParameterWarnFlags |= SSM_WARN_TRAJECTORIES;
4158 : }
4159 : }
4160 5188 : return trajectories;
4161 : }
4162 :
4163 :
4164 : bool
4165 5196 : MSDevice_SSM::getMeasuresAndThresholds(const SUMOVehicle& v, std::string deviceID, std::map<std::string, double>& thresholds) {
4166 5196 : OptionsCont& oc = OptionsCont::getOptions();
4167 :
4168 : // Measures
4169 5196 : std::string measures_str = "";
4170 10392 : if (v.getParameter().hasParameter("device.ssm.measures")) {
4171 : try {
4172 816 : measures_str = v.getParameter().getParameter("device.ssm.measures", "");
4173 0 : } catch (...) {
4174 0 : WRITE_WARNINGF(TL("Invalid value '%' for vehicle parameter 'ssm.measures'."), v.getParameter().getParameter("device.ssm.measures", ""));
4175 0 : }
4176 9576 : } else if (v.getVehicleType().getParameter().hasParameter("device.ssm.measures")) {
4177 : try {
4178 4272 : measures_str = v.getVehicleType().getParameter().getParameter("device.ssm.measures", "");
4179 0 : } catch (...) {
4180 0 : WRITE_WARNINGF(TL("Invalid value '%' for vType parameter 'ssm.measures'."), v.getVehicleType().getParameter().getParameter("device.ssm.measures", ""));
4181 0 : }
4182 : } else {
4183 2652 : measures_str = oc.getString("device.ssm.measures");
4184 5304 : if (oc.isDefault("device.ssm.measures") && (myIssuedParameterWarnFlags & SSM_WARN_MEASURES) == 0) {
4185 156 : WRITE_MESSAGEF(TL("Vehicle '%' does not supply vehicle parameter 'device.ssm.measures'. Using default of '%'."), v.getID(), measures_str);
4186 52 : myIssuedParameterWarnFlags |= SSM_WARN_MEASURES;
4187 : }
4188 : }
4189 :
4190 : // Check retrieved measures
4191 5196 : if (measures_str == "") {
4192 192 : WRITE_WARNINGF("No measures specified for ssm device of vehicle '%'. Registering all available SSMs.", v.getID());
4193 : measures_str = AVAILABLE_SSMS;
4194 : }
4195 5196 : StringTokenizer st = StringTokenizer(AVAILABLE_SSMS);
4196 5196 : std::vector<std::string> available = st.getVector();
4197 15588 : st = (measures_str.find(",") != std::string::npos) ? StringTokenizer(measures_str, ",") : StringTokenizer(measures_str);
4198 5196 : std::vector<std::string> measures = st.getVector();
4199 22508 : for (std::vector<std::string>::const_iterator i = measures.begin(); i != measures.end(); ++i) {
4200 17312 : if (std::find(available.begin(), available.end(), *i) == available.end()) {
4201 : // Given identifier is unknown
4202 0 : WRITE_ERRORF(TL("SSM identifier '%' is not supported. Aborting construction of SSM device '%'."), *i, deviceID);
4203 : return false;
4204 : }
4205 : }
4206 :
4207 : // Thresholds
4208 5196 : std::string thresholds_str = "";
4209 10392 : if (v.getParameter().hasParameter("device.ssm.thresholds")) {
4210 : try {
4211 744 : thresholds_str = v.getParameter().getParameter("device.ssm.thresholds", "");
4212 0 : } catch (...) {
4213 0 : WRITE_WARNINGF(TL("Invalid value '%' for vehicle parameter 'ssm.thresholds'."), v.getParameter().getParameter("device.ssm.thresholds", ""));
4214 0 : }
4215 9648 : } else if (v.getVehicleType().getParameter().hasParameter("device.ssm.thresholds")) {
4216 : try {
4217 4208 : thresholds_str = v.getVehicleType().getParameter().getParameter("device.ssm.thresholds", "");
4218 0 : } catch (...) {
4219 0 : WRITE_WARNINGF(TL("Invalid value '%' for vType parameter 'ssm.thresholds'."), v.getVehicleType().getParameter().getParameter("device.ssm.thresholds", ""));
4220 0 : }
4221 : } else {
4222 2720 : thresholds_str = oc.getString("device.ssm.thresholds");
4223 5440 : if (oc.isDefault("device.ssm.thresholds") && (myIssuedParameterWarnFlags & SSM_WARN_THRESHOLDS) == 0) {
4224 372 : WRITE_MESSAGEF(TL("Vehicle '%' does not supply vehicle parameter 'device.ssm.thresholds'. Using default of '%'."), v.getID(), thresholds_str);
4225 124 : myIssuedParameterWarnFlags |= SSM_WARN_THRESHOLDS;
4226 : }
4227 : }
4228 :
4229 : // Parse vector of doubles from threshold_str
4230 : int count = 0;
4231 5196 : if (thresholds_str != "") {
4232 8292 : st = (thresholds_str.find(",") != std::string::npos) ? StringTokenizer(thresholds_str, ",") : StringTokenizer(thresholds_str);
4233 11912 : while (count < (int)measures.size() && st.hasNext()) {
4234 9148 : double thresh = StringUtils::toDouble(st.next());
4235 9148 : thresholds.insert(std::make_pair(measures[count], thresh));
4236 9148 : ++count;
4237 : }
4238 2764 : if (thresholds.size() < measures.size() || st.hasNext()) {
4239 24 : WRITE_ERRORF(TL("Given list of thresholds ('%') is not of the same size as the list of measures ('%').\nPlease specify exactly one threshold for each measure."), thresholds_str, measures_str);
4240 8 : return false;
4241 : }
4242 : } else {
4243 : // assume default thresholds if none are given
4244 10592 : for (std::vector<std::string>::const_iterator i = measures.begin(); i != measures.end(); ++i) {
4245 8160 : if (*i == "TTC") {
4246 4760 : thresholds.insert(std::make_pair(*i, DEFAULT_THRESHOLD_TTC));
4247 5780 : } else if (*i == "DRAC") {
4248 2736 : thresholds.insert(std::make_pair(*i, DEFAULT_THRESHOLD_DRAC));
4249 4412 : } else if (*i == "MDRAC") {
4250 2712 : thresholds.insert(std::make_pair(*i, DEFAULT_THRESHOLD_MDRAC));
4251 3056 : } else if (*i == "PET") {
4252 2816 : thresholds.insert(std::make_pair(*i, DEFAULT_THRESHOLD_PET));
4253 1648 : } else if (*i == "PPET") {
4254 2696 : thresholds.insert(std::make_pair(*i, DEFAULT_THRESHOLD_PPET));
4255 300 : } else if (*i == "BR") {
4256 200 : thresholds.insert(std::make_pair(*i, DEFAULT_THRESHOLD_BR));
4257 200 : } else if (*i == "SGAP") {
4258 200 : thresholds.insert(std::make_pair(*i, DEFAULT_THRESHOLD_SGAP));
4259 100 : } else if (*i == "TGAP") {
4260 200 : thresholds.insert(std::make_pair(*i, DEFAULT_THRESHOLD_TGAP));
4261 : } else {
4262 0 : WRITE_ERROR("Unknown SSM identifier '" + (*i) + "'. Aborting construction of ssm device."); // should never occur
4263 : return false;
4264 : }
4265 : }
4266 : }
4267 : return true;
4268 5196 : }
4269 :
4270 :
4271 : std::string
4272 2400 : MSDevice_SSM::getParameter(const std::string& key) const {
4273 2400 : if (key == "minTTC" && !myComputeTTC) {
4274 0 : throw InvalidArgument("Measure TTC is not tracked by ssm device");
4275 : }
4276 2400 : if (key == "maxDRAC" && !myComputeDRAC) {
4277 0 : throw InvalidArgument("Measure DRAC is not tracked by ssm device");
4278 : }
4279 2400 : if (key == "maxMDRAC" && !myComputeMDRAC) {
4280 0 : throw InvalidArgument("Measure MDRAC is not tracked by ssm device");
4281 : }
4282 2400 : if (key == "minPET" && !myComputePET) {
4283 0 : throw InvalidArgument("Measure PET is not tracked by ssm device");
4284 : }
4285 2400 : if (key == "minPPET" && !myComputePPET) {
4286 0 : throw InvalidArgument("Measure PPET is not tracked by ssm device");
4287 : }
4288 1800 : if (key == "minTTC" ||
4289 1200 : key == "maxDRAC" ||
4290 1200 : key == "maxMDRAC" ||
4291 3000 : key == "minPET" ||
4292 : key == "minPPET") {
4293 2400 : double value = INVALID_DOUBLE;
4294 : double minTTC = INVALID_DOUBLE;
4295 : double minPET = INVALID_DOUBLE;
4296 : double maxDRAC = -INVALID_DOUBLE;
4297 : double maxMDRAC = -INVALID_DOUBLE;
4298 : double minPPET = INVALID_DOUBLE;
4299 4288 : for (Encounter* e : myActiveEncounters) {
4300 1888 : minTTC = MIN2(minTTC, e->minTTC.value);
4301 1888 : minPET = MIN2(minPET, e->PET.value);
4302 1888 : maxDRAC = MAX2(maxDRAC, e->maxDRAC.value);
4303 1888 : maxMDRAC = MAX2(maxMDRAC, e->maxMDRAC.value);
4304 1888 : minPPET = MIN2(minPPET, e->minPPET.value);
4305 : }
4306 2400 : if (key == "minTTC") {
4307 600 : value = minTTC;
4308 1800 : } else if (key == "maxDRAC") {
4309 600 : value = maxDRAC;
4310 1200 : } else if (key == "maxMDRAC") {
4311 0 : value = maxMDRAC;
4312 1200 : } else if (key == "minPET") {
4313 600 : value = minPET;
4314 600 : } else if (key == "minPPET") {
4315 600 : value = minPPET;
4316 : }
4317 2400 : if (fabs(value) == INVALID_DOUBLE) {
4318 1556 : return "";
4319 : } else {
4320 844 : return toString(value);
4321 : }
4322 : }
4323 0 : throw InvalidArgument("Parameter '" + key + "' is not supported for device of type '" + deviceName() + "'.");
4324 : }
4325 :
4326 :
4327 : void
4328 0 : MSDevice_SSM::setParameter(const std::string& key, const std::string& value) {
4329 : double doubleValue;
4330 : try {
4331 0 : doubleValue = StringUtils::toDouble(value);
4332 0 : } catch (NumberFormatException&) {
4333 0 : throw InvalidArgument("Setting parameter '" + key + "' requires a number for device of type '" + deviceName() + "'.");
4334 0 : }
4335 0 : if (false || key == "foo") {
4336 : UNUSED_PARAMETER(doubleValue);
4337 : } else {
4338 0 : throw InvalidArgument("Setting parameter '" + key + "' is not supported for device of type '" + deviceName() + "'.");
4339 : }
4340 0 : }
4341 :
4342 :
4343 : /****************************************************************************/
|
