Eclipse SUMO - Simulation of Urban MObility
MSDevice_ToC.cpp
Go to the documentation of this file.
1/****************************************************************************/
2// Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3// Copyright (C) 2013-2023 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/****************************************************************************/
21// The ToC Device controls the transition of control between automated and manual driving.
22//
23/****************************************************************************/
24#include <config.h>
25
26#include <algorithm>
27#include <memory>
33#include <microsim/MSNet.h>
34#include <microsim/MSLane.h>
35#include <microsim/MSVehicle.h>
41#include <microsim/MSStop.h>
42#include "MSDevice_ToC.h"
43
44
45// ===========================================================================
46// debug constants
47// ===========================================================================
48//#define DEBUG_TOC
49//#define DEBUG_DYNAMIC_TOC
50
51// ===========================================================================
52// parameter defaults
53// ===========================================================================
54
55// default value for the average response time, that a driver needs to take back control
56#define DEFAULT_RESPONSE_TIME -1.0
57// default value for the average rate at which the driver's awareness recovers to
58// 1.0 after a ToC has been performed
59#define DEFAULT_RECOVERY_RATE 0.1
60// Default value of the awareness below which no lane-changes are performed
61#define DEFAULT_LCABSTINENCE 0.0
62// The default value for the average awareness a driver has initially after a ToC
63#define DEFAULT_INITIAL_AWARENESS 0.5
64// The default value for the deceleration rate applied during a 'minimum risk maneuver'
65#define DEFAULT_MRM_DECEL 1.5
66// The default value for the dynamic ToC threshold indicates that the dynamic ToCs are deactivated
67#define DEFAULT_DYNAMIC_TOC_THRESHOLD 0.0
68// The default value for the probability of an MRM to occur after a dynamically triggered ToC
69// (Note that these MRMs will not induce full stops in most cases)
70#define DEFAULT_MRM_PROBABILITY 0.05
71
72// The factor by which the dynamic ToC threshold time is multiplied to yield the lead time given for the corresponding ToC
73#define DYNAMIC_TOC_LEADTIME_FACTOR 0.75
74// A factor applied to the check for the dynamic ToC condition to resist aborting an ongoing dynamic ToC (and prevent oscillations)
75#define DYNAMIC_TOC_ABORT_RESISTANCE_FACTOR 2.0
76
77
78// The default values for the openGap parameters applied for gap creation in preparation for a ToC
79#define DEFAULT_OPENGAP_TIMEGAP -1.0
80#define DEFAULT_OPENGAP_SPACING 0.0
81#define DEFAULT_OPENGAP_CHANGERATE 1.0
82#define DEFAULT_OPENGAP_MAXDECEL 1.0
83
84
85#define DEFAULT_MANUAL_TYPE ""
86#define DEFAULT_AUTOMATED_TYPE ""
87
88// Maximal tries to sample a positive value from the gaussian distribution
89// used for the driver response time when a TOR is issued. (the distribution is assumed truncated at zero)
90#define MAX_RESPONSETIME_SAMPLE_TRIES 100
91// Maximal variance of responsetimes (returned for pMRM outside lookup table, i.e. pMRM>0.5), see interpolateVariance()
92#define MAX_RESPONSETIME_VARIANCE 10000
93
94
95// ---------------------------------------------------------------------------
96// static members
97// ---------------------------------------------------------------------------
98std::set<MSDevice_ToC*, ComparatorNumericalIdLess> MSDevice_ToC::myInstances = std::set<MSDevice_ToC*, ComparatorNumericalIdLess>();
99std::set<std::string> MSDevice_ToC::createdOutputFiles;
100int MSDevice_ToC::LCModeMRM = 768; // = 0b001100000000 - no autonomous changes, no speed adaptation
102
103
104// ===========================================================================
105// method definitions
106// ===========================================================================
107// ---------------------------------------------------------------------------
108// static initialisation methods
109// ---------------------------------------------------------------------------
110void
112 oc.addOptionSubTopic("ToC Device");
113 insertDefaultAssignmentOptions("toc", "ToC Device", oc);
114
115 oc.doRegister("device.toc.manualType", new Option_String());
116 oc.addDescription("device.toc.manualType", "ToC Device", TL("Vehicle type for manual driving regime."));
117 oc.doRegister("device.toc.automatedType", new Option_String());
118 oc.addDescription("device.toc.automatedType", "ToC Device", TL("Vehicle type for automated driving regime."));
119 oc.doRegister("device.toc.responseTime", new Option_Float(DEFAULT_RESPONSE_TIME));
120 oc.addDescription("device.toc.responseTime", "ToC Device", TL("Average response time needed by a driver to take back control."));
121 oc.doRegister("device.toc.recoveryRate", new Option_Float(DEFAULT_RECOVERY_RATE));
122 oc.addDescription("device.toc.recoveryRate", "ToC Device", TL("Recovery rate for the driver's awareness after a ToC."));
123 oc.doRegister("device.toc.lcAbstinence", new Option_Float(DEFAULT_LCABSTINENCE));
124 oc.addDescription("device.toc.lcAbstinence", "ToC Device", TL("Attention level below which a driver restrains from performing lane changes (value in [0,1])."));
125 oc.doRegister("device.toc.initialAwareness", new Option_Float(DEFAULT_INITIAL_AWARENESS));
126 oc.addDescription("device.toc.initialAwareness", "ToC Device", TL("Average awareness a driver has initially after a ToC (value in [0,1])."));
127 oc.doRegister("device.toc.mrmDecel", new Option_Float(DEFAULT_MRM_DECEL));
128 oc.addDescription("device.toc.mrmDecel", "ToC Device", TL("Deceleration rate applied during a 'minimum risk maneuver'."));
129 oc.doRegister("device.toc.dynamicToCThreshold", new Option_Float(DEFAULT_DYNAMIC_TOC_THRESHOLD));
130 oc.addDescription("device.toc.dynamicToCThreshold", "ToC Device", TL("Time, which the vehicle requires to have ahead to continue in automated mode. The default value of 0 indicates no dynamic triggering of ToCs."));
131 oc.doRegister("device.toc.dynamicMRMProbability", new Option_Float(DEFAULT_MRM_PROBABILITY));
132 oc.addDescription("device.toc.dynamicMRMProbability", "ToC Device", TL("Probability that a dynamically triggered TOR is not answered in time."));
133 oc.doRegister("device.toc.mrmKeepRight", new Option_Bool(false));
134 oc.addDescription("device.toc.mrmKeepRight", "ToC Device", TL("If true, the vehicle tries to change to the right during an MRM."));
135 oc.doRegister("device.toc.mrmSafeSpot", new Option_String());
136 oc.addDescription("device.toc.mrmSafeSpot", "ToC Device", TL("If set, the vehicle tries to reach the given named stopping place during an MRM."));
137 oc.doRegister("device.toc.mrmSafeSpotDuration", new Option_Float(60.));
138 oc.addDescription("device.toc.mrmSafeSpotDuration", "ToC Device", TL("Duration the vehicle stays at the safe spot after an MRM."));
139 oc.doRegister("device.toc.maxPreparationAccel", new Option_Float(0.0));
140 oc.addDescription("device.toc.maxPreparationAccel", "ToC Device", TL("Maximal acceleration that may be applied during the ToC preparation phase."));
141 oc.doRegister("device.toc.ogNewTimeHeadway", new Option_Float(-1.0));
142 oc.addDescription("device.toc.ogNewTimeHeadway", "ToC Device", TL("Timegap for ToC preparation phase."));
143 oc.doRegister("device.toc.ogNewSpaceHeadway", new Option_Float(-1.0));
144 oc.addDescription("device.toc.ogNewSpaceHeadway", "ToC Device", TL("Additional spacing for ToC preparation phase."));
145 oc.doRegister("device.toc.ogMaxDecel", new Option_Float(-1.0));
146 oc.addDescription("device.toc.ogMaxDecel", "ToC Device", TL("Maximal deceleration applied for establishing increased gap in ToC preparation phase."));
147 oc.doRegister("device.toc.ogChangeRate", new Option_Float(-1.0));
148 oc.addDescription("device.toc.ogChangeRate", "ToC Device", TL("Rate of adaptation towards the increased headway during ToC preparation."));
149 oc.doRegister("device.toc.useColorScheme", new Option_Bool(true));
150 oc.addDescription("device.toc.useColorScheme", "ToC Device", TL("Whether a coloring scheme shall by applied to indicate the different ToC stages."));
151 oc.doRegister("device.toc.file", new Option_String());
152 oc.addDescription("device.toc.file", "ToC Device", TL("Switches on output by specifying an output filename."));
153}
154
155
156void
157MSDevice_ToC::buildVehicleDevices(SUMOVehicle& v, std::vector<MSVehicleDevice*>& into) {
159 if (equippedByDefaultAssignmentOptions(oc, "toc", v, false)) {
161 WRITE_WARNING(TL("ToC device is not supported by the mesoscopic simulation."));
162 return;
163 }
164 const std::string manualType = getStringParam(v, oc, "toc.manualType", DEFAULT_MANUAL_TYPE, true);
165 const std::string automatedType = getStringParam(v, oc, "toc.automatedType", DEFAULT_AUTOMATED_TYPE, true);
166 const SUMOTime responseTime = TIME2STEPS(getFloatParam(v, oc, "toc.responseTime", DEFAULT_RESPONSE_TIME, false));
167 const double recoveryRate = getFloatParam(v, oc, "toc.recoveryRate", DEFAULT_RECOVERY_RATE, false);
168 const double lcAbstinence = getFloatParam(v, oc, "toc.lcAbstinence", DEFAULT_LCABSTINENCE, false);
169 const double initialAwareness = getFloatParam(v, oc, "toc.initialAwareness", DEFAULT_INITIAL_AWARENESS, false);
170 const double mrmDecel = getFloatParam(v, oc, "toc.mrmDecel", DEFAULT_MRM_DECEL, false);
171 const bool useColoring = getBoolParam(v, oc, "toc.useColorScheme", true, false);
172 const std::string deviceID = "toc_" + v.getID();
173 const std::string file = getOutputFilename(v, oc);
174 const OpenGapParams ogp = getOpenGapParams(v, oc);
175 const double dynamicToCThreshold = getFloatParam(v, oc, "toc.dynamicToCThreshold", DEFAULT_DYNAMIC_TOC_THRESHOLD, false);
176 const double dynamicMRMProbability = getDynamicMRMProbability(v, oc);
177 const bool mrmKeepRight = getBoolParam(v, oc, "toc.mrmKeepRight", false, false);
178 const std::string mrmSafeSpot = getStringParam(v, oc, "toc.mrmSafeSpot", "", false);
179 const SUMOTime mrmSafeSpotDuration = TIME2STEPS(getFloatParam(v, oc, "toc.mrmSafeSpotDuration", 60., false));
180 const double maxPreparationAccel = getFloatParam(v, oc, "toc.maxPreparationAccel", 0.0, false);
181 // build the device
182 MSDevice_ToC* device = new MSDevice_ToC(v, deviceID, file,
183 manualType, automatedType, responseTime, recoveryRate,
184 lcAbstinence, initialAwareness, mrmDecel, dynamicToCThreshold,
185 dynamicMRMProbability, maxPreparationAccel, mrmKeepRight,
186 mrmSafeSpot, mrmSafeSpotDuration, useColoring, ogp);
187 into.push_back(device);
188 }
189}
190
191
192std::string
194 // Default of "" means no output
195 std::string file = "";
196 if (v.getParameter().knowsParameter("device.toc.file")) {
197 try {
198 file = v.getParameter().getParameter("device.toc.file", file);
199 } catch (...) {
200 WRITE_WARNINGF(TL("Invalid value '%'for vehicle parameter 'ssm.measures'"), v.getParameter().getParameter("device.toc.file", file));
201 }
202 } else if (v.getVehicleType().getParameter().knowsParameter("device.toc.file")) {
203 try {
204 file = v.getVehicleType().getParameter().getParameter("device.toc.file", file);
205 } catch (...) {
206 WRITE_WARNINGF(TL("Invalid value '%'for vType parameter 'ssm.measures'"), v.getVehicleType().getParameter().getParameter("device.toc.file", file));
207 }
208 } else {
209 file = oc.getString("device.toc.file") == "" ? file : oc.getString("device.toc.file");
210 }
211 return file;
212}
213
214
215double
217 double pMRM = getFloatParam(v, oc, "toc.dynamicMRMProbability", DEFAULT_MRM_PROBABILITY, false);
218 if (pMRM < 0 || pMRM > 0.5) {
219 const double pMRMTrunc = MAX2(0.0, MIN2(0.5, pMRM));
220 WRITE_WARNINGF(TL("Given value for ToC device parameter 'dynamicMRMProbability' (=%) is not in the admissible range [0,0.5]. Truncated to %."), toString(pMRM), toString(pMRMTrunc));
221 return pMRMTrunc;
222 }
223 return pMRM;
224}
225
226
229 double timegap = getFloatParam(v, oc, "toc.ogNewTimeHeadway", -1.0, false);
230 double spacing = getFloatParam(v, oc, "toc.ogNewSpaceHeadway", -1.0, false);
231 double changeRate = getFloatParam(v, oc, "toc.ogChangeRate", -1.0, false);
232 double maxDecel = getFloatParam(v, oc, "toc.ogMaxDecel", -1.0, false);
233 bool specifiedAny = false;
234
235 if (changeRate == -1.0) {
236 changeRate = DEFAULT_OPENGAP_CHANGERATE;
237 } else {
238 specifiedAny = true;
239 }
240 if (maxDecel == -1.0) {
241 maxDecel = DEFAULT_OPENGAP_MAXDECEL;
242 } else {
243 specifiedAny = true;
244 }
245 if (specifiedAny && timegap == -1 && spacing == -1) {
246 WRITE_ERROR(TL("If any openGap parameters for the ToC model are specified, then at least one of toc.ogNewTimeHeadway and toc.ogNewSpaceHeadway must be defined."))
247 }
248 if (timegap == -1) {
249 timegap = DEFAULT_OPENGAP_TIMEGAP;
250 } else {
251 specifiedAny = true;
252 }
253 if (spacing == -1) {
254 spacing = DEFAULT_OPENGAP_SPACING;
255 } else {
256 specifiedAny = true;
257 }
258#ifdef DEBUG_TOC
259 std::cout << "Parsed openGapParams: \n"
260 << " timegap=" << timegap
261 << ", spacing=" << spacing
262 << ", changeRate=" << changeRate
263 << ", maxDecel=" << maxDecel
264 << std::endl;
265#endif
266 return OpenGapParams(timegap, spacing, changeRate, maxDecel, specifiedAny);
267}
268
269// ---------------------------------------------------------------------------
270// MSDevice_ToC-methods
271// ---------------------------------------------------------------------------
272MSDevice_ToC::MSDevice_ToC(SUMOVehicle& holder, const std::string& id, const std::string& outputFilename,
273 const std::string& manualType, const std::string& automatedType, SUMOTime responseTime, double recoveryRate,
274 double lcAbstinence, double initialAwareness, double mrmDecel,
275 double dynamicToCThreshold, double dynamicMRMProbability, double maxPreparationAccel,
276 bool mrmKeepRight, const std::string& mrmSafeSpot, SUMOTime mrmSafeSpotDuration, bool useColorScheme, OpenGapParams ogp) :
277 MSVehicleDevice(holder, id),
278 myManualTypeID(manualType),
279 myAutomatedTypeID(automatedType),
280 myResponseTime(responseTime),
281 myRecoveryRate(recoveryRate),
282 myLCAbstinence(lcAbstinence),
283 myInitialAwareness(initialAwareness),
284 myMRMDecel(mrmDecel),
285 myCurrentAwareness(1.),
286 myUseColorScheme(useColorScheme),
287 myTriggerMRMCommand(nullptr),
288 myTriggerToCCommand(nullptr),
289 myRecoverAwarenessCommand(nullptr),
290 myExecuteMRMCommand(nullptr),
291 myPrepareToCCommand(nullptr),
292 myOutputFile(nullptr),
293 myEvents(),
294 myEventLanes(),
295 myEventXY(),
296 myPreviousLCMode(-1),
297 myOpenGapParams(ogp),
298 myDynamicToCThreshold(dynamicToCThreshold),
299 myMRMProbability(dynamicMRMProbability),
300 myDynamicToCActive(dynamicToCThreshold > 0),
301 myIssuedDynamicToC(false),
302 myDynamicToCLane(-1),
303 myMRMKeepRight(mrmKeepRight),
304 myMRMSafeSpot(mrmSafeSpot),
305 myMRMSafeSpotDuration(mrmSafeSpotDuration),
306 myMaxPreparationAccel(maxPreparationAccel),
307 myOriginalMaxAccel(-1) {
308 // Take care! Holder is currently being constructed. Cast occurs before completion.
309 myHolderMS = static_cast<MSVehicle*>(&holder);
310
311 if (outputFilename != "") {
312 myOutputFile = &OutputDevice::getDevice(outputFilename);
313 // TODO: make xsd, include header
314 // myOutputFile.writeXMLHeader("ToCDeviceLog", "ToCDeviceLog.xsd");
315 if (createdOutputFiles.count(outputFilename) == 0) {
316 myOutputFile->writeXMLHeader("ToCDeviceLog", "");
317 createdOutputFiles.insert(outputFilename);
318 }
319 }
320
321 // Check if the given vTypes for the ToC Device are vTypeDistributions
323 const bool automatedVTypeIsDist = vehCtrl.hasVTypeDistribution(myAutomatedTypeID);
324 const bool manualVTypeIsDist = vehCtrl.hasVTypeDistribution(myManualTypeID);
325
326 // Check if the vType of the holder matches one of the given vTypes
327 std::string holderVTypeID = holder.getVehicleType().getID();
328 if (holderVTypeID == myManualTypeID) {
329 myState = ToCState::MANUAL;
330 } else if (holderVTypeID == myAutomatedTypeID) {
331 myState = ToCState::AUTOMATED;
332 } else if (manualVTypeIsDist && vehCtrl.getVTypeDistributionMembership(holderVTypeID).count(myManualTypeID) > 0) {
333 // Holder type id is from the given manual type distribution.
334 myState = ToCState::MANUAL;
335 myManualTypeID = holderVTypeID;
336 } else if (automatedVTypeIsDist && vehCtrl.getVTypeDistributionMembership(holderVTypeID).count(myAutomatedTypeID) > 0) {
337 // Holder type id is from the given automated type distribution.
338 myState = ToCState::AUTOMATED;
339 myAutomatedTypeID = holderVTypeID;
340 } else {
341 throw ProcessError("Vehicle type of vehicle '" + holder.getID() + "' ('" +
342 holder.getVehicleType().getID() + "') must coincide with manualType ('" +
343 manualType + "') or automatedType ('" + automatedType +
344 "') specified for its ToC-device (or drawn from the specified vTypeDistributions).");
345 }
346 if (!vehCtrl.hasVType(myAutomatedTypeID)) {
347 throw ProcessError("The automated vehicle type '" + myAutomatedTypeID +
348 "' of vehicle '" + holder.getID() + "' is not known.");
349 }
350 if (!vehCtrl.hasVType(myManualTypeID)) {
351 throw ProcessError("The manual vehicle type '" + myManualTypeID +
352 "' of vehicle '" + holder.getID() + "' is not known.");
353 }
354
355 // Eventually instantiate given vTypes from distributions
356 if (myState == ToCState::MANUAL && automatedVTypeIsDist) {
358 } else if (myState == ToCState::AUTOMATED && manualVTypeIsDist) {
360 }
361
362 // register at static instance container
363 myInstances.insert(this);
365
366#ifdef DEBUG_TOC
367 std::cout << "initialized device '" << id << "' with "
368 << "outputFilename=" << outputFilename << ", "
369 << "myManualType=" << myManualTypeID << ", "
370 << "myAutomatedType=" << myAutomatedTypeID << ", "
371 << "myResponseTime=" << myResponseTime << ", "
372 << "myRecoveryRate=" << myRecoveryRate << ", "
373 << "myInitialAwareness=" << myInitialAwareness << ", "
374 << "myMRMDecel=" << myMRMDecel << ", "
375 << "ogTimeHeadway=" << myOpenGapParams.newTimeHeadway << ", "
376 << "ogSpaceHeadway=" << myOpenGapParams.newSpaceHeadway << ", "
377 << "ogChangeRate=" << myOpenGapParams.changeRate << ", "
378 << "ogMaxDecel=" << myOpenGapParams.maxDecel << ", "
379 << "ogActive=" << myOpenGapParams.active << ", "
380 << "myCurrentAwareness=" << myCurrentAwareness << ", "
381 << "myState=" << _2string(myState) << std::endl;
382#endif
383
384 assert(myInitialAwareness <= 1.0 && myInitialAwareness >= 0.0);
385}
386
387
388
389void
391 //RGBColor(red, green, blue)
394 myColorScheme[PREPARING_TOC] = RGBColor(200, 200, 250); // light blue
395 myColorScheme[MRM] = RGBColor(250, 50, 50); // red
396 myColorScheme[RECOVERING] = RGBColor(250, 210, 150); // light yellow
397 myColorScheme[UNDEFINED] = RGBColor(150, 150, 150); // gray
398}
399
400
402 // unregister from static instance container
403 myInstances.erase(this);
404 // deschedule commands associated to this device
405 if (myTriggerMRMCommand != nullptr) {
407 }
408 if (myTriggerToCCommand != nullptr) {
410 }
411 if (myRecoverAwarenessCommand != nullptr) {
413 }
414 if (myExecuteMRMCommand != nullptr) {
417 }
418 if (myPrepareToCCommand != nullptr) {
420 }
421}
422
423void
425 if (value > 1.0 || value < 0.0) {
426 std::stringstream ss;
427 ss << "Truncating invalid value for awareness (" << value << ") to lie in [0,1].";
428 WRITE_WARNING(ss.str());
429 value = MAX2(0.0, MIN2(1.0, value));
430 }
432 // Awareness is now below LC abstinence level -> prevent deliberate LCs
434 } else if (myCurrentAwareness < myLCAbstinence && value >= myLCAbstinence) {
435 // Awareness is now above LC abstinence level -> allow deliberate LCs
437 }
438 myCurrentAwareness = value;
439 myHolderMS->getDriverState()->setAwareness(value);
440}
441
442
443void
445#ifdef DEBUG_TOC
446 std::cout << SIMTIME << " MSDevice_ToC::setState()" << std::endl;
447#endif
448 if (myState == state) {
449 // No state change
450 return;
451 }
452
453 if (myState == MRM) {
454 // reset the vehicle's maxAccel
457 } else if (myState == PREPARING_TOC) {
459 // Deactivate gap control at preparation phase end
461 }
462 if (state != MRM) {
463 // Aborting preparation
466 }
467 } else if (state == PREPARING_TOC || state == MRM) {
468#ifdef DEBUG_TOC
469 std::cout << " Entering ToC preparation... " << std::endl;
470#endif
471 // Prevent lane changing during takeover preparation
473 // Store original value of maxAccel for restoring it after preparation phase
475 // Impose acceleration limit during preparation
477 }
478
479 if (myIssuedDynamicToC) {
480 // Reset dynamic ToC flag
481 myIssuedDynamicToC = false;
482 }
483
484 myState = state;
485 if (myUseColorScheme) {
487 }
488}
489
490void
495}
496
497void
499 // Remove any preparatory process
501 // .. and any recovery process
503 // ... and any pending ToC to manual
505 // Immediately trigger the MRM process
506 triggerMRM(0);
507}
508
509
510void
511MSDevice_ToC::requestToC(SUMOTime timeTillMRM, SUMOTime responseTime) {
512#ifdef DEBUG_TOC
513 std::cout << SIMTIME << " requestToC() for vehicle '" << myHolder.getID() << "', timeTillMRM=" << timeTillMRM << ", responseTime=" << responseTime << std::endl;
514#endif
515 if (myState == AUTOMATED) {
516 // Initialize preparation phase
517 if (responseTime == -1000) {
518 // Sample response time from distribution
519 const double sample = sampleResponseTime(STEPS2TIME(timeTillMRM));
520 // this needs to be a separate line because TIME2STEPS may otherwise do two calls to sampleResponseTime
521 responseTime = TIME2STEPS(sample);
522 }
523
524 // Schedule ToC Event
527
528 assert(myExecuteMRMCommand == nullptr);
529 assert(myTriggerMRMCommand == nullptr);
530 if (responseTime > timeTillMRM && myState != MRM) {
531 // Schedule new MRM if driver response time is higher than permitted
534 }
535
536 // Start ToC preparation process
541 // Start gap controller
542 double originalTau = myHolderMS->getCarFollowModel().getHeadwayTime();
546 }
547 // Record event
548 if (generatesOutput()) {
549 myEvents.push(std::make_pair(SIMSTEP, "TOR"));
550 myEventLanes.push(std::make_pair(myHolder.getLane()->getID(), myHolder.getPositionOnLane())); // add lane and lanepos
551 myEventXY.push(std::make_pair(myHolder.getPosition().x(), myHolder.getPosition().y())); // add (x, y) position
552 }
553 } else {
554 // Switch to automated mode is performed immediately
555 if (timeTillMRM > 0) {
556 std::stringstream ss;
557 ss << "[t=" << SIMTIME << "] Positive transition time (" << STEPS2TIME(timeTillMRM) << "s.) for upward ToC of vehicle '" << myHolder.getID() << "' is ignored.";
558 WRITE_WARNING(ss.str());
559 }
561 }
562}
563
564
567#ifdef DEBUG_TOC
568 std::cout << SIMTIME << " triggerMRM() for vehicle '" << myHolder.getID() << "'" << std::endl;
569#endif
570 // Clear ongoing MRM
572
573 // Start MRM process
574 if (myMRMSafeSpot != "") {
577 if (s == nullptr) {
578 WRITE_WARNINGF(TL("Ignoring unknown safe spot '%' for vehicle '%'."), myMRMSafeSpot, myHolder.getID());
579 } else {
582 stop.lane = s->getLane().getID();
583 stop.endPos = s->getEndLanePosition();
584 stop.startPos = s->getBeginLanePosition();
587 std::string error;
588 if (!myHolder.addStop(stop, error)) {
589 WRITE_WARNING("Could not set safe spot '" + myMRMSafeSpot + "' for vehicle '" + myHolder.getID() + "'. " + error);
590 }
591 }
592 } else {
595 }
596 if (myState == MANUAL || myState == RECOVERING) {
598 }
599 setState(MRM);
600 setAwareness(1.);
601
602 // Record event
603 if (generatesOutput()) {
604 myEvents.push(std::make_pair(SIMSTEP, "MRM"));
605 myEventLanes.push(std::make_pair(myHolder.getLane()->getID(), myHolder.getPositionOnLane())); // add lane and lanepos
606 myEventXY.push(std::make_pair(myHolder.getPosition().x(), myHolder.getPosition().y())); // add (x, y) position
607 }
608
609 return 0;
610}
611
612
615#ifdef DEBUG_TOC
616 std::cout << SIMTIME << " triggerUpwardToC() for vehicle '" << myHolder.getID() << "'" << std::endl;
617#endif
619 // Eventually stop ToC preparation process
621 // Eventually abort MRM
623 // Eventually abort awareness recovery process
625
626 if (myState == MANUAL || myState == RECOVERING) {
628 }
629 setAwareness(1.);
631
632 // Record event
633 if (generatesOutput()) {
634 myEvents.push(std::make_pair(SIMSTEP, "ToCup"));
635 myEventLanes.push(std::make_pair(myHolder.getLane()->getID(), myHolder.getPositionOnLane())); // add lane and lanepos
636 myEventXY.push(std::make_pair(myHolder.getPosition().x(), myHolder.getPosition().y())); // add (x, y) position
637 }
638
639 return 0;
640}
641
642
645#ifdef DEBUG_TOC
646 std::cout << SIMTIME << " triggerDownwardToC() for vehicle '" << myHolder.getID() << "'" << std::endl;
647#endif
649 // Eventually stop ToC preparation process
651 // Eventually abort MRM
653
654#ifdef DEBUG_TOC
655 std::cout << SIMTIME << " Initial awareness after ToC: " << myCurrentAwareness << std::endl;
656#endif
657
658 // Start awareness recovery process
662
663 // @todo: Sample initial awareness
664 double initialAwareness = myInitialAwareness;
665 setAwareness(initialAwareness);
666
668
669 // Record event
670 if (generatesOutput()) {
671 myEvents.push(std::make_pair(SIMSTEP, "ToCdown"));
672 myEventLanes.push(std::make_pair(myHolder.getLane()->getID(), myHolder.getPositionOnLane())); // add lane and lanepos
673 myEventXY.push(std::make_pair(myHolder.getPosition().x(), myHolder.getPosition().y())); // add (x, y) position
674 }
675 return 0;
676}
677
678void
680 // Eventually abort scheduled MRM
681 if (myTriggerMRMCommand != nullptr) {
683 myTriggerMRMCommand = nullptr;
684 }
685 // Eventually abort ongoing MRM
686 if (myExecuteMRMCommand != nullptr) {
689 myExecuteMRMCommand = nullptr;
690 }
691}
692
693
694void
696 if (myTriggerToCCommand != nullptr) {
698 myTriggerToCCommand = nullptr;
699 }
700}
701
702void
704 // Eventually stop ToC preparation process
705 if (myPrepareToCCommand != nullptr) {
707 if (myState != MRM) {
709 }
710 myPrepareToCCommand = nullptr;
711 }
712}
713
714void
716 // Eventually stop ToC preparation process
717 if (myRecoverAwarenessCommand != nullptr) {
720 }
721}
722
723
724void
725MSDevice_ToC::switchHolderType(const std::string& targetTypeID) {
726#ifdef DEBUG_TOC
727 std::cout << SIMTIME << " Switching type of vehicle '" << myHolder.getID() << "' to '" << targetTypeID << "'" << std::endl;
728#endif
729 MSVehicleType* targetType = MSNet::getInstance()->getVehicleControl().getVType(targetTypeID);
730 if (targetType == nullptr) {
731 WRITE_ERRORF(TL("vType '%' for vehicle '%' is not known."), targetType->getID(), myHolder.getID());
732 return;
733 }
734 myHolderMS->replaceVehicleType(targetType);
735}
736
737
740#ifdef DEBUG_TOC
741 std::cout << SIMTIME << " ToC preparation step for vehicle '" << myHolder.getID() << "'" << std::endl;
742#endif
743 if (myState == PREPARING_TOC) {
744 return DELTA_T;
745 } else {
746#ifdef DEBUG_TOC
747 std::cout << SIMTIME << " Aborting ToC preparation for vehicle '" << myHolder.getID() << "'" << std::endl;
748#endif
750 return 0;
751 }
752}
753
754
758 const double currentSpeed = myHolderMS->getSpeed();
759#ifdef DEBUG_TOC
760 std::cout << SIMTIME << " MRM step for vehicle '" << myHolder.getID() << "', currentSpeed=" << currentSpeed << std::endl;
761#endif
762
763 // Induce slowdown with MRMDecel
764 std::vector<std::pair<SUMOTime, double> > speedTimeLine;
765 const double nextSpeed = MAX2(0., currentSpeed - ACCEL2SPEED(myMRMDecel));
766 speedTimeLine.push_back(std::make_pair(t - DELTA_T, currentSpeed));
767 speedTimeLine.push_back(std::make_pair(t, nextSpeed));
768 myHolderMS->getInfluencer().setSpeedTimeLine(speedTimeLine);
769
770 if (myMRMKeepRight) {
771 // Try to change to the right
772 std::vector<std::pair<SUMOTime, int> > laneTimeLine;
773 laneTimeLine.push_back(std::make_pair(MSNet::getInstance()->getCurrentTimeStep(), 0));
774 laneTimeLine.push_back(std::make_pair(MSNet::getInstance()->getCurrentTimeStep() + 1, 0));
776// std::cout << "Keeping right..." << std::endl;
777 }
778
779 if (myState == MRM) {
780 return DELTA_T;
781 } else {
782#ifdef DEBUG_TOC
783 std::cout << SIMTIME << " Aborting MRM for vehicle '" << myHolder.getID() << "'" << std::endl;
784#endif
786 return 0;
787 }
788}
789
790
793#ifdef DEBUG_TOC
794 std::cout << SIMTIME << " Awareness recovery step for vehicle '" << myHolder.getID() << "'" << std::endl;
795#endif
796 // Proceed with awareness recovery
797 if (myCurrentAwareness < 1.0) {
799 }
800
801#ifdef DEBUG_TOC
802 std::cout << SIMTIME << " currentAwareness = " << myCurrentAwareness << std::endl;
803#endif
804
805 const bool awarenessRecoveryCompleted = myCurrentAwareness == 1.0;
806 if (awarenessRecoveryCompleted) {
807#ifdef DEBUG_TOC
808 std::cout << SIMTIME << " Awareness recovery completed for veh '" << myHolder.getID() << "'" << std::endl;
809#endif
813 return 0;
814 }
815 return DELTA_T;
816}
817
818bool
820 double /*oldPos*/,
821 double /*newPos*/,
822 double /*newSpeed*/) {
823 if (myState == AUTOMATED && checkDynamicToC()) {
824 // Initiate a ToC
825 // Record event
826 if (generatesOutput()) {
827 myEvents.push(std::make_pair(SIMSTEP, "DYNTOR"));
828 myEventLanes.push(std::make_pair(myHolder.getLane()->getID(), myHolder.getPositionOnLane())); // add lane and lanepos
829 myEventXY.push(std::make_pair(myHolder.getPosition().x(), myHolder.getPosition().y())); // add (x, y) position
830 }
831 // Leadtime for dynamic ToC is proportional to the time assumed for the dynamic ToC threshold
832 const double leadTime = myDynamicToCThreshold * 1000 * DYNAMIC_TOC_LEADTIME_FACTOR;
833 requestToC((SUMOTime) leadTime);
834 // TODO: Alter the response time according to the given lead time. Consider re-sampling it at each call of requestToC(). (Conditional to whether a non-negative response time was given in the configuration)
835 myIssuedDynamicToC = true;
838 // Abort dynamic ToC, FIXME: This could abort an externally requested ToC in rare occasions... (needs test)
839 // Record event
840 if (generatesOutput()) {
841 myEvents.push(std::make_pair(SIMSTEP, "DYNTOR"));
842 myEventLanes.push(std::make_pair(myHolder.getLane()->getID(), myHolder.getPositionOnLane())); // add lane and lanepos
843 myEventXY.push(std::make_pair(myHolder.getPosition().x(), myHolder.getPosition().y())); // add (x, y) position
844 }
845 // NOTE: This should not occur if lane changing is prevented during ToC preparation...
846 // TODO: Reset response time to the original value (unnecessary if re-sampling for each call to requestToC)
848 }
849 return true;
850}
851
852std::string
853MSDevice_ToC::getParameter(const std::string& key) const {
854 if (key == "manualType") {
855 return myManualTypeID;
856 } else if (key == "automatedType") {
857 return myAutomatedTypeID;
858 } else if (key == "responseTime") {
860 } else if (key == "recoveryRate") {
861 return toString(myRecoveryRate);
862 } else if (key == "initialAwareness") {
864 } else if (key == "mrmDecel") {
865 return toString(myMRMDecel);
866 } else if (key == "currentAwareness") {
868 } else if (key == "lcAbstinence") {
869 return toString(myLCAbstinence);
870 } else if (key == "state") {
871 return _2string(myState);
872 } else if (key == "holder") {
873 return myHolder.getID();
874 } else if (key == "hasDynamicToC") {
876 } else if (key == "dynamicToCThreshold") {
878 } else if (key == "dynamicMRMProbability") {
880 } else if (key == "mrmKeepRight") {
881 return toString(myMRMKeepRight);
882 } else if (key == "mrmSafeSpot") {
883 return myMRMSafeSpot;
884 } else if (key == "mrmSafeSpotDuration") {
886 } else if (key == "maxPreparationAccel") {
888 } else if (key == "ogNewTimeHeadway") {
890 } else if (key == "ogNewSpaceHeadway") {
892 } else if (key == "ogChangeRate") {
894 } else if (key == "ogMaxDecel") {
896 }
897 throw InvalidArgument("Parameter '" + key + "' is not supported for device of type '" + deviceName() + "'");
898}
899
900
901void
902MSDevice_ToC::setParameter(const std::string& key, const std::string& value) {
903#ifdef DEBUG_TOC
904 std::cout << SIMTIME << " MSDevice_ToC::setParameter(key=" << key << ", value=" << value << ")" << std::endl;
905#endif
906 if (key == "manualType") {
907 myManualTypeID = value;
909 if (myState == MANUAL) {
910 switchHolderType(value);
911 }
912 } else if (key == "automatedType") {
913 myAutomatedTypeID = value;
915 if (myState == AUTOMATED || myState == PREPARING_TOC || myState == MRM) {
916 switchHolderType(value);
917 }
918 } else if (key == "responseTime") {
920 } else if (key == "recoveryRate") {
922 } else if (key == "initialAwareness") {
924 } else if (key == "lcAbstinence") {
926 if (isManuallyDriven()) {
927 setAwareness(myCurrentAwareness); // to eventually trigger LC-prevention
928 }
929 } else if (key == "currentAwareness") {
930 if (isManuallyDriven()) {
932 } else {
933 WRITE_WARNING(TL("Setting device.toc.currentAwareness during automated mode has no effect."))
934 }
935 } else if (key == "mrmDecel") {
937 } else if (key == "requestToC") {
938 // setting this magic parameter gives the interface for inducing a ToC
939 const SUMOTime timeTillMRM = TIME2STEPS(StringUtils::toDouble(value));
940 requestToC(timeTillMRM, myResponseTime);
941 } else if (key == "requestMRM") {
942 // setting this magic parameter gives the interface for inducing an MRM
943 requestMRM();
944 } else if (key == "awareness") {
945 // setting this magic parameter gives the interface for setting the driverstate's awareness
947 } else if (key == "dynamicToCThreshold") {
948 const double newValue = StringUtils::toDouble(value);
949 if (newValue < 0) {
950 WRITE_WARNINGF(TL("Value of dynamicToCThreshold must be non-negative. (Given value % for vehicle % is ignored)"), value, myHolderMS->getID());
951 } else if (newValue == 0) {
952 myDynamicToCThreshold = newValue;
953 myDynamicToCActive = false;
954 } else {
955 myDynamicToCThreshold = newValue;
956 myDynamicToCActive = true;
957 }
958 } else if (key == "dynamicMRMProbability") {
959 const double newValue = StringUtils::toDouble(value);
960 if (newValue < 0) {
961 WRITE_WARNINGF(TL("Value of dynamicMRMProbability must be non-negative. (Given value % for vehicle % is ignored)"), value, myHolderMS->getID());
962 } else {
963 myMRMProbability = newValue;
964 }
965 } else if (key == "mrmKeepRight") {
966 const bool newValue = StringUtils::toBool(value);
967 myMRMKeepRight = newValue;
968 } else if (key == "mrmSafeSpot") {
969 myMRMSafeSpot = value;
970 } else if (key == "mrmSafeSpotDuration") {
972 } else if (key == "maxPreparationAccel") {
973 const double newValue = StringUtils::toDouble(value);
974 if (newValue < 0) {
975 WRITE_WARNINGF(TL("Value of maxPreparationAccel must be non-negative. (Given value % for vehicle % is ignored)"), value, myHolderMS->getID());
976 } else {
977 myMaxPreparationAccel = newValue;
978 }
979 } else {
980 throw InvalidArgument("Parameter '" + key + "' is not supported for device of type '" + deviceName() + "'");
981 }
982}
983
984
986MSDevice_ToC::_2ToCState(const std::string& str) {
987 if (str == "UNDEFINED") {
988 return UNDEFINED;
989 } else if (str == "MANUAL") {
990 return MANUAL;
991 } else if (str == "AUTOMATED") {
992 return AUTOMATED;
993 } else if (str == "PREPARING_TOC") {
994 return PREPARING_TOC;
995 } else if (str == "MRM") {
996 return MRM;
997 } else if (str == "RECOVERING") {
998 return RECOVERING;
999 } else {
1000 WRITE_WARNINGF(TL("Unknown ToCState '%'"), str);
1001 return UNDEFINED;
1002 }
1003}
1004
1005
1006std::string
1008 if (state == UNDEFINED) {
1009 return "UNDEFINED";
1010 } else if (state == MANUAL) {
1011 return "MANUAL";
1012 } else if (state == AUTOMATED) {
1013 return "AUTOMATED";
1014 } else if (state == PREPARING_TOC) {
1015 return "PREPARING_TOC";
1016 } else if (state == MRM) {
1017 return "MRM";
1018 } else if (state == RECOVERING) {
1019 return "RECOVERING";
1020 } else {
1021 WRITE_WARNINGF(TL("Unknown ToCState '%'"), toString(state));
1022 return toString(state);
1023 }
1024}
1025
1026
1027void
1029 if (!generatesOutput()) {
1030 assert(myEvents.empty());
1031 return;
1032 }
1033 while (!myEvents.empty()) {
1034 const std::pair<SUMOTime, std::string> e = myEvents.front(); // make a copy, it is used after pop
1035 const std::pair<std::string, double>& l = myEventLanes.front();
1036 const std::pair<double, double>& p = myEventXY.front();
1037 myOutputFile->openTag(e.second);
1038 myOutputFile->writeAttr("id", myHolder.getID()).writeAttr("t", STEPS2TIME(e.first));
1039 myOutputFile->writeAttr("lane", l.first).writeAttr("lanePos", l.second);
1040 myOutputFile->writeAttr("x", p.first).writeAttr("y", p.second);
1042 myEvents.pop();
1043 myEventLanes.pop();
1044 myEventXY.pop();
1045
1046 if (e.second.compare("DYNTOR") == 0 && !myEvents.empty()) { // skip "TOR" events if duplicate of "DYNTOR"
1047 std::pair<SUMOTime, std::string>& eNext = myEvents.front();
1048 if (eNext.second.compare("TOR") == 0 && eNext.first == e.first) {
1049 myEvents.pop();
1050 myEventLanes.pop();
1051 myEventXY.pop();
1052 }
1053 }
1054 }
1055}
1056
1057
1058void
1060 // Close xml bodies for all existing files
1061 // TODO: Check if required
1062 for (auto& fn : createdOutputFiles) {
1064 file->closeTag();
1065 }
1066}
1067
1068
1069void
1071 if (myPreviousLCMode != -1) {
1073#ifdef DEBUG_TOC
1074 std::cout << SIMTIME << " MSDevice_ToC::resetLCMode() restoring LC Mode of vehicle '" << myHolder.getID() << "' to " << myPreviousLCMode << std::endl;
1075#endif
1076 }
1077 myPreviousLCMode = -1;
1078}
1079
1080
1081void
1083 const int lcModeHolder = myHolderMS->getInfluencer().getLaneChangeMode();
1084 if (lcModeHolder != LCModeMRM) {
1085 myPreviousLCMode = lcModeHolder;
1086#ifdef DEBUG_TOC
1087 std::cout << SIMTIME << " MSDevice_ToC::setLCModeMRM() setting LC Mode of vehicle '" << myHolder.getID()
1088 << "' from " << myPreviousLCMode << " to " << LCModeMRM << std::endl;
1089#endif
1091 }
1092}
1093
1094bool
1096 return (myState == MANUAL || myState == RECOVERING);
1097}
1098
1099bool
1101 return (myState == AUTOMATED || myState == PREPARING_TOC || myState == MRM);
1102}
1103
1104bool
1106#ifdef DEBUG_DYNAMIC_TOC
1107 std::cout << SIMTIME << " # MSDevice_ToC::checkDynamicToC() for veh '" << myHolder.getID() << "'" << std::endl;
1108#endif
1109 if (!myDynamicToCActive) {
1110 return false;
1111 }
1112 // The vehicle's current lane
1113 const MSLane* currentLane = myHolderMS->getLane();
1114
1115 if (currentLane->isInternal()) {
1116 // Don't start or abort dynamic ToCs on internal lanes
1117 return myIssuedDynamicToC;
1118 }
1119
1120 if (myIssuedDynamicToC) {
1121#ifdef DEBUG_DYNAMIC_TOC
1122 std::cout << SIMTIME << " Dynamic ToC is ongoing." << std::endl;
1123#endif
1124 // Dynamic ToC in progress. Resist to aborting it if lane was not changed.
1125 if (myDynamicToCLane == currentLane->getNumericalID()) {
1126 return true;
1127 }
1128 }
1129 // Length for which the current route can be followed
1130 const std::vector<MSVehicle::LaneQ>& bestLanes = myHolderMS->getBestLanes();
1131 // Maximal distance for route continuation without LCs over the possible start lanes
1132 double maximalContinuationDistance = 0;
1133 // Distance for route continuation without LCs from the vehicle's current lane
1134 double continuationDistanceOnCurrent = 0;
1135 // Lane of the next stop
1136 const MSLane* nextStopLane = nullptr;
1137
1138 if (myHolderMS->hasStops()) {
1139 nextStopLane = myHolderMS->getNextStop().lane;
1140 }
1141 for (auto& i : bestLanes) {
1142 maximalContinuationDistance = MAX2(maximalContinuationDistance, i.length);
1143 if (currentLane == i.lane) {
1144 if (myHolderMS->hasStops()) {
1145 // Check if the next stop lies on the route continuation from the current lane
1146 for (MSLane* l : i.bestContinuations) {
1147 if (l == nextStopLane) {
1148#ifdef DEBUG_DYNAMIC_TOC
1149 std::cout << SIMTIME << " Stop found on the route continuation from the current lane. => No ToC" << std::endl;
1150#endif
1151 // Stop found on the route continuation from the current lane => no ToC necessary
1152 return false;
1153 }
1154 }
1155 }
1156 continuationDistanceOnCurrent = i.length;
1157 }
1158 }
1159 if (continuationDistanceOnCurrent == maximalContinuationDistance) {
1160 // There is no better lane than the current, hence no desire to change lanes,
1161 // which the driver could pursue better than the automation => no reason for ToC.
1162 return false;
1163 }
1164 const double distFromCurrent = continuationDistanceOnCurrent - myHolderMS->getPositionOnLane();
1165 const double MRMDist = 0.5 * myHolderMS->getSpeed() * myHolderMS->getSpeed() / MAX2(myMRMDecel, 0.0001);
1166 double distThreshold = myHolderMS->getSpeed() * myDynamicToCThreshold + MRMDist;
1167#ifdef DEBUG_DYNAMIC_TOC
1168 std::cout << " speed=" << myHolderMS->getSpeed()
1169 << ", distFromCurrent=" << distFromCurrent
1170 << ", maximal dist=" << maximalContinuationDistance - myHolderMS->getPositionOnLane()
1171 << ", distThreshold=" << distThreshold
1172 << std::endl;
1173#endif
1174
1175 if (myIssuedDynamicToC) {
1176 // In case of an ongoing ToC, add an additional resistance to abort it.
1177 // (The lane-check above does not capture lanes subsequent to the dynamic ToC lane)
1178 distThreshold *= DYNAMIC_TOC_ABORT_RESISTANCE_FACTOR;
1179 }
1180
1181 if (distFromCurrent < distThreshold) {
1182 // TODO: Make this more sophisticated in dealing with low speeds/stops and route ends
1183#ifdef DEBUG_DYNAMIC_TOC
1184 std::cout << SIMTIME << " * distAlongBest is below threshold! *" << std::endl;
1185#endif
1186 return true;
1187 }
1188
1189 return false;
1190}
1191
1192double
1193MSDevice_ToC::sampleResponseTime(double leadTime) const {
1194#ifdef DEBUG_DYNAMIC_TOC
1195 std::cout << "sampleResponseTime() leadTime=" << leadTime << std::endl;
1196#endif
1197 const double mean = responseTimeMean(leadTime);
1198 const double var = interpolateVariance(leadTime, myMRMProbability);
1199 double rt = RandHelper::randNorm(mean, var, &myResponseTimeRNG);
1200#ifdef DEBUG_DYNAMIC_TOC
1201 std::cout << " mean=" << mean << ", variance=" << var << " => sampled responseTime=" << rt << std::endl;
1202#endif
1203 int it_count = 0;
1204 while (rt < 0 && it_count < MAX_RESPONSETIME_SAMPLE_TRIES) {
1205 rt = RandHelper::randNorm(mean, var, &myResponseTimeRNG);
1206 it_count++;
1207 }
1208 if (rt < 0) {
1209 // Didn't generate a positive random response time => use mean
1210 rt = mean;
1211 }
1212 return rt;
1213}
1214
1215double
1216MSDevice_ToC::interpolateVariance(double leadTime, double pMRM) {
1217#ifdef DEBUG_DYNAMIC_TOC
1218 std::cout << "interpolateVariance() leadTime=" << leadTime << ", pMRM=" << pMRM << std::endl;
1219#endif
1220 // Calculate indices for surrounding values in lookup tables
1221
1222 // Find largest p_{i-1} < pMRM < p_{i}
1223 const auto pi = std::lower_bound(lookupResponseTimeMRMProbs.begin(), lookupResponseTimeMRMProbs.end(), pMRM);
1224 if (pi == lookupResponseTimeMRMProbs.end()) {
1225 // requested probability lies outside lookup table.
1226 // => return maximal variance value
1228 }
1229 const size_t pi1 = pi - lookupResponseTimeMRMProbs.begin();
1230 assert(pi1 > 0);
1231 const size_t pi0 = pi1 - 1;
1232 const double cp = (pMRM - * (pi - 1)) / (*pi - * (pi - 1));
1233
1234#ifdef DEBUG_DYNAMIC_TOC
1235 std::cout << " p[=" << pi0 << "]=" << *(pi - 1) << ", p[=" << pi1 << "]=" << *pi << " => cp=" << cp << std::endl;
1236#endif
1237
1238 // Find largest p_{i-1} < pMRM < p_{i}
1239 auto li = std::lower_bound(lookupResponseTimeLeadTimes.begin(), lookupResponseTimeLeadTimes.end(), leadTime);
1240 if (li == lookupResponseTimeLeadTimes.begin()) {
1241 // Given lead time smaller than minimal lookup-value.
1242 // Use minimal value from lookup table instead
1243 leadTime = *li;
1244 li = lookupResponseTimeLeadTimes.begin() + 1;
1245 } else if (li == lookupResponseTimeLeadTimes.end()) {
1246 // Given leadTime exceeds values in lookup table
1247 // => induce extrapolation
1248 li--;
1249 }
1250 const size_t li1 = li - lookupResponseTimeLeadTimes.begin();
1251 const size_t li0 = li1 - 1;
1252 const double cl = (leadTime - * (li - 1)) / (*li - * (li - 1));
1253
1254#ifdef DEBUG_DYNAMIC_TOC
1255 std::cout << " l[=" << li0 << "]=" << *(li - 1) << ", l[=" << li1 << "]=" << *li << " => cp=" << cl << std::endl;
1256#endif
1257
1258 // 2D interpolation for variance
1259 // First, interpolate (or extrapolate) variances along leadTimes
1260 const double var00 = lookupResponseTimeVariances[pi0][li0];
1261 const double var01 = lookupResponseTimeVariances[pi0][li1];
1262 const double var10 = lookupResponseTimeVariances[pi1][li0];
1263 const double var11 = lookupResponseTimeVariances[pi1][li1];
1264 const double var_0 = var00 + (var01 - var00) * cl;
1265 const double var_1 = var10 + (var11 - var10) * cl;
1266 // From these, interpolate along the pMRM-axis
1267 const double var = var_0 + (var_1 - var_0) * cp;
1268#ifdef DEBUG_DYNAMIC_TOC
1269 std::cout << " var00=" << var00 << ", var01=" << var01 << " var10=" << var10 << ", var11=" << var11
1270 << " var_0=" << var_0 << ", var_1=" << var_1 << ", var=" << var << std::endl;
1271#endif
1272 return var;
1273}
1274
1275// Grid of the response time distribution.
1276// Generated by the script generateResponseTimeDistributions.py, see Appendix to TransAID Deliverable 3.1v2.
1277// Probability for an MRM to occur (start with 0.0, end with 0.5)
1278std::vector<double> MSDevice_ToC::lookupResponseTimeMRMProbs = {0.0, 0.05, 0.1, 0.15000000000000002, 0.2, 0.25, 0.30000000000000004, 0.35000000000000003, 0.4, 0.45, 0.5};
1279// Lead time grid
1280std::vector<double> MSDevice_ToC::lookupResponseTimeLeadTimes = {0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.7999999999999999, 0.8999999999999999, 0.9999999999999999, 1.0999999999999999, 1.2, 1.3, 1.4, 1.5, 1.5999999999999999, 1.7, 1.8, 1.9, 2.0, 2.25, 2.5, 2.75, 3.0, 3.25, 3.5, 3.75, 4.0, 4.25, 4.5, 4.75, 5.0, 5.5, 6.0, 6.5, 7.0, 7.5, 8.0, 8.5, 9.0, 9.5, 10.0, 10.5, 11.0, 11.5, 12.0, 12.5, 13.0, 13.5, 14.0, 14.5, 15.0, 15.5, 16.0, 16.5, 17.0, 17.5, 18.0, 18.5, 19.0, 19.5, 20.0, 21.0, 22.0, 23.0, 24.0, 25.0, 26.0, 27.0, 28.0, 29.0, 30.0, 31.0, 32.0, 33.0, 34.0, 35.0, 36.0, 37.0, 38.0, 39.0, 40.0, 41.0, 42.0, 43.0, 44.0, 45.0, 46.0, 47.0, 48.0, 49.0, 50.0};
1281
1282// Variances of the response time distribution.
1283std::vector<std::vector<double> > MSDevice_ToC::lookupResponseTimeVariances = {
1284 {0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001},
1285 {0.018238371642696278, 0.03647674328603705, 0.054715114926535656, 0.07295348656987645, 0.09119185821321724, 0.10943022985371582, 0.12766860149705656, 0.14590697314039733, 0.16414534478089599, 0.18238371642423673, 0.2006220880675775, 0.21886045971091828, 0.2370988313514169, 0.25533720299475765, 0.27357557463809845, 0.291813946278597, 0.3100523179219377, 0.32829068956527846, 0.3465290612057772, 0.36476743284911795, 0.4103633619560487, 0.45595929106297967, 0.5015552201670682, 0.5471511492739992, 0.59274707838093, 0.6383430074850186, 0.6839389365919495, 0.7295348656988803, 0.7751307948058112, 0.8207267239098999, 0.8663226530168309, 0.9119185821237615, 1.003110440334781, 1.0943022985486428, 1.1854941567596624, 1.2766860149735242, 1.3678778731845436, 1.4590697313984053, 1.622588042723657, 1.8236013818166044, 2.0271872430355344, 2.2329896377033402, 2.440681612959606, 2.6499766330096066, 2.8606328744048484, 3.0724517481497657, 3.285273075453899, 3.4989689745182173, 3.713437758931686, 3.928598499444084, 4.144386478335934, 4.360749535794346, 4.577645204319768, 4.795038495182951, 5.012900204026633, 5.231205620052272, 5.449933544477286, 5.669065543877604, 5.888585381094657, 6.108478580034079, 6.328732091514834, 6.5493340353396325, 6.991540386888257, 7.4350193801571836, 7.8797023900653835, 8.325528107903486, 8.772441019472117, 9.220390365425358, 9.66932940241786, 10.11921485679467, 10.570006505095746, 11.021666840703753, 11.474160800924851, 11.927455537955435, 12.381520222795276, 12.836325874663427, 13.291845210806684, 13.748052512926236, 14.204923507573522, 14.662435258383752, 15.120566068535554, 15.57929539219183, 16.03860375377652, 16.498472674274336, 16.958884603774735, 17.41982285960362, 17.881271569514066, 18.343215619413176, 18.805640605235663, 19.268532788517863, 19.731879055399546, 20.195666878723525},
1286 {0.023394708584543455, 0.04678941716973141, 0.07018412575207719, 0.09357883433726513, 0.11697354292245306, 0.14036825150764104, 0.16376296009282898, 0.18715766867517475, 0.2105523772603627, 0.23394708584555063, 0.25734179443073857, 0.2807365030130844, 0.3041312115982723, 0.3275259201834602, 0.3509206287686481, 0.37431533735099387, 0.3977100459361818, 0.42110475452136986, 0.4444994631065578, 0.46789417168890357, 0.5263809431504523, 0.5848677146120012, 0.6433544860735498, 0.7018412575350985, 0.7603280289966473, 0.8188148004581961, 0.8773015719197449, 0.9357883433784517, 0.9942751148400004, 1.0527618863015489, 1.111248657763098, 1.169735429224647, 1.2867089721477445, 1.4036825150708419, 1.520656057991097, 1.6376296009141948, 1.7546031438372918, 1.8715766867603902, 2.080324923070349, 2.3356056603653466, 2.592695753086048, 2.851065033244823, 3.110348235805368, 3.37030701064756, 3.630792243424829, 3.8917140872434826, 4.153020535352749, 4.414682984212653, 4.676686852454919, 4.939025633083471, 5.201697195938797, 5.4647015339779985, 5.728039423002128, 5.991711652753062, 6.255718611539178, 6.520060086487768, 6.784735192689795, 7.0497423770254475, 7.315079462951026, 7.580743715621537, 7.846731914823575, 8.113040428416943, 8.646602223970579, 9.181394311584102, 9.717379133745458, 10.25451769188949, 10.792770521511661, 11.332098388641393, 11.872462773672844, 12.41382619675905, 12.956152426938662, 13.499406606935544, 14.04355531739462, 14.588566598200957, 15.134409939740127, 15.681056253544272, 16.228477829207687, 16.776648282531472, 17.32554249848686, 17.875136571609353, 18.425407745596857, 18.976334353419617, 19.527895758814104, 20.080072299738145, 20.632845234156534, 21.18619668838177, 21.740109608063758, 22.294567711842525, 22.849555447666962, 23.405057951674344, 23.96106100953682, 24.517551020220964},
1287 {0.028809965676139145, 0.05761993135292278, 0.08642989702686427, 0.11523986270364789, 0.14404982838043154, 0.172859794054373, 0.20166975973115658, 0.23047972540794023, 0.2592896910818817, 0.2880996567586654, 0.31690962243544896, 0.34571958811223263, 0.37452955378617403, 0.4033395194629576, 0.4321494851397413, 0.4609594508136828, 0.4897694164904665, 0.51857938216725, 0.5473893478411916, 0.5761993135179752, 0.6482242277085132, 0.7202491418990512, 0.7922740560867471, 0.864298970277285, 0.9363238844678232, 1.0083487986555189, 1.0803737128460569, 1.152398627036595, 1.224423541227133, 1.296448455414829, 1.368473369605367, 1.4404982837959048, 1.584548112174139, 1.7285979405552145, 1.8726477689334486, 2.0166975973145242, 2.1607474256927586, 2.3047972540738346, 2.5572219533483644, 2.8623099438447785, 3.167154753839855, 3.4715923078311928, 3.775618300633292, 4.079301412505033, 4.3827364323759, 4.686020985499979, 4.989245061612653, 5.2924871133699085, 5.595813357348367, 5.89927852360174, 6.202927167199209, 6.506795109670751, 6.810910812533207, 7.115296603265157, 7.419969732096102, 7.724943265058281, 8.030226829842663, 8.33582723446787, 8.641748978492437, 8.947994674556652, 9.254565395777925, 9.561460961999748, 10.17622101728854, 10.792256328037844, 11.409539737303032, 12.02803871883203, 12.647717622463388, 13.268539213916304, 13.890465727714453, 14.513459582113466, 15.137483858085837, 15.76250261298258, 16.388481078046894, 17.015385774331957, 17.643184571383188, 18.271846706039163, 18.901342773672106, 19.531644700723955, 20.16272570482796, 20.79456024708863, 21.427123979740387, 22.06039369148417, 22.694347252144688, 23.32896355779389, 23.964222477085105, 24.600104799357926, 25.23659218482918, 25.873667117046345, 26.51131285772261, 27.149513403967962, 27.788253447896256, 28.427518338543063},
1288 {0.03496845765860337, 0.06993691531785123, 0.1049053729770991, 0.13987383063634692, 0.17484228829559478, 0.20981074595200053, 0.24477920361124836, 0.2797476612704962, 0.314716118929744, 0.34968457658899194, 0.38465303424823977, 0.41962149190748765, 0.4545899495638933, 0.48955840722314126, 0.5245268648823892, 0.5594953225416369, 0.5944637802008848, 0.6294322378601326, 0.6644006955193805, 0.6993691531757862, 0.7867902973239058, 0.8742114414720256, 0.961632585617303, 1.0490537297654225, 1.1364748739135424, 1.2238960180588196, 1.3113171622069395, 1.3987383063550594, 1.4861594505003368, 1.5735805946484562, 1.6610017387965759, 1.748422882941853, 1.9232651712352506, 2.0981074595314895, 2.272949747824887, 2.4477920361182846, 2.6226343244145234, 2.7974766127079205, 3.0926745755509586, 3.44395841916428, 3.7929654946149927, 4.140199155580832, 4.48610487276643, 4.8310499148399675, 5.175327720605737, 5.519169410160118, 5.862755881539858, 6.20622837304298, 6.549697059107542, 6.893247832651301, 7.236947584569466, 7.580848293751031, 7.924990195098607, 8.2694042393908, 8.614114010475241, 8.959137226336916, 9.30448692001335, 9.650172373181427, 9.996199857775714, 10.342573227835842, 10.689294393844476, 11.036363704475946, 11.73154213703699, 12.428090426873897, 13.125980076789245, 13.825176212794954, 14.52564028426291, 15.227331883606062, 15.930209972250445, 16.634233702118422, 17.339362959284276, 18.045558715641334, 18.752783247333983, 19.461000260660008, 20.170174953762714, 20.880274034013603, 21.591265705145904, 22.303119634055196, 23.015806904341318, 23.7292999616137, 24.443572554090043, 25.158599671023588, 25.874357480669964, 26.590823269047004, 27.30797538027189, 28.025793159035764, 28.74425689552527, 29.46334777297606, 30.183047817969005, 30.903339853423454, 31.624207454305022, 32.345634905899644},
1289 {0.04208452197242317, 0.08416904394549082, 0.12625356591855852, 0.16833808789162616, 0.21042260986469383, 0.25250713183776147, 0.294591653807987, 0.33667617578105463, 0.3787606977541223, 0.42084521972719, 0.46292974170025764, 0.5050142636733252, 0.5470987856463931, 0.5891833076194606, 0.6312678295925284, 0.6733523515627537, 0.7154368735358214, 0.7575213955088891, 0.7996059174819568, 0.8416904394550245, 0.9469017443876938, 1.0521130493203628, 1.1573243542501899, 1.2625356591828591, 1.3677469641155282, 1.472958269045355, 1.5781695739780248, 1.6833808789106937, 1.7885921838433625, 1.8938034887731896, 1.9990147937058587, 2.104226098638527, 2.314648708501024, 2.5250713183663627, 2.7354939282288586, 2.9459165380941967, 3.1563391479566936, 3.3667617578220317, 3.703839316684887, 4.098607599735211, 4.490071039515135, 4.879192386567447, 5.266673871161598, 5.6530296960510835, 6.038639278868162, 6.423784955842712, 6.808678484059117, 7.1934797498117895, 7.578310096881422, 7.9632619279700085, 8.348405702022065, 8.733795092290785, 9.119470830182808, 9.505463599150936, 9.891796234154242, 10.278485408041451, 10.665542934966393, 11.05297678524097, 11.440791880843797, 11.828990722803214, 12.217573888696098, 12.606540429161571, 13.385614043672865, 14.166184035906006, 14.948213302277296, 15.731659164963906, 16.51647601334669, 17.302617053034968, 18.09003546045466, 18.87868513730469, 19.66852119352658, 20.459500245021562, 21.251580584675224, 22.044722266798285, 22.838887132598156, 23.634038796019215, 24.430142603281702, 25.22716557558935, 26.025076341551085, 26.823845063912813, 27.62344336382784, 28.423844244828874, 29.22502201803638, 30.026952229552563, 30.82961159071451, 31.63297791152784, 32.43703003753926, 33.24174779014256, 34.047111910360435, 34.85310400598881, 35.65970650197814, 36.46690259392711},
1290 {0.05029020480396514, 0.10058040960573261, 0.15087061441034225, 0.2011608192121097, 0.2514510240167194, 0.301741228821329, 0.35203143362309647, 0.40232163842770624, 0.45261184322947356, 0.5029020480340831, 0.5531922528358507, 0.6034824576404602, 0.6537726624450699, 0.7040628672468374, 0.7543530720514469, 0.8046432768532146, 0.8549334816578241, 0.9052236864595916, 0.9555138912642014, 1.0058040960688106, 1.1315296080760717, 1.2572551200833324, 1.382980632093436, 1.5087061441006968, 1.6344316561107992, 1.7601571681180603, 1.8858826801253215, 2.0116081921354243, 2.137333704142684, 2.263059216152788, 2.3887847281600485, 2.514510240170152, 2.765961264184674, 3.017412288202038, 3.2688633122194015, 3.520314336236765, 3.7717653602541295, 4.023216384271493, 4.402923686706604, 4.840923169412718, 5.275628140231794, 5.708106387011112, 6.139096012777341, 6.569117835239806, 6.998545742885471, 7.427651941064436, 7.856636809801444, 8.28564907733453, 8.714799743029685, 9.144171870687785, 9.573827594895418, 10.00381321001211, 10.43416291693517, 10.864901615202994, 11.296047006509264, 11.72761119486633, 12.159601914603718, 12.592023480161025, 13.024877525833228, 13.458163585558754, 13.891879549849177, 14.326022027631314, 15.195568219985953, 16.066758980781113, 16.93954397110994, 17.813869202173244, 18.68967930447209, 19.566918991345418, 20.44553399181503, 21.325471630737475, 22.206681173178218, 23.089114010781095, 23.972723742457497, 24.857466184805716, 25.743299336515662, 26.63018331333154, 27.518080264958197, 28.4069542817556, 29.296771296561822, 30.187498985305254, 31.07910666877645, 31.97156521721369, 32.86484695863455, 33.75892559154721, 34.653776102328635, 35.54937468734068, 36.44569867980954, 37.34272648128694, 38.24043749755077, 39.13881207875172, 40.03783146350794, 40.937477726773984},
1291 {0.05974016468300759, 0.11948032936381749, 0.17922049404462742, 0.23896065872827943, 0.29870082340908927, 0.35844098808989927, 0.4181811527735513, 0.47792131745436117, 0.5376614821380133, 0.5974016468188231, 0.657141811499633, 0.7168819761832851, 0.7766221408640949, 0.8363623055449051, 0.8961024702285572, 0.955842634909367, 1.0155827995930191, 1.0753229642738293, 1.135063128954639, 1.1948032936382913, 1.344153705341737, 1.493504117048025, 1.6428545287514704, 1.7922049404549163, 1.9415553521612046, 2.0909057638646504, 2.2402561755709387, 2.389606587274385, 2.5389569989806726, 2.688307410684118, 2.8376578223904056, 2.987008234093852, 3.2857090575035857, 3.58440988091332, 3.8831107043230535, 4.181811527729945, 4.48051235113968, 4.779213174549414, 5.204876111923692, 5.688848562937568, 6.170159044583231, 6.6497495644421285, 7.128261753669921, 7.606142949678828, 8.083710709627423, 8.561193555361694, 9.038757543068261, 9.516524068724227, 9.99458208377549, 10.47299664830626, 10.951815027051936, 11.431071102373838, 11.910788612986488, 12.39098355983018, 12.871666012589971, 13.352841479046571, 13.834511951858257, 14.316676714667919, 14.799332966945224, 15.282476311040416, 15.766101133711286, 16.250200906162327, 17.219795978620663, 18.1911988298489, 19.164342959599296, 20.139160814458737, 21.115585503946967, 22.093551853025446, 23.07299702417972, 24.053860858945782, 25.036086036434696, 26.019618113131642, 27.004405486541614, 27.990399311257423, 28.977553386435417, 29.965824027565322, 30.95516993094154, 31.94555203660525, 32.93693339332716, 33.92927902800424, 34.92255582087156, 35.91673238727074, 36.91177896637648, 37.907667316880286, 38.90437061956949, 39.90186338652862, 40.90012137664726, 41.899121517093974, 42.89884183037187, 43.8992613666098, 44.90036014068208, 45.90211907383619},
1292 {0.07067515415184052, 0.14135030830148337, 0.2120254624511261, 0.2827006166036112, 0.35337577075325394, 0.4240509249028968, 0.49472607905538185, 0.5654012332050246, 0.6360763873575095, 0.7067515415071527, 0.7774266956567951, 0.8481018498092803, 0.9187770039589231, 0.9894521581085659, 1.060127312261051, 1.1308024664106937, 1.2014776205603366, 1.2721527747128218, 1.3428279288624643, 1.41350308301495, 1.5901909683904774, 1.7668788537660056, 1.943566739144376, 2.120254624519904, 2.2969425098982743, 2.473630395273803, 2.6503182806493313, 2.8270061660277013, 3.0036940514032295, 3.1803819367816004, 3.3570698221571273, 3.5337577075354982, 3.8871334782865548, 4.240509249040453, 4.593885019794351, 4.947260790548251, 5.300636561302148, 5.654012332053204, 6.131630444292413, 6.6672022906208355, 7.200987739963464, 7.733716006955079, 8.265882445019285, 8.797830454475601, 9.32980159851235, 9.861967438834494, 10.394450401173598, 10.927337816806162, 11.460691587201293, 11.994554966496775, 12.528957402009762, 13.06391803975272, 13.599448295828148, 14.135553763816139, 14.672235643520898, 15.209491820318211, 15.747317686564726, 16.285706770680626, 16.82465122144653, 17.364142182462015, 17.904170082649248, 18.444724862108774, 19.52737339128757, 20.612003289072593, 21.69853012990063, 22.786871401229217, 23.87694757433194, 24.968682680039276, 26.062004572321317, 27.156844995848896, 28.253139532174295, 29.350827472729005, 30.449851649907924, 31.55015824661291, 32.651696597281045, 33.75441898881355, 34.85828046658357, 35.96323864863796, 37.06925354984671, 38.176287416815526, 39.2843045737864, 40.39327127936956, 41.50315559373177, 42.613927255723794, 43.72555756930443, 44.838019298684486, 45.951286571522466, 47.065334789581065, 48.18014054623036, 49.29568155028052, 50.41193655559852, 51.52888529603219},
1293 {0.0834623671324666, 0.16692473426273555, 0.2503871013958466, 0.33384946852611547, 0.4173118356592266, 0.5007742027894957, 0.5842365699226064, 0.6676989370528754, 0.7511613041859866, 0.8346236713162553, 0.9180860384493664, 1.0015484055796355, 1.0850107727127467, 1.1684731398430155, 1.2519355069761264, 1.3353978741063957, 1.4188602412395066, 1.5023226083697758, 1.5857849755028866, 1.6692473426331556, 1.8779032604630912, 2.0865591782930264, 2.29521509612012, 2.5038710139500555, 2.7125269317799905, 2.921182849609927, 3.1298387674370196, 3.338494685266956, 3.547150603096892, 3.7558065209268268, 3.96446243875392, 4.173118356583855, 4.590430192243725, 5.007742027900754, 5.425053863557784, 5.842365699217655, 6.259677534874682, 6.676989370534554, 7.215284811865045, 7.810942636066625, 8.405713438558344, 9.000109193262631, 9.594476065866042, 10.189051324756685, 10.783998757795331, 11.379431470741277, 11.975426983124095, 12.572037465005534, 13.169296821117609, 13.767225679430393, 14.365834957075359, 14.965128442509108, 15.565104686191797, 16.165758398192054, 16.767081489665355, 17.369063854126264, 17.971693956723634, 18.574959280506526, 19.178846665295886, 19.78334256540095, 20.38843324553279, 20.994104929469565, 22.20713664504607, 23.42233029997001, 24.63958259994195, 25.858795291719396, 27.079875555554814, 28.302736078822026, 29.527294941445533, 30.75347539415742, 31.981205580215, 33.21041823195959, 34.44105036161415, 35.67304295805245, 36.90634069627536, 38.14089166337484, 39.3766471025938, 40.61356117597085, 41.851590745280404, 43.090695170440334, 44.33083612445286, 45.57197742365028, 46.814084872185866, 48.05712611949615, 49.301070529748834, 50.54588906213925, 51.79155416112571, 53.0380396556748, 54.285320666751495, 55.533373522224736, 56.782175678619716, 58.031705649011975},
1294 {0.09864342769295685, 0.19728685538371601, 0.2959302830773173, 0.39457371076807657, 0.49321713846167803, 0.5918605661524371, 0.6905039938460386, 0.7891474215367978, 0.887790849230399, 0.9864342769211584, 1.0850777046147597, 1.1837211323055188, 1.2823645599991202, 1.38100798768988, 1.4796514153834808, 1.5782948430742403, 1.6769382707678415, 1.7755816984586006, 1.874225126152202, 1.972868553842961, 2.219477123074122, 2.4660856923052834, 2.7126942615364444, 2.9593028307647646, 3.2059113999959257, 3.4525199692270867, 3.6991285384582473, 3.945737107686567, 4.192345676917727, 4.438954246148888, 4.68556281538005, 4.932171384608369, 5.425388523070691, 5.918605661530172, 6.411822799992494, 6.905039938451975, 7.398257076914296, 7.891474215373778, 8.502335361374866, 9.169831074405188, 9.837265003168877, 10.504960425335845, 11.173131332846934, 11.841919360545138, 12.511417180930938, 13.181683762841976, 13.85275456878272, 14.524648510968875, 15.197372779403622, 15.870926242469201, 16.545301871890324, 17.220488489846183, 17.896472038316816, 18.573236507380663, 19.250764617377587, 19.929038321651447, 20.608039177427425, 21.287748619021162, 21.968148158299684, 22.64921953063529, 23.330944799863957, 24.013306432352234, 25.37987095153259, 26.74878238692951, 28.119918223260417, 29.4931641334551, 30.8684136985658, 32.245567980998565, 33.624535031317535, 35.005229375702534, 36.38757151118004, 37.77148742362557, 39.156908136182956, 40.54376929127801, 41.93201076678101, 43.32157632534552, 44.712413295099445, 46.1044722794653, 47.497706893719666, 48.892073525949804, 50.2875311201024, 51.68404097903562, 53.081566585544806, 54.48007343965984, 55.87952891050276, 57.27990210130182, 58.68116372625177, 60.08328599798667, 61.48624252468206, 62.890008215820075, 64.29455919573915, 65.6998727243063},
1295};
1296
1297
1298/****************************************************************************/
long long int SUMOTime
Definition: GUI.h:36
#define DEFAULT_DYNAMIC_TOC_THRESHOLD
#define DEFAULT_RESPONSE_TIME
#define DEFAULT_OPENGAP_SPACING
#define DEFAULT_MRM_DECEL
#define DEFAULT_OPENGAP_CHANGERATE
#define DEFAULT_INITIAL_AWARENESS
#define DEFAULT_OPENGAP_TIMEGAP
#define DYNAMIC_TOC_ABORT_RESISTANCE_FACTOR
#define DEFAULT_MANUAL_TYPE
#define DYNAMIC_TOC_LEADTIME_FACTOR
#define DEFAULT_RECOVERY_RATE
#define DEFAULT_LCABSTINENCE
#define MAX_RESPONSETIME_VARIANCE
#define DEFAULT_MRM_PROBABILITY
#define MAX_RESPONSETIME_SAMPLE_TRIES
#define DEFAULT_OPENGAP_MAXDECEL
#define DEFAULT_AUTOMATED_TYPE
#define WRITE_WARNINGF(...)
Definition: MsgHandler.h:271
#define WRITE_ERRORF(...)
Definition: MsgHandler.h:280
#define WRITE_ERROR(msg)
Definition: MsgHandler.h:279
#define WRITE_WARNING(msg)
Definition: MsgHandler.h:270
#define TL(string)
Definition: MsgHandler.h:287
SUMOTime DELTA_T
Definition: SUMOTime.cpp:38
#define STEPS2TIME(x)
Definition: SUMOTime.h:55
#define SIMSTEP
Definition: SUMOTime.h:61
#define ACCEL2SPEED(x)
Definition: SUMOTime.h:51
#define TS
Definition: SUMOTime.h:42
#define SIMTIME
Definition: SUMOTime.h:62
#define TIME2STEPS(x)
Definition: SUMOTime.h:57
const int VEHPARS_COLOR_SET
@ SUMO_TAG_PARKING_AREA
A parking area.
T MIN2(T a, T b)
Definition: StdDefs.h:76
T MAX2(T a, T b)
Definition: StdDefs.h:82
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:46
MSVehicleType & getSingularType()
Replaces the current vehicle type with a new one used by this vehicle only.
MSStop & getNextStop()
bool hasStops() const
Returns whether the vehicle has to stop somewhere.
virtual void setMaxAccel(double accel)
Sets a new value for maximum acceleration [m/s^2].
Definition: MSCFModel.h:528
double getMaxAccel() const
Get the vehicle type's maximum acceleration [m/s^2].
Definition: MSCFModel.h:256
virtual double getHeadwayTime() const
Get the driver's desired headway [s].
Definition: MSCFModel.h:311
The ToC Device controls transition of control between automated and manual driving.
Definition: MSDevice_ToC.h:52
SUMOTime MRMExecutionStep(SUMOTime t)
Continue the MRM for one time step.
std::string myAutomatedTypeID
vehicle type ID for automated driving
Definition: MSDevice_ToC.h:267
void requestMRM()
Request an MRM to be initiated immediately. No downward ToC will be scheduled.
std::string myManualTypeID
vehicle type ID for manual driving
Definition: MSDevice_ToC.h:265
static void cleanup()
Closes root tags of output files.
double myRecoveryRate
Recovery rate for the driver's awareness after a ToC.
Definition: MSDevice_ToC.h:273
bool myDynamicToCActive
Switch for considering dynamic ToCs,.
Definition: MSDevice_ToC.h:337
double myMRMDecel
Deceleration rate applied during MRM.
Definition: MSDevice_ToC.h:280
WrappingCommand< MSDevice_ToC > * myTriggerToCCommand
Definition: MSDevice_ToC.h:303
static double interpolateVariance(double leadTime, double pMRM)
Two-dimensional interpolation of variance from lookup table assumes pMRM >= 0, leadTime >= 0.
static int LCModeMRM
LC mode operational during an MRM.
Definition: MSDevice_ToC.h:325
double myInitialAwareness
Average awareness the driver has initially after a ToC.
Definition: MSDevice_ToC.h:277
bool myUseColorScheme
Whether a coloring scheme shall by applied to indicate the different toc stages,.
Definition: MSDevice_ToC.h:289
OpenGapParams myOpenGapParams
Parameters for the openGap mechanism applied during ToC preparation phase.
Definition: MSDevice_ToC.h:328
static std::vector< std::vector< double > > lookupResponseTimeVariances
Variances of the response time distribution. Given the lead time and the MRM probability the variance...
Definition: MSDevice_ToC.h:369
bool notifyMove(SUMOTrafficObject &veh, double oldPos, double newPos, double newSpeed) override
Return value indicates whether the device still wants to be notified about the vehicle movement.
SUMOTime myMRMSafeSpotDuration
duration at stop vehicle tries to reach during MRM
Definition: MSDevice_ToC.h:350
void resetDeliberateLCs()
Resets the holder's LC mode to the last differing to LCModeMRM.
double myOriginalMaxAccel
Storage for original maximal acceleration of vehicle.
Definition: MSDevice_ToC.h:357
const std::string deviceName() const override
return the name for this type of device
Definition: MSDevice_ToC.h:133
static std::vector< double > lookupResponseTimeLeadTimes
Definition: MSDevice_ToC.h:361
WrappingCommand< MSDevice_ToC > * myExecuteMRMCommand
Definition: MSDevice_ToC.h:305
static std::set< std::string > createdOutputFiles
Definition: MSDevice_ToC.h:57
SUMOTime ToCPreparationStep(SUMOTime t)
Continue the ToC preparation for one time step.
~MSDevice_ToC()
Destructor.
bool myIssuedDynamicToC
Flag to indicate that a dynamically triggered ToC is in preparation.
Definition: MSDevice_ToC.h:339
double myMRMProbability
Probability of an MRM to occur after a dynamically triggered ToC.
Definition: MSDevice_ToC.h:335
static std::set< MSDevice_ToC *, ComparatorNumericalIdLess > myInstances
Definition: MSDevice_ToC.h:55
void descheduleRecovery()
Remove ongoing awareness recovery process from the event-queue.
int myPreviousLCMode
LC mode overridden during MRM, stored for restoration.
Definition: MSDevice_ToC.h:322
void requestToC(SUMOTime timeTillMRM, SUMOTime responseTime=-1000)
Request a ToC. If the device is in AUTOMATED or MRM state, a driver response time is sampled and the ...
double sampleResponseTime(double leadTime) const
Samples a random driver response time from a truncated Gaussian with parameters according to the look...
static OpenGapParams getOpenGapParams(const SUMOVehicle &v, const OptionsCont &oc)
std::queue< std::pair< std::string, double > > myEventLanes
Storage for events to be written to the output.
Definition: MSDevice_ToC.h:316
static void insertOptions(OptionsCont &oc)
Inserts MSDevice_ToC-options.
bool myMRMKeepRight
Whether vehicle tries to change to the right during an MRM.
Definition: MSDevice_ToC.h:344
bool isManuallyDriven()
Whether the current operation mode is manual.
static void buildVehicleDevices(SUMOVehicle &v, std::vector< MSVehicleDevice * > &into)
Build devices for the given vehicle, if needed.
void setVehicleColor()
ToCState myState
Current state of the device.
Definition: MSDevice_ToC.h:292
double myMaxPreparationAccel
Maximal acceleration that may be applied during the ToC preparation phase TODO: Make effective.
Definition: MSDevice_ToC.h:354
static ToCState _2ToCState(const std::string &)
static std::string getOutputFilename(const SUMOVehicle &v, const OptionsCont &oc)
MSDevice_ToC(SUMOVehicle &holder, const std::string &id, const std::string &outputFilename, const std::string &manualType, const std::string &automatedType, SUMOTime responseTime, double recoveryRate, double lcAbstinence, double initialAwareness, double mrmDecel, double dynamicToCThreshold, double dynamicMRMProbability, double maxPreparationAccel, bool mrmKeepRight, const std::string &mrmSafeSpot, SUMOTime mrmSafeSpotDuration, bool useColorScheme, OpenGapParams ogp)
Constructor.
int myDynamicToCLane
Lane, on which the ongoing dynamic ToC was issued. It can only be aborted if the lane was changed.
Definition: MSDevice_ToC.h:341
bool isAutomated()
Whether the current operation mode is automated.
std::map< ToCState, RGBColor > myColorScheme
Coloring scheme,.
Definition: MSDevice_ToC.h:286
static std::string _2string(ToCState state)
static SumoRNG myResponseTimeRNG
Random generator for ToC devices.
Definition: MSDevice_ToC.h:372
void setState(ToCState state)
Set the ToC device's state.
SUMOTime myResponseTime
Average response time needed by the driver to take back control.
Definition: MSDevice_ToC.h:271
void setParameter(const std::string &key, const std::string &value) override
try to set the given parameter for this device. Throw exception for unsupported key
bool generatesOutput()
Whether this device requested to write output.
Definition: MSDevice_ToC.h:172
double myLCAbstinence
Level of the awareness below which no lane-changes are performed.
Definition: MSDevice_ToC.h:275
SUMOTime triggerDownwardToC(SUMOTime t)
Trigger execution of a ToC X-->MANUAL ("downwards")
void initColorScheme()
Initialize vehicle colors for different states.
MSVehicle * myHolderMS
The holder vehicle casted to MSVehicle*.
Definition: MSDevice_ToC.h:297
WrappingCommand< MSDevice_ToC > * myPrepareToCCommand
Definition: MSDevice_ToC.h:306
std::string getParameter(const std::string &key) const override
try to retrieve the given parameter from this device. Throw exception for unsupported key
void descheduleMRM()
Break MRM Process or remove MRM-Trigger command from the event-queue.
static double responseTimeMean(double leadTime)
Mean of the response time distribution. (Only depends on given lead time)
Definition: MSDevice_ToC.h:363
void descheduleToC()
Remove scheduled ToC-Trigger command from the event-queue.
void descheduleToCPreparation()
Remove ongoing ToC-Preparation process from the event-queue.
void deactivateDeliberateLCs()
Resets the holder's LC mode to the operational LC-mode of the ToC Device (.
WrappingCommand< MSDevice_ToC > * myRecoverAwarenessCommand
Definition: MSDevice_ToC.h:304
void setAwareness(double value)
Set the awareness to the given value.
WrappingCommand< MSDevice_ToC > * myTriggerMRMCommand
Definition: MSDevice_ToC.h:302
double myDynamicToCThreshold
Duration in s. for which the vehicle needs to be able to follow its route without a lane change to co...
Definition: MSDevice_ToC.h:332
bool checkDynamicToC()
Check if the vehicle should induce a ToC due to internal reasons. That is, if the route cannot be fol...
ToCState
Enum describing the different regimes for the device,.
Definition: MSDevice_ToC.h:108
static double getDynamicMRMProbability(const SUMOVehicle &v, const OptionsCont &oc)
void writeOutput()
Write output to file given by option device.toc.file.
std::queue< std::pair< double, double > > myEventXY
Storage for events to be written to the output.
Definition: MSDevice_ToC.h:319
OutputDevice * myOutputFile
The file the devices output goes to.
Definition: MSDevice_ToC.h:310
double myCurrentAwareness
Current awareness-level of the driver in [0,1].
Definition: MSDevice_ToC.h:283
SUMOTime triggerMRM(SUMOTime t)
Trigger execution of an MRM.
SUMOTime triggerUpwardToC(SUMOTime t)
Trigger execution of a ToC X-->AUTOMATED ("upwards")
std::queue< std::pair< SUMOTime, std::string > > myEvents
Storage for events to be written to the output.
Definition: MSDevice_ToC.h:313
SUMOTime awarenessRecoveryStep(SUMOTime t)
Continue the awareness recovery for one time step.
std::string myMRMSafeSpot
stop vehicle tries to reach during MRM
Definition: MSDevice_ToC.h:347
static std::vector< double > lookupResponseTimeMRMProbs
Grid of the response time distribution.
Definition: MSDevice_ToC.h:360
void switchHolderType(const std::string &targetTypeID)
Switch the device holder's vehicle type.
static bool getBoolParam(const SUMOVehicle &v, const OptionsCont &oc, std::string paramName, bool deflt, bool required=false)
Definition: MSDevice.cpp:212
static double getFloatParam(const SUMOVehicle &v, const OptionsCont &oc, std::string paramName, double deflt, bool required=false)
Definition: MSDevice.cpp:199
static void insertDefaultAssignmentOptions(const std::string &deviceName, const std::string &optionsTopic, OptionsCont &oc, const bool isPerson=false)
Adds common command options that allow to assign devices to vehicles.
Definition: MSDevice.cpp:148
static bool equippedByDefaultAssignmentOptions(const OptionsCont &oc, const std::string &deviceName, DEVICEHOLDER &v, bool outputOptionSet, const bool isPerson=false)
Determines whether a vehicle should get a certain device.
Definition: MSDevice.h:202
static std::string getStringParam(const SUMOVehicle &v, const OptionsCont &oc, std::string paramName, std::string deflt, bool required=false)
Definition: MSDevice.cpp:175
virtual void addEvent(Command *operation, SUMOTime execTimeStep=-1)
Adds an Event.
static bool gUseMesoSim
Definition: MSGlobals.h:103
Representation of a lane in the micro simulation.
Definition: MSLane.h:84
bool isInternal() const
Definition: MSLane.cpp:2456
int getNumericalID() const
Returns this lane's numerical id.
Definition: MSLane.h:516
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
Definition: MSNet.cpp:183
MSEventControl * getBeginOfTimestepEvents()
Returns the event control for events executed at the begin of a time step.
Definition: MSNet.h:473
MSStoppingPlace * getStoppingPlace(const std::string &id, const SumoXMLTag category) const
Returns the named stopping place of the given category.
Definition: MSNet.cpp:1363
MSVehicleControl & getVehicleControl()
Returns the vehicle control.
Definition: MSNet.h:380
static SumoRNG * getParsingRNG()
get parsing RNG
const MSLane * lane
The lane to stop at (microsim only)
Definition: MSStop.h:50
A lane area vehicles can halt at.
double getBeginLanePosition() const
Returns the begin position of this stop.
double getEndLanePosition() const
Returns the end position of this stop.
const MSLane & getLane() const
Returns the lane this stop is located at.
void setLaneChangeMode(int value)
Sets lane changing behavior.
Definition: MSVehicle.cpp:781
void deactivateGapController()
Deactivates the gap control.
Definition: MSVehicle.cpp:409
void setLaneTimeLine(const std::vector< std::pair< SUMOTime, int > > &laneTimeLine)
Sets a new lane timeline.
Definition: MSVehicle.cpp:416
int getLaneChangeMode() const
return the current lane change mode
Definition: MSVehicle.cpp:447
void setSpeedTimeLine(const std::vector< std::pair< SUMOTime, double > > &speedTimeLine)
Sets a new velocity timeline.
Definition: MSVehicle.cpp:395
void activateGapController(double originalTau, double newTimeHeadway, double newSpaceHeadway, double duration, double changeRate, double maxDecel, MSVehicle *refVeh=nullptr)
Activates the gap control with the given parameters,.
Definition: MSVehicle.cpp:401
The class responsible for building and deletion of vehicles.
bool hasVType(const std::string &id) const
Asks for existence of a vehicle type.
bool hasVTypeDistribution(const std::string &id) const
Asks for a vehicle type distribution.
MSVehicleType * getVType(const std::string &id=DEFAULT_VTYPE_ID, SumoRNG *rng=nullptr, bool readOnly=false)
Returns the named vehicle type or a sample from the named distribution.
const std::set< std::string > getVTypeDistributionMembership(const std::string &id) const
Return the distribution IDs the vehicle type is a member of.
Abstract in-vehicle device.
SUMOVehicle & myHolder
The vehicle that stores the device.
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:77
std::shared_ptr< MSSimpleDriverState > getDriverState() const
Returns the vehicle driver's state.
Definition: MSVehicle.cpp:7486
void replaceVehicleType(MSVehicleType *type)
Replaces the current vehicle type by the one given.
Definition: MSVehicle.cpp:4737
const MSLane * getLane() const
Returns the lane the vehicle is on.
Definition: MSVehicle.h:584
Influencer & getInfluencer()
Definition: MSVehicle.cpp:7076
double getSpeed() const
Returns the vehicle's current speed.
Definition: MSVehicle.h:493
const std::vector< LaneQ > & getBestLanes() const
Returns the description of best lanes to use in order to continue the route.
Definition: MSVehicle.cpp:5638
const MSCFModel & getCarFollowModel() const
Returns the vehicle's car following model definition.
Definition: MSVehicle.h:978
double getPositionOnLane() const
Get the vehicle's position along the lane.
Definition: MSVehicle.h:377
The car-following model and parameter.
Definition: MSVehicleType.h:63
const std::string & getID() const
Returns the name of the vehicle type.
Definition: MSVehicleType.h:91
void setDecel(double decel)
Set a new value for this type's deceleration.
const MSCFModel & getCarFollowModel() const
Returns the vehicle type's car following model definition (const version)
const SUMOVTypeParameter & getParameter() const
const RGBColor & getColor() const
Returns this type's color.
const std::string & getID() const
Returns the id.
Definition: Named.h:74
A storage for options typed value containers)
Definition: OptionsCont.h:89
void addDescription(const std::string &name, const std::string &subtopic, const std::string &description)
Adds a description for an option.
std::string getString(const std::string &name) const
Returns the string-value of the named option (only for Option_String)
void doRegister(const std::string &name, Option *o)
Adds an option under the given name.
Definition: OptionsCont.cpp:76
void addOptionSubTopic(const std::string &topic)
Adds an option subtopic.
static OptionsCont & getOptions()
Retrieves the options.
Definition: OptionsCont.cpp:60
Static storage of an output device and its base (abstract) implementation.
Definition: OutputDevice.h:61
OutputDevice & writeAttr(const SumoXMLAttr attr, const T &val)
writes a named attribute
Definition: OutputDevice.h:254
OutputDevice & openTag(const std::string &xmlElement)
Opens an XML tag.
bool closeTag(const std::string &comment="")
Closes the most recently opened tag and optionally adds a comment.
static OutputDevice & getDevice(const std::string &name, bool usePrefix=true)
Returns the described OutputDevice.
bool writeXMLHeader(const std::string &rootElement, const std::string &schemaFile, std::map< SumoXMLAttr, std::string > attrs=std::map< SumoXMLAttr, std::string >(), bool includeConfig=true)
Writes an XML header with optional configuration.
virtual const std::string getParameter(const std::string &key, const std::string defaultValue="") const
Returns the value for a given key.
bool knowsParameter(const std::string &key) const
Returns whether the parameter is known.
double x() const
Returns the x-position.
Definition: Position.h:55
double y() const
Returns the y-position.
Definition: Position.h:60
static double randNorm(double mean, double variance, SumoRNG *rng=nullptr)
Access to a random number from a normal distribution.
Definition: RandHelper.cpp:137
Representation of a vehicle, person, or container.
virtual const MSVehicleType & getVehicleType() const =0
Returns the object's "vehicle" type.
virtual const MSLane * getLane() const =0
Returns the lane the object is currently at.
virtual const SUMOVehicleParameter & getParameter() const =0
Returns the vehicle's parameter (including departure definition)
virtual Position getPosition(const double offset=0) const =0
Return current position (x/y, cartesian)
virtual double getPositionOnLane() const =0
Get the object's position along the lane.
Representation of a vehicle.
Definition: SUMOVehicle.h:62
virtual bool addStop(const SUMOVehicleParameter::Stop &stopPar, std::string &errorMsg, SUMOTime untilOffset=0, ConstMSEdgeVector::const_iterator *searchStart=0)=0
Adds a stop.
Definition of vehicle stop (position and duration)
ParkingType parking
whether the vehicle is removed from the net while stopping
std::string lane
The lane to stop at.
std::string parkingarea
(Optional) parking area if one is assigned to the stop
double startPos
The stopping position start.
double endPos
The stopping position end.
SUMOTime duration
The stopping duration.
Structure representing possible vehicle parameter.
int parametersSet
Information for the router which parameter were set, TraCI may modify this (when changing color)
RGBColor color
The vehicle's color, TraCI may change this.
static double toDouble(const std::string &sData)
converts a string into the double value described by it by calling the char-type converter
static bool toBool(const std::string &sData)
converts a string into the bool value described by it by calling the char-type converter
void deschedule()
Marks this Command as being descheduled.
static double fn[10]
Definition: odrSpiral.cpp:87