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-2018 German Aerospace Center (DLR) and others.
4 // This program and the accompanying materials
5 // are made available under the terms of the Eclipse Public License v2.0
6 // which accompanies this distribution, and is available at
7 // http://www.eclipse.org/legal/epl-v20.html
8 // SPDX-License-Identifier: EPL-2.0
9 /****************************************************************************/
18 // The ToC Device controls the transition of control between automated and manual driving.
19 //
20 /****************************************************************************/
21 
22 // ===========================================================================
23 // included modules
24 // ===========================================================================
25 #include <config.h>
26 
27 #include <memory>
32 #include <utils/common/RGBColor.h>
33 #include <microsim/MSNet.h>
34 #include <microsim/MSVehicle.h>
38 #include <microsim/MSDriverState.h>
39 #include "MSDevice_ToC.h"
40 
41 
42 // ===========================================================================
43 // debug constants
44 // ===========================================================================
45 //#define DEBUG_TOC
46 
47 
48 // ===========================================================================
49 // parameter defaults
50 // ===========================================================================
51 
52 // default value for the average response time, that a driver needs to take back control
53 #define DEFAULT_RESPONSE_TIME 5.0
54 // default value for the average rate at which the driver's awareness recovers to
55 // 1.0 after a ToC has been performed
56 #define DEFAULT_RECOVERY_RATE 0.1
57 // Default value of the awareness below which no lane-changes are performed
58 #define DEFAULT_LCABSTINENCE 0.0
59 // The default value for the average awareness a driver has initially after a ToC
60 #define DEFAULT_INITIAL_AWARENESS 0.5
61 // The default value for the deceleration rate applied during a 'minimum risk maneuver'
62 #define DEFAULT_MRM_DECEL 1.5
63 
64 // The default values for the openGap parameters applied for gap creation in preparation for a ToC
65 #define DEFAULT_OPENGAP_TIMEGAP -1.0
66 #define DEFAULT_OPENGAP_SPACING 0.0
67 #define DEFAULT_OPENGAP_CHANGERATE 1.0
68 #define DEFAULT_OPENGAP_MAXDECEL 1.0
69 
70 
71 #define DEFAULT_MANUAL_TYPE ""
72 #define DEFAULT_AUTOMATED_TYPE ""
73 
74 
75 
76 
77 // ---------------------------------------------------------------------------
78 // static members
79 // ---------------------------------------------------------------------------
80 std::set<MSDevice_ToC*> MSDevice_ToC::instances = std::set<MSDevice_ToC*>();
81 std::set<std::string> MSDevice_ToC::createdOutputFiles;
82 int MSDevice_ToC::LCModeMRM = 768; // = 0b001100000000 - no autonomous changes, no speed adaptation
83 
84 // ===========================================================================
85 // method definitions
86 // ===========================================================================
87 // ---------------------------------------------------------------------------
88 // static initialisation methods
89 // ---------------------------------------------------------------------------
90 void
92  oc.addOptionSubTopic("ToC Device");
93  insertDefaultAssignmentOptions("toc", "ToC Device", oc);
94 
95  oc.doRegister("device.toc.manualType", new Option_String());
96  oc.addDescription("device.toc.manualType", "ToC Device", "Vehicle type for manual driving regime.");
97  oc.doRegister("device.toc.automatedType", new Option_String());
98  oc.addDescription("device.toc.automatedType", "ToC Device", "Vehicle type for automated driving regime.");
99  oc.doRegister("device.toc.responseTime", new Option_Float(DEFAULT_RESPONSE_TIME));
100  oc.addDescription("device.toc.responseTime", "ToC Device", "Average response time needed by a driver to take back control.");
101  oc.doRegister("device.toc.recoveryRate", new Option_Float(DEFAULT_RECOVERY_RATE));
102  oc.addDescription("device.toc.recoveryRate", "ToC Device", "Recovery rate for the driver's awareness after a ToC.");
103  oc.doRegister("device.toc.lcAbstinence", new Option_Float(DEFAULT_LCABSTINENCE));
104  oc.addDescription("device.toc.lcAbstinence", "ToC Device", "Attention level below which a driver restrains from performing lane changes (value in [0,1]).");
105  oc.doRegister("device.toc.initialAwareness", new Option_Float(DEFAULT_INITIAL_AWARENESS));
106  oc.addDescription("device.toc.initialAwareness", "ToC Device", "Average awareness a driver has initially after a ToC (value in [0,1]).");
107  oc.doRegister("device.toc.mrmDecel", new Option_Float(DEFAULT_MRM_DECEL));
108  oc.addDescription("device.toc.mrmDecel", "ToC Device", "Deceleration rate applied during a 'minimum risk maneuver'.");
109  oc.doRegister("device.toc.ogNewTimeHeadway", new Option_Float(-1.0));
110  oc.addDescription("device.toc.ogNewTimeHeadway", "ToC Device", "Timegap for ToC preparation phase.");
111  oc.doRegister("device.toc.ogNewSpaceHeadway", new Option_Float(-1.0));
112  oc.addDescription("device.toc.ogNewSpaceHeadway", "ToC Device", "Additional spacing for ToC preparation phase.");
113  oc.doRegister("device.toc.ogMaxDecel", new Option_Float(-1.0));
114  oc.addDescription("device.toc.ogMaxDecel", "ToC Device", "Maximal deceleration applied for establishing increased gap in ToC preparation phase.");
115  oc.doRegister("device.toc.ogChangeRate", new Option_Float(-1.0));
116  oc.addDescription("device.toc.ogChangeRate", "ToC Device", "Rate of adaptation towards the increased headway during ToC preparation.");
117  oc.doRegister("device.toc.useColorScheme", new Option_Bool(true));
118  oc.addDescription("device.toc.useColorScheme", "ToC Device", "Whether a coloring scheme shall by applied to indicate the different ToC stages.");
119  oc.doRegister("device.toc.file", new Option_String());
120  oc.addDescription("device.toc.file", "ToC Device", "Switches on output by specifying an output filename.");
121 }
122 
123 
124 void
125 MSDevice_ToC::buildVehicleDevices(SUMOVehicle& v, std::vector<MSVehicleDevice*>& into) {
127  if (equippedByDefaultAssignmentOptions(oc, "toc", v, false)) {
128  std::string manualType = getManualType(v, oc);
129  std::string automatedType = getAutomatedType(v, oc);
130  SUMOTime responseTime = TIME2STEPS(getResponseTime(v, oc));
131  double recoveryRate = getRecoveryRate(v, oc);
132  double lcAbstinence = getLCAbstinence(v, oc);
133  double initialAwareness = getInitialAwareness(v, oc);
134  double mrmDecel = getMRMDecel(v, oc);
135  bool useColoring = useColorScheme(v, oc);
136  std::string deviceID = "toc_" + v.getID();
137  std::string file = getOutputFilename(v, oc);
138  OpenGapParams ogp = getOpenGapParams(v, oc);
139  // build the device
140  MSDevice_ToC* device = new MSDevice_ToC(v, deviceID, file,
141  manualType, automatedType, responseTime, recoveryRate,
142  lcAbstinence, initialAwareness, mrmDecel, useColoring, ogp);
143  into.push_back(device);
144  }
145 }
146 
147 
148 std::string
150  // Default of "" means no output
151  std::string file = "";
152  if (v.getParameter().knowsParameter("device.toc.file")) {
153  try {
154  file = v.getParameter().getParameter("device.toc.file", file);
155  } catch (...) {
156  WRITE_WARNING("Invalid value '" + v.getParameter().getParameter("device.toc.file", file) + "'for vehicle parameter 'ssm.measures'");
157  }
158  } else if (v.getVehicleType().getParameter().knowsParameter("device.toc.file")) {
159  try {
160  file = v.getVehicleType().getParameter().getParameter("device.toc.file", file);
161  } catch (...) {
162  WRITE_WARNING("Invalid value '" + v.getVehicleType().getParameter().getParameter("device.toc.file", file) + "'for vType parameter 'ssm.measures'");
163  }
164  } else {
165  file = oc.getString("device.toc.file") == "" ? file : oc.getString("device.toc.file");
166  }
167  return file;
168 }
169 
170 std::string
172  return getStringParam(v, oc, "toc.manualType", DEFAULT_MANUAL_TYPE, true);
173 }
174 
175 std::string
177  return getStringParam(v, oc, "toc.automatedType", DEFAULT_AUTOMATED_TYPE, true);
178 }
179 
180 double
182  return getFloatParam(v, oc, "toc.responseTime", DEFAULT_RESPONSE_TIME, false);
183 }
184 
185 double
187  return getFloatParam(v, oc, "toc.recoveryRate", DEFAULT_RECOVERY_RATE, false);
188 }
189 
190 double
192  return getFloatParam(v, oc, "toc.lcAbstinence", DEFAULT_LCABSTINENCE, false);
193 }
194 
195 double
197  return getFloatParam(v, oc, "toc.initialAwareness", DEFAULT_INITIAL_AWARENESS, false);
198 }
199 
200 double
202  return getFloatParam(v, oc, "toc.mrmDecel", DEFAULT_MRM_DECEL, false);
203 }
204 
205 bool
207  return getBoolParam(v, oc, "toc.useColorScheme", "false", false);
208 }
209 
212  double timegap = getFloatParam(v, oc, "toc.ogNewTimeHeadway", -1.0, false);
213  double spacing = getFloatParam(v, oc, "toc.ogNewSpaceHeadway", -1.0, false);
214  double changeRate = getFloatParam(v, oc, "toc.ogChangeRate", -1.0, false);
215  double maxDecel = getFloatParam(v, oc, "toc.ogMaxDecel", -1.0, false);
216 
217  bool specifiedAny = false;
218  if (changeRate == -1.0) {
219  changeRate = DEFAULT_OPENGAP_CHANGERATE;
220  } else {
221  specifiedAny = true;
222  }
223  if (maxDecel == -1.0) {
224  maxDecel = DEFAULT_OPENGAP_MAXDECEL;
225  } else {
226  specifiedAny = true;
227  }
228  if (specifiedAny && timegap == -1 && spacing == -1) {
229  WRITE_ERROR("If any openGap parameters for the ToC model are specified, then at least one of ogTimeGap and ogSpacing must be defined.")
230  }
231  if (timegap == -1) {
232  timegap = DEFAULT_OPENGAP_TIMEGAP;
233  } else {
234  specifiedAny = true;
235  }
236  if (spacing == -1) {
237  spacing = DEFAULT_OPENGAP_SPACING;
238  } else {
239  specifiedAny = true;
240  }
241 
242 
243 #ifdef DEBUG_TOC
244  std::cout << "Parsed openGapParams: \n"
245  << " timegap=" << timegap
246  << ", spacing=" << spacing
247  << ", changeRate=" << changeRate
248  << ", maxDecel=" << maxDecel
249  << std::endl;
250 #endif
251 
252  return OpenGapParams(timegap, spacing, changeRate, maxDecel, specifiedAny);
253 }
254 
255 // ---------------------------------------------------------------------------
256 // MSDevice_ToC-methods
257 // ---------------------------------------------------------------------------
258 MSDevice_ToC::MSDevice_ToC(SUMOVehicle& holder, const std::string& id, const std::string& outputFilename,
259  std::string manualType, std::string automatedType, SUMOTime responseTime, double recoveryRate,
260  double lcAbstinence, double initialAwareness, double mrmDecel, bool useColoring, OpenGapParams ogp) :
261  MSVehicleDevice(holder, id),
262  myManualTypeID(manualType),
263  myAutomatedTypeID(automatedType),
264  myResponseTime(responseTime),
265  myRecoveryRate(recoveryRate),
266  myLCAbstinence(lcAbstinence),
267  myInitialAwareness(initialAwareness),
268  myMRMDecel(mrmDecel),
269  myCurrentAwareness(1.),
270  myUseColorScheme(useColoring),
271  myTriggerMRMCommand(nullptr),
272  myTriggerToCCommand(nullptr),
273  myRecoverAwarenessCommand(nullptr),
274  myExecuteMRMCommand(nullptr),
275  myPrepareToCCommand(nullptr),
276  myOutputFile(nullptr),
277  myEvents(),
278  myPreviousLCMode(-1),
279  myOpenGapParams(ogp) {
280  // Take care! Holder is currently being constructed. Cast occurs before completion.
281  myHolderMS = static_cast<MSVehicle*>(&holder);
282  // Ensure that the holder receives a driver state as soon as it is created (can't be done here, since myHolderMS is incomplete)
284 
285  if (outputFilename != "") {
286  myOutputFile = &OutputDevice::getDevice(outputFilename);
287  // TODO: make xsd, include header
288  // myOutputFile.writeXMLHeader("ToCDeviceLog", "ToCDeviceLog.xsd");
289  if (createdOutputFiles.count(outputFilename) == 0) {
290  myOutputFile->openTag("ToCDeviceLog");
291  createdOutputFiles.insert(outputFilename);
292  }
293  }
294  // register at static instance container
295  instances.insert(this);
296 
297  // Check if the given vTypes for the ToC Device are vTypeDistributions
299  const bool automatedVTypeIsDist = vehCtrl.hasVTypeDistribution(myAutomatedTypeID);
300  const bool manualVTypeIsDist = vehCtrl.hasVTypeDistribution(myManualTypeID);
301 
302  // Check if the vType of the holder matches one of the given vTypes
303  std::string holderVTypeID = holder.getVehicleType().getID();
304  if (holderVTypeID == myManualTypeID) {
305  myState = ToCState::MANUAL;
306  } else if (holderVTypeID == myAutomatedTypeID) {
307  myState = ToCState::AUTOMATED;
308  } else if (manualVTypeIsDist && holderVTypeID.find(myManualTypeID) == 0) {
309  // Holder type id starts with type distribution name.
310  // We assume that this means that it is from the given distribution.
311  myState = ToCState::MANUAL;
312  myManualTypeID = holderVTypeID;
313  } else if (automatedVTypeIsDist && holderVTypeID.find(myAutomatedTypeID) == 0) {
314  // Holder type id starts with type distribution name.
315  // We assume that this means that it is from the given distribution.
316  myState = ToCState::AUTOMATED;
317  myAutomatedTypeID = holderVTypeID;
318  } else {
319  throw ProcessError("Vehicle type of vehicle '" + holder.getID() + "' ('" + holder.getVehicleType().getID()
320  + "') must coincide with manualType ('" + manualType + "') or automatedType ('" + automatedType
321  + "') specified for its ToC-device (or drawn from the specified vTypeDistributions).");
322  }
323 
324  // Eventually instantiate given vTypes from distributions
325  if (myState == ToCState::MANUAL && automatedVTypeIsDist) {
327  } else if (myState == ToCState::AUTOMATED && manualVTypeIsDist) {
329  }
330 
331  initColorScheme();
332 
333 #ifdef DEBUG_TOC
334  std::cout << "initialized device '" << id << "' with "
335  << "outputFilename=" << outputFilename << ", "
336  << "myManualType=" << myManualTypeID << ", "
337  << "myAutomatedType=" << myAutomatedTypeID << ", "
338  << "myResponseTime=" << myResponseTime << ", "
339  << "myRecoveryRate=" << myRecoveryRate << ", "
340  << "myInitialAwareness=" << myInitialAwareness << ", "
341  << "myMRMDecel=" << myMRMDecel << ", "
342  << "ogTimeHeadway=" << myOpenGapParams.newTimeHeadway << ", "
343  << "ogSpaceHeadway=" << myOpenGapParams.newSpaceHeadway << ", "
344  << "ogChangeRate=" << myOpenGapParams.changeRate << ", "
345  << "ogMaxDecel=" << myOpenGapParams.maxDecel << ", "
346  << "ogActive=" << myOpenGapParams.active << ", "
347  << "myCurrentAwareness=" << myCurrentAwareness << ", "
348  << "myState=" << _2string(myState) << std::endl;
349 #endif
350 
351  assert(myInitialAwareness <= 1.0 && myInitialAwareness >= 0.0);
352 }
353 
354 
355 
356 void
358  //RGBColor(red, green, blue)
361  myColorScheme[PREPARING_TOC] = RGBColor(200, 200, 250); // light blue
362  myColorScheme[MRM] = RGBColor(250, 50, 50); // red
363  myColorScheme[RECOVERING] = RGBColor(250, 210, 150); // light yellow
364  myColorScheme[UNDEFINED] = RGBColor(150, 150, 150); // gray
365 }
366 
367 
368 SUMOTime
370 #ifdef DEBUG_TOC
371  std::cout << SIMTIME << " ensureDriverStateExistence() for vehicle '" << myHolder.getID() << "'" << std::endl;
372 #endif
373  // Ensure that the holder has a driver state
374  if (myHolderMS->getDriverState() == nullptr
375  && !myHolderMS->hasDevice("driverstate")) {
376  // Create an MSDriverState for the vehicle if it hasn't one already,
377  // and has no DriverState Device attached (that will create the driver state, then)
379  }
380  return 0;
381 }
382 
384  // unregister from static instance container
385  instances.erase(this);
386  // deschedule commands associated to this device
387  if (myTriggerMRMCommand != nullptr) {
389  }
390  if (myTriggerToCCommand != nullptr) {
392  }
393  if (myRecoverAwarenessCommand != nullptr) {
395  }
396  if (myExecuteMRMCommand != nullptr) {
399  }
400  if (myPrepareToCCommand != nullptr) {
402  }
403 }
404 
405 void
407  if (value > 1.0 || value < 0.0) {
408  std::stringstream ss;
409  ss << "Truncating invalid value for awareness (" << value << ") to lie in [0,1].";
410  WRITE_WARNING(ss.str());
411  value = MAX2(0.0, MIN2(1.0, value));
412  }
414  // Awareness is now below LC abstinence level -> prevent deliberate LCs
416  } else if (myCurrentAwareness < myLCAbstinence && value >= myLCAbstinence) {
417  // Awareness is now above LC abstinence level -> allow deliberate LCs
419  }
420  myCurrentAwareness = value;
421  std::shared_ptr<MSSimpleDriverState> ds = myHolderMS->getDriverState();
422  ds->setAwareness(value);
423 }
424 
425 
426 void
428  if (myOpenGapParams.active && myState == PREPARING_TOC && state != PREPARING_TOC) {
429  // Deactivate gap control at preparation phase end
431  }
432 
433  myState = state;
434  if (myUseColorScheme) {
435  setVehicleColor();
436  }
437 }
438 
439 void
444 }
445 
446 void
448  // Remove any preparatory process
450  // .. and any recovery process
452  // ... and any pending ToC to manual
453  descheduleToC();
454  // Immediately trigger the MRM process
455  triggerMRM(0);
456 }
457 
458 
459 void
461 #ifdef DEBUG_TOC
462  std::cout << SIMTIME << " requestToC() for vehicle '" << myHolder.getID() << "' , timeTillMRM=" << timeTillMRM << std::endl;
463 #endif
464  if (myState == AUTOMATED) {
465  // Initialize preparation phase
466 
467  // @todo: Sample response time from distribution
468  SUMOTime responseTime = myResponseTime;
469 
470  // Schedule ToC Event
473 
474  // Clear eventually prior scheduled MRM
475 // descheduleMRM();
476  assert(myExecuteMRMCommand == nullptr);
477  assert(myTriggerMRMCommand == nullptr);
478  if (responseTime > timeTillMRM) {
479  // Schedule new MRM if driver response time is higher than permitted
482  }
483 
484  // Start ToC preparation process
488  if (myOpenGapParams.active) {
489  // Start gap controller
490  double originalTau = myHolderMS->getCarFollowModel().getHeadwayTime();
494  }
495  // Record event
496  if (generatesOutput()) {
497  myEvents.push(std::make_pair(SIMSTEP, "TOR"));
498  }
499  } else {
500  // Switch to automated mode is performed immediately
501  // Note that the transition MRM/PREPARING_TOC->AUTOMATED, where a downward ToC is aborted, is handled here as well.
502  if (timeTillMRM > 0.) {
503  std::stringstream ss;
504  ss << "[t=" << SIMTIME << "] Positive transition time (" << timeTillMRM / 1000. << "s.) for upward ToC of vehicle '" << myHolder.getID() << "' is ignored.";
505  WRITE_WARNING(ss.str());
506  }
508  }
509 }
510 
511 
512 SUMOTime
514 #ifdef DEBUG_TOC
515  std::cout << SIMTIME << " triggerMRM() for vehicle '" << myHolder.getID() << "'" << std::endl;
516 #endif
517  // Clear ongoing MRM
518  descheduleMRM();
519 
520  // Start MRM process
523  setState(MRM);
525  setAwareness(1.);
527 
528  // Record event
529  if (generatesOutput()) {
530  myEvents.push(std::make_pair(SIMSTEP, "MRM"));
531  }
532 
533  return 0;
534 }
535 
536 
537 SUMOTime
539 #ifdef DEBUG_TOC
540  std::cout << SIMTIME << " triggerUpwardToC() for vehicle '" << myHolder.getID() << "'" << std::endl;
541 #endif
542  descheduleToC();
543  // Eventually stop ToC preparation process
545  // Eventually abort MRM
546  descheduleMRM();
547  // Eventually abort awareness recovery process
549 
551  setAwareness(1.);
553 
554  // Record event
555  if (generatesOutput()) {
556  myEvents.push(std::make_pair(SIMSTEP, "ToCup"));
557  }
558 
559  return 0;
560 }
561 
562 
563 SUMOTime
565 #ifdef DEBUG_TOC
566  std::cout << SIMTIME << " triggerDownwardToC() for vehicle '" << myHolder.getID() << "'" << std::endl;
567 #endif
568  descheduleToC();
569  // Eventually stop ToC preparation process
571  // Eventually abort MRM
572  descheduleMRM();
573 
575 
576  // @todo: Sample initial awareness
577  double initialAwareness = myInitialAwareness;
578  setAwareness(initialAwareness);
579 
580 #ifdef DEBUG_TOC
581  std::cout << SIMTIME << " Initial awareness after ToC: " << myCurrentAwareness << std::endl;
582 #endif
583 
584  // Start awareness recovery process
588 
589  // Record event
590  if (generatesOutput()) {
591  myEvents.push(std::make_pair(SIMSTEP, "ToCdown"));
592  }
593  return 0;
594 }
595 
596 void
598  // Eventually abort scheduled MRM
599  if (myTriggerMRMCommand != nullptr) {
601  myTriggerMRMCommand = nullptr;
602  }
603  // Eventually abort ongoing MRM
604  if (myExecuteMRMCommand != nullptr) {
607  myExecuteMRMCommand = nullptr;
608  }
609 }
610 
611 
612 void
614  if (myTriggerToCCommand != nullptr) {
616  myTriggerToCCommand = nullptr;
617  }
618 }
619 
620 void
622  // Eventually stop ToC preparation process
623  if (myPrepareToCCommand != nullptr) {
625  myPrepareToCCommand = nullptr;
626  }
627 }
628 
629 void
631  // Eventually stop ToC preparation process
632  if (myRecoverAwarenessCommand != nullptr) {
634  myRecoverAwarenessCommand = nullptr;
635  }
636 }
637 
638 
639 void
640 MSDevice_ToC::switchHolderType(const std::string& targetTypeID) {
641 #ifdef DEBUG_TOC
642  std::cout << SIMTIME << " Switching type of vehicle '" << myHolder.getID() << "' to '" << targetTypeID << "'" << std::endl;
643 #endif
644  MSVehicleType* targetType = MSNet::getInstance()->getVehicleControl().getVType(targetTypeID);
645  if (targetType == nullptr) {
646  WRITE_ERROR("vType '" + targetType->getID() + "' for vehicle '" + myHolder.getID() + "' is not known.");
647  return;
648  }
649  myHolderMS->replaceVehicleType(targetType);
650 }
651 
652 
653 SUMOTime
655 #ifdef DEBUG_TOC
656  std::cout << SIMTIME << " ToC preparation step for vehicle '" << myHolder.getID() << "'" << std::endl;
657 #endif
658  // TODO: Devise preparation of ToC (still needs discussion). At least: do not overtake. Perhaps, increase gap to leader.
659 
660  if (myState == PREPARING_TOC) {
661  return DELTA_T;
662  } else {
663 #ifdef DEBUG_TOC
664  std::cout << SIMTIME << " Aborting ToC preparation for vehicle '" << myHolder.getID() << "'" << std::endl;
665 #endif
667  return 0;
668  }
669 }
670 
671 
672 SUMOTime
675  const double currentSpeed = myHolderMS->getSpeed();
676 #ifdef DEBUG_TOC
677  std::cout << SIMTIME << " MRM step for vehicle '" << myHolder.getID() << "', currentSpeed=" << currentSpeed << std::endl;
678 #endif
679 
680  // Induce slowdown with MRMDecel
681  std::vector<std::pair<SUMOTime, double> > speedTimeLine;
682  const double nextSpeed = MAX2(0., currentSpeed - ACCEL2SPEED(myMRMDecel));
683  speedTimeLine.push_back(std::make_pair(t - DELTA_T, currentSpeed));
684  speedTimeLine.push_back(std::make_pair(t, nextSpeed));
685  myHolderMS->getInfluencer().setSpeedTimeLine(speedTimeLine);
686 
687  if (myState == MRM) {
688  return DELTA_T;
689  } else {
690 #ifdef DEBUG_TOC
691  std::cout << SIMTIME << " Aborting MRM for vehicle '" << myHolder.getID() << "'" << std::endl;
692 #endif
694  return 0;
695  }
696 }
697 
698 
699 SUMOTime
701 #ifdef DEBUG_TOC
702  std::cout << SIMTIME << " Awareness recovery step for vehicle '" << myHolder.getID() << "'" << std::endl;
703 #endif
704  // Proceed with awareness recovery
705  if (myCurrentAwareness < 1.0) {
707  }
708 
709 #ifdef DEBUG_TOC
710  std::cout << SIMTIME << " currentAwareness = " << myCurrentAwareness << std::endl;
711 #endif
712 
713  const bool awarenessRecoveryCompleted = myCurrentAwareness == 1.0;
714  if (awarenessRecoveryCompleted) {
715 #ifdef DEBUG_TOC
716  std::cout << SIMTIME << " Awareness recovery completed for veh '" << myHolder.getID() << "'" << std::endl;
717 #endif
719  myRecoverAwarenessCommand = nullptr;
720  setState(MANUAL);
721  return 0;
722  }
723  return DELTA_T;
724 }
725 
726 
727 std::string
728 MSDevice_ToC::getParameter(const std::string& key) const {
729  if (key == "manualType") {
730  return myManualTypeID;
731  } else if (key == "automatedType") {
732  return myAutomatedTypeID;
733  } else if (key == "responseTime") {
735  } else if (key == "recoveryRate") {
736  return toString(myRecoveryRate);
737  } else if (key == "initialAwareness") {
739  } else if (key == "mrmDecel") {
740  return toString(myMRMDecel);
741  } else if (key == "currentAwareness") {
743  } else if (key == "lcAbstinence") {
744  return toString(myLCAbstinence);
745  } else if (key == "state") {
746  return _2string(myState);
747  } else if (key == "holder") {
748  return myHolder.getID();
749  }
750  throw InvalidArgument("Parameter '" + key + "' is not supported for device of type '" + deviceName() + "'");
751 }
752 
753 
754 void
755 MSDevice_ToC::setParameter(const std::string& key, const std::string& value) {
756 #ifdef DEBUG_TOC
757  std::cout << "MSDevice_ToC::setParameter(key=" << key << ", value=" << value << ")" << std::endl;
758 #endif
759  if (key == "manualType") {
760  myManualTypeID = value;
762  if (myState == MANUAL) {
763  switchHolderType(value);
764  }
765  } else if (key == "automatedType") {
766  myAutomatedTypeID = value;
768  if (myState == AUTOMATED || myState == PREPARING_TOC || myState == MRM) {
769  switchHolderType(value);
770  }
771  } else if (key == "responseTime") {
773  } else if (key == "recoveryRate") {
775  } else if (key == "initialAwareness") {
777  } else if (key == "lcAbstinence") {
779  if (isManuallyDriven()) {
780  setAwareness(myCurrentAwareness); // to eventually trigger LC-prevention
781  }
782  } else if (key == "currentAwareness") {
783  if (isManuallyDriven()) {
785  } else {
786  WRITE_WARNING("Setting device.toc.currentAwareness during automated mode has no effect.")
787  }
788  } else if (key == "mrmDecel") {
790  } else if (key == "requestToC") {
791  // setting this magic parameter gives the interface for inducing a ToC
792  const SUMOTime timeTillMRM = TIME2STEPS(StringUtils::toDouble(value));
793  requestToC(timeTillMRM);
794  } else if (key == "requestMRM") {
795  // setting this magic parameter gives the interface for inducing an MRM
796  requestMRM();
797  } else if (key == "awareness") {
798  // setting this magic parameter gives the interface for setting the driverstate's awareness
800  } else {
801  throw InvalidArgument("Parameter '" + key + "' is not supported for device of type '" + deviceName() + "'");
802  }
803 }
804 
805 
807 MSDevice_ToC::_2ToCState(const std::string& str) {
808  if (str == "UNDEFINED") {
809  return UNDEFINED;
810  } else if (str == "MANUAL") {
811  return MANUAL;
812  } else if (str == "AUTOMATED") {
813  return AUTOMATED;
814  } else if (str == "PREPARING_TOC") {
815  return PREPARING_TOC;
816  } else if (str == "MRM") {
817  return MRM;
818  } else if (str == "RECOVERING") {
819  return RECOVERING;
820  } else {
821  WRITE_WARNING("Unknown ToCState '" + str + "'");
822  return UNDEFINED;
823  }
824 }
825 
826 
827 std::string
829  if (state == UNDEFINED) {
830  return "UNDEFINED";
831  } else if (state == MANUAL) {
832  return "MANUAL";
833  } else if (state == AUTOMATED) {
834  return "AUTOMATED";
835  } else if (state == PREPARING_TOC) {
836  return "PREPARING_TOC";
837  } else if (state == MRM) {
838  return "MRM";
839  } else if (state == RECOVERING) {
840  return "RECOVERING";
841  } else {
842  WRITE_WARNING("Unknown ToCState '" + toString(state) + "'");
843  return toString(state);
844  }
845 }
846 
847 
848 void
850  if (!generatesOutput()) {
851  assert(myEvents.empty());
852  return;
853  }
854  while(!myEvents.empty()) {
855  std::pair<SUMOTime, std::string>& e = myEvents.front();
856  myOutputFile->openTag(e.second);
857  myOutputFile->writeAttr("id", myHolder.getID()).writeAttr("t", STEPS2TIME(e.first));
859  myEvents.pop();
860  }
861 }
862 
863 
864 void
866  // Close xml bodies for all existing files
867  // TODO: Check if required
868  for (auto& fn : createdOutputFiles) {
870  file->closeTag();
871  }
872 }
873 
874 
875 void
877  if (myPreviousLCMode != -1) {
879 #ifdef DEBUG_TOC
880  std::cout << "MSDevice_ToC::resetLCMode() restoring LC Mode of vehicle '" << myHolder.getID() << "' to " << myPreviousLCMode << std::endl;
881 #endif
882  }
883  myPreviousLCMode = -1;
884 }
885 
886 
887 void
889  const int lcModeHolder = myHolderMS->getInfluencer().getLaneChangeMode();
890  if (lcModeHolder != LCModeMRM) {
891  myPreviousLCMode = lcModeHolder;
892 #ifdef DEBUG_TOC
893  std::cout << "MSDevice_ToC::setLCModeMRM() setting LC Mode of vehicle '" << myHolder.getID()
894  << "' from " << myPreviousLCMode << " to " << LCModeMRM << std::endl;
895 #endif
896  }
898 }
899 
900 bool
902  return (myState == MANUAL || myState == RECOVERING);
903 }
904 
905 bool
907  return (myState == AUTOMATED || myState == PREPARING_TOC || myState == MRM);
908 }
909 
910 /****************************************************************************/
911 
static bool getBoolParam(const SUMOVehicle &v, const OptionsCont &oc, std::string paramName, bool deflt, bool required)
Definition: MSDevice.cpp:226
int myPreviousLCMode
LC mode overridden during MRM, stored for restoration.
Definition: MSDevice_ToC.h:305
static int LCModeMRM
LC mode operational during an MRM.
Definition: MSDevice_ToC.h:308
void doRegister(const std::string &name, Option *v)
Adds an option under the given name.
Definition: OptionsCont.cpp:75
OutputDevice & writeAttr(const SumoXMLAttr attr, const T &val)
writes a named attribute
Definition: OutputDevice.h:256
RGBColor color
The vehicle&#39;s color, TraCI may change this.
static std::string getManualType(const SUMOVehicle &v, const OptionsCont &oc)
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:79
long long int SUMOTime
Definition: SUMOTime.h:36
void descheduleRecovery()
Remove ongoing awareness recovery process from the event-queue.
ToCState myState
Current state of the device.
Definition: MSDevice_ToC.h:281
std::queue< std::pair< SUMOTime, std::string > > myEvents
Storage for events to be written to the output.
Definition: MSDevice_ToC.h:302
#define ACCEL2SPEED(x)
Definition: SUMOTime.h:54
bool isAutomated()
Whether the current operation mode is automated.
static void insertOptions(OptionsCont &oc)
Inserts MSDevice_ToC-options.
static double getLCAbstinence(const SUMOVehicle &v, const OptionsCont &oc)
static double getRecoveryRate(const SUMOVehicle &v, const OptionsCont &oc)
SUMOTime MRMExecutionStep(SUMOTime t)
Continue the MRM for one time step.
bool isManuallyDriven()
Whether the current operation mode is manual.
void descheduleToCPreparation()
Remove ongoing ToC-Preparation process from the event-queue.
bool hasVTypeDistribution(const std::string &id) const
Asks for a vehicle type distribution.
static ToCState _2ToCState(const std::string &)
static std::set< MSDevice_ToC * > instances
Definition: MSDevice_ToC.h:56
int parametersSet
Information for the router which parameter were set, TraCI may modify this (whe changing color) ...
#define DEFAULT_OPENGAP_CHANGERATE
void descheduleMRM()
Break MRM Process or remove MRM-Trigger command from the event-queue.
SUMOTime triggerUpwardToC(SUMOTime t)
Trigger execution of a ToC X–>AUTOMATED ("upwards")
SUMOVehicle & myHolder
The vehicle that stores the device.
static OpenGapParams getOpenGapParams(const SUMOVehicle &v, const OptionsCont &oc)
#define DEFAULT_LCABSTINENCE
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
Definition: MSNet.cpp:165
T MAX2(T a, T b)
Definition: StdDefs.h:76
void createDriverState()
Create a DriverState for the vehicle.
Definition: MSVehicle.cpp:5735
SUMOTime DELTA_T
Definition: SUMOTime.cpp:35
WrappingCommand< MSDevice_ToC > * myExecuteMRMCommand
Definition: MSDevice_ToC.h:294
#define DEFAULT_OPENGAP_TIMEGAP
static double getResponseTime(const SUMOVehicle &v, const OptionsCont &oc)
static std::set< std::string > createdOutputFiles
Definition: MSDevice_ToC.h:58
#define DEFAULT_OPENGAP_MAXDECEL
void initColorScheme()
Initialize vehicle colors for different states.
static std::string _2string(ToCState state)
SUMOTime triggerDownwardToC(SUMOTime t)
Trigger execution of a ToC X–>MANUAL ("downwards")
static bool useColorScheme(const SUMOVehicle &v, const OptionsCont &oc)
int getLaneChangeMode() const
return the current lane change mode
Definition: MSVehicle.cpp:346
#define TIME2STEPS(x)
Definition: SUMOTime.h:60
#define TS
Definition: SUMOTime.h:45
std::string myAutomatedTypeID
vehicle type ID for automated driving
Definition: MSDevice_ToC.h:257
void requestToC(SUMOTime timeTillMRM)
Request a ToC. If the device is in AUTOMATED or MRM state, a driver response time is sampled and the ...
void requestMRM()
Request an MRM to be initiated immediately. No downward ToC will be scheduled.
#define DEFAULT_INITIAL_AWARENESS
void deactivateDeliberateLCs()
Resets the holder&#39;s LC mode to the operational LC-mode of the ToC Device (.
#define WRITE_WARNING(msg)
Definition: MsgHandler.h:241
The car-following model and parameter.
Definition: MSVehicleType.h:66
MSDevice_ToC(SUMOVehicle &holder, const std::string &id, const std::string &outputFilename, std::string manualType, std::string automatedType, SUMOTime responseTime, double recoveryRate, double lcAbstinence, double initialAwareness, double mrmDecel, bool useColorScheme, OpenGapParams ogp)
Constructor.
static OptionsCont & getOptions()
Retrieves the options.
Definition: OptionsCont.cpp:58
static void buildVehicleDevices(SUMOVehicle &v, std::vector< MSVehicleDevice *> &into)
Build devices for the given vehicle, if needed.
#define SIMTIME
Definition: SUMOTime.h:65
#define DEFAULT_OPENGAP_SPACING
static std::mt19937 * getParsingRNG()
WrappingCommand< MSDevice_ToC > * myTriggerMRMCommand
Definition: MSDevice_ToC.h:291
static void cleanup()
Closes root tags of output files.
void setSpeedTimeLine(const std::vector< std::pair< SUMOTime, double > > &speedTimeLine)
Sets a new velocity timeline.
Definition: MSVehicle.cpp:305
static std::string getOutputFilename(const SUMOVehicle &v, const OptionsCont &oc)
void setAwareness(double value)
Set the awareness to the given value.
const MSCFModel & getCarFollowModel() const
Returns the vehicle&#39;s car following model definition.
Definition: MSVehicle.h:891
bool knowsParameter(const std::string &key) const
Returns whether the parameter is known.
SUMOTime ensureDriverStateExistence(SUMOTime)
Ensure existence of DriverState for equipped vehicles.
bool myUseColorScheme
Whether a coloring scheme shall by applied to indicate the different toc stages,. ...
Definition: MSDevice_ToC.h:278
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:49
#define DEFAULT_MANUAL_TYPE
Representation of a vehicle.
Definition: SUMOVehicle.h:60
OutputDevice * myOutputFile
The file the devices output goes to.
Definition: MSDevice_ToC.h:299
double myLCAbstinence
Level of the awareness below which no lane-changes are performed.
Definition: MSDevice_ToC.h:264
static double toDouble(const std::string &sData)
converts a string into the double value described by it by calling the char-type converter ...
ToCState
Enum describing the different regimes for the device,.
Definition: MSDevice_ToC.h:109
MSVehicleControl & getVehicleControl()
Returns the vehicle control.
Definition: MSNet.h:316
#define SIMSTEP
Definition: SUMOTime.h:64
MSEventControl * getBeginOfTimestepEvents()
Returns the event control for events executed at the begin of a time step.
Definition: MSNet.h:409
std::string getString(const std::string &name) const
Returns the string-value of the named option (only for Option_String)
virtual void addEvent(Command *operation, SUMOTime execTimeStep=-1)
Adds an Event.
std::string getParameter(const std::string &key) const
try to retrieve the given parameter from this device. Throw exception for unsupported key ...
#define STEPS2TIME(x)
Definition: SUMOTime.h:58
MSVehicle * myHolderMS
The holder vehicle casted to MSVehicle*.
Definition: MSDevice_ToC.h:286
static double getFloatParam(const SUMOVehicle &v, const OptionsCont &oc, std::string paramName, double deflt, bool required)
Definition: MSDevice.cpp:193
T MIN2(T a, T b)
Definition: StdDefs.h:70
SUMOTime myResponseTime
Average response time needed by the driver to take back control.
Definition: MSDevice_ToC.h:260
void addOptionSubTopic(const std::string &topic)
Adds an option subtopic.
static double getMRMDecel(const SUMOVehicle &v, const OptionsCont &oc)
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:121
#define DEFAULT_AUTOMATED_TYPE
std::string myManualTypeID
vehicle type ID for manual driving
Definition: MSDevice_ToC.h:255
double myCurrentAwareness
Current awareness-level of the driver in [0,1].
Definition: MSDevice_ToC.h:272
const int VEHPARS_COLOR_SET
const SUMOVTypeParameter & getParameter() const
#define DEFAULT_MRM_DECEL
const RGBColor & getColor() const
Returns this type&#39;s color.
static double getInitialAwareness(const SUMOVehicle &v, const OptionsCont &oc)
SUMOTime ToCPreparationStep(SUMOTime t)
Continue the ToC preparation for one time step.
MSVehicleType * getVType(const std::string &id=DEFAULT_VTYPE_ID, std::mt19937 *rng=0)
Returns the named vehicle type or a sample from the named distribution.
#define WRITE_ERROR(msg)
Definition: MsgHandler.h:247
Influencer & getInfluencer()
Returns the velocity/lane influencer.
Definition: MSVehicle.cpp:5488
void activateGapController(double originalTau, double newTimeHeadway, double newSpaceHeadway, double duration, double changeRate, double maxDecel)
Activates the gap control with the given parameters,.
Definition: MSVehicle.cpp:311
Structure representing possible vehicle parameter.
static std::string getStringParam(const SUMOVehicle &v, const OptionsCont &oc, std::string paramName, std::string deflt, bool required)
Definition: MSDevice.cpp:160
void deschedule()
Marks this Command as being descheduled.
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:208
virtual double getHeadwayTime() const
Get the driver&#39;s desired headway [s].
Definition: MSCFModel.h:259
static OutputDevice & getDevice(const std::string &name)
Returns the described OutputDevice.
virtual const SUMOVehicleParameter & getParameter() const =0
Returns the vehicle&#39;s parameter (including departure definition)
void setVehicleColor()
A storage for options typed value containers)
Definition: OptionsCont.h:92
const std::string & getID() const
Returns the name of the vehicle type.
Definition: MSVehicleType.h:94
const std::string getParameter(const std::string &key, const std::string &defaultValue="") const
Returns the value for a given key.
Abstract in-vehicle device.
void setState(ToCState state)
Set the ToC device&#39;s state.
void descheduleToC()
Remove scheduled ToC-Trigger command from the event-queue.
void setLaneChangeMode(int value)
Sets lane changing behavior.
Definition: MSVehicle.cpp:653
#define DEFAULT_RECOVERY_RATE
double myRecoveryRate
Recovery rate for the driver&#39;s awareness after a ToC.
Definition: MSDevice_ToC.h:262
Static storage of an output device and its base (abstract) implementation.
Definition: OutputDevice.h:64
bool closeTag(const std::string &comment="")
Closes the most recently opened tag and optionally adds a comment.
WrappingCommand< MSDevice_ToC > * myRecoverAwarenessCommand
Definition: MSDevice_ToC.h:293
std::shared_ptr< MSSimpleDriverState > getDriverState() const
Returns the vehicle driver&#39;s state.
Definition: MSVehicle.h:900
bool hasDevice(const std::string &deviceName) const
check whether the vehicle is equiped with a device of the given type
SUMOTime awarenessRecoveryStep(SUMOTime t)
Continue the awareness recovery for one time step.
~MSDevice_ToC()
Destructor.
void resetDeliberateLCs()
Resets the holder&#39;s LC mode to the last differing to LCModeMRM.
WrappingCommand< MSDevice_ToC > * myTriggerToCCommand
Definition: MSDevice_ToC.h:292
The class responsible for building and deletion of vehicles.
void addDescription(const std::string &name, const std::string &subtopic, const std::string &description)
Adds a description for an option.
double getSpeed() const
Returns the vehicle&#39;s current speed.
Definition: MSVehicle.h:483
const std::string deviceName() const
return the name for this type of device
Definition: MSDevice_ToC.h:141
void switchHolderType(const std::string &targetTypeID)
Switch the device holder&#39;s vehicle type.
void setParameter(const std::string &key, const std::string &value)
try to set the given parameter for this device. Throw exception for unsupported key ...
static double fn[10]
Definition: odrSpiral.cpp:82
void deactivateGapController()
Deactivates the gap control.
Definition: MSVehicle.cpp:319
OpenGapParams myOpenGapParams
Parameters for the openGap mechanism applied during ToC preparation phase.
Definition: MSDevice_ToC.h:311
void replaceVehicleType(MSVehicleType *type)
Replaces the current vehicle type by the one given.
virtual const std::string & getID() const =0
Get the vehicle&#39;s ID.
SUMOTime triggerMRM(SUMOTime t)
Trigger execution of an MRM.
WrappingCommand< MSDevice_ToC > * myPrepareToCCommand
Definition: MSDevice_ToC.h:295
OutputDevice & openTag(const std::string &xmlElement)
Opens an XML tag.
std::map< ToCState, RGBColor > myColorScheme
Coloring scheme,.
Definition: MSDevice_ToC.h:275
#define DEFAULT_RESPONSE_TIME
bool generatesOutput()
Whether this device requested to write output.
Definition: MSDevice_ToC.h:177
The ToC Device controls transition of control between automated and manual driving.
Definition: MSDevice_ToC.h:53
void writeOutput()
Write output to file given by option device.toc.file.
virtual const MSVehicleType & getVehicleType() const =0
Returns the vehicle&#39;s type.
double myMRMDecel
Deceleration rate applied during MRM.
Definition: MSDevice_ToC.h:269
double myInitialAwareness
Average awareness the driver has initially after a ToC.
Definition: MSDevice_ToC.h:266
static std::string getAutomatedType(const SUMOVehicle &v, const OptionsCont &oc)