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.org/sumo
3 // Copyright (C) 2013-2020 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>
32 #include <utils/common/RGBColor.h>
33 #include <microsim/MSNet.h>
34 #include <microsim/MSVehicle.h>
38 #include <microsim/MSDriverState.h>
40 #include <microsim/MSStop.h>
41 #include "MSDevice_ToC.h"
42 
43 
44 // ===========================================================================
45 // debug constants
46 // ===========================================================================
47 //#define DEBUG_TOC
48 //#define DEBUG_DYNAMIC_TOC
49 
50 // ===========================================================================
51 // parameter defaults
52 // ===========================================================================
53 
54 // default value for the average response time, that a driver needs to take back control
55 #define DEFAULT_RESPONSE_TIME -1.0
56 // default value for the average rate at which the driver's awareness recovers to
57 // 1.0 after a ToC has been performed
58 #define DEFAULT_RECOVERY_RATE 0.1
59 // Default value of the awareness below which no lane-changes are performed
60 #define DEFAULT_LCABSTINENCE 0.0
61 // The default value for the average awareness a driver has initially after a ToC
62 #define DEFAULT_INITIAL_AWARENESS 0.5
63 // The default value for the deceleration rate applied during a 'minimum risk maneuver'
64 #define DEFAULT_MRM_DECEL 1.5
65 // The default value for the dynamic ToC threshold indicates that the dynamic ToCs are deactivated
66 #define DEFAULT_DYNAMIC_TOC_THRESHOLD 0.0
67 // The default value for the probability of an MRM to occur after a dynamically triggered ToC
68 // (Note that these MRMs will not induce full stops in most cases)
69 #define DEFAULT_MRM_PROBABILITY 0.05
70 
71 // The factor by which the dynamic ToC threshold time is multiplied to yield the lead time given for the corresponding ToC
72 #define DYNAMIC_TOC_LEADTIME_FACTOR 0.75
73 // A factor applied to the check for the dynamic ToC condition to resist aborting an ongoing dynamic ToC (and prevent oscillations)
74 #define DYNAMIC_TOC_ABORT_RESISTANCE_FACTOR 2.0
75 
76 
77 // The default values for the openGap parameters applied for gap creation in preparation for a ToC
78 #define DEFAULT_OPENGAP_TIMEGAP -1.0
79 #define DEFAULT_OPENGAP_SPACING 0.0
80 #define DEFAULT_OPENGAP_CHANGERATE 1.0
81 #define DEFAULT_OPENGAP_MAXDECEL 1.0
82 
83 
84 #define DEFAULT_MANUAL_TYPE ""
85 #define DEFAULT_AUTOMATED_TYPE ""
86 
87 // Maximal tries to sample a positive value from the gaussian distribution
88 // used for the driver response time when a TOR is issued. (the distribution is assumed truncated at zero)
89 #define MAX_RESPONSETIME_SAMPLE_TRIES 100
90 // Maximal variance of responsetimes (returned for pMRM outside lookup table, i.e. pMRM>0.5), see interpolateVariance()
91 #define MAX_RESPONSETIME_VARIANCE 10000
92 
93 
94 // ---------------------------------------------------------------------------
95 // static members
96 // ---------------------------------------------------------------------------
97 std::set<MSDevice_ToC*, ComparatorNumericalIdLess> MSDevice_ToC::myInstances = std::set<MSDevice_ToC*, ComparatorNumericalIdLess>();
98 std::set<std::string> MSDevice_ToC::createdOutputFiles;
99 int MSDevice_ToC::LCModeMRM = 768; // = 0b001100000000 - no autonomous changes, no speed adaptation
101 
102 
103 // ===========================================================================
104 // method definitions
105 // ===========================================================================
106 // ---------------------------------------------------------------------------
107 // static initialisation methods
108 // ---------------------------------------------------------------------------
109 void
111  oc.addOptionSubTopic("ToC Device");
112  insertDefaultAssignmentOptions("toc", "ToC Device", oc);
113 
114  oc.doRegister("device.toc.manualType", new Option_String());
115  oc.addDescription("device.toc.manualType", "ToC Device", "Vehicle type for manual driving regime.");
116  oc.doRegister("device.toc.automatedType", new Option_String());
117  oc.addDescription("device.toc.automatedType", "ToC Device", "Vehicle type for automated driving regime.");
118  oc.doRegister("device.toc.responseTime", new Option_Float(DEFAULT_RESPONSE_TIME));
119  oc.addDescription("device.toc.responseTime", "ToC Device", "Average response time needed by a driver to take back control.");
120  oc.doRegister("device.toc.recoveryRate", new Option_Float(DEFAULT_RECOVERY_RATE));
121  oc.addDescription("device.toc.recoveryRate", "ToC Device", "Recovery rate for the driver's awareness after a ToC.");
122  oc.doRegister("device.toc.lcAbstinence", new Option_Float(DEFAULT_LCABSTINENCE));
123  oc.addDescription("device.toc.lcAbstinence", "ToC Device", "Attention level below which a driver restrains from performing lane changes (value in [0,1]).");
124  oc.doRegister("device.toc.initialAwareness", new Option_Float(DEFAULT_INITIAL_AWARENESS));
125  oc.addDescription("device.toc.initialAwareness", "ToC Device", "Average awareness a driver has initially after a ToC (value in [0,1]).");
126  oc.doRegister("device.toc.mrmDecel", new Option_Float(DEFAULT_MRM_DECEL));
127  oc.addDescription("device.toc.mrmDecel", "ToC Device", "Deceleration rate applied during a 'minimum risk maneuver'.");
128  oc.doRegister("device.toc.dynamicToCThreshold", new Option_Float(DEFAULT_DYNAMIC_TOC_THRESHOLD));
129  oc.addDescription("device.toc.dynamicToCThreshold", "ToC Device", "Time, which the vehicle requires to have ahead to continue in automated mode. The default value of 0 indicates no dynamic triggering of ToCs.");
130  oc.doRegister("device.toc.dynamicMRMProbability", new Option_Float(DEFAULT_MRM_PROBABILITY));
131  oc.addDescription("device.toc.dynamicMRMProbability", "ToC Device", "Probability that a dynamically triggered TOR is not answered in time.");
132  oc.doRegister("device.toc.mrmKeepRight", new Option_Bool(false));
133  oc.addDescription("device.toc.mrmKeepRight", "ToC Device", "If true, the vehicle tries to change to the right during an MRM.");
134  oc.doRegister("device.toc.mrmSafeSpot", new Option_String());
135  oc.addDescription("device.toc.mrmSafeSpot", "ToC Device", "If set, the vehicle tries to reach the given named stopping place during an MRM.");
136  oc.doRegister("device.toc.mrmSafeSpotDuration", new Option_Float(60.));
137  oc.addDescription("device.toc.mrmSafeSpotDuration", "ToC Device", "Duration the vehicle stays at the safe spot after an MRM.");
138  oc.doRegister("device.toc.maxPreparationAccel", new Option_Float(0.0));
139  oc.addDescription("device.toc.maxPreparationAccel", "ToC Device", "Maximal acceleration that may be applied during the ToC preparation phase.");
140  oc.doRegister("device.toc.ogNewTimeHeadway", new Option_Float(-1.0));
141  oc.addDescription("device.toc.ogNewTimeHeadway", "ToC Device", "Timegap for ToC preparation phase.");
142  oc.doRegister("device.toc.ogNewSpaceHeadway", new Option_Float(-1.0));
143  oc.addDescription("device.toc.ogNewSpaceHeadway", "ToC Device", "Additional spacing for ToC preparation phase.");
144  oc.doRegister("device.toc.ogMaxDecel", new Option_Float(-1.0));
145  oc.addDescription("device.toc.ogMaxDecel", "ToC Device", "Maximal deceleration applied for establishing increased gap in ToC preparation phase.");
146  oc.doRegister("device.toc.ogChangeRate", new Option_Float(-1.0));
147  oc.addDescription("device.toc.ogChangeRate", "ToC Device", "Rate of adaptation towards the increased headway during ToC preparation.");
148  oc.doRegister("device.toc.useColorScheme", new Option_Bool(true));
149  oc.addDescription("device.toc.useColorScheme", "ToC Device", "Whether a coloring scheme shall by applied to indicate the different ToC stages.");
150  oc.doRegister("device.toc.file", new Option_String());
151  oc.addDescription("device.toc.file", "ToC Device", "Switches on output by specifying an output filename.");
152 }
153 
154 
155 void
156 MSDevice_ToC::buildVehicleDevices(SUMOVehicle& v, std::vector<MSVehicleDevice*>& into) {
158  if (equippedByDefaultAssignmentOptions(oc, "toc", v, false)) {
160  WRITE_WARNING("ToC device is not supported by the mesoscopic simulation.");
161  return;
162  }
163  const std::string manualType = getStringParam(v, oc, "toc.manualType", DEFAULT_MANUAL_TYPE, true);
164  const std::string automatedType = getStringParam(v, oc, "toc.automatedType", DEFAULT_AUTOMATED_TYPE, true);
165  const SUMOTime responseTime = TIME2STEPS(getFloatParam(v, oc, "toc.responseTime", DEFAULT_RESPONSE_TIME, false));
166  const double recoveryRate = getFloatParam(v, oc, "toc.recoveryRate", DEFAULT_RECOVERY_RATE, false);
167  const double lcAbstinence = getFloatParam(v, oc, "toc.lcAbstinence", DEFAULT_LCABSTINENCE, false);
168  const double initialAwareness = getFloatParam(v, oc, "toc.initialAwareness", DEFAULT_INITIAL_AWARENESS, false);
169  const double mrmDecel = getFloatParam(v, oc, "toc.mrmDecel", DEFAULT_MRM_DECEL, false);
170  const bool useColoring = getBoolParam(v, oc, "toc.useColorScheme", "false", false);
171  const std::string deviceID = "toc_" + v.getID();
172  const std::string file = getOutputFilename(v, oc);
173  const OpenGapParams ogp = getOpenGapParams(v, oc);
174  const double dynamicToCThreshold = getFloatParam(v, oc, "toc.dynamicToCThreshold", DEFAULT_DYNAMIC_TOC_THRESHOLD, false);
175  const double dynamicMRMProbability = getDynamicMRMProbability(v, oc);
176  const bool mrmKeepRight = getBoolParam(v, oc, "toc.mrmKeepRight", false, false);
177  const std::string mrmSafeSpot = getStringParam(v, oc, "toc.mrmSafeSpot", "", false);
178  const SUMOTime mrmSafeSpotDuration = TIME2STEPS(getFloatParam(v, oc, "toc.mrmSafeSpotDuration", 60., false));
179  const double maxPreparationAccel = getFloatParam(v, oc, "toc.maxPreparationAccel", 0.0, false);
180  // build the device
181  MSDevice_ToC* device = new MSDevice_ToC(v, deviceID, file,
182  manualType, automatedType, responseTime, recoveryRate,
183  lcAbstinence, initialAwareness, mrmDecel, dynamicToCThreshold,
184  dynamicMRMProbability, maxPreparationAccel, mrmKeepRight,
185  mrmSafeSpot, mrmSafeSpotDuration, useColoring, ogp);
186  into.push_back(device);
187  }
188 }
189 
190 
191 std::string
193  // Default of "" means no output
194  std::string file = "";
195  if (v.getParameter().knowsParameter("device.toc.file")) {
196  try {
197  file = v.getParameter().getParameter("device.toc.file", file);
198  } catch (...) {
199  WRITE_WARNING("Invalid value '" + v.getParameter().getParameter("device.toc.file", file) + "'for vehicle parameter 'ssm.measures'");
200  }
201  } else if (v.getVehicleType().getParameter().knowsParameter("device.toc.file")) {
202  try {
203  file = v.getVehicleType().getParameter().getParameter("device.toc.file", file);
204  } catch (...) {
205  WRITE_WARNING("Invalid value '" + v.getVehicleType().getParameter().getParameter("device.toc.file", file) + "'for vType parameter 'ssm.measures'");
206  }
207  } else {
208  file = oc.getString("device.toc.file") == "" ? file : oc.getString("device.toc.file");
209  }
210  return file;
211 }
212 
213 
214 double
216  double pMRM = getFloatParam(v, oc, "toc.dynamicMRMProbability", DEFAULT_MRM_PROBABILITY, false);
217  if (pMRM < 0 || pMRM > 0.5) {
218  const double pMRMTrunc = MAX2(0.0, MIN2(0.5, pMRM));
219  WRITE_WARNING("Given value for ToC device parameter 'dynamicMRMProbability' (=" + toString(pMRM) + ") is not in the admissible range [0,0.5]. Truncated to " + toString(pMRMTrunc) + ".");
220  return pMRMTrunc;
221  }
222  return pMRM;
223 }
224 
225 
228  double timegap = getFloatParam(v, oc, "toc.ogNewTimeHeadway", -1.0, false);
229  double spacing = getFloatParam(v, oc, "toc.ogNewSpaceHeadway", -1.0, false);
230  double changeRate = getFloatParam(v, oc, "toc.ogChangeRate", -1.0, false);
231  double maxDecel = getFloatParam(v, oc, "toc.ogMaxDecel", -1.0, false);
232  bool specifiedAny = false;
233 
234  if (changeRate == -1.0) {
235  changeRate = DEFAULT_OPENGAP_CHANGERATE;
236  } else {
237  specifiedAny = true;
238  }
239  if (maxDecel == -1.0) {
240  maxDecel = DEFAULT_OPENGAP_MAXDECEL;
241  } else {
242  specifiedAny = true;
243  }
244  if (specifiedAny && timegap == -1 && spacing == -1) {
245  WRITE_ERROR("If any openGap parameters for the ToC model are specified, then at least one of toc.ogNewTimeHeadway and toc.ogNewSpaceHeadway must be defined.")
246  }
247  if (timegap == -1) {
248  timegap = DEFAULT_OPENGAP_TIMEGAP;
249  } else {
250  specifiedAny = true;
251  }
252  if (spacing == -1) {
253  spacing = DEFAULT_OPENGAP_SPACING;
254  } else {
255  specifiedAny = true;
256  }
257 #ifdef DEBUG_TOC
258  std::cout << "Parsed openGapParams: \n"
259  << " timegap=" << timegap
260  << ", spacing=" << spacing
261  << ", changeRate=" << changeRate
262  << ", maxDecel=" << maxDecel
263  << std::endl;
264 #endif
265  return OpenGapParams(timegap, spacing, changeRate, maxDecel, specifiedAny);
266 }
267 
268 // ---------------------------------------------------------------------------
269 // MSDevice_ToC-methods
270 // ---------------------------------------------------------------------------
271 MSDevice_ToC::MSDevice_ToC(SUMOVehicle& holder, const std::string& id, const std::string& outputFilename,
272  const std::string& manualType, const std::string& automatedType, SUMOTime responseTime, double recoveryRate,
273  double lcAbstinence, double initialAwareness, double mrmDecel,
274  double dynamicToCThreshold, double dynamicMRMProbability, double maxPreparationAccel,
275  bool mrmKeepRight, const std::string& mrmSafeSpot, SUMOTime mrmSafeSpotDuration, bool useColorScheme, OpenGapParams ogp) :
276  MSVehicleDevice(holder, id),
277  myManualTypeID(manualType),
278  myAutomatedTypeID(automatedType),
279  myResponseTime(responseTime),
280  myRecoveryRate(recoveryRate),
281  myLCAbstinence(lcAbstinence),
282  myInitialAwareness(initialAwareness),
283  myMRMDecel(mrmDecel),
284  myCurrentAwareness(1.),
285  myUseColorScheme(useColorScheme),
286  myTriggerMRMCommand(nullptr),
287  myTriggerToCCommand(nullptr),
288  myRecoverAwarenessCommand(nullptr),
289  myExecuteMRMCommand(nullptr),
290  myPrepareToCCommand(nullptr),
291  myOutputFile(nullptr),
292  myEvents(),
293  myEventLanes(),
294  myEventXY(),
295  myPreviousLCMode(-1),
296  myOpenGapParams(ogp),
297  myDynamicToCThreshold(dynamicToCThreshold),
298  myMRMProbability(dynamicMRMProbability),
299  myDynamicToCActive(dynamicToCThreshold > 0),
300  myIssuedDynamicToC(false),
301  myDynamicToCLane(-1),
302  myMRMKeepRight(mrmKeepRight),
303  myMRMSafeSpot(mrmSafeSpot),
304  myMRMSafeSpotDuration(mrmSafeSpotDuration),
305  myMaxPreparationAccel(maxPreparationAccel),
306  myOriginalMaxAccel(-1) {
307  // Take care! Holder is currently being constructed. Cast occurs before completion.
308  myHolderMS = static_cast<MSVehicle*>(&holder);
309 
310  if (outputFilename != "") {
311  myOutputFile = &OutputDevice::getDevice(outputFilename);
312  // TODO: make xsd, include header
313  // myOutputFile.writeXMLHeader("ToCDeviceLog", "ToCDeviceLog.xsd");
314  if (createdOutputFiles.count(outputFilename) == 0) {
315  myOutputFile->writeXMLHeader("ToCDeviceLog", "");
316  createdOutputFiles.insert(outputFilename);
317  }
318  }
319 
320  // Check if the given vTypes for the ToC Device are vTypeDistributions
322  const bool automatedVTypeIsDist = vehCtrl.hasVTypeDistribution(myAutomatedTypeID);
323  const bool manualVTypeIsDist = vehCtrl.hasVTypeDistribution(myManualTypeID);
324 
325  // Check if the vType of the holder matches one of the given vTypes
326  std::string holderVTypeID = holder.getVehicleType().getID();
327  if (holderVTypeID == myManualTypeID) {
328  myState = ToCState::MANUAL;
329  } else if (holderVTypeID == myAutomatedTypeID) {
330  myState = ToCState::AUTOMATED;
331  } else if (manualVTypeIsDist && vehCtrl.getVTypeDistributionMembership(holderVTypeID).count(myManualTypeID) > 0) {
332  // Holder type id is from the given manual type distribution.
333  myState = ToCState::MANUAL;
334  myManualTypeID = holderVTypeID;
335  } else if (automatedVTypeIsDist && vehCtrl.getVTypeDistributionMembership(holderVTypeID).count(myAutomatedTypeID) > 0) {
336  // Holder type id is from the given automated type distribution.
337  myState = ToCState::AUTOMATED;
338  myAutomatedTypeID = holderVTypeID;
339  } else {
340  throw ProcessError("Vehicle type of vehicle '" + holder.getID() + "' ('" +
341  holder.getVehicleType().getID() + "') must coincide with manualType ('" +
342  manualType + "') or automatedType ('" + automatedType +
343  "') specified for its ToC-device (or drawn from the specified vTypeDistributions).");
344  }
345  if (!vehCtrl.hasVType(myAutomatedTypeID)) {
346  throw ProcessError("The automated vehicle type '" + myAutomatedTypeID +
347  "' of vehicle '" + holder.getID() + "' is not known.");
348  }
349  if (!vehCtrl.hasVType(myManualTypeID)) {
350  throw ProcessError("The manual vehicle type '" + myManualTypeID +
351  "' of vehicle '" + holder.getID() + "' is not known.");
352  }
353 
354  // Eventually instantiate given vTypes from distributions
355  if (myState == ToCState::MANUAL && automatedVTypeIsDist) {
357  } else if (myState == ToCState::AUTOMATED && manualVTypeIsDist) {
359  }
360 
361  // register at static instance container
362  myInstances.insert(this);
363  initColorScheme();
364 
365 #ifdef DEBUG_TOC
366  std::cout << "initialized device '" << id << "' with "
367  << "outputFilename=" << outputFilename << ", "
368  << "myManualType=" << myManualTypeID << ", "
369  << "myAutomatedType=" << myAutomatedTypeID << ", "
370  << "myResponseTime=" << myResponseTime << ", "
371  << "myRecoveryRate=" << myRecoveryRate << ", "
372  << "myInitialAwareness=" << myInitialAwareness << ", "
373  << "myMRMDecel=" << myMRMDecel << ", "
374  << "ogTimeHeadway=" << myOpenGapParams.newTimeHeadway << ", "
375  << "ogSpaceHeadway=" << myOpenGapParams.newSpaceHeadway << ", "
376  << "ogChangeRate=" << myOpenGapParams.changeRate << ", "
377  << "ogMaxDecel=" << myOpenGapParams.maxDecel << ", "
378  << "ogActive=" << myOpenGapParams.active << ", "
379  << "myCurrentAwareness=" << myCurrentAwareness << ", "
380  << "myState=" << _2string(myState) << std::endl;
381 #endif
382 
383  assert(myInitialAwareness <= 1.0 && myInitialAwareness >= 0.0);
384 }
385 
386 
387 
388 void
390  //RGBColor(red, green, blue)
393  myColorScheme[PREPARING_TOC] = RGBColor(200, 200, 250); // light blue
394  myColorScheme[MRM] = RGBColor(250, 50, 50); // red
395  myColorScheme[RECOVERING] = RGBColor(250, 210, 150); // light yellow
396  myColorScheme[UNDEFINED] = RGBColor(150, 150, 150); // gray
397 }
398 
399 
401  // unregister from static instance container
402  myInstances.erase(this);
403  // deschedule commands associated to this device
404  if (myTriggerMRMCommand != nullptr) {
406  }
407  if (myTriggerToCCommand != nullptr) {
409  }
410  if (myRecoverAwarenessCommand != nullptr) {
412  }
413  if (myExecuteMRMCommand != nullptr) {
416  }
417  if (myPrepareToCCommand != nullptr) {
419  }
420 }
421 
422 void
424  if (value > 1.0 || value < 0.0) {
425  std::stringstream ss;
426  ss << "Truncating invalid value for awareness (" << value << ") to lie in [0,1].";
427  WRITE_WARNING(ss.str());
428  value = MAX2(0.0, MIN2(1.0, value));
429  }
431  // Awareness is now below LC abstinence level -> prevent deliberate LCs
433  } else if (myCurrentAwareness < myLCAbstinence && value >= myLCAbstinence) {
434  // Awareness is now above LC abstinence level -> allow deliberate LCs
436  }
437  myCurrentAwareness = value;
438  myHolderMS->getDriverState()->setAwareness(value);
439 }
440 
441 
442 void
444 #ifdef DEBUG_TOC
445  std::cout << SIMTIME << " MSDevice_ToC::setState()" << std::endl;
446 #endif
447  if (myState == state) {
448  // No state change
449  return;
450  }
451 
452  if (myState == MRM) {
453  // reset the vehicle's maxAccel
456  } else if (myState == PREPARING_TOC) {
457  if (myOpenGapParams.active) {
458  // Deactivate gap control at preparation phase end
460  }
461  if (state != MRM) {
462  // Aborting preparation
465  }
466  } else if (state == PREPARING_TOC || state == MRM) {
467 #ifdef DEBUG_TOC
468  std::cout << " Entering ToC preparation... " << std::endl;
469 #endif
470  // Prevent lane changing during takeover preparation
472  // Store original value of maxAccel for restoring it after preparation phase
474  // Impose acceleration limit during preparation
476  }
477 
478  if (myIssuedDynamicToC) {
479  // Reset dynamic ToC flag
480  myIssuedDynamicToC = false;
481  }
482 
483  myState = state;
484  if (myUseColorScheme) {
485  setVehicleColor();
486  }
487 }
488 
489 void
494 }
495 
496 void
498  // Remove any preparatory process
500  // .. and any recovery process
502  // ... and any pending ToC to manual
503  descheduleToC();
504  // Immediately trigger the MRM process
505  triggerMRM(0);
506 }
507 
508 
509 void
510 MSDevice_ToC::requestToC(SUMOTime timeTillMRM, SUMOTime responseTime) {
511 #ifdef DEBUG_TOC
512  std::cout << SIMTIME << " requestToC() for vehicle '" << myHolder.getID() << "', timeTillMRM=" << timeTillMRM << ", responseTime=" << responseTime << std::endl;
513 #endif
514  if (myState == AUTOMATED) {
515  // Initialize preparation phase
516  if (responseTime == -1000) {
517  // Sample response time from distribution
518  responseTime = TIME2STEPS(sampleResponseTime(STEPS2TIME(timeTillMRM)));
519  }
520 
521  // Schedule ToC Event
524 
525  assert(myExecuteMRMCommand == nullptr);
526  assert(myTriggerMRMCommand == nullptr);
527  if (responseTime > timeTillMRM && myState != MRM) {
528  // Schedule new MRM if driver response time is higher than permitted
531  }
532 
533  // Start ToC preparation process
537  if (myOpenGapParams.active) {
538  // Start gap controller
539  double originalTau = myHolderMS->getCarFollowModel().getHeadwayTime();
543  }
544  // Record event
545  if (generatesOutput()) {
546  myEvents.push(std::make_pair(SIMSTEP, "TOR"));
547  myEventLanes.push(std::make_pair(myHolder.getLane()->getID(), myHolder.getPositionOnLane())); // add lane and lanepos
548  myEventXY.push(std::make_pair(myHolder.getPosition().x(), myHolder.getPosition().y())); // add (x, y) position
549  }
550  } else {
551  // Switch to automated mode is performed immediately
552  if (timeTillMRM > 0.) {
553  std::stringstream ss;
554  ss << "[t=" << SIMTIME << "] Positive transition time (" << timeTillMRM / 1000. << "s.) for upward ToC of vehicle '" << myHolder.getID() << "' is ignored.";
555  WRITE_WARNING(ss.str());
556  }
558  }
559 }
560 
561 
562 SUMOTime
564 #ifdef DEBUG_TOC
565  std::cout << SIMTIME << " triggerMRM() for vehicle '" << myHolder.getID() << "'" << std::endl;
566 #endif
567  // Clear ongoing MRM
568  descheduleMRM();
569 
570  // Start MRM process
571  if (myMRMSafeSpot != "") {
574  if (s == nullptr) {
575  WRITE_WARNING("Ignoring unknown safe spot '" + myMRMSafeSpot + "' for vehicle '" + myHolder.getID() + "'.");
576  } else {
577  stop.parkingarea = myMRMSafeSpot;
578  stop.parking = true;
579  stop.lane = s->getLane().getID();
580  stop.endPos = s->getEndLanePosition();
581  stop.startPos = s->getBeginLanePosition();
584  std::string error;
585  if (!myHolder.addStop(stop, error)) {
586  WRITE_WARNING("Could not set safe spot '" + myMRMSafeSpot + "' for vehicle '" + myHolder.getID() + "'. " + error);
587  }
588  }
589  } else {
592  }
593  if (myState == MANUAL || myState == RECOVERING) {
595  }
596  setState(MRM);
597  setAwareness(1.);
598 
599  // Record event
600  if (generatesOutput()) {
601  myEvents.push(std::make_pair(SIMSTEP, "MRM"));
602  myEventLanes.push(std::make_pair(myHolder.getLane()->getID(), myHolder.getPositionOnLane())); // add lane and lanepos
603  myEventXY.push(std::make_pair(myHolder.getPosition().x(), myHolder.getPosition().y())); // add (x, y) position
604  }
605 
606  return 0;
607 }
608 
609 
610 SUMOTime
612 #ifdef DEBUG_TOC
613  std::cout << SIMTIME << " triggerUpwardToC() for vehicle '" << myHolder.getID() << "'" << std::endl;
614 #endif
615  descheduleToC();
616  // Eventually stop ToC preparation process
618  // Eventually abort MRM
619  descheduleMRM();
620  // Eventually abort awareness recovery process
622 
623  if (myState == MANUAL || myState == RECOVERING) {
625  }
626  setAwareness(1.);
628 
629  // Record event
630  if (generatesOutput()) {
631  myEvents.push(std::make_pair(SIMSTEP, "ToCup"));
632  myEventLanes.push(std::make_pair(myHolder.getLane()->getID(), myHolder.getPositionOnLane())); // add lane and lanepos
633  myEventXY.push(std::make_pair(myHolder.getPosition().x(), myHolder.getPosition().y())); // add (x, y) position
634  }
635 
636  return 0;
637 }
638 
639 
640 SUMOTime
642 #ifdef DEBUG_TOC
643  std::cout << SIMTIME << " triggerDownwardToC() for vehicle '" << myHolder.getID() << "'" << std::endl;
644 #endif
645  descheduleToC();
646  // Eventually stop ToC preparation process
648  // Eventually abort MRM
649  descheduleMRM();
650 
651 #ifdef DEBUG_TOC
652  std::cout << SIMTIME << " Initial awareness after ToC: " << myCurrentAwareness << std::endl;
653 #endif
654 
655  // Start awareness recovery process
659 
660  // @todo: Sample initial awareness
661  double initialAwareness = myInitialAwareness;
662  setAwareness(initialAwareness);
663 
665 
666  // Record event
667  if (generatesOutput()) {
668  myEvents.push(std::make_pair(SIMSTEP, "ToCdown"));
669  myEventLanes.push(std::make_pair(myHolder.getLane()->getID(), myHolder.getPositionOnLane())); // add lane and lanepos
670  myEventXY.push(std::make_pair(myHolder.getPosition().x(), myHolder.getPosition().y())); // add (x, y) position
671  }
672  return 0;
673 }
674 
675 void
677  // Eventually abort scheduled MRM
678  if (myTriggerMRMCommand != nullptr) {
680  myTriggerMRMCommand = nullptr;
681  }
682  // Eventually abort ongoing MRM
683  if (myExecuteMRMCommand != nullptr) {
686  myExecuteMRMCommand = nullptr;
687  }
688 }
689 
690 
691 void
693  if (myTriggerToCCommand != nullptr) {
695  myTriggerToCCommand = nullptr;
696  }
697 }
698 
699 void
701  // Eventually stop ToC preparation process
702  if (myPrepareToCCommand != nullptr) {
704  if (myState != MRM) {
706  }
707  myPrepareToCCommand = nullptr;
708  }
709 }
710 
711 void
713  // Eventually stop ToC preparation process
714  if (myRecoverAwarenessCommand != nullptr) {
716  myRecoverAwarenessCommand = nullptr;
717  }
718 }
719 
720 
721 void
722 MSDevice_ToC::switchHolderType(const std::string& targetTypeID) {
723 #ifdef DEBUG_TOC
724  std::cout << SIMTIME << " Switching type of vehicle '" << myHolder.getID() << "' to '" << targetTypeID << "'" << std::endl;
725 #endif
726  MSVehicleType* targetType = MSNet::getInstance()->getVehicleControl().getVType(targetTypeID);
727  if (targetType == nullptr) {
728  WRITE_ERROR("vType '" + targetType->getID() + "' for vehicle '" + myHolder.getID() + "' is not known.");
729  return;
730  }
731  myHolderMS->replaceVehicleType(targetType);
732 }
733 
734 
735 SUMOTime
737 #ifdef DEBUG_TOC
738  std::cout << SIMTIME << " ToC preparation step for vehicle '" << myHolder.getID() << "'" << std::endl;
739 #endif
740  if (myState == PREPARING_TOC) {
741  return DELTA_T;
742  } else {
743 #ifdef DEBUG_TOC
744  std::cout << SIMTIME << " Aborting ToC preparation for vehicle '" << myHolder.getID() << "'" << std::endl;
745 #endif
747  return 0;
748  }
749 }
750 
751 
752 SUMOTime
755  const double currentSpeed = myHolderMS->getSpeed();
756 #ifdef DEBUG_TOC
757  std::cout << SIMTIME << " MRM step for vehicle '" << myHolder.getID() << "', currentSpeed=" << currentSpeed << std::endl;
758 #endif
759 
760  // Induce slowdown with MRMDecel
761  std::vector<std::pair<SUMOTime, double> > speedTimeLine;
762  const double nextSpeed = MAX2(0., currentSpeed - ACCEL2SPEED(myMRMDecel));
763  speedTimeLine.push_back(std::make_pair(t - DELTA_T, currentSpeed));
764  speedTimeLine.push_back(std::make_pair(t, nextSpeed));
765  myHolderMS->getInfluencer().setSpeedTimeLine(speedTimeLine);
766 
767  if (myMRMKeepRight) {
768  // Try to change to the right
769  std::vector<std::pair<SUMOTime, int> > laneTimeLine;
770  laneTimeLine.push_back(std::make_pair(MSNet::getInstance()->getCurrentTimeStep(), 0));
771  laneTimeLine.push_back(std::make_pair(MSNet::getInstance()->getCurrentTimeStep() + 1, 0));
772  myHolderMS->getInfluencer().setLaneTimeLine(laneTimeLine);
773 // std::cout << "Keeping right..." << std::endl;
774  }
775 
776  if (myState == MRM) {
777  return DELTA_T;
778  } else {
779 #ifdef DEBUG_TOC
780  std::cout << SIMTIME << " Aborting MRM for vehicle '" << myHolder.getID() << "'" << std::endl;
781 #endif
783  return 0;
784  }
785 }
786 
787 
788 SUMOTime
790 #ifdef DEBUG_TOC
791  std::cout << SIMTIME << " Awareness recovery step for vehicle '" << myHolder.getID() << "'" << std::endl;
792 #endif
793  // Proceed with awareness recovery
794  if (myCurrentAwareness < 1.0) {
796  }
797 
798 #ifdef DEBUG_TOC
799  std::cout << SIMTIME << " currentAwareness = " << myCurrentAwareness << std::endl;
800 #endif
801 
802  const bool awarenessRecoveryCompleted = myCurrentAwareness == 1.0;
803  if (awarenessRecoveryCompleted) {
804 #ifdef DEBUG_TOC
805  std::cout << SIMTIME << " Awareness recovery completed for veh '" << myHolder.getID() << "'" << std::endl;
806 #endif
808  myRecoverAwarenessCommand = nullptr;
809  setState(MANUAL);
810  return 0;
811  }
812  return DELTA_T;
813 }
814 
815 bool
817  double /*oldPos*/,
818  double /*newPos*/,
819  double /*newSpeed*/) {
820  if (myState == AUTOMATED && checkDynamicToC()) {
821  // Initiate a ToC
822  // Record event
823  if (generatesOutput()) {
824  myEvents.push(std::make_pair(SIMSTEP, "DYNTOR"));
825  myEventLanes.push(std::make_pair(myHolder.getLane()->getID(), myHolder.getPositionOnLane())); // add lane and lanepos
826  myEventXY.push(std::make_pair(myHolder.getPosition().x(), myHolder.getPosition().y())); // add (x, y) position
827  }
828  // Leadtime for dynamic ToC is proportional to the time assumed for the dynamic ToC threshold
829  const double leadTime = myDynamicToCThreshold * 1000 * DYNAMIC_TOC_LEADTIME_FACTOR;
830  requestToC((SUMOTime) leadTime);
831  // 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)
832  myIssuedDynamicToC = true;
834  } else if (myIssuedDynamicToC && myState == PREPARING_TOC && !checkDynamicToC()) {
835  // Abort dynamic ToC, FIXME: This could abort an externally requested ToC in rare occasions... (needs test)
836  // Record event
837  if (generatesOutput()) {
838  myEvents.push(std::make_pair(SIMSTEP, "DYNTOR"));
839  myEventLanes.push(std::make_pair(myHolder.getLane()->getID(), myHolder.getPositionOnLane())); // add lane and lanepos
840  myEventXY.push(std::make_pair(myHolder.getPosition().x(), myHolder.getPosition().y())); // add (x, y) position
841  }
842  // NOTE: This should not occur if lane changing is prevented during ToC preparation...
843  // TODO: Reset response time to the original value (unnecessary if re-sampling for each call to requestToC)
844  triggerUpwardToC(0);
845  }
846  return true;
847 }
848 
849 std::string
850 MSDevice_ToC::getParameter(const std::string& key) const {
851  if (key == "manualType") {
852  return myManualTypeID;
853  } else if (key == "automatedType") {
854  return myAutomatedTypeID;
855  } else if (key == "responseTime") {
857  } else if (key == "recoveryRate") {
858  return toString(myRecoveryRate);
859  } else if (key == "initialAwareness") {
861  } else if (key == "mrmDecel") {
862  return toString(myMRMDecel);
863  } else if (key == "currentAwareness") {
865  } else if (key == "lcAbstinence") {
866  return toString(myLCAbstinence);
867  } else if (key == "state") {
868  return _2string(myState);
869  } else if (key == "holder") {
870  return myHolder.getID();
871  } else if (key == "hasDynamicToC") {
873  } else if (key == "dynamicToCThreshold") {
875  } else if (key == "dynamicMRMProbability") {
876  return toString(myMRMProbability);
877  } else if (key == "mrmKeepRight") {
878  return toString(myMRMKeepRight);
879  } else if (key == "mrmSafeSpot") {
880  return myMRMSafeSpot;
881  } else if (key == "mrmSafeSpotDuration") {
883  } else if (key == "maxPreparationAccel") {
885  } else if (key == "ogNewTimeHeadway") {
887  } else if (key == "ogNewSpaceHeadway") {
889  } else if (key == "ogChangeRate") {
891  } else if (key == "ogMaxDecel") {
893  }
894  throw InvalidArgument("Parameter '" + key + "' is not supported for device of type '" + deviceName() + "'");
895 }
896 
897 
898 void
899 MSDevice_ToC::setParameter(const std::string& key, const std::string& value) {
900 #ifdef DEBUG_TOC
901  std::cout << SIMTIME << " MSDevice_ToC::setParameter(key=" << key << ", value=" << value << ")" << std::endl;
902 #endif
903  if (key == "manualType") {
904  myManualTypeID = value;
906  if (myState == MANUAL) {
907  switchHolderType(value);
908  }
909  } else if (key == "automatedType") {
910  myAutomatedTypeID = value;
912  if (myState == AUTOMATED || myState == PREPARING_TOC || myState == MRM) {
913  switchHolderType(value);
914  }
915  } else if (key == "responseTime") {
917  } else if (key == "recoveryRate") {
919  } else if (key == "initialAwareness") {
921  } else if (key == "lcAbstinence") {
923  if (isManuallyDriven()) {
924  setAwareness(myCurrentAwareness); // to eventually trigger LC-prevention
925  }
926  } else if (key == "currentAwareness") {
927  if (isManuallyDriven()) {
929  } else {
930  WRITE_WARNING("Setting device.toc.currentAwareness during automated mode has no effect.")
931  }
932  } else if (key == "mrmDecel") {
934  } else if (key == "requestToC") {
935  // setting this magic parameter gives the interface for inducing a ToC
936  const SUMOTime timeTillMRM = TIME2STEPS(StringUtils::toDouble(value));
937  requestToC(timeTillMRM, myResponseTime);
938  } else if (key == "requestMRM") {
939  // setting this magic parameter gives the interface for inducing an MRM
940  requestMRM();
941  } else if (key == "awareness") {
942  // setting this magic parameter gives the interface for setting the driverstate's awareness
944  } else if (key == "dynamicToCThreshold") {
945  const double newValue = StringUtils::toDouble(value);
946  if (newValue < 0) {
947  WRITE_WARNING("Value of dynamicToCThreshold must be non-negative. (Given value " + value + " for vehicle " + myHolderMS->getID() + " is ignored)");
948  } else if (newValue == 0) {
949  myDynamicToCThreshold = newValue;
950  myDynamicToCActive = false;
951  } else {
952  myDynamicToCThreshold = newValue;
953  myDynamicToCActive = true;
954  }
955  } else if (key == "dynamicMRMProbability") {
956  const double newValue = StringUtils::toDouble(value);
957  if (newValue < 0) {
958  WRITE_WARNING("Value of dynamicMRMProbability must be non-negative. (Given value " + value + " for vehicle " + myHolderMS->getID() + " is ignored)");
959  } else {
960  myMRMProbability = newValue;
961  }
962  } else if (key == "mrmKeepRight") {
963  const bool newValue = StringUtils::toBool(value);
964  myMRMKeepRight = newValue;
965  } else if (key == "mrmSafeSpot") {
966  myMRMSafeSpot = value;
967  } else if (key == "mrmSafeSpotDuration") {
969  } else if (key == "maxPreparationAccel") {
970  const double newValue = StringUtils::toDouble(value);
971  if (newValue < 0) {
972  WRITE_WARNING("Value of maxPreparationAccel must be non-negative. (Given value " + value + " for vehicle " + myHolderMS->getID() + " is ignored)");
973  } else {
974  myMaxPreparationAccel = newValue;
975  }
976  } else {
977  throw InvalidArgument("Parameter '" + key + "' is not supported for device of type '" + deviceName() + "'");
978  }
979 }
980 
981 
983 MSDevice_ToC::_2ToCState(const std::string& str) {
984  if (str == "UNDEFINED") {
985  return UNDEFINED;
986  } else if (str == "MANUAL") {
987  return MANUAL;
988  } else if (str == "AUTOMATED") {
989  return AUTOMATED;
990  } else if (str == "PREPARING_TOC") {
991  return PREPARING_TOC;
992  } else if (str == "MRM") {
993  return MRM;
994  } else if (str == "RECOVERING") {
995  return RECOVERING;
996  } else {
997  WRITE_WARNING("Unknown ToCState '" + str + "'");
998  return UNDEFINED;
999  }
1000 }
1001 
1002 
1003 std::string
1005  if (state == UNDEFINED) {
1006  return "UNDEFINED";
1007  } else if (state == MANUAL) {
1008  return "MANUAL";
1009  } else if (state == AUTOMATED) {
1010  return "AUTOMATED";
1011  } else if (state == PREPARING_TOC) {
1012  return "PREPARING_TOC";
1013  } else if (state == MRM) {
1014  return "MRM";
1015  } else if (state == RECOVERING) {
1016  return "RECOVERING";
1017  } else {
1018  WRITE_WARNING("Unknown ToCState '" + toString(state) + "'");
1019  return toString(state);
1020  }
1021 }
1022 
1023 
1024 void
1026  if (!generatesOutput()) {
1027  assert(myEvents.empty());
1028  return;
1029  }
1030  while (!myEvents.empty()) {
1031  std::pair<SUMOTime, std::string>& e = myEvents.front();
1032  std::pair<std::string, double>& l = myEventLanes.front();
1033  std::pair<double, double>& p = myEventXY.front();
1034  myOutputFile->openTag(e.second);
1035  myOutputFile->writeAttr("id", myHolder.getID()).writeAttr("t", STEPS2TIME(e.first));
1036  myOutputFile->writeAttr("lane", l.first).writeAttr("lanePos", l.second);
1037  myOutputFile->writeAttr("x", p.first).writeAttr("y", p.second);
1039  myEvents.pop();
1040  myEventLanes.pop();
1041  myEventXY.pop();
1042 
1043  if (e.second.compare("DYNTOR") == 0 && !myEvents.empty()) { // skip "TOR" events if duplicate of "DYNTOR"
1044  std::pair<SUMOTime, std::string>& eNext = myEvents.front();
1045  if (eNext.second.compare("TOR") == 0 && eNext.first == e.first) {
1046  myEvents.pop();
1047  myEventLanes.pop();
1048  myEventXY.pop();
1049  }
1050  }
1051  }
1052 }
1053 
1054 
1055 void
1057  // Close xml bodies for all existing files
1058  // TODO: Check if required
1059  for (auto& fn : createdOutputFiles) {
1061  file->closeTag();
1062  }
1063 }
1064 
1065 
1066 void
1068  if (myPreviousLCMode != -1) {
1070 #ifdef DEBUG_TOC
1071  std::cout << SIMTIME << " MSDevice_ToC::resetLCMode() restoring LC Mode of vehicle '" << myHolder.getID() << "' to " << myPreviousLCMode << std::endl;
1072 #endif
1073  }
1074  myPreviousLCMode = -1;
1075 }
1076 
1077 
1078 void
1080  const int lcModeHolder = myHolderMS->getInfluencer().getLaneChangeMode();
1081  if (lcModeHolder != LCModeMRM) {
1082  myPreviousLCMode = lcModeHolder;
1083 #ifdef DEBUG_TOC
1084  std::cout << SIMTIME << " MSDevice_ToC::setLCModeMRM() setting LC Mode of vehicle '" << myHolder.getID()
1085  << "' from " << myPreviousLCMode << " to " << LCModeMRM << std::endl;
1086 #endif
1088  }
1089 }
1090 
1091 bool
1093  return (myState == MANUAL || myState == RECOVERING);
1094 }
1095 
1096 bool
1098  return (myState == AUTOMATED || myState == PREPARING_TOC || myState == MRM);
1099 }
1100 
1101 bool
1103 #ifdef DEBUG_DYNAMIC_TOC
1104  std::cout << SIMTIME << " # MSDevice_ToC::checkDynamicToC() for veh '" << myHolder.getID() << "'" << std::endl;
1105 #endif
1106  if (!myDynamicToCActive) {
1107  return false;
1108  }
1109  // The vehicle's current lane
1110  const MSLane* currentLane = myHolderMS->getLane();
1111 
1112  if (currentLane->isInternal()) {
1113  // Don't start or abort dynamic ToCs on internal lanes
1114  return myIssuedDynamicToC;
1115  }
1116 
1117  if (myIssuedDynamicToC) {
1118 #ifdef DEBUG_DYNAMIC_TOC
1119  std::cout << SIMTIME << " Dynamic ToC is ongoing." << std::endl;
1120 #endif
1121  // Dynamic ToC in progress. Resist to aborting it if lane was not changed.
1122  if (myDynamicToCLane == currentLane->getNumericalID()) {
1123  return true;
1124  }
1125  }
1126  // Length for which the current route can be followed
1127  const std::vector<MSVehicle::LaneQ>& bestLanes = myHolderMS->getBestLanes();
1128  // Maximal distance for route continuation without LCs over the possible start lanes
1129  double maximalContinuationDistance = 0;
1130  // Distance for route continuation without LCs from the vehicle's current lane
1131  double continuationDistanceOnCurrent = 0;
1132  // Lane of the next stop
1133  const MSLane* nextStopLane = nullptr;
1134 
1135  if (myHolderMS->hasStops()) {
1136  nextStopLane = myHolderMS->getNextStop().lane;
1137  }
1138  for (auto& i : bestLanes) {
1139  maximalContinuationDistance = MAX2(maximalContinuationDistance, i.length);
1140  if (currentLane == i.lane) {
1141  if (myHolderMS->hasStops()) {
1142  // Check if the next stop lies on the route continuation from the current lane
1143  for (MSLane* l : i.bestContinuations) {
1144  if (l == nextStopLane) {
1145 #ifdef DEBUG_DYNAMIC_TOC
1146  std::cout << SIMTIME << " Stop found on the route continuation from the current lane. => No ToC" << std::endl;
1147 #endif
1148  // Stop found on the route continuation from the current lane => no ToC necessary
1149  return false;
1150  }
1151  }
1152  }
1153  continuationDistanceOnCurrent = i.length;
1154  }
1155  }
1156  if (continuationDistanceOnCurrent == maximalContinuationDistance) {
1157  // There is no better lane than the current, hence no desire to change lanes,
1158  // which the driver could pursue better than the automation => no reason for ToC.
1159  return false;
1160  }
1161  const double distFromCurrent = continuationDistanceOnCurrent - myHolderMS->getPositionOnLane();
1162  const double MRMDist = 0.5 * myHolderMS->getSpeed() * myHolderMS->getSpeed() / MAX2(myMRMDecel, 0.0001);
1163  double distThreshold = myHolderMS->getSpeed() * myDynamicToCThreshold + MRMDist;
1164 #ifdef DEBUG_DYNAMIC_TOC
1165  std::cout << " speed=" << myHolderMS->getSpeed()
1166  << ", distFromCurrent=" << distFromCurrent
1167  << ", maximal dist=" << maximalContinuationDistance - myHolderMS->getPositionOnLane()
1168  << ", distThreshold=" << distThreshold
1169  << std::endl;
1170 #endif
1171 
1172  if (myIssuedDynamicToC) {
1173  // In case of an ongoing ToC, add an additional resistance to abort it.
1174  // (The lane-check above does not capture lanes subsequent to the dynamic ToC lane)
1175  distThreshold *= DYNAMIC_TOC_ABORT_RESISTANCE_FACTOR;
1176  }
1177 
1178  if (distFromCurrent < distThreshold) {
1179  // TODO: Make this more sophisticated in dealing with low speeds/stops and route ends
1180 #ifdef DEBUG_DYNAMIC_TOC
1181  std::cout << SIMTIME << " * distAlongBest is below threshold! *" << std::endl;
1182 #endif
1183  return true;
1184  }
1185 
1186  return false;
1187 }
1188 
1189 double
1190 MSDevice_ToC::sampleResponseTime(double leadTime) const {
1191 #ifdef DEBUG_DYNAMIC_TOC
1192  std::cout << "sampleResponseTime() leadTime=" << leadTime << std::endl;
1193 #endif
1194  const double mean = responseTimeMean(leadTime);
1195  const double var = interpolateVariance(leadTime, myMRMProbability);
1196  std::normal_distribution<double> d(mean, var);
1197  double rt = d(myResponseTimeRNG);
1198 #ifdef DEBUG_DYNAMIC_TOC
1199  std::cout << " mean=" << mean << ", variance=" << var << " => sampled responseTime=" << rt << std::endl;
1200 #endif
1201  int it_count = 0;
1202  while (rt < 0 && it_count < MAX_RESPONSETIME_SAMPLE_TRIES) {
1203  rt = d(myResponseTimeRNG);
1204  it_count++;
1205  }
1206  if (rt < 0) {
1207  // Didn't generate a positive random response time => use mean
1208  rt = mean;
1209  }
1210  return rt;
1211 }
1212 
1213 double
1214 MSDevice_ToC::interpolateVariance(double leadTime, double pMRM) {
1215 #ifdef DEBUG_DYNAMIC_TOC
1216  std::cout << "interpolateVariance() leadTime=" << leadTime << ", pMRM=" << pMRM << std::endl;
1217 #endif
1218  // Calculate indices for surrounding values in lookup tables
1219 
1220  // Find largest p_{i-1} < pMRM < p_{i}
1221  const auto pi = std::lower_bound(lookupResponseTimeMRMProbs.begin(), lookupResponseTimeMRMProbs.end(), pMRM);
1222  if (pi == lookupResponseTimeMRMProbs.end()) {
1223  // requested probability lies outside lookup table.
1224  // => return maximal variance value
1226  }
1227  const size_t pi1 = pi - lookupResponseTimeMRMProbs.begin();
1228  assert(pi1 > 0);
1229  const size_t pi0 = pi1 - 1;
1230  const double cp = (pMRM - * (pi - 1)) / (*pi - * (pi - 1));
1231 
1232 #ifdef DEBUG_DYNAMIC_TOC
1233  std::cout << " p[=" << pi0 << "]=" << *(pi - 1) << ", p[=" << pi1 << "]=" << *pi << " => cp=" << cp << std::endl;
1234 #endif
1235 
1236  // Find largest p_{i-1} < pMRM < p_{i}
1237  auto li = std::lower_bound(lookupResponseTimeLeadTimes.begin(), lookupResponseTimeLeadTimes.end(), leadTime);
1238  if (li == lookupResponseTimeLeadTimes.begin()) {
1239  // Given lead time smaller than minimal lookup-value.
1240  // Use minimal value from lookup table instead
1241  leadTime = *li;
1242  li = lookupResponseTimeLeadTimes.begin() + 1;
1243  } else if (li == lookupResponseTimeLeadTimes.end()) {
1244  // Given leadTime exceeds values in lookup table
1245  // => induce extrapolation
1246  li--;
1247  }
1248  const size_t li1 = li - lookupResponseTimeLeadTimes.begin();
1249  const size_t li0 = li1 - 1;
1250  const double cl = (leadTime - * (li - 1)) / (*li - * (li - 1));
1251 
1252 #ifdef DEBUG_DYNAMIC_TOC
1253  std::cout << " l[=" << li0 << "]=" << *(li - 1) << ", l[=" << li1 << "]=" << *li << " => cp=" << cl << std::endl;
1254 #endif
1255 
1256  // 2D interpolation for variance
1257  // First, interpolate (or extrapolate) variances along leadTimes
1258  const double var00 = lookupResponseTimeVariances[pi0][li0];
1259  const double var01 = lookupResponseTimeVariances[pi0][li1];
1260  const double var10 = lookupResponseTimeVariances[pi1][li0];
1261  const double var11 = lookupResponseTimeVariances[pi1][li1];
1262  const double var_0 = var00 + (var01 - var00) * cl;
1263  const double var_1 = var10 + (var11 - var10) * cl;
1264  // From these, interpolate along the pMRM-axis
1265  const double var = var_0 + (var_1 - var_0) * cp;
1266 #ifdef DEBUG_DYNAMIC_TOC
1267  std::cout << " var00=" << var00 << ", var01=" << var01 << " var10=" << var10 << ", var11=" << var11
1268  << " var_0=" << var_0 << ", var_1=" << var_1 << ", var=" << var << std::endl;
1269 #endif
1270  return var;
1271 }
1272 
1273 // Grid of the response time distribution.
1274 // Generated by the script generateResponseTimeDistributions.py, see Appendix to TransAID Deliverable 3.1v2.
1275 // Probability for an MRM to occur (start with 0.0, end with 0.5)
1276 std::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};
1277 // Lead time grid
1278 std::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};
1279 
1280 // Variances of the response time distribution.
1281 std::vector<std::vector<double> > MSDevice_ToC::lookupResponseTimeVariances = {
1282  {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},
1283  {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},
1284  {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},
1285  {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},
1286  {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},
1287  {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},
1288  {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},
1289  {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},
1290  {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},
1291  {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},
1292  {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},
1293 };
1294 
1295 
1296 /****************************************************************************/
#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_ERROR(msg)
Definition: MsgHandler.h:284
#define WRITE_WARNING(msg)
Definition: MsgHandler.h:276
SUMOTime DELTA_T
Definition: SUMOTime.cpp:37
#define STEPS2TIME(x)
Definition: SUMOTime.h:53
#define SIMSTEP
Definition: SUMOTime.h:59
#define ACCEL2SPEED(x)
Definition: SUMOTime.h:49
#define TS
Definition: SUMOTime.h:40
#define SIMTIME
Definition: SUMOTime.h:60
#define TIME2STEPS(x)
Definition: SUMOTime.h:55
long long int SUMOTime
Definition: SUMOTime.h:31
const int VEHPARS_COLOR_SET
@ SUMO_TAG_PARKING_AREA
A parking area.
T MIN2(T a, T b)
Definition: StdDefs.h:73
T MAX2(T a, T b)
Definition: StdDefs.h:79
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:44
MSVehicleType & getSingularType()
Replaces the current vehicle type with a new one used by this vehicle only.
void replaceVehicleType(MSVehicleType *type)
Replaces the current vehicle type by the one given.
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:466
double getMaxAccel() const
Get the vehicle type's maximum acceleration [m/s^2].
Definition: MSCFModel.h:208
virtual double getHeadwayTime() const
Get the driver's desired headway [s].
Definition: MSCFModel.h:257
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:365
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)
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
static std::mt19937 myResponseTimeRNG
Random generator for ToC devices.
Definition: MSDevice_ToC.h:372
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)
Definition: MSDevice.cpp:198
static double getFloatParam(const SUMOVehicle &v, const OptionsCont &oc, std::string paramName, double deflt, bool required)
Definition: MSDevice.cpp:185
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:134
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:204
static std::string getStringParam(const SUMOVehicle &v, const OptionsCont &oc, std::string paramName, std::string deflt, bool required)
Definition: MSDevice.cpp:161
virtual void addEvent(Command *operation, SUMOTime execTimeStep=-1)
Adds an Event.
static bool gUseMesoSim
Definition: MSGlobals.h:88
Representation of a lane in the micro simulation.
Definition: MSLane.h:82
bool isInternal() const
Definition: MSLane.cpp:2036
int getNumericalID() const
Returns this lane's numerical id.
Definition: MSLane.h:468
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
Definition: MSNet.cpp:171
MSVehicleControl & getVehicleControl()
Returns the vehicle control.
Definition: MSNet.h:371
MSStoppingPlace * getStoppingPlace(const std::string &id, const SumoXMLTag category) const
Returns the named stopping place of the given category.
Definition: MSNet.cpp:1098
MSEventControl * getBeginOfTimestepEvents()
Returns the event control for events executed at the begin of a time step.
Definition: MSNet.h:464
static std::mt19937 * 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:772
void deactivateGapController()
Deactivates the gap control.
Definition: MSVehicle.cpp:405
void setLaneTimeLine(const std::vector< std::pair< SUMOTime, int > > &laneTimeLine)
Sets a new lane timeline.
Definition: MSVehicle.cpp:412
int getLaneChangeMode() const
return the current lane change mode
Definition: MSVehicle.cpp:441
void setSpeedTimeLine(const std::vector< std::pair< SUMOTime, double > > &speedTimeLine)
Sets a new velocity timeline.
Definition: MSVehicle.cpp:391
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:397
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, std::mt19937 *rng=nullptr)
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:6404
Influencer & getInfluencer()
Definition: MSVehicle.cpp:6059
double getSpeed() const
Returns the vehicle's current speed.
Definition: MSVehicle.h:458
const std::vector< LaneQ > & getBestLanes() const
Returns the description of best lanes to use in order to continue the route.
Definition: MSVehicle.cpp:4711
double getPositionOnLane() const
Get the vehicle's position along the lane.
Definition: MSVehicle.h:374
const MSLane * getLane() const
Returns the lane the vehicle is on.
Definition: MSVehicle.h:550
MSStop & getNextStop()
Definition: MSVehicle.cpp:6046
const MSCFModel & getCarFollowModel() const
Returns the vehicle's car following model definition.
Definition: MSVehicle.h:930
The car-following model and parameter.
Definition: MSVehicleType.h:62
void setDecel(double decel)
Set a new value for this type's deceleration.
const RGBColor & getColor() const
Returns this type's color.
const std::string & getID() const
Returns the name of the vehicle type.
Definition: MSVehicleType.h:90
const MSCFModel & getCarFollowModel() const
Returns the vehicle type's car following model definition (const version)
const SUMOVTypeParameter & getParameter() const
const std::string & getID() const
Returns the id.
Definition: Named.h:73
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.
void doRegister(const std::string &name, Option *v)
Adds an option under the given name.
Definition: OptionsCont.cpp:75
std::string getString(const std::string &name) const
Returns the string-value of the named option (only for Option_String)
void addOptionSubTopic(const std::string &topic)
Adds an option subtopic.
static OptionsCont & getOptions()
Retrieves the options.
Definition: OptionsCont.cpp:58
Static storage of an output device and its base (abstract) implementation.
Definition: OutputDevice.h:60
bool writeXMLHeader(const std::string &rootElement, const std::string &schemaFile, std::map< SumoXMLAttr, std::string > attrs=std::map< SumoXMLAttr, std::string >())
Writes an XML header with optional configuration.
OutputDevice & openTag(const std::string &xmlElement)
Opens an XML tag.
OutputDevice & writeAttr(const SumoXMLAttr attr, const T &val)
writes a named attribute
Definition: OutputDevice.h:239
bool closeTag(const std::string &comment="")
Closes the most recently opened tag and optionally adds a comment.
static OutputDevice & getDevice(const std::string &name)
Returns the described OutputDevice.
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:54
double y() const
Returns the y-position.
Definition: Position.h:59
Representation of a vehicle, person, or container.
virtual const MSVehicleType & getVehicleType() const =0
Returns the object's "vehicle" type.
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 const MSLane * getLane() const =0
Returns the lane the object is currently at.
virtual double getPositionOnLane() const =0
Get the object's position along the lane.
Representation of a vehicle.
Definition: SUMOVehicle.h:58
virtual bool addStop(const SUMOVehicleParameter::Stop &stopPar, std::string &errorMsg, SUMOTime untilOffset=0, bool collision=false, ConstMSEdgeVector::const_iterator *searchStart=0)=0
Adds a stop.
Definition of vehicle stop (position and duration)
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.
bool parking
whether the vehicle is removed from the net while stopping
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:82