Eclipse SUMO - Simulation of Urban MObility
MSDriverState.cpp
Go to the documentation of this file.
1 /****************************************************************************/
2 // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3 // Copyright (C) 2001-2022 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 /****************************************************************************/
20 // The common superclass for modelling transportable objects like persons and containers
21 /****************************************************************************/
22 #include <config.h>
23 
24 #include <math.h>
25 #include <cmath>
27 #include <utils/common/SUMOTime.h>
28 //#include <microsim/MSVehicle.h>
30 //#include <microsim/MSLane.h>
31 #include <microsim/MSEdge.h>
32 //#include <microsim/MSGlobals.h>
33 //#include <microsim/MSNet.h>
36 #include "MSDriverState.h"
37 
38 // ===========================================================================
39 // DEBUG constants
40 // ===========================================================================
41 //#define DEBUG_OUPROCESS
42 //#define DEBUG_TRAFFIC_ITEMS
43 //#define DEBUG_AWARENESS
44 //#define DEBUG_PERCEPTION_ERRORS
45 //#define DEBUG_DRIVERSTATE
46 #define DEBUG_COND (true)
47 //#define DEBUG_COND (myVehicle->isSelected())
48 
49 
50 /* -------------------------------------------------------------------------
51  * static member definitions
52  * ----------------------------------------------------------------------- */
53 // hash function
54 //std::hash<std::string> MSDriverState::MSTrafficItem::hash = std::hash<std::string>();
56 
57 // ===========================================================================
58 // Default value definitions
59 // ===========================================================================
60 //double TCIDefaults::myMinTaskCapability = 0.1;
61 //double TCIDefaults::myMaxTaskCapability = 10.0;
62 //double TCIDefaults::myMaxTaskDemand = 20.0;
63 //double TCIDefaults::myMaxDifficulty = 10.0;
64 //double TCIDefaults::mySubCriticalDifficultyCoefficient = 0.1;
65 //double TCIDefaults::mySuperCriticalDifficultyCoefficient = 1.0;
66 //double TCIDefaults::myOppositeDirectionDrivingFactor = 1.3;
67 //double TCIDefaults::myHomeostasisDifficulty = 1.5;
68 //double TCIDefaults::myCapabilityTimeScale = 0.5;
69 //double TCIDefaults::myAccelerationErrorTimeScaleCoefficient = 1.0;
70 //double TCIDefaults::myAccelerationErrorNoiseIntensityCoefficient = 1.0;
71 //double TCIDefaults::myActionStepLengthCoefficient = 1.0;
72 //double TCIDefaults::myMinActionStepLength = 0.0;
73 //double TCIDefaults::myMaxActionStepLength = 3.0;
74 //double TCIDefaults::mySpeedPerceptionErrorTimeScaleCoefficient = 1.0;
75 //double TCIDefaults::mySpeedPerceptionErrorNoiseIntensityCoefficient = 1.0;
76 //double TCIDefaults::myHeadwayPerceptionErrorTimeScaleCoefficient = 1.0;
77 //double TCIDefaults::myHeadwayPerceptionErrorNoiseIntensityCoefficient = 1.0;
78 
88 
89 
90 // ===========================================================================
91 // method definitions
92 // ===========================================================================
93 
94 OUProcess::OUProcess(double initialState, double timeScale, double noiseIntensity)
95  : myState(initialState),
96  myTimeScale(timeScale),
97  myNoiseIntensity(noiseIntensity) {}
98 
99 
101 
102 
103 void
104 OUProcess::step(double dt) {
105 #ifdef DEBUG_OUPROCESS
106  const double oldstate = myState;
107 #endif
108  myState = exp(-dt / myTimeScale) * myState + myNoiseIntensity * sqrt(2 * dt / myTimeScale) * RandHelper::randNorm(0, 1, &myRNG);
109 #ifdef DEBUG_OUPROCESS
110  std::cout << " OU-step (" << dt << " s.): " << oldstate << "->" << myState << std::endl;
111 #endif
112 }
113 
114 double
115 OUProcess::step(double state, double dt, double timeScale, double noiseIntensity) {
117  return exp(-dt / timeScale) * state + noiseIntensity * sqrt(2 * dt / timeScale) * RandHelper::randNorm(0, 1, &myRNG);
118 }
119 
120 double
122  return myState;
123 }
124 
125 
127  myVehicle(veh),
128  myAwareness(1.),
129  myMinAwareness(DriverStateDefaults::minAwareness),
130  myError(0., 1., 1.),
131  myErrorTimeScaleCoefficient(DriverStateDefaults::errorTimeScaleCoefficient),
132  myErrorNoiseIntensityCoefficient(DriverStateDefaults::errorNoiseIntensityCoefficient),
133  mySpeedDifferenceErrorCoefficient(DriverStateDefaults::speedDifferenceErrorCoefficient),
134  myHeadwayErrorCoefficient(DriverStateDefaults::headwayErrorCoefficient),
135  myHeadwayChangePerceptionThreshold(DriverStateDefaults::headwayChangePerceptionThreshold),
136  mySpeedDifferenceChangePerceptionThreshold(DriverStateDefaults::speedDifferenceChangePerceptionThreshold),
137  myOriginalReactionTime(veh->getActionStepLengthSecs()),
138  myMaximalReactionTime(DriverStateDefaults::maximalReactionTimeFactor * myOriginalReactionTime),
139 // myActionStepLength(TS),
140  myStepDuration(TS),
141  myLastUpdateTime(SIMTIME - TS),
142  myDebugLock(false) {
143 #ifdef DEBUG_DRIVERSTATE
144  std::cout << "Constructing driver state for veh '" << veh->getID() << "'." << std::endl;
145 #endif
146  updateError();
148 }
149 
150 
151 void
153 #ifdef DEBUG_AWARENESS
154  if (DEBUG_COND) {
155  std::cout << SIMTIME << " veh=" << myVehicle->getID() << ", DriverState::update()" << std::endl;
156  }
157 #endif
158  // Adapt step duration
160  // Update error
161  updateError();
162  // Update actionStepLength, aka reaction time
164  // Update assumed gaps
166 #ifdef DEBUG_AWARENESS
167  if (DEBUG_COND) {
168  std::cout << SIMTIME << " stepDuration=" << myStepDuration << ", error=" << myError.getState() << std::endl;
169  }
170 #endif
171 }
172 
173 void
177 }
178 
179 void
181  if (myAwareness == 1.0 || myAwareness == 0.0) {
182  myError.setState(0.);
183  } else {
187  }
188 }
189 
190 void
192  if (myAwareness == 1.0 || myAwareness == 0.0) {
194  } else {
195  const double theta = (myAwareness - myMinAwareness) / (1.0 - myMinAwareness);
197  // Round to multiple of simstep length
198  int quotient;
199  remquo(myActionStepLength, TS, &quotient);
200  myActionStepLength = TS * MAX2(quotient, 1);
201  }
202 }
203 
204 void
206  assert(value >= 0.);
207  assert(value <= 1.);
208 #ifdef DEBUG_AWARENESS
209  if (DEBUG_COND) {
210  std::cout << SIMTIME << " veh=" << myVehicle->getID() << ", setAwareness(" << MAX2(value, myMinAwareness) << ")" << std::endl;
211  }
212 #endif
213  myAwareness = MAX2(value, myMinAwareness);
214  if (myAwareness == 1.) {
215  myError.setState(0.);
216  }
218 }
219 
220 
221 double
222 MSSimpleDriverState::getPerceivedHeadway(const double trueGap, const void* objID) {
223 #ifdef DEBUG_PERCEPTION_ERRORS
224  if (DEBUG_COND) {
225  if (!debugLocked()) {
226  std::cout << SIMTIME << " getPerceivedHeadway() for veh '" << myVehicle->getID() << "'\n"
227  << " trueGap=" << trueGap << " objID=" << objID << std::endl;
228  }
229  }
230 #endif
231 
232  const double perceivedGap = trueGap + myHeadwayErrorCoefficient * myError.getState() * trueGap;
233  const auto assumedGap = myAssumedGap.find(objID);
234  if (assumedGap == myAssumedGap.end()
235  || fabs(perceivedGap - assumedGap->second) > myHeadwayChangePerceptionThreshold * trueGap * (1.0 - myAwareness)) {
236 
237 #ifdef DEBUG_PERCEPTION_ERRORS
238  if (!debugLocked()) {
239  std::cout << " new perceived gap (=" << perceivedGap << ") differs significantly from the assumed (="
240  << (assumedGap == myAssumedGap.end() ? "NA" : toString(assumedGap->second)) << ")" << std::endl;
241  }
242 #endif
243 
244  // new perceived gap differs significantly from the previous
245  myAssumedGap[objID] = perceivedGap;
246  return perceivedGap;
247  } else {
248 
249 #ifdef DEBUG_PERCEPTION_ERRORS
250  if (DEBUG_COND) {
251  if (!debugLocked()) {
252  std::cout << " new perceived gap (=" << perceivedGap << ") does *not* differ significantly from the assumed (="
253  << (assumedGap->second) << ")" << std::endl;
254  }
255  }
256 #endif
257  // new perceived gap doesn't differ significantly from the previous
258  return myAssumedGap[objID];
259  }
260 }
261 
262 void
264  for (auto& p : myAssumedGap) {
265  const void* objID = p.first;
266  const auto speedDiff = myLastPerceivedSpeedDifference.find(objID);
267  double assumedSpeedDiff;
268  if (speedDiff != myLastPerceivedSpeedDifference.end()) {
269  // update the assumed gap with the last perceived speed difference
270  assumedSpeedDiff = speedDiff->second;
271  } else {
272  // Assume the object is not moving, if no perceived speed difference is known.
273  assumedSpeedDiff = -myVehicle->getSpeed();
274  }
275  p.second += SPEED2DIST(assumedSpeedDiff);
276  }
277 }
278 
279 double
280 MSSimpleDriverState::getPerceivedSpeedDifference(const double trueSpeedDifference, const double trueGap, const void* objID) {
281 #ifdef DEBUG_PERCEPTION_ERRORS
282  if (DEBUG_COND) {
283  if (!debugLocked()) {
284  std::cout << SIMTIME << " getPerceivedSpeedDifference() for veh '" << myVehicle->getID() << "'\n"
285  << " trueGap=" << trueGap << " trueSpeedDifference=" << trueSpeedDifference << " objID=" << objID << std::endl;
286  }
287  }
288 #endif
289  const double perceivedSpeedDifference = trueSpeedDifference + mySpeedDifferenceErrorCoefficient * myError.getState() * trueGap;
290  const auto lastPerceivedSpeedDifference = myLastPerceivedSpeedDifference.find(objID);
291  if (lastPerceivedSpeedDifference == myLastPerceivedSpeedDifference.end()
292  || fabs(perceivedSpeedDifference - lastPerceivedSpeedDifference->second) > mySpeedDifferenceChangePerceptionThreshold * trueGap * (1.0 - myAwareness)) {
293 
294 #ifdef DEBUG_PERCEPTION_ERRORS
295  if (DEBUG_COND) {
296  if (!debugLocked()) {
297  std::cout << " new perceived speed difference (=" << perceivedSpeedDifference << ") differs significantly from the last perceived (="
298  << (lastPerceivedSpeedDifference == myLastPerceivedSpeedDifference.end() ? "NA" : toString(lastPerceivedSpeedDifference->second)) << ")"
299  << std::endl;
300  }
301  }
302 #endif
303 
304  // new perceived speed difference differs significantly from the previous
305  myLastPerceivedSpeedDifference[objID] = perceivedSpeedDifference;
306  return perceivedSpeedDifference;
307  } else {
308 #ifdef DEBUG_PERCEPTION_ERRORS
309  if (!debugLocked()) {
310  std::cout << " new perceived speed difference (=" << perceivedSpeedDifference << ") does *not* differ significantly from the last perceived (="
311  << (lastPerceivedSpeedDifference->second) << ")" << std::endl;
312  }
313 #endif
314  // new perceived speed difference doesn't differ significantly from the previous
315  return lastPerceivedSpeedDifference->second;
316  }
317 }
318 
319 
320 //MSDriverState::MSTrafficItem::MSTrafficItem(MSTrafficItemType type, const std::string& id, std::shared_ptr<MSTrafficItemCharacteristics> data) :
321 // type(type),
322 // id_hash(hash(id)),
323 // data(data),
324 // remainingIntegrationTime(0.),
325 // integrationDemand(0.),
326 // latentDemand(0.)
327 //{}
328 //
329 //MSDriverState::MSDriverState(MSVehicle* veh) :
330 // myVehicle(veh),
331 // myMinTaskCapability(TCIDefaults::myMinTaskCapability),
332 // myMaxTaskCapability(TCIDefaults::myMaxTaskCapability),
333 // myMaxTaskDemand(TCIDefaults::myMaxTaskDemand),
334 // myMaxDifficulty(TCIDefaults::myMaxDifficulty),
335 // mySubCriticalDifficultyCoefficient(TCIDefaults::mySubCriticalDifficultyCoefficient),
336 // mySuperCriticalDifficultyCoefficient(TCIDefaults::mySuperCriticalDifficultyCoefficient),
337 // myOppositeDirectionDrivingDemandFactor(TCIDefaults::myOppositeDirectionDrivingFactor),
338 // myHomeostasisDifficulty(TCIDefaults::myHomeostasisDifficulty),
339 // myCapabilityTimeScale(TCIDefaults::myCapabilityTimeScale),
340 // myAccelerationErrorTimeScaleCoefficient(TCIDefaults::myAccelerationErrorTimeScaleCoefficient),
341 // myAccelerationErrorNoiseIntensityCoefficient(TCIDefaults::myAccelerationErrorNoiseIntensityCoefficient),
342 // myActionStepLengthCoefficient(TCIDefaults::myActionStepLengthCoefficient),
343 // myMinActionStepLength(TCIDefaults::myMinActionStepLength),
344 // myMaxActionStepLength(TCIDefaults::myMaxActionStepLength),
345 // mySpeedPerceptionErrorTimeScaleCoefficient(TCIDefaults::mySpeedPerceptionErrorTimeScaleCoefficient),
346 // mySpeedPerceptionErrorNoiseIntensityCoefficient(TCIDefaults::mySpeedPerceptionErrorNoiseIntensityCoefficient),
347 // myHeadwayPerceptionErrorTimeScaleCoefficient(TCIDefaults::myHeadwayPerceptionErrorTimeScaleCoefficient),
348 // myHeadwayPerceptionErrorNoiseIntensityCoefficient(TCIDefaults::myHeadwayPerceptionErrorNoiseIntensityCoefficient),
349 // myAmOpposite(false),
350 // myAccelerationError(0., 1.,1.),
351 // myHeadwayPerceptionError(0., 1.,1.),
352 // mySpeedPerceptionError(0., 1.,1.),
353 // myTaskDemand(0.),
354 // myTaskCapability(myMaxTaskCapability),
355 // myCurrentDrivingDifficulty(myTaskDemand/myTaskCapability),
356 // myActionStepLength(TS),
357 // myStepDuration(TS),
358 // myLastUpdateTime(SIMTIME-TS),
359 // myCurrentSpeed(0.),
360 // myCurrentAcceleration(0.)
361 //{}
362 //
363 //
364 //void
365 //MSDriverState::updateStepDuration() {
366 // myStepDuration = SIMTIME - myLastUpdateTime;
367 // myLastUpdateTime = SIMTIME;
368 //}
369 //
370 //
371 //void
372 //MSDriverState::calculateDrivingDifficulty() {
373 // if (myAmOpposite) {
374 // myCurrentDrivingDifficulty = difficultyFunction(myOppositeDirectionDrivingDemandFactor*myTaskDemand/myTaskCapability);
375 // } else {
376 // myCurrentDrivingDifficulty = difficultyFunction(myTaskDemand/myTaskCapability);
377 // }
378 //}
379 //
380 //
381 //double
382 //MSDriverState::difficultyFunction(double demandCapabilityQuotient) const {
383 // double difficulty;
384 // if (demandCapabilityQuotient <= 1) {
385 // // demand does not exceed capability -> we are in the region for a slight ascend of difficulty
386 // difficulty = mySubCriticalDifficultyCoefficient*demandCapabilityQuotient;
387 // } else {
388 // // demand exceeds capability -> we are in the region for a steeper ascend of the effect of difficulty
389 // difficulty = mySubCriticalDifficultyCoefficient + (demandCapabilityQuotient - 1)*mySuperCriticalDifficultyCoefficient;
390 // }
391 // return MIN2(myMaxDifficulty, difficulty);
392 //}
393 //
394 //
395 //void
396 //MSDriverState::adaptTaskCapability() {
397 // myTaskCapability = myTaskCapability + myCapabilityTimeScale*myStepDuration*(myTaskDemand - myHomeostasisDifficulty*myTaskCapability);
398 //}
399 //
400 //
401 //void
402 //MSDriverState::updateAccelerationError() {
403 //#ifdef DEBUG_OUPROCESS
404 // if (DEBUG_COND) {
405 // std::cout << SIMTIME << " Updating acceleration error (for " << myStepDuration << " s.):\n "
406 // << myAccelerationError.getState() << " -> ";
407 // }
408 //#endif
409 //
410 // updateErrorProcess(myAccelerationError, myAccelerationErrorTimeScaleCoefficient, myAccelerationErrorNoiseIntensityCoefficient);
411 //
412 //#ifdef DEBUG_OUPROCESS
413 // if (DEBUG_COND) {
414 // std::cout << myAccelerationError.getState() << std::endl;
415 // }
416 //#endif
417 //}
418 //
419 //void
420 //MSDriverState::updateSpeedPerceptionError() {
421 //#ifdef DEBUG_OUPROCESS
422 // if (DEBUG_COND) {
423 // std::cout << SIMTIME << " Updating speed perception error (for " << myStepDuration << " s.):\n "
424 // << mySpeedPerceptionError.getState() << " -> ";
425 // }
426 //#endif
427 //
428 // updateErrorProcess(mySpeedPerceptionError, mySpeedPerceptionErrorTimeScaleCoefficient, mySpeedPerceptionErrorNoiseIntensityCoefficient);
429 //
430 //#ifdef DEBUG_OUPROCESS
431 // if (DEBUG_COND) {
432 // std::cout << mySpeedPerceptionError.getState() << std::endl;
433 // }
434 //#endif
435 //}
436 //
437 //void
438 //MSDriverState::updateHeadwayPerceptionError() {
439 //#ifdef DEBUG_OUPROCESS
440 // if (DEBUG_COND) {
441 // std::cout << SIMTIME << " Updating headway perception error (for " << myStepDuration << " s.):\n "
442 // << myHeadwayPerceptionError.getState() << " -> ";
443 // }
444 //#endif
445 //
446 // updateErrorProcess(myHeadwayPerceptionError, myHeadwayPerceptionErrorTimeScaleCoefficient, myHeadwayPerceptionErrorNoiseIntensityCoefficient);
447 //
448 //#ifdef DEBUG_OUPROCESS
449 // if (DEBUG_COND) {
450 // std::cout << myHeadwayPerceptionError.getState() << std::endl;
451 // }
452 //#endif
453 //}
454 //
455 //void
456 //MSDriverState::updateActionStepLength() {
457 //#ifdef DEBUG_OUPROCESS
458 // if (DEBUG_COND) {
459 // std::cout << SIMTIME << " Updating action step length (for " << myStepDuration << " s.): \n" << myActionStepLength;
460 // }
461 //#endif
462 // if (myActionStepLengthCoefficient*myCurrentDrivingDifficulty <= myMinActionStepLength) {
463 // myActionStepLength = myMinActionStepLength;
464 // } else {
465 // myActionStepLength = MIN2(myActionStepLengthCoefficient*myCurrentDrivingDifficulty - myMinActionStepLength, myMaxActionStepLength);
466 // }
467 //#ifdef DEBUG_OUPROCESS
468 // if (DEBUG_COND) {
469 // std::cout << " -> " << myActionStepLength << std::endl;
470 // }
471 //#endif
472 //}
473 //
474 //
475 //void
476 //MSDriverState::updateErrorProcess(OUProcess& errorProcess, double timeScaleCoefficient, double noiseIntensityCoefficient) const {
477 // if (myCurrentDrivingDifficulty == 0) {
478 // errorProcess.setState(0.);
479 // } else {
480 // errorProcess.setTimeScale(timeScaleCoefficient/myCurrentDrivingDifficulty);
481 // errorProcess.setNoiseIntensity(myCurrentDrivingDifficulty*noiseIntensityCoefficient);
482 // errorProcess.step(myStepDuration);
483 // }
484 //}
485 //
486 //void
487 //MSDriverState::registerLeader(const MSVehicle* leader, double gap, double relativeSpeed, double latGap) {
488 // std::shared_ptr<MSTrafficItemCharacteristics> tic = std::dynamic_pointer_cast<MSTrafficItemCharacteristics>(std::make_shared<VehicleCharacteristics>(leader, gap, latGap, relativeSpeed));
489 // std::shared_ptr<MSTrafficItem> ti = std::make_shared<MSTrafficItem>(TRAFFIC_ITEM_VEHICLE, leader->getID(), tic);
490 // registerTrafficItem(ti);
491 //}
492 //
493 //void
494 //MSDriverState::registerPedestrian(const MSPerson* pedestrian, double gap) {
495 // std::shared_ptr<MSTrafficItemCharacteristics> tic = std::dynamic_pointer_cast<MSTrafficItemCharacteristics>(std::make_shared<PedestrianCharacteristics>(pedestrian, gap));
496 // std::shared_ptr<MSTrafficItem> ti = std::make_shared<MSTrafficItem>(TRAFFIC_ITEM_PEDESTRIAN, pedestrian->getID(), tic);
497 // registerTrafficItem(ti);
498 //}
499 //
500 //void
501 //MSDriverState::registerSpeedLimit(const MSLane* lane, double speedLimit, double dist) {
502 // std::shared_ptr<MSTrafficItemCharacteristics> tic = std::dynamic_pointer_cast<MSTrafficItemCharacteristics>(std::make_shared<SpeedLimitCharacteristics>(lane, dist, speedLimit));
503 // std::shared_ptr<MSTrafficItem> ti = std::make_shared<MSTrafficItem>(TRAFFIC_ITEM_SPEED_LIMIT, lane->getID(), tic);
504 // registerTrafficItem(ti);
505 //}
506 //
507 //void
508 //MSDriverState::registerJunction(MSLink* link, double dist) {
509 // const MSJunction* junction = link->getJunction();
510 // std::shared_ptr<MSTrafficItemCharacteristics> tic = std::dynamic_pointer_cast<MSTrafficItemCharacteristics>(std::make_shared<JunctionCharacteristics>(junction, link, dist));
511 // std::shared_ptr<MSTrafficItem> ti = std::make_shared<MSTrafficItem>(TRAFFIC_ITEM_JUNCTION, junction->getID(), tic);
512 // registerTrafficItem(ti);
513 //}
514 //
515 //void
516 //MSDriverState::registerEgoVehicleState() {
517 // myAmOpposite = myVehicle->getLaneChangeModel().isOpposite();
518 // myCurrentSpeed = myVehicle->getSpeed();
519 // myCurrentAcceleration = myVehicle->getAcceleration();
520 //}
521 //
522 //void
523 //MSDriverState::update() {
524 // // Adapt step duration
525 // updateStepDuration();
526 //
527 // // Replace traffic items from previous step with the newly encountered.
528 // myTrafficItems = myNewTrafficItems;
529 //
530 // // Iterate through present traffic items and take into account the corresponding
531 // // task demands. Further update the item's integration progress.
532 // for (auto& hashItemPair : myTrafficItems) {
533 // // Traffic item
534 // auto ti = hashItemPair.second;
535 // // Take into account the task demand associated with the item
536 // integrateDemand(ti);
537 // // Update integration progress
538 // if (ti->remainingIntegrationTime>0) {
539 // updateItemIntegration(ti);
540 // }
541 // }
542 //
543 // // Update capability (~attention) according to the changed demand
544 // // NOTE: Doing this before recalculating the errors seems more adequate
545 // // than after adjusting the errors, since a very fast time scale
546 // // for the capability could not be captured otherwise. A slow timescale
547 // // could still be tuned to have a desired effect.
548 // adaptTaskCapability();
549 //
550 // // Update driving difficulty
551 // calculateDrivingDifficulty();
552 //
553 // // Update errors
554 // updateAccelerationError();
555 // updateSpeedPerceptionError();
556 // updateHeadwayPerceptionError();
557 // updateActionStepLength();
558 //}
559 //
560 //
561 //void
562 //MSDriverState::integrateDemand(std::shared_ptr<MSTrafficItem> ti) {
563 // myMaxTaskDemand += ti->integrationDemand;
564 // myMaxTaskDemand += ti->latentDemand;
565 //}
566 //
567 //
568 //void
569 //MSDriverState::registerTrafficItem(std::shared_ptr<MSTrafficItem> ti) {
570 // if (myNewTrafficItems.find(ti->id_hash) == myNewTrafficItems.end()) {
571 //
572 // // Update demand associated with the item
573 // auto knownTiIt = myTrafficItems.find(ti->id_hash);
574 // if (knownTiIt == myTrafficItems.end()) {
575 // // new item --> init integration demand and latent task demand
576 // calculateIntegrationDemandAndTime(ti);
577 // } else {
578 // // known item --> only update latent task demand associated with the item
579 // ti = knownTiIt->second;
580 // }
581 // calculateLatentDemand(ti);
582 //
583 // // Track item
584 // myNewTrafficItems[ti->id_hash] = ti;
585 // }
586 //}
587 //
588 //
589 //void
590 //MSDriverState::updateItemIntegration(std::shared_ptr<MSTrafficItem> ti) const {
591 // // Eventually decrease integration time and take into account integration cost.
592 // ti->remainingIntegrationTime -= myStepDuration;
593 // if (ti->remainingIntegrationTime <= 0.) {
594 // ti->remainingIntegrationTime = 0.;
595 // ti->integrationDemand = 0.;
596 // }
597 //}
598 //
599 //
600 //void
601 //MSDriverState::calculateIntegrationDemandAndTime(std::shared_ptr<MSTrafficItem> ti) const {
602 // // @todo Idea is that the integration demand is the quantitatively the same for a specific
603 // // item type with definite characteristics but it can be stretched over time,
604 // // if the integration is less urgent (item farther away), thus resulting in
605 // // smaller effort for a longer time.
606 // switch (ti->type) {
607 // case TRAFFIC_ITEM_JUNCTION: {
608 // std::shared_ptr<JunctionCharacteristics> ch = std::dynamic_pointer_cast<JunctionCharacteristics>(ti->data);
609 // const double totalIntegrationDemand = calculateJunctionIntegrationDemand(ch);
610 // const double integrationTime = calculateIntegrationTime(ch->dist, myVehicle->getSpeed());
611 // ti->integrationDemand = totalIntegrationDemand/integrationTime;
612 // ti->remainingIntegrationTime = integrationTime;
613 // }
614 // break;
615 // case TRAFFIC_ITEM_PEDESTRIAN: {
616 // std::shared_ptr<PedestrianCharacteristics> ch = std::dynamic_pointer_cast<PedestrianCharacteristics>(ti->data);
617 // const double totalIntegrationDemand = calculatePedestrianIntegrationDemand(ch);
618 // const double integrationTime = calculateIntegrationTime(ch->dist, myVehicle->getSpeed());
619 // ti->integrationDemand = totalIntegrationDemand/integrationTime;
620 // ti->remainingIntegrationTime = integrationTime;
621 // }
622 // break;
623 // case TRAFFIC_ITEM_SPEED_LIMIT: {
624 // std::shared_ptr<SpeedLimitCharacteristics> ch = std::dynamic_pointer_cast<SpeedLimitCharacteristics>(ti->data);
625 // const double totalIntegrationDemand = calculateSpeedLimitIntegrationDemand(ch);
626 // const double integrationTime = calculateIntegrationTime(ch->dist, myVehicle->getSpeed());
627 // ti->integrationDemand = totalIntegrationDemand/integrationTime;
628 // ti->remainingIntegrationTime = integrationTime;
629 // }
630 // break;
631 // case TRAFFIC_ITEM_VEHICLE: {
632 // std::shared_ptr<VehicleCharacteristics> ch = std::dynamic_pointer_cast<VehicleCharacteristics>(ti->data);
633 // ti->latentDemand = calculateLatentVehicleDemand(ch);
634 // const double totalIntegrationDemand = calculateVehicleIntegrationDemand(ch);
635 // const double integrationTime = calculateIntegrationTime(ch->longitudinalDist, ch->relativeSpeed);
636 // ti->integrationDemand = totalIntegrationDemand/integrationTime;
637 // ti->remainingIntegrationTime = integrationTime;
638 // }
639 // break;
640 // default:
641 // WRITE_WARNING("Unknown traffic item type!")
642 // break;
643 // }
644 //}
645 //
646 //
647 //double
648 //MSDriverState::calculatePedestrianIntegrationDemand(std::shared_ptr<PedestrianCharacteristics> ch) const {
649 // // Integration demand for a pedestrian
650 // const double INTEGRATION_DEMAND_PEDESTRIAN = 0.5;
651 // return INTEGRATION_DEMAND_PEDESTRIAN;
652 //}
653 //
654 //
655 //double
656 //MSDriverState::calculateSpeedLimitIntegrationDemand(std::shared_ptr<SpeedLimitCharacteristics> ch) const {
657 // // Integration demand for speed limit
658 // const double INTEGRATION_DEMAND_SPEEDLIMIT = 0.1;
659 // return INTEGRATION_DEMAND_SPEEDLIMIT;
660 //}
661 //
662 //
663 //double
664 //MSDriverState::calculateJunctionIntegrationDemand(std::shared_ptr<JunctionCharacteristics> ch) const {
665 // // Latent demand for junction is proportional to number of conflicting lanes
666 // // for the vehicle's path plus a factor for the total number of incoming lanes
667 // // at the junction. Further, the distance to the junction is inversely proportional
668 // // to the induced demand [~1/(c*dist + 1)].
669 // // Traffic lights induce an additional demand
670 // const MSJunction* j = ch->junction;
671 //
672 // // Basic junction integration demand
673 // const double INTEGRATION_DEMAND_JUNCTION_BASE = 0.3;
674 //
675 // // Surplus integration demands
676 // const double INTEGRATION_DEMAND_JUNCTION_TLS = 0.2;
677 // const double INTEGRATION_DEMAND_JUNCTION_FOE_LANE = 0.3; // per foe lane
678 // const double INTEGRATION_DEMAND_JUNCTION_LANE = 0.1; // per lane
679 // const double INTEGRATION_DEMAND_JUNCTION_RAIL = 0.2;
680 // const double INTEGRATION_DEMAND_JUNCTION_ZIPPER = 0.3;
681 //
682 // double result = INTEGRATION_DEMAND_JUNCTION_BASE;
684 // switch (ch->junction->getType()) {
685 // case SumoXMLNodeType::NOJUNCTION:
686 // case SumoXMLNodeType::UNKNOWN:
687 // case SumoXMLNodeType::DISTRICT:
688 // case SumoXMLNodeType::DEAD_END:
689 // case SumoXMLNodeType::DEAD_END_DEPRECATED:
690 // case SumoXMLNodeType::RAIL_SIGNAL: {
691 // result = 0.;
692 // }
693 // break;
694 // case SumoXMLNodeType::RAIL_CROSSING: {
695 // result += INTEGRATION_DEMAND_JUNCTION_RAIL;
696 // }
697 // break;
698 // case SumoXMLNodeType::TRAFFIC_LIGHT:
699 // case SumoXMLNodeType::TRAFFIC_LIGHT_NOJUNCTION:
700 // case SumoXMLNodeType::TRAFFIC_LIGHT_RIGHT_ON_RED: {
701 // // TODO: Take into account traffic light state?
713 // result += INTEGRATION_DEMAND_JUNCTION_TLS;
714 // }
715 // // no break. TLS has extra integration demand.
716 // case SumoXMLNodeType::PRIORITY:
717 // case SumoXMLNodeType::PRIORITY_STOP:
718 // case SumoXMLNodeType::RIGHT_BEFORE_LEFT:
719 // case SumoXMLNodeType::ALLWAY_STOP:
720 // case SumoXMLNodeType::INTERNAL: {
721 // // TODO: Consider link type (major or minor...)
722 // double junctionComplexity = (INTEGRATION_DEMAND_JUNCTION_LANE*j->getNrOfIncomingLanes()
723 // + INTEGRATION_DEMAND_JUNCTION_FOE_LANE*j->getFoeLinks(ch->approachingLink).size());
724 // result += junctionComplexity;
725 // }
726 // break;
727 // case SumoXMLNodeType::ZIPPER: {
728 // result += INTEGRATION_DEMAND_JUNCTION_ZIPPER;
729 // }
730 // break;
731 // default:
732 // assert(false);
733 // result = 0.;
734 // }
735 // return result;
736 //
737 //}
738 //
739 //
740 //double
741 //MSDriverState::calculateVehicleIntegrationDemand(std::shared_ptr<VehicleCharacteristics> ch) const {
742 // // TODO
743 // return 0.;
744 //}
745 //
746 //
747 //double
748 //MSDriverState::calculateIntegrationTime(double dist, double speed) const {
749 // // Fraction of encounter time, which is accounted for the corresponding traffic item's integration
750 // const double INTEGRATION_TIME_COEFF = 0.5;
751 // // Maximal time to be accounted for integration
752 // const double MAX_INTEGRATION_TIME = 5.;
753 // if (speed <= 0.) {
754 // return MAX_INTEGRATION_TIME;
755 // } else {
756 // return MIN2(MAX_INTEGRATION_TIME, INTEGRATION_TIME_COEFF*dist/speed);
757 // }
758 //}
759 //
760 //
761 //void
762 //MSDriverState::calculateLatentDemand(std::shared_ptr<MSTrafficItem> ti) const {
763 // switch (ti->type) {
764 // case TRAFFIC_ITEM_JUNCTION: {
765 // std::shared_ptr<JunctionCharacteristics> ch = std::dynamic_pointer_cast<JunctionCharacteristics>(ti->data);
766 // ti->latentDemand = calculateLatentJunctionDemand(ch);
767 // }
768 // break;
769 // case TRAFFIC_ITEM_PEDESTRIAN: {
770 // std::shared_ptr<PedestrianCharacteristics> ch = std::dynamic_pointer_cast<PedestrianCharacteristics>(ti->data);
771 // ti->latentDemand = calculateLatentPedestrianDemand(ch);
772 // }
773 // break;
774 // case TRAFFIC_ITEM_SPEED_LIMIT: {
775 // std::shared_ptr<SpeedLimitCharacteristics> ch = std::dynamic_pointer_cast<SpeedLimitCharacteristics>(ti->data);
776 // ti->latentDemand = calculateLatentSpeedLimitDemand(ch);
777 // }
778 // break;
779 // case TRAFFIC_ITEM_VEHICLE: {
780 // std::shared_ptr<VehicleCharacteristics> ch = std::dynamic_pointer_cast<VehicleCharacteristics>(ti->data);
781 // ti->latentDemand = calculateLatentVehicleDemand(ch);
782 // }
783 // break;
784 // default:
785 // WRITE_WARNING("Unknown traffic item type!")
786 // break;
787 // }
788 //}
789 //
790 //
791 //double
792 //MSDriverState::calculateLatentPedestrianDemand(std::shared_ptr<PedestrianCharacteristics> ch) const {
793 // // Latent demand for pedestrian is proportional to the euclidean distance to the
794 // // pedestrian (i.e. its potential to 'jump in front of the car) [~1/(c*dist + 1)]
795 // const double LATENT_DEMAND_COEFF_PEDESTRIAN_DIST = 0.1;
796 // const double LATENT_DEMAND_COEFF_PEDESTRIAN = 0.5;
797 // double result = LATENT_DEMAND_COEFF_PEDESTRIAN/(1. + LATENT_DEMAND_COEFF_PEDESTRIAN_DIST*ch->dist);
798 // return result;
799 //}
800 //
801 //
802 //double
803 //MSDriverState::calculateLatentSpeedLimitDemand(std::shared_ptr<SpeedLimitCharacteristics> ch) const {
804 // // Latent demand for speed limit is proportional to speed difference to current vehicle speed
805 // // during approach [~c*(1+deltaV) if dist<threshold].
806 // const double LATENT_DEMAND_COEFF_SPEEDLIMIT_TIME_THRESH = 5;
807 // const double LATENT_DEMAND_COEFF_SPEEDLIMIT = 0.1;
808 // double dist_thresh = LATENT_DEMAND_COEFF_SPEEDLIMIT_TIME_THRESH*myVehicle->getSpeed();
809 // double result = 0.;
810 // if (ch->dist <= dist_thresh && myVehicle->getSpeed() > ch->limit*myVehicle->getChosenSpeedFactor()) {
811 // // Upcoming speed limit does require a slowdown and is close enough.
812 // double dv = myVehicle->getSpeed() - ch->limit*myVehicle->getChosenSpeedFactor();
813 // result = LATENT_DEMAND_COEFF_SPEEDLIMIT*(1 + dv);
814 // }
815 // return result;
816 //}
817 //
818 //
819 //double
820 //MSDriverState::calculateLatentVehicleDemand(std::shared_ptr<VehicleCharacteristics> ch) const {
821 //
822 //
823 // // TODO
824 //
825 //
826 // // Latent demand for neighboring vehicle is determined from the relative and absolute speed,
827 // // and from the lateral and longitudinal distance.
828 // double result = 0.;
829 // const MSVehicle* foe = ch->foe;
830 // if (foe->getEdge() == myVehicle->getEdge()) {
831 // // on same edge
832 // } else if (foe->getEdge() == myVehicle->getEdge()->getOppositeEdge()) {
833 // // on opposite edges
834 // }
835 // return result;
836 //}
837 //
838 //
839 //
840 //double
841 //MSDriverState::calculateLatentJunctionDemand(std::shared_ptr<JunctionCharacteristics> ch) const {
842 // // Latent demand for junction is proportional to number of conflicting lanes
843 // // for the vehicle's path plus a factor for the total number of incoming lanes
844 // // at the junction. Further, the distance to the junction is inversely proportional
845 // // to the induced demand [~1/(c*dist + 1)].
846 // // Traffic lights induce an additional demand
847 // const MSJunction* j = ch->junction;
848 // const double LATENT_DEMAND_COEFF_JUNCTION_TIME_DIST_THRESH = 5; // seconds till arrival, below which junction is relevant
849 // const double LATENT_DEMAND_COEFF_JUNCTION_INCOMING = 0.1;
850 // const double LATENT_DEMAND_COEFF_JUNCTION_FOES = 0.5;
851 // const double LATENT_DEMAND_COEFF_JUNCTION_DIST = 0.1;
852 //
853 // double v = myVehicle->getSpeed();
854 // double dist_thresh = LATENT_DEMAND_COEFF_JUNCTION_TIME_DIST_THRESH*v;
855 //
856 // if (ch->dist > dist_thresh) {
857 // return 0.;
858 // }
859 // double result = 0.;
860 // LinkState linkState = ch->approachingLink->getState();
861 // switch (ch->junction->getType()) {
862 // case SumoXMLNodeType::NOJUNCTION:
863 // case SumoXMLNodeType::UNKNOWN:
864 // case SumoXMLNodeType::DISTRICT:
865 // case SumoXMLNodeType::DEAD_END:
866 // case SumoXMLNodeType::DEAD_END_DEPRECATED:
867 // case SumoXMLNodeType::RAIL_SIGNAL: {
868 // result = 0.;
869 // }
870 // break;
871 // case SumoXMLNodeType::RAIL_CROSSING: {
872 // result = 0.5;
873 // }
874 // break;
875 // case SumoXMLNodeType::TRAFFIC_LIGHT:
876 // case SumoXMLNodeType::TRAFFIC_LIGHT_NOJUNCTION:
877 // case SumoXMLNodeType::TRAFFIC_LIGHT_RIGHT_ON_RED: {
878 // // Take into account traffic light state
879 // switch (linkState) {
880 // case LINKSTATE_TL_GREEN_MAJOR:
881 // result = 0;
882 // break;
883 // case LINKSTATE_TL_GREEN_MINOR:
884 // result = 0.2*(1. + 0.1*v);
885 // break;
886 // case LINKSTATE_TL_RED:
887 // result = 0.1*(1. + 0.1*v);
888 // break;
889 // case LINKSTATE_TL_REDYELLOW:
890 // result = 0.2*(1. + 0.1*v);
891 // break;
892 // case LINKSTATE_TL_YELLOW_MAJOR:
893 // result = 0.1*(1. + 0.1*v);
894 // break;
895 // case LINKSTATE_TL_YELLOW_MINOR:
896 // result = 0.2*(1. + 0.1*v);
897 // break;
898 // case LINKSTATE_TL_OFF_BLINKING:
899 // result = 0.3*(1. + 0.1*v);
900 // break;
901 // case LINKSTATE_TL_OFF_NOSIGNAL:
902 // result = 0.2*(1. + 0.1*v);
903 // }
904 // }
905 // // no break, TLS is accounted extra
906 // case SumoXMLNodeType::PRIORITY:
907 // case SumoXMLNodeType::PRIORITY_STOP:
908 // case SumoXMLNodeType::RIGHT_BEFORE_LEFT:
909 // case SumoXMLNodeType::ALLWAY_STOP:
910 // case SumoXMLNodeType::INTERNAL: {
911 // // TODO: Consider link type (major or minor...)
912 // double junctionComplexity = (LATENT_DEMAND_COEFF_JUNCTION_INCOMING*j->getNrOfIncomingLanes()
913 // + LATENT_DEMAND_COEFF_JUNCTION_FOES*j->getFoeLinks(ch->approachingLink).size())
914 // /(1 + ch->dist*LATENT_DEMAND_COEFF_JUNCTION_DIST);
915 // result += junctionComplexity;
916 // }
917 // break;
918 // case SumoXMLNodeType::ZIPPER: {
919 // result = 0.5*(1. + 0.1*v);
920 // }
921 // break;
922 // default:
923 // assert(false);
924 // result = 0.;
925 // }
926 // return result;
927 //}
928 //
929 
930 
931 /****************************************************************************/
#define DEBUG_COND
#define SPEED2DIST(x)
Definition: SUMOTime.h:43
#define TS
Definition: SUMOTime.h:40
#define SIMTIME
Definition: SUMOTime.h:60
T MAX2(T a, T b)
Definition: StdDefs.h:80
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:46
double getPerceivedSpeedDifference(const double trueSpeedDifference, const double trueGap, const void *objID=nullptr)
This method checks whether the errorneous speed difference that would be perceived for this step diff...
double myMinAwareness
Minimal value for 'awareness' \in [0,1].
double myHeadwayErrorCoefficient
double myHeadwayChangePerceptionThreshold
Thresholds above a change in the corresponding quantity is perceived.
OUProcess myError
Driver's 'error',.
double myActionStepLength
Action step length (~current maximal reaction time) induced by awareness level.
double myErrorNoiseIntensityCoefficient
Coefficient controlling the impact of awareness on the noise intensity of the error process.
double getPerceivedHeadway(const double trueGap, const void *objID=nullptr)
MSSimpleDriverState(MSVehicle *veh)
double myAwareness
Driver's 'awareness' \in [0,1].
double mySpeedDifferenceErrorCoefficient
Scaling coefficients for the magnitude of errors.
double myMaximalReactionTime
Maximal reaction time (value set for the actionStepLength at awareness=myMinAwareness)
std::map< const void *, double > myAssumedGap
The assumed gaps to different objects.
bool debugLocked() const
MSVehicle * myVehicle
Vehicle corresponding to this driver state.
double myOriginalReactionTime
Maximal reaction time (value set for the actionStepLength at awareness=1)
void setAwareness(const double value)
void update()
Trigger updates for the errorProcess, assumed gaps, etc.
std::map< const void *, double > myLastPerceivedSpeedDifference
The last perceived speed differences to the corresponding objects.
void updateAssumedGaps()
Update the assumed gaps to the known objects according to the corresponding perceived speed differenc...
double mySpeedDifferenceChangePerceptionThreshold
double myErrorTimeScaleCoefficient
Coefficient controlling the impact of awareness on the time scale of the error process.
double myLastUpdateTime
Time point of the last state update.
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:75
double getSpeed() const
Returns the vehicle's current speed.
Definition: MSVehicle.h:462
const std::string & getID() const
Returns the id.
Definition: Named.h:74
double getState() const
Obtain the current state of the process.
static SumoRNG myRNG
Random generator for OUProcesses.
Definition: MSDriverState.h:96
void setTimeScale(double timeScale)
set the process' timescale to a new value
Definition: MSDriverState.h:51
OUProcess(double initialState, double timeScale, double noiseIntensity)
constructor
double myState
The current state of the process.
Definition: MSDriverState.h:85
double myTimeScale
The time scale of the process.
Definition: MSDriverState.h:89
~OUProcess()
destructor
double myNoiseIntensity
The noise intensity of the process.
Definition: MSDriverState.h:93
void step(double dt)
evolve for a time step of length dt.
void setNoiseIntensity(double noiseIntensity)
set the process' noise intensity to a new value
Definition: MSDriverState.h:56
void setState(double state)
set the process' state to a new value
Definition: MSDriverState.h:61
static double randNorm(double mean, double variance, SumoRNG *rng=nullptr)
Access to a random number from a normal distribution.
Definition: RandHelper.cpp:82
Default values for the MSDriverState parameters.
static double speedDifferenceChangePerceptionThreshold
static double headwayChangePerceptionThreshold
static double initialAwareness
static double maximalReactionTimeFactor
static double minAwareness
static double headwayErrorCoefficient
static double errorTimeScaleCoefficient
static double errorNoiseIntensityCoefficient
static double speedDifferenceErrorCoefficient