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