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