1 /****************************************************************************/
2 // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3 // Copyright (C) 2013-2019 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 /****************************************************************************/
10 /// @file MSDevice_ToC.cpp
11 /// @author Leonhard Luecken
12 /// @author Daniel Krajzewicz
13 /// @author Michael Behrisch
14 /// @author Jakob Erdmann
15 /// @date 01.04.2018
16 /// @version $Id$
17 ///
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>
28 #include <utils/common/StringUtils.h>
29 #include <utils/options/OptionsCont.h>
30 #include <utils/vehicle/SUMOVehicle.h>
31 #include <utils/common/WrappingCommand.h>
32 #include <utils/common/RGBColor.h>
33 #include <microsim/MSNet.h>
34 #include <microsim/MSVehicle.h>
35 #include <microsim/MSRouteHandler.h>
36 #include <microsim/MSVehicleControl.h>
37 #include <microsim/MSEventControl.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
insertOptions(OptionsCont & oc)91 MSDevice_ToC::insertOptions(OptionsCont& oc) {
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
buildVehicleDevices(SUMOVehicle & v,std::vector<MSVehicleDevice * > & into)125 MSDevice_ToC::buildVehicleDevices(SUMOVehicle& v, std::vector<MSVehicleDevice*>& into) {
126 OptionsCont& oc = OptionsCont::getOptions();
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
getOutputFilename(const SUMOVehicle & v,const OptionsCont & oc)149 MSDevice_ToC::getOutputFilename(const SUMOVehicle& v, const OptionsCont& oc) {
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
getManualType(const SUMOVehicle & v,const OptionsCont & oc)171 MSDevice_ToC::getManualType(const SUMOVehicle& v, const OptionsCont& oc) {
172 return getStringParam(v, oc, "toc.manualType", DEFAULT_MANUAL_TYPE, true);
173 }
174
175 std::string
getAutomatedType(const SUMOVehicle & v,const OptionsCont & oc)176 MSDevice_ToC::getAutomatedType(const SUMOVehicle& v, const OptionsCont& oc) {
177 return getStringParam(v, oc, "toc.automatedType", DEFAULT_AUTOMATED_TYPE, true);
178 }
179
180 double
getResponseTime(const SUMOVehicle & v,const OptionsCont & oc)181 MSDevice_ToC::getResponseTime(const SUMOVehicle& v, const OptionsCont& oc) {
182 return getFloatParam(v, oc, "toc.responseTime", DEFAULT_RESPONSE_TIME, false);
183 }
184
185 double
getRecoveryRate(const SUMOVehicle & v,const OptionsCont & oc)186 MSDevice_ToC::getRecoveryRate(const SUMOVehicle& v, const OptionsCont& oc) {
187 return getFloatParam(v, oc, "toc.recoveryRate", DEFAULT_RECOVERY_RATE, false);
188 }
189
190 double
getLCAbstinence(const SUMOVehicle & v,const OptionsCont & oc)191 MSDevice_ToC::getLCAbstinence(const SUMOVehicle& v, const OptionsCont& oc) {
192 return getFloatParam(v, oc, "toc.lcAbstinence", DEFAULT_LCABSTINENCE, false);
193 }
194
195 double
getInitialAwareness(const SUMOVehicle & v,const OptionsCont & oc)196 MSDevice_ToC::getInitialAwareness(const SUMOVehicle& v, const OptionsCont& oc) {
197 return getFloatParam(v, oc, "toc.initialAwareness", DEFAULT_INITIAL_AWARENESS, false);
198 }
199
200 double
getMRMDecel(const SUMOVehicle & v,const OptionsCont & oc)201 MSDevice_ToC::getMRMDecel(const SUMOVehicle& v, const OptionsCont& oc) {
202 return getFloatParam(v, oc, "toc.mrmDecel", DEFAULT_MRM_DECEL, false);
203 }
204
205 bool
useColorScheme(const SUMOVehicle & v,const OptionsCont & oc)206 MSDevice_ToC::useColorScheme(const SUMOVehicle& v, const OptionsCont& oc) {
207 return getBoolParam(v, oc, "toc.useColorScheme", "false", false);
208 }
209
210 MSDevice_ToC::OpenGapParams
getOpenGapParams(const SUMOVehicle & v,const OptionsCont & oc)211 MSDevice_ToC::getOpenGapParams(const SUMOVehicle& v, const OptionsCont& oc) {
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 // ---------------------------------------------------------------------------
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 useColoring,OpenGapParams ogp)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
283 if (outputFilename != "") {
284 myOutputFile = &OutputDevice::getDevice(outputFilename);
285 // TODO: make xsd, include header
286 // myOutputFile.writeXMLHeader("ToCDeviceLog", "ToCDeviceLog.xsd");
287 if (createdOutputFiles.count(outputFilename) == 0) {
288 myOutputFile->openTag("ToCDeviceLog");
289 createdOutputFiles.insert(outputFilename);
290 }
291 }
292 // register at static instance container
293 instances.insert(this);
294
295 // Check if the given vTypes for the ToC Device are vTypeDistributions
296 MSVehicleControl& vehCtrl = MSNet::getInstance()->getVehicleControl();
297 const bool automatedVTypeIsDist = vehCtrl.hasVTypeDistribution(myAutomatedTypeID);
298 const bool manualVTypeIsDist = vehCtrl.hasVTypeDistribution(myManualTypeID);
299
300 // Check if the vType of the holder matches one of the given vTypes
301 std::string holderVTypeID = holder.getVehicleType().getID();
302 if (holderVTypeID == myManualTypeID) {
303 myState = ToCState::MANUAL;
304 } else if (holderVTypeID == myAutomatedTypeID) {
305 myState = ToCState::AUTOMATED;
306 } else if (manualVTypeIsDist && holderVTypeID.find(myManualTypeID) == 0) {
307 // Holder type id starts with type distribution name.
308 // We assume that this means that it is from the given distribution.
309 myState = ToCState::MANUAL;
310 myManualTypeID = holderVTypeID;
311 } else if (automatedVTypeIsDist && holderVTypeID.find(myAutomatedTypeID) == 0) {
312 // Holder type id starts with type distribution name.
313 // We assume that this means that it is from the given distribution.
314 myState = ToCState::AUTOMATED;
315 myAutomatedTypeID = holderVTypeID;
316 } else {
317 throw ProcessError("Vehicle type of vehicle '" + holder.getID() + "' ('" + holder.getVehicleType().getID()
318 + "') must coincide with manualType ('" + manualType + "') or automatedType ('" + automatedType
319 + "') specified for its ToC-device (or drawn from the specified vTypeDistributions).");
320 }
321
322 // Eventually instantiate given vTypes from distributions
323 if (myState == ToCState::MANUAL && automatedVTypeIsDist) {
324 myAutomatedTypeID = vehCtrl.getVType(myAutomatedTypeID, MSRouteHandler::getParsingRNG())->getID();
325 } else if (myState == ToCState::AUTOMATED && manualVTypeIsDist) {
326 myManualTypeID = vehCtrl.getVType(myManualTypeID, MSRouteHandler::getParsingRNG())->getID();
327 }
328
329 initColorScheme();
330
331 #ifdef DEBUG_TOC
332 std::cout << "initialized device '" << id << "' with "
333 << "outputFilename=" << outputFilename << ", "
334 << "myManualType=" << myManualTypeID << ", "
335 << "myAutomatedType=" << myAutomatedTypeID << ", "
336 << "myResponseTime=" << myResponseTime << ", "
337 << "myRecoveryRate=" << myRecoveryRate << ", "
338 << "myInitialAwareness=" << myInitialAwareness << ", "
339 << "myMRMDecel=" << myMRMDecel << ", "
340 << "ogTimeHeadway=" << myOpenGapParams.newTimeHeadway << ", "
341 << "ogSpaceHeadway=" << myOpenGapParams.newSpaceHeadway << ", "
342 << "ogChangeRate=" << myOpenGapParams.changeRate << ", "
343 << "ogMaxDecel=" << myOpenGapParams.maxDecel << ", "
344 << "ogActive=" << myOpenGapParams.active << ", "
345 << "myCurrentAwareness=" << myCurrentAwareness << ", "
346 << "myState=" << _2string(myState) << std::endl;
347 #endif
348
349 assert(myInitialAwareness <= 1.0 && myInitialAwareness >= 0.0);
350 }
351
352
353
354 void
initColorScheme()355 MSDevice_ToC::initColorScheme() {
356 //RGBColor(red, green, blue)
357 myColorScheme[MANUAL] = MSNet::getInstance()->getVehicleControl().getVType(myManualTypeID)->getColor();
358 myColorScheme[AUTOMATED] = MSNet::getInstance()->getVehicleControl().getVType(myAutomatedTypeID)->getColor();
359 myColorScheme[PREPARING_TOC] = RGBColor(200, 200, 250); // light blue
360 myColorScheme[MRM] = RGBColor(250, 50, 50); // red
361 myColorScheme[RECOVERING] = RGBColor(250, 210, 150); // light yellow
362 myColorScheme[UNDEFINED] = RGBColor(150, 150, 150); // gray
363 }
364
365
~MSDevice_ToC()366 MSDevice_ToC::~MSDevice_ToC() {
367 // unregister from static instance container
368 instances.erase(this);
369 // deschedule commands associated to this device
370 if (myTriggerMRMCommand != nullptr) {
371 myTriggerMRMCommand->deschedule();
372 }
373 if (myTriggerToCCommand != nullptr) {
374 myTriggerToCCommand->deschedule();
375 }
376 if (myRecoverAwarenessCommand != nullptr) {
377 myRecoverAwarenessCommand->deschedule();
378 }
379 if (myExecuteMRMCommand != nullptr) {
380 myExecuteMRMCommand->deschedule();
381 resetDeliberateLCs();
382 }
383 if (myPrepareToCCommand != nullptr) {
384 myPrepareToCCommand->deschedule();
385 }
386 }
387
388 void
setAwareness(double value)389 MSDevice_ToC::setAwareness(double value) {
390 if (value > 1.0 || value < 0.0) {
391 std::stringstream ss;
392 ss << "Truncating invalid value for awareness (" << value << ") to lie in [0,1].";
393 WRITE_WARNING(ss.str());
394 value = MAX2(0.0, MIN2(1.0, value));
395 }
396 if (myCurrentAwareness >= myLCAbstinence && value < myLCAbstinence) {
397 // Awareness is now below LC abstinence level -> prevent deliberate LCs
398 deactivateDeliberateLCs();
399 } else if (myCurrentAwareness < myLCAbstinence && value >= myLCAbstinence) {
400 // Awareness is now above LC abstinence level -> allow deliberate LCs
401 resetDeliberateLCs();
402 }
403 myCurrentAwareness = value;
404 myHolderMS->getDriverState()->setAwareness(value);
405 }
406
407
408 void
setState(ToCState state)409 MSDevice_ToC::setState(ToCState state) {
410 if (myOpenGapParams.active && myState == PREPARING_TOC && state != PREPARING_TOC) {
411 // Deactivate gap control at preparation phase end
412 myHolderMS->getInfluencer().deactivateGapController();
413 }
414
415 myState = state;
416 if (myUseColorScheme) {
417 setVehicleColor();
418 }
419 }
420
421 void
setVehicleColor()422 MSDevice_ToC::setVehicleColor() {
423 const SUMOVehicleParameter& p = myHolder.getParameter();
424 p.color = myColorScheme[myState];
425 p.parametersSet |= VEHPARS_COLOR_SET;
426 }
427
428 void
requestMRM()429 MSDevice_ToC::requestMRM() {
430 // Remove any preparatory process
431 descheduleToCPreparation();
432 // .. and any recovery process
433 descheduleRecovery();
434 // ... and any pending ToC to manual
435 descheduleToC();
436 // Immediately trigger the MRM process
437 triggerMRM(0);
438 }
439
440
441 void
requestToC(SUMOTime timeTillMRM)442 MSDevice_ToC::requestToC(SUMOTime timeTillMRM) {
443 #ifdef DEBUG_TOC
444 std::cout << SIMTIME << " requestToC() for vehicle '" << myHolder.getID() << "' , timeTillMRM=" << timeTillMRM << std::endl;
445 #endif
446 if (myState == AUTOMATED) {
447 // Initialize preparation phase
448
449 // @todo: Sample response time from distribution
450 SUMOTime responseTime = myResponseTime;
451
452 // Schedule ToC Event
453 myTriggerToCCommand = new WrappingCommand<MSDevice_ToC>(this, &MSDevice_ToC::triggerDownwardToC);
454 MSNet::getInstance()->getBeginOfTimestepEvents()->addEvent(myTriggerToCCommand, SIMSTEP + responseTime);
455
456 // Clear eventually prior scheduled MRM
457 // descheduleMRM();
458 assert(myExecuteMRMCommand == nullptr);
459 assert(myTriggerMRMCommand == nullptr);
460 if (responseTime > timeTillMRM) {
461 // Schedule new MRM if driver response time is higher than permitted
462 myTriggerMRMCommand = new WrappingCommand<MSDevice_ToC>(this, &MSDevice_ToC::triggerMRM);
463 MSNet::getInstance()->getBeginOfTimestepEvents()->addEvent(myTriggerMRMCommand, SIMSTEP + timeTillMRM);
464 }
465
466 // Start ToC preparation process
467 myPrepareToCCommand = new WrappingCommand<MSDevice_ToC>(this, &MSDevice_ToC::ToCPreparationStep);
468 MSNet::getInstance()->getBeginOfTimestepEvents()->addEvent(myPrepareToCCommand, SIMSTEP + DELTA_T);
469 setState(PREPARING_TOC);
470 if (myOpenGapParams.active) {
471 // Start gap controller
472 double originalTau = myHolderMS->getCarFollowModel().getHeadwayTime();
473 myHolderMS->getInfluencer().activateGapController(originalTau,
474 myOpenGapParams.newTimeHeadway, myOpenGapParams.newSpaceHeadway, -1,
475 myOpenGapParams.changeRate, myOpenGapParams.maxDecel);
476 }
477 // Record event
478 if (generatesOutput()) {
479 myEvents.push(std::make_pair(SIMSTEP, "TOR"));
480 }
481 } else {
482 // Switch to automated mode is performed immediately
483 // Note that the transition MRM/PREPARING_TOC->AUTOMATED, where a downward ToC is aborted, is handled here as well.
484 if (timeTillMRM > 0.) {
485 std::stringstream ss;
486 ss << "[t=" << SIMTIME << "] Positive transition time (" << timeTillMRM / 1000. << "s.) for upward ToC of vehicle '" << myHolder.getID() << "' is ignored.";
487 WRITE_WARNING(ss.str());
488 }
489 triggerUpwardToC(SIMSTEP + DELTA_T);
490 }
491 }
492
493
494 SUMOTime
triggerMRM(SUMOTime)495 MSDevice_ToC::triggerMRM(SUMOTime /* t */) {
496 #ifdef DEBUG_TOC
497 std::cout << SIMTIME << " triggerMRM() for vehicle '" << myHolder.getID() << "'" << std::endl;
498 #endif
499 // Clear ongoing MRM
500 descheduleMRM();
501
502 // Start MRM process
503 myExecuteMRMCommand = new WrappingCommand<MSDevice_ToC>(this, &MSDevice_ToC::MRMExecutionStep);
504 MSNet::getInstance()->getBeginOfTimestepEvents()->addEvent(myExecuteMRMCommand, SIMSTEP + DELTA_T);
505 setState(MRM);
506 switchHolderType(myAutomatedTypeID);
507 setAwareness(1.);
508 deactivateDeliberateLCs();
509
510 // Record event
511 if (generatesOutput()) {
512 myEvents.push(std::make_pair(SIMSTEP, "MRM"));
513 }
514
515 return 0;
516 }
517
518
519 SUMOTime
triggerUpwardToC(SUMOTime)520 MSDevice_ToC::triggerUpwardToC(SUMOTime /* t */) {
521 #ifdef DEBUG_TOC
522 std::cout << SIMTIME << " triggerUpwardToC() for vehicle '" << myHolder.getID() << "'" << std::endl;
523 #endif
524 descheduleToC();
525 // Eventually stop ToC preparation process
526 descheduleToCPreparation();
527 // Eventually abort MRM
528 descheduleMRM();
529 // Eventually abort awareness recovery process
530 descheduleRecovery();
531
532 switchHolderType(myAutomatedTypeID);
533 setAwareness(1.);
534 setState(AUTOMATED);
535
536 // Record event
537 if (generatesOutput()) {
538 myEvents.push(std::make_pair(SIMSTEP, "ToCup"));
539 }
540
541 return 0;
542 }
543
544
545 SUMOTime
triggerDownwardToC(SUMOTime)546 MSDevice_ToC::triggerDownwardToC(SUMOTime /* t */) {
547 #ifdef DEBUG_TOC
548 std::cout << SIMTIME << " triggerDownwardToC() for vehicle '" << myHolder.getID() << "'" << std::endl;
549 #endif
550 descheduleToC();
551 // Eventually stop ToC preparation process
552 descheduleToCPreparation();
553 // Eventually abort MRM
554 descheduleMRM();
555
556 switchHolderType(myManualTypeID);
557
558 // @todo: Sample initial awareness
559 double initialAwareness = myInitialAwareness;
560 setAwareness(initialAwareness);
561
562 #ifdef DEBUG_TOC
563 std::cout << SIMTIME << " Initial awareness after ToC: " << myCurrentAwareness << std::endl;
564 #endif
565
566 // Start awareness recovery process
567 myRecoverAwarenessCommand = new WrappingCommand<MSDevice_ToC>(this, &MSDevice_ToC::awarenessRecoveryStep);
568 MSNet::getInstance()->getBeginOfTimestepEvents()->addEvent(myRecoverAwarenessCommand, SIMSTEP + DELTA_T);
569 setState(RECOVERING);
570
571 // Record event
572 if (generatesOutput()) {
573 myEvents.push(std::make_pair(SIMSTEP, "ToCdown"));
574 }
575 return 0;
576 }
577
578 void
descheduleMRM()579 MSDevice_ToC::descheduleMRM() {
580 // Eventually abort scheduled MRM
581 if (myTriggerMRMCommand != nullptr) {
582 myTriggerMRMCommand->deschedule();
583 myTriggerMRMCommand = nullptr;
584 }
585 // Eventually abort ongoing MRM
586 if (myExecuteMRMCommand != nullptr) {
587 myExecuteMRMCommand->deschedule();
588 resetDeliberateLCs();
589 myExecuteMRMCommand = nullptr;
590 }
591 }
592
593
594 void
descheduleToC()595 MSDevice_ToC::descheduleToC() {
596 if (myTriggerToCCommand != nullptr) {
597 myTriggerToCCommand->deschedule();
598 myTriggerToCCommand = nullptr;
599 }
600 }
601
602 void
descheduleToCPreparation()603 MSDevice_ToC::descheduleToCPreparation() {
604 // Eventually stop ToC preparation process
605 if (myPrepareToCCommand != nullptr) {
606 myPrepareToCCommand->deschedule();
607 myPrepareToCCommand = nullptr;
608 }
609 }
610
611 void
descheduleRecovery()612 MSDevice_ToC::descheduleRecovery() {
613 // Eventually stop ToC preparation process
614 if (myRecoverAwarenessCommand != nullptr) {
615 myRecoverAwarenessCommand->deschedule();
616 myRecoverAwarenessCommand = nullptr;
617 }
618 }
619
620
621 void
switchHolderType(const std::string & targetTypeID)622 MSDevice_ToC::switchHolderType(const std::string& targetTypeID) {
623 #ifdef DEBUG_TOC
624 std::cout << SIMTIME << " Switching type of vehicle '" << myHolder.getID() << "' to '" << targetTypeID << "'" << std::endl;
625 #endif
626 MSVehicleType* targetType = MSNet::getInstance()->getVehicleControl().getVType(targetTypeID);
627 if (targetType == nullptr) {
628 WRITE_ERROR("vType '" + targetType->getID() + "' for vehicle '" + myHolder.getID() + "' is not known.");
629 return;
630 }
631 myHolderMS->replaceVehicleType(targetType);
632 }
633
634
635 SUMOTime
ToCPreparationStep(SUMOTime)636 MSDevice_ToC::ToCPreparationStep(SUMOTime /* t */) {
637 #ifdef DEBUG_TOC
638 std::cout << SIMTIME << " ToC preparation step for vehicle '" << myHolder.getID() << "'" << std::endl;
639 #endif
640 // TODO: Devise preparation of ToC (still needs discussion). At least: do not overtake. Perhaps, increase gap to leader.
641
642 if (myState == PREPARING_TOC) {
643 return DELTA_T;
644 } else {
645 #ifdef DEBUG_TOC
646 std::cout << SIMTIME << " Aborting ToC preparation for vehicle '" << myHolder.getID() << "'" << std::endl;
647 #endif
648 descheduleToCPreparation();
649 return 0;
650 }
651 }
652
653
654 SUMOTime
MRMExecutionStep(SUMOTime t)655 MSDevice_ToC::MRMExecutionStep(SUMOTime t) {
656 deactivateDeliberateLCs();
657 const double currentSpeed = myHolderMS->getSpeed();
658 #ifdef DEBUG_TOC
659 std::cout << SIMTIME << " MRM step for vehicle '" << myHolder.getID() << "', currentSpeed=" << currentSpeed << std::endl;
660 #endif
661
662 // Induce slowdown with MRMDecel
663 std::vector<std::pair<SUMOTime, double> > speedTimeLine;
664 const double nextSpeed = MAX2(0., currentSpeed - ACCEL2SPEED(myMRMDecel));
665 speedTimeLine.push_back(std::make_pair(t - DELTA_T, currentSpeed));
666 speedTimeLine.push_back(std::make_pair(t, nextSpeed));
667 myHolderMS->getInfluencer().setSpeedTimeLine(speedTimeLine);
668
669 if (myState == MRM) {
670 return DELTA_T;
671 } else {
672 #ifdef DEBUG_TOC
673 std::cout << SIMTIME << " Aborting MRM for vehicle '" << myHolder.getID() << "'" << std::endl;
674 #endif
675 resetDeliberateLCs();
676 return 0;
677 }
678 }
679
680
681 SUMOTime
awarenessRecoveryStep(SUMOTime)682 MSDevice_ToC::awarenessRecoveryStep(SUMOTime /* t */) {
683 #ifdef DEBUG_TOC
684 std::cout << SIMTIME << " Awareness recovery step for vehicle '" << myHolder.getID() << "'" << std::endl;
685 #endif
686 // Proceed with awareness recovery
687 if (myCurrentAwareness < 1.0) {
688 setAwareness(MIN2(1.0, myCurrentAwareness + TS * myRecoveryRate));
689 }
690
691 #ifdef DEBUG_TOC
692 std::cout << SIMTIME << " currentAwareness = " << myCurrentAwareness << std::endl;
693 #endif
694
695 const bool awarenessRecoveryCompleted = myCurrentAwareness == 1.0;
696 if (awarenessRecoveryCompleted) {
697 #ifdef DEBUG_TOC
698 std::cout << SIMTIME << " Awareness recovery completed for veh '" << myHolder.getID() << "'" << std::endl;
699 #endif
700 myRecoverAwarenessCommand->deschedule();
701 myRecoverAwarenessCommand = nullptr;
702 setState(MANUAL);
703 return 0;
704 }
705 return DELTA_T;
706 }
707
708
709 std::string
getParameter(const std::string & key) const710 MSDevice_ToC::getParameter(const std::string& key) const {
711 if (key == "manualType") {
712 return myManualTypeID;
713 } else if (key == "automatedType") {
714 return myAutomatedTypeID;
715 } else if (key == "responseTime") {
716 return toString(STEPS2TIME(myResponseTime));
717 } else if (key == "recoveryRate") {
718 return toString(myRecoveryRate);
719 } else if (key == "initialAwareness") {
720 return toString(myInitialAwareness);
721 } else if (key == "mrmDecel") {
722 return toString(myMRMDecel);
723 } else if (key == "currentAwareness") {
724 return toString(myCurrentAwareness);
725 } else if (key == "lcAbstinence") {
726 return toString(myLCAbstinence);
727 } else if (key == "state") {
728 return _2string(myState);
729 } else if (key == "holder") {
730 return myHolder.getID();
731 }
732 throw InvalidArgument("Parameter '" + key + "' is not supported for device of type '" + deviceName() + "'");
733 }
734
735
736 void
setParameter(const std::string & key,const std::string & value)737 MSDevice_ToC::setParameter(const std::string& key, const std::string& value) {
738 #ifdef DEBUG_TOC
739 std::cout << "MSDevice_ToC::setParameter(key=" << key << ", value=" << value << ")" << std::endl;
740 #endif
741 if (key == "manualType") {
742 myManualTypeID = value;
743 myColorScheme[MANUAL] = MSNet::getInstance()->getVehicleControl().getVType(myManualTypeID)->getColor();
744 if (myState == MANUAL) {
745 switchHolderType(value);
746 }
747 } else if (key == "automatedType") {
748 myAutomatedTypeID = value;
749 myColorScheme[AUTOMATED] = MSNet::getInstance()->getVehicleControl().getVType(myAutomatedTypeID)->getColor();
750 if (myState == AUTOMATED || myState == PREPARING_TOC || myState == MRM) {
751 switchHolderType(value);
752 }
753 } else if (key == "responseTime") {
754 myResponseTime = TIME2STEPS(StringUtils::toDouble(value));
755 } else if (key == "recoveryRate") {
756 myRecoveryRate = StringUtils::toDouble(value);
757 } else if (key == "initialAwareness") {
758 myInitialAwareness = StringUtils::toDouble(value);
759 } else if (key == "lcAbstinence") {
760 myLCAbstinence = StringUtils::toDouble(value);
761 if (isManuallyDriven()) {
762 setAwareness(myCurrentAwareness); // to eventually trigger LC-prevention
763 }
764 } else if (key == "currentAwareness") {
765 if (isManuallyDriven()) {
766 setAwareness(StringUtils::toDouble(value));
767 } else {
768 WRITE_WARNING("Setting device.toc.currentAwareness during automated mode has no effect.")
769 }
770 } else if (key == "mrmDecel") {
771 myMRMDecel = StringUtils::toDouble(value);
772 } else if (key == "requestToC") {
773 // setting this magic parameter gives the interface for inducing a ToC
774 const SUMOTime timeTillMRM = TIME2STEPS(StringUtils::toDouble(value));
775 requestToC(timeTillMRM);
776 } else if (key == "requestMRM") {
777 // setting this magic parameter gives the interface for inducing an MRM
778 requestMRM();
779 } else if (key == "awareness") {
780 // setting this magic parameter gives the interface for setting the driverstate's awareness
781 setAwareness(StringUtils::toDouble(value));
782 } else {
783 throw InvalidArgument("Parameter '" + key + "' is not supported for device of type '" + deviceName() + "'");
784 }
785 }
786
787
788 MSDevice_ToC::ToCState
_2ToCState(const std::string & str)789 MSDevice_ToC::_2ToCState(const std::string& str) {
790 if (str == "UNDEFINED") {
791 return UNDEFINED;
792 } else if (str == "MANUAL") {
793 return MANUAL;
794 } else if (str == "AUTOMATED") {
795 return AUTOMATED;
796 } else if (str == "PREPARING_TOC") {
797 return PREPARING_TOC;
798 } else if (str == "MRM") {
799 return MRM;
800 } else if (str == "RECOVERING") {
801 return RECOVERING;
802 } else {
803 WRITE_WARNING("Unknown ToCState '" + str + "'");
804 return UNDEFINED;
805 }
806 }
807
808
809 std::string
_2string(ToCState state)810 MSDevice_ToC::_2string(ToCState state) {
811 if (state == UNDEFINED) {
812 return "UNDEFINED";
813 } else if (state == MANUAL) {
814 return "MANUAL";
815 } else if (state == AUTOMATED) {
816 return "AUTOMATED";
817 } else if (state == PREPARING_TOC) {
818 return "PREPARING_TOC";
819 } else if (state == MRM) {
820 return "MRM";
821 } else if (state == RECOVERING) {
822 return "RECOVERING";
823 } else {
824 WRITE_WARNING("Unknown ToCState '" + toString(state) + "'");
825 return toString(state);
826 }
827 }
828
829
830 void
writeOutput()831 MSDevice_ToC::writeOutput() {
832 if (!generatesOutput()) {
833 assert(myEvents.empty());
834 return;
835 }
836 while (!myEvents.empty()) {
837 std::pair<SUMOTime, std::string>& e = myEvents.front();
838 myOutputFile->openTag(e.second);
839 myOutputFile->writeAttr("id", myHolder.getID()).writeAttr("t", STEPS2TIME(e.first));
840 myOutputFile->closeTag();
841 myEvents.pop();
842 }
843 }
844
845
846 void
cleanup()847 MSDevice_ToC::cleanup() {
848 // Close xml bodies for all existing files
849 // TODO: Check if required
850 for (auto& fn : createdOutputFiles) {
851 OutputDevice* file = &OutputDevice::getDevice(fn);
852 file->closeTag();
853 }
854 }
855
856
857 void
resetDeliberateLCs()858 MSDevice_ToC::resetDeliberateLCs() {
859 if (myPreviousLCMode != -1) {
860 myHolderMS->getInfluencer().setLaneChangeMode(myPreviousLCMode);
861 #ifdef DEBUG_TOC
862 std::cout << "MSDevice_ToC::resetLCMode() restoring LC Mode of vehicle '" << myHolder.getID() << "' to " << myPreviousLCMode << std::endl;
863 #endif
864 }
865 myPreviousLCMode = -1;
866 }
867
868
869 void
deactivateDeliberateLCs()870 MSDevice_ToC::deactivateDeliberateLCs() {
871 const int lcModeHolder = myHolderMS->getInfluencer().getLaneChangeMode();
872 if (lcModeHolder != LCModeMRM) {
873 myPreviousLCMode = lcModeHolder;
874 #ifdef DEBUG_TOC
875 std::cout << "MSDevice_ToC::setLCModeMRM() setting LC Mode of vehicle '" << myHolder.getID()
876 << "' from " << myPreviousLCMode << " to " << LCModeMRM << std::endl;
877 #endif
878 }
879 myHolderMS->getInfluencer().setLaneChangeMode(LCModeMRM);
880 }
881
882 bool
isManuallyDriven()883 MSDevice_ToC::isManuallyDriven() {
884 return (myState == MANUAL || myState == RECOVERING);
885 }
886
887 bool
isAutomated()888 MSDevice_ToC::isAutomated() {
889 return (myState == AUTOMATED || myState == PREPARING_TOC || myState == MRM);
890 }
891
892 /****************************************************************************/
893
894