Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2013-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 MSDevice_GLOSA.cpp
15 : /// @author Jakob Erdmann
16 : /// @author Mirko Barthauer
17 : /// @date 21.04.2021
18 : ///
19 : // A device for Green Light Optimal Speed Advisory
20 : /****************************************************************************/
21 : #include <config.h>
22 :
23 : #include <utils/common/StringUtils.h>
24 : #include <utils/options/OptionsCont.h>
25 : #include <utils/iodevices/OutputDevice.h>
26 : #include <utils/vehicle/SUMOVehicle.h>
27 : #include <microsim/traffic_lights/MSTrafficLightLogic.h>
28 : #include <microsim/MSNet.h>
29 : #include <microsim/MSLane.h>
30 : #include <microsim/MSEdge.h>
31 : #include <microsim/MSLink.h>
32 : #include <microsim/MSVehicle.h>
33 : #include "MSDevice_GLOSA.h"
34 :
35 : //#define DEBUG_GLOSA
36 : //#define DEBUG_QUEUE
37 : #define DEBUG_COND (true)
38 :
39 : // ===========================================================================
40 : // method definitions
41 : // ===========================================================================
42 : // ---------------------------------------------------------------------------
43 : // static initialisation methods
44 : // ---------------------------------------------------------------------------
45 : void
46 43644 : MSDevice_GLOSA::insertOptions(OptionsCont& oc) {
47 43644 : oc.addOptionSubTopic("GLOSA Device");
48 87288 : insertDefaultAssignmentOptions("glosa", "GLOSA Device", oc);
49 :
50 43644 : oc.doRegister("device.glosa.range", new Option_Float(100.0));
51 87288 : oc.addDescription("device.glosa.range", "GLOSA Device", TL("The communication range to the traffic light"));
52 :
53 43644 : oc.doRegister("device.glosa.max-speedfactor", new Option_Float(1.1));
54 87288 : oc.addDescription("device.glosa.max-speedfactor", "GLOSA Device", TL("The maximum speed factor when approaching a green light"));
55 :
56 43644 : oc.doRegister("device.glosa.min-speed", new Option_Float(5.0));
57 87288 : oc.addDescription("device.glosa.min-speed", "GLOSA Device", TL("Minimum speed when coasting towards a red light"));
58 :
59 43644 : oc.doRegister("device.glosa.add-switchtime", new Option_Float(0.0));
60 87288 : oc.addDescription("device.glosa.add-switchtime", "GLOSA Device", TL("Additional time the vehicle shall need to reach the intersection after the signal turns green"));
61 :
62 43644 : oc.doRegister("device.glosa.use-queue", new Option_Bool(false));
63 87288 : oc.addDescription("device.glosa.use-queue", "GLOSA Device", TL("Use queue in front of the tls for GLOSA calculation"));
64 :
65 43644 : oc.doRegister("device.glosa.override-safety", new Option_Bool(false));
66 87288 : oc.addDescription("device.glosa.override-safety", "GLOSA Device", TL("Override safety features - ignore the current light state, always follow GLOSA's predicted state"));
67 :
68 43644 : oc.doRegister("device.glosa.ignore-cfmodel", new Option_Bool(false));
69 87288 : oc.addDescription("device.glosa.ignore-cfmodel", "GLOSA Device", TL("Vehicles follow a perfect speed calculation - ignore speed calculations from the CF model if not safety critical"));
70 43644 : }
71 :
72 :
73 : void
74 5104363 : MSDevice_GLOSA::buildVehicleDevices(SUMOVehicle& v, std::vector<MSVehicleDevice*>& into) {
75 5104363 : OptionsCont& oc = OptionsCont::getOptions();
76 9484152 : if (!MSGlobals::gUseMesoSim && equippedByDefaultAssignmentOptions(oc, "glosa", v, false)) {
77 48 : MSDevice_GLOSA* device = new MSDevice_GLOSA(v, "glosa_" + v.getID(),
78 : v.getFloatParam("device.glosa.min-speed", true),
79 : v.getFloatParam("device.glosa.range", true),
80 : v.getFloatParam("device.glosa.max-speedfactor", true),
81 : v.getFloatParam("device.glosa.add-switchtime", true),
82 48 : v.getBoolParam("device.glosa.use-queue", true),
83 48 : v.getBoolParam("device.glosa.override-safety", true),
84 120 : v.getBoolParam("device.glosa.ignore-cfmodel", true));
85 24 : into.push_back(device);
86 : }
87 5104363 : }
88 :
89 : void
90 0 : MSDevice_GLOSA::cleanup() {
91 : // cleaning up global state (if any)
92 0 : }
93 :
94 : // ---------------------------------------------------------------------------
95 : // MSDevice_GLOSA-methods
96 : // ---------------------------------------------------------------------------
97 24 : MSDevice_GLOSA::MSDevice_GLOSA(SUMOVehicle& holder, const std::string& id, double minSpeed, double range, double maxSpeedFactor,
98 24 : double addSwitchTime, bool useQueue, bool overrideSafety, bool ignoreCFModel) :
99 : MSVehicleDevice(holder, id),
100 0 : myVeh(dynamic_cast<MSVehicle&>(holder)),
101 24 : myNextTLSLink(nullptr),
102 24 : myDistance(0),
103 24 : myMinSpeed(minSpeed),
104 24 : myRange(range),
105 24 : myMaxSpeedFactor(maxSpeedFactor),
106 24 : myAddSwitchTime(addSwitchTime),
107 24 : myOverrideSafety(overrideSafety),
108 24 : myIgnoreCFModel(ignoreCFModel),
109 24 : mySpeedAdviceActive(false),
110 24 : myUseQueue(useQueue)
111 :
112 : {
113 24 : myOriginalSpeedFactor = myVeh.getChosenSpeedFactor();
114 24 : }
115 :
116 :
117 48 : MSDevice_GLOSA::~MSDevice_GLOSA() {
118 48 : }
119 :
120 :
121 : bool
122 816 : MSDevice_GLOSA::notifyMove(SUMOTrafficObject& /*tObject*/, double oldPos,
123 : double newPos, double /*newSpeed*/) {
124 816 : myDistance -= (newPos - oldPos);
125 816 : if (myNextTLSLink != nullptr && myDistance <= myRange) {
126 412 : const double vMax = myVeh.getLane()->getVehicleMaxSpeed(&myVeh);
127 412 : double timeToJunction = earliest_arrival(myDistance, vMax);
128 412 : int countOld = 0;
129 : // calculate "first" next phase, all coming Phases are calculated via "getTimeToNextSwitch"
130 412 : double timeToSwitch = getTimeToSwitch(myNextTLSLink, countOld);
131 412 : bool currentPhaseGreen = false;
132 412 : bool currentPhaseStop = false;
133 412 : bool solved = false;
134 : double nextSwitch = 0;
135 : nextSwitch = timeToSwitch;
136 : double QueueLength = 0;
137 : double greenTime = 0;
138 : double additionalJunctionTime = 0;
139 : // It takes factor [seconds/per meter queue] to dissolve the queue at drive off
140 : // experimental value from calibrated drone data
141 : double factor = 0.21;
142 : // Additional time (offset) for the leading vehicle to accelerate
143 : // experimental value from calibrated drone data
144 : double addition = 3;
145 : // Vehicles are often not able to drive with their desired speed vMax.
146 : // The further away from the junction, the more uncertain the arrival prediction becomes.
147 : // For a vehicle trying to reach the begin/end of a green phase, this is critical.
148 : // The vehicle should then rather opt for the next phase (trade travel time for traffic flow)
149 : int switchOffset = 0;
150 :
151 412 : if (myNextTLSLink->haveGreen()) { currentPhaseGreen = true; }
152 348 : else if (myNextTLSLink->haveRed() || myNextTLSLink->haveYellow()) { currentPhaseStop = true; }
153 : // else if any other phase, GLOSA does not interfere
154 : #if defined DEBUG_GLOSA || defined DEBUG_QUEUE
155 : if (DEBUG_COND) {
156 : std::cout << SIMTIME << " veh=" << myVeh.getID() << " d=" << myDistance << " ttJ=" << timeToJunction << " ttS=" << timeToSwitch << "\n";
157 : }
158 : #endif
159 412 : if (myUseQueue) {
160 : // Detect queue length at tls
161 0 : QueueLength = myNextTLSLink->getTLLogic()->getTLQueueLength(myNextTLSLink->getLaneBefore()->getID());
162 : #ifdef DEBUG_QUEUE
163 : if (DEBUG_COND) {
164 : std::cout << SIMTIME << " veh=" << myVeh.getID() << " Queuelength=" << QueueLength << "\n";
165 : }
166 : #endif
167 0 : if (currentPhaseGreen) {
168 : // how long has it already been green in this phase
169 0 : greenTime = timeGreen(myNextTLSLink);
170 0 : additionalJunctionTime = (QueueLength * factor + addition) - greenTime;
171 0 : if ((additionalJunctionTime > 0) && (additionalJunctionTime < nextSwitch)) {
172 : // do note use queue system if queue is to long
173 0 : timeToJunction += additionalJunctionTime;
174 : } else {
175 : // important for case: "speed can be increased to arrive at tls while green"
176 : additionalJunctionTime = 0;
177 : }
178 : #ifdef DEBUG_QUEUE
179 : if (DEBUG_COND) {
180 : std::cout << SIMTIME << " veh=" << myVeh.getID() << " Additonaljunctiontime=" << additionalJunctionTime << " Greentime=" << greenTime << " TimetoJunction(GreenQueue)=" << timeToJunction << "\n";
181 : }
182 : #endif
183 : }
184 :
185 0 : if (currentPhaseStop) {
186 0 : nextSwitch += QueueLength * factor + addition;
187 : #ifdef DEBUG_QUEUE
188 : if (DEBUG_COND) {
189 : std::cout << SIMTIME << " veh=" << myVeh.getID() << " Nextswitch(RedQueue)=" << nextSwitch << "\n";
190 : }
191 : #endif
192 : }
193 : }
194 :
195 : // Search for the next passable phase, maximum 10 Phases
196 472 : for (int countwhile = 1; countwhile < 11; countwhile++) {
197 472 : if (currentPhaseGreen) {
198 : // reset nextSwitch because the queue matters only for the current Phase
199 124 : if (countwhile == 2 && myUseQueue) {
200 0 : nextSwitch -= QueueLength * factor + addition;
201 : }
202 124 : if (mySpeedAdviceActive && myOriginalSpeedFactor > myVeh.getChosenSpeedFactor()) {
203 12 : myVeh.setChosenSpeedFactor(myOriginalSpeedFactor);
204 12 : mySpeedAdviceActive = false;
205 : }
206 : #ifdef DEBUG_GLOSA
207 : if (DEBUG_COND) {
208 : std::cout << SIMTIME << " veh=" << myVeh.getID() << " traffic light will be/is green" << "\n";
209 : std::cout << SIMTIME << " veh=" << myVeh.getID()
210 : << " timeToJunction=" << timeToJunction
211 : << " nextSwitch=" << nextSwitch
212 : << " myDistance=" << myDistance << "\n";
213 : }
214 : #endif
215 : // if we arrive at the tls after it switched to red (else do nothing)
216 124 : if (timeToJunction > nextSwitch) {
217 : // if speed can be increased to arrive at tls while green (else look for next phases)
218 44 : if (myMaxSpeedFactor > myOriginalSpeedFactor) {
219 44 : const double vMax2 = vMax / myVeh.getChosenSpeedFactor() * myMaxSpeedFactor;
220 44 : const double timetoJunction2 = earliest_arrival(myDistance, vMax2) + additionalJunctionTime;
221 : // reaching the signal at yellow might be sufficient
222 44 : const double yellowSlack = myVeh.getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_DRIVE_AFTER_YELLOW_TIME, 0);
223 : #ifdef DEBUG_GLOSA
224 : if (DEBUG_COND) {
225 : std::cout << SIMTIME << " veh=" << myVeh.getID()
226 : << " vMax2=" << vMax2
227 : << " timetoJunction2=" << timetoJunction2
228 : << " yellowSlack=" << yellowSlack << "\n";
229 : }
230 : #endif
231 : // if increased speed is fast enough to arrive at tls while green (else look for next phases)
232 44 : if (timetoJunction2 <= (nextSwitch + yellowSlack)) {
233 : // increase speed factor up to a maximum if necessary and useful
234 : // XXX could compute optimal speed factor here
235 44 : myVeh.setChosenSpeedFactor(myMaxSpeedFactor);
236 44 : mySpeedAdviceActive = true;
237 44 : break; // solved
238 : } else {
239 : // speed can not be increased to arrive at tls while green
240 : // next switch is calculated at the end of the for-loop
241 : }
242 : } else {
243 : // speed increase is not enough to reach tls while green
244 : // next switch is calculated at the end of the for-loop
245 : }
246 : } else {
247 : // vehicle will arrive at tls while green
248 : break; // solved
249 : }
250 348 : } else if (currentPhaseStop) {
251 : // tls is red at the moment
252 : #ifdef DEBUG_GLOSA
253 : if (DEBUG_COND) {
254 : std::cout << SIMTIME << " veh=" << myVeh.getID() << " traffic light will be/is red" << "\n";
255 : std::cout << SIMTIME << " veh=" << myVeh.getID()
256 : << " timeToJunction=" << timeToJunction
257 : << " nextSwitch=" << nextSwitch
258 : << " myDistance=" << myDistance << "\n";
259 : }
260 : #endif
261 348 : if (countwhile == 2 && myUseQueue) {
262 : // take queue into account if current green phase can not be passed and next red phase is used instead
263 0 : nextSwitch += QueueLength * factor + addition;
264 : #ifdef DEBUG_QUEUE
265 : if (DEBUG_COND) {
266 : std::cout << SIMTIME << " veh=" << myVeh.getID() << " nextSwitch(redadditon): " << nextSwitch << "\n";
267 : }
268 : #endif
269 : }
270 348 : adaptSpeed(myDistance, timeToJunction, nextSwitch + myAddSwitchTime, solved);
271 348 : if (countwhile == 2 && myUseQueue) {
272 : // reset queue because following phases should not calculate with current queue
273 0 : nextSwitch -= QueueLength * factor + addition;
274 0 : timeToJunction -= additionalJunctionTime;
275 : }
276 348 : if (solved) {
277 : break; // solved
278 : }
279 : }
280 : // calculate next Phase
281 60 : nextSwitch += getTimeToNextSwitch(myNextTLSLink, currentPhaseGreen, currentPhaseStop, countOld);
282 : // For vehicles far away from the junction we add an offset
283 60 : if (nextSwitch > 80.) {
284 4 : nextSwitch += (double)switchOffset;
285 : switchOffset = 6;
286 4 : nextSwitch -= (double)switchOffset;
287 56 : } else if (nextSwitch > 60.) {
288 0 : nextSwitch += (double)switchOffset;
289 : switchOffset = 4;
290 0 : nextSwitch -= (double)switchOffset;
291 56 : } else if (nextSwitch > 40.) {
292 24 : nextSwitch += (double)switchOffset;
293 : switchOffset = 3;
294 24 : nextSwitch -= (double)switchOffset;
295 32 : } else if (nextSwitch > 20.) {
296 32 : nextSwitch += (double)switchOffset;
297 : switchOffset = 2;
298 32 : nextSwitch -= (double)switchOffset;
299 : }
300 : }
301 : }
302 816 : return true; // keep the device
303 : }
304 :
305 :
306 : bool
307 80 : MSDevice_GLOSA::notifyEnter(SUMOTrafficObject& /*veh*/, MSMoveReminder::Notification /*reason*/, const MSLane* /* enteredLane */) {
308 80 : const MSLink* prevLink = myNextTLSLink;
309 80 : myNextTLSLink = nullptr;
310 80 : const MSLane* lane = myVeh.getLane();
311 80 : if (myVeh.getDeparture() < SIMSTEP) {
312 : // no need to call at insertion
313 56 : myVeh.updateBestLanes();
314 : }
315 80 : const std::vector<MSLane*>& bestLaneConts = myVeh.getBestLanesContinuation(lane);
316 80 : double seen = lane->getLength() - myVeh.getPositionOnLane();
317 : int view = 1;
318 80 : std::vector<MSLink*>::const_iterator linkIt = MSLane::succLinkSec(myVeh, view, *lane, bestLaneConts);
319 116 : while (!lane->isLinkEnd(linkIt)) {
320 68 : if (!lane->getEdge().isInternal()) {
321 36 : if ((*linkIt)->isTLSControlled()) {
322 32 : myNextTLSLink = *linkIt;
323 32 : myDistance = seen;
324 32 : break;
325 : }
326 : }
327 36 : lane = (*linkIt)->getViaLaneOrLane();
328 36 : if (!lane->getEdge().isInternal()) {
329 32 : view++;
330 : }
331 36 : seen += lane->getLength();
332 36 : linkIt = MSLane::succLinkSec(myVeh, view, *lane, bestLaneConts);
333 : }
334 80 : if (prevLink != nullptr && myNextTLSLink == nullptr) {
335 : // moved pass tls
336 24 : myVeh.setChosenSpeedFactor(myOriginalSpeedFactor);
337 24 : mySpeedAdviceActive = false;
338 56 : } else if (myNextTLSLink != nullptr && prevLink != myNextTLSLink) {
339 : // approaching new tls
340 : double tlsRange = 1e10;
341 48 : const std::string val = myNextTLSLink->getTLLogic()->getParameter("device.glosa.range", "1e10");
342 : try {
343 24 : tlsRange = StringUtils::toDouble(val);
344 0 : } catch (const NumberFormatException&) {
345 0 : WRITE_WARNINGF(TL("Invalid value '%' for parameter 'device.glosa.range' of traffic light '%'"),
346 : val, myNextTLSLink->getTLLogic()->getID());
347 0 : }
348 48 : myRange = MIN2(myVeh.getFloatParam("device.glosa.range", true), tlsRange);
349 : }
350 :
351 : #ifdef DEBUG_GLOSA
352 : if (DEBUG_COND) {
353 : std::cout << SIMTIME << " veh=" << myVeh.getID() << " enter=" << myVeh.getLane()->getID() << " tls=" << (myNextTLSLink == nullptr ? "NULL" : myNextTLSLink->getTLLogic()->getID()) << " dist=" << myDistance << "\n";
354 : }
355 : #endif
356 80 : return true; // keep the device
357 : }
358 :
359 :
360 : double
361 412 : MSDevice_GLOSA::getTimeToSwitch(const MSLink* tlsLink, int& countOld) {
362 : assert(tlsLink != nullptr);
363 : const MSTrafficLightLogic* const tl = tlsLink->getTLLogic();
364 : assert(tl != nullptr);
365 412 : const auto& phases = tl->getPhases();
366 412 : const int n = (int)phases.size();
367 412 : const int cur = tl->getCurrentPhaseIndex();
368 412 : SUMOTime result = tl->getNextSwitchTime() - SIMSTEP;
369 :
370 1008 : for (int i = 1; i < n; i++) {
371 1008 : const auto& phase = phases[(cur + i) % n];
372 1008 : const char ls = phase->getState()[tlsLink->getTLIndex()];
373 944 : if (((tlsLink->haveRed() || tlsLink->haveYellow()) && (ls == 'g' || ls == 'G'))
374 660 : || (tlsLink->haveGreen() && ls != 'g' && ls != 'G')) {
375 412 : countOld = cur + i;
376 412 : break;
377 : }
378 596 : result += phase->duration;
379 : }
380 412 : return STEPS2TIME(result);
381 : }
382 :
383 :
384 : double
385 60 : MSDevice_GLOSA::getTimeToNextSwitch(const MSLink* tlsLink, bool& currentPhaseGreen, bool& currentPhaseStop, int& countOld) {
386 : // get time till the tls switches to a phase the vehicle can reach
387 : assert(tlsLink != nullptr);
388 : const MSTrafficLightLogic* const tl = tlsLink->getTLLogic();
389 : assert(tl != nullptr);
390 60 : const auto& phases = tl->getPhases();
391 60 : const int n = (int)phases.size();
392 60 : const int cur = countOld;
393 : SUMOTime result = 0;
394 :
395 120 : for (int i = 0; i < n; i++) {
396 120 : const auto& phase = phases[(cur + i) % n];
397 120 : const char ls = phase->getState()[tlsLink->getTLIndex()];
398 120 : if (currentPhaseGreen && (ls == 'g' || ls == 'G')) {
399 0 : countOld = (cur + i)%n;
400 0 : break;
401 : }
402 120 : if (currentPhaseStop && (ls != 'g' && ls != 'G')) {
403 60 : countOld = (cur + i)%n;
404 60 : break;
405 : }
406 60 : result += phase->duration;
407 : }
408 60 : currentPhaseGreen = !currentPhaseGreen;
409 60 : currentPhaseStop = !currentPhaseStop;
410 60 : return STEPS2TIME(result);
411 : }
412 :
413 : double
414 0 : MSDevice_GLOSA::timeGreen(const MSLink* tlsLink) {
415 : assert(tlsLink != nullptr);
416 : const MSTrafficLightLogic* const tl = tlsLink->getTLLogic();
417 : assert(tl != nullptr);
418 0 : const auto& phases = tl->getPhases();
419 0 : const int n = (int)phases.size();
420 0 : const int cur = tl->getCurrentPhaseIndex();
421 : // As there are multiple "microphases G" in one Greenphase this function only gives back
422 : // the already spent time in the current microphase
423 0 : SUMOTime result = tl->getSpentDuration();
424 :
425 0 : for (int i = 1; i < n; i++) {
426 0 : const auto& phase = phases[(cur - i) % n];
427 0 : const char ls = phase->getState()[tlsLink->getTLIndex()];
428 0 : if ( ls != 'g' && ls != 'G') {
429 : break;
430 : }
431 0 : result += phase->duration;
432 : }
433 0 : return STEPS2TIME(result);
434 : }
435 :
436 :
437 :
438 : double
439 456 : MSDevice_GLOSA::earliest_arrival(double distance, double vMax) {
440 : // assume we keep acceleration until we hit maximum speed
441 456 : const double v = myVeh.getSpeed();
442 456 : const double a = myVeh.getCarFollowModel().getMaxAccel();
443 456 : const double accel_time = MIN2((vMax - v) / a, time_to_junction_at_continuous_accel(distance, v));
444 456 : const double remaining_dist = distance - distance_at_continuous_accel(v, accel_time);
445 456 : const double remaining_time = remaining_dist / vMax;
446 456 : return accel_time + remaining_time;
447 : }
448 :
449 :
450 : /*
451 : double
452 : MSDevice_GLOSA::latest_arrival(speed, distance, earliest) {
453 : // assume we keep current speed until within myRange and then decelerate to myMinSpeed
454 : speed = max(speed, GLOSA_MIN_SPEED)
455 : potential_decel_dist = min(distance, GLOSA_RANGE)
456 : decel_time = (speed - GLOSA_MIN_SPEED) / GLOSA_DECEL
457 : avg_decel_speed = (speed + GLOSA_MIN_SPEED) / 2.0
458 : decel_dist = decel_time * avg_decel_speed
459 : if decel_dist > potential_decel_dist:
460 : decel_dist = potential_decel_dist
461 : # XXX actually avg_decel_speed is higher in this case
462 : decel_time = decel_dist / avg_decel_speed
463 : slow_dist = potential_decel_dist - decel_dist
464 : fast_dist = distance - (decel_dist + slow_dist)
465 : result = fast_dist / speed + decel_time + slow_dist / GLOSA_MIN_SPEED
466 : if result < earliest:
467 : if (distance > 15):
468 : print("DEBUG: fixing latest arrival of %s to match earliest of %s" % (result, earliest))
469 : result = earliest
470 : return result
471 : return 0;
472 : }
473 : */
474 :
475 :
476 : double
477 456 : MSDevice_GLOSA::distance_at_continuous_accel(double speed, double time) {
478 : const double v = speed;
479 : const double t = time;
480 456 : const double a = myVeh.getCarFollowModel().getMaxAccel();
481 : // integrated area composed of a rectangle and a triangle
482 456 : return v * t + a * t * t / 2;
483 : }
484 :
485 :
486 : double
487 456 : MSDevice_GLOSA::time_to_junction_at_continuous_accel(double d, double v) {
488 : // see distance_at_continuous_accel
489 : // t^2 + (2v/a)t - 2d/a = 0
490 456 : const double a = myVeh.getCarFollowModel().getMaxAccel();
491 456 : const double p_half = v / a;
492 456 : const double t = -p_half + sqrt(p_half * p_half + 2 * d / a);
493 456 : return t;
494 : }
495 :
496 :
497 : void
498 348 : MSDevice_GLOSA::adaptSpeed(double distance, double /*timeToJunction*/, double timeToSwitch, bool &solved) {
499 : // ensure that myVehicle arrives at the
500 : // junction with maximum speed when it switches to green
501 : // car performs a slowDown at time z to speed x for duration y
502 : // there are two basic strategies
503 : // a) maximize z -> this saves road space but leads to low x and thus excessive braking
504 : // b) maximize x -> this saves fuel but wastes road
505 : // c) compromise: b) but only when distance to junction is below a threshold
506 :
507 348 : const double vMax = myVeh.getLane()->getSpeedLimit() * myOriginalSpeedFactor;
508 :
509 : // need to start/continue maneuver
510 : const double t = timeToSwitch;
511 348 : const double a = myVeh.getCarFollowModel().getMaxAccel();
512 348 : const double u = myMinSpeed;
513 : const double w = vMax;
514 : const double s = distance;
515 348 : const double v = myVeh.getSpeed();
516 : // x : target speed
517 : // y : slow down duration
518 : // s is composed of 1 trapezoid (decel), 1 rectangle (maintain), 1 trapezoid (accel)
519 : // s = (v^2-x^2)/2d + x*(t-y-(w-x)/a) + (w^2-x^2)/2a
520 : // y = (v-x)/d
521 : // solution for x curtesy of mathomatic.org
522 :
523 : // First, we calculate targetSpeed assuming we are driving that speed already (v=x)
524 : // If this results in targetSpeed < currentSpeed, then we need to decelerate (and vice versa)
525 348 : const double rootConst = a * a * t * t - 2.0 * a * w * t + 2 * a * s;
526 : double vConst = 0;
527 348 : if (rootConst >= 0) {
528 324 : vConst = sqrt(rootConst) - a * t + w;
529 : }
530 348 : double d = myVeh.getCarFollowModel().getMaxDecel();
531 348 : if (v < vConst) {
532 : d = a;
533 : }
534 :
535 : // Second, we calculate the correct targetSpeed, knowing if we need to accelerate or decelerate
536 : const double sign0 = -1; // quadratic formula solution x1 (choose solution with higher speed)
537 348 : const double root_argument = a * d * ((2.0 * d * (s - (w * t))) - ((v - w) * (v - w)) + (a * ((d * (t * t)) + (2.0 * (s - (t * v))))));
538 348 : if (root_argument < 0) {
539 : #ifdef DEBUG_GLOSA
540 : WRITE_WARNINGF("GLOSA error 1 root_argument=% s=% t=% v=%", root_argument, s, t, v);
541 : #endif
542 : // no reset of speedFactor because in this case, current speed can be kept
543 32 : solved = true;
544 :
545 32 : return;
546 : }
547 316 : const double x = (((a * (v - (d * t))) + (d * w) - sign0 * sqrt(root_argument)) / (d + a));
548 316 : double y = (v - x) / d;
549 316 : if (v < x) {
550 176 : y = (x - v) / d;
551 : }
552 316 : if (s < (w * w - x * x) / 2.0 / a) {
553 : // end maneuver
554 4 : if (myIgnoreCFModel) {
555 : std::vector<std::pair<SUMOTime, double> > speedTimeLine;
556 0 : speedTimeLine.push_back(std::make_pair(MSNet::getInstance()->getCurrentTimeStep(), myVeh.getSpeed()));
557 0 : speedTimeLine.push_back(std::make_pair(MSNet::getInstance()->getCurrentTimeStep() + TIME2STEPS(w - x / a), vMax));
558 0 : myVeh.getInfluencer().setSpeedTimeLine(speedTimeLine);
559 0 : } else {
560 4 : myVeh.setChosenSpeedFactor(myOriginalSpeedFactor);
561 : }
562 4 : mySpeedAdviceActive = false;
563 4 : return;
564 : }
565 312 : if (!(x >= u && x <= w && y < t)) {
566 : #ifdef DEBUG_GLOSA
567 : WRITE_WARNINGF("GLOSA error 2 x=% y=% s=% t=% v=%", x, y, s, t, v);
568 : #endif
569 56 : if (x < u) {
570 : #ifdef DEBUG_GLOSA
571 : if (DEBUG_COND) {
572 : std::cout << "veh=" << myVeh.getID() << " cant go slow enough" << "\n";
573 : }
574 : #endif
575 : // no reset of speedFactor because in this case, current speed can be kept
576 : //myVeh.setChosenSpeedFactor(myOriginalSpeedFactor);
577 : //mySpeedAdviceActive = false;
578 0 : solved = true;
579 : }
580 : if (x > w) {
581 : #ifdef DEBUG_GLOSA
582 : if (DEBUG_COND) {
583 : std::cout << "veh=" << myVeh.getID() << " cant go fast enough" << "\n";
584 : }
585 : #endif
586 : }
587 56 : return;
588 : }
589 : const double targetSpeed = x;
590 256 : const double duration = MAX2(y, TS);
591 256 : solved = true;
592 : #ifdef DEBUG_GLOSA
593 : if (DEBUG_COND) {
594 : std::cout << " targetSpeed=" << targetSpeed << " duration=" << duration << "\n";
595 : }
596 : #endif
597 256 : if (myIgnoreCFModel) {
598 : std::vector<std::pair<SUMOTime, double> > speedTimeLine;
599 0 : speedTimeLine.push_back(std::make_pair(MSNet::getInstance()->getCurrentTimeStep(), myVeh.getSpeed()));
600 0 : speedTimeLine.push_back(std::make_pair(MSNet::getInstance()->getCurrentTimeStep() + TIME2STEPS(duration), targetSpeed));
601 0 : myVeh.getInfluencer().setSpeedTimeLine(speedTimeLine);
602 0 : } else {
603 256 : myVeh.setChosenSpeedFactor(targetSpeed / myVeh.getLane()->getSpeedLimit());
604 : }
605 256 : mySpeedAdviceActive = true;
606 : }
607 :
608 :
609 : void
610 24 : MSDevice_GLOSA::generateOutput(OutputDevice* /*tripinfoOut*/) const {
611 : /*
612 : if (tripinfoOut != nullptr) {
613 : tripinfoOut->openTag("glosa_device");
614 : tripinfoOut->closeTag();
615 : }
616 : */
617 24 : }
618 :
619 : std::string
620 0 : MSDevice_GLOSA::getParameter(const std::string& key) const {
621 0 : if (key == "minSpeed") {
622 0 : return toString(myMinSpeed);
623 : }
624 0 : throw InvalidArgument("Parameter '" + key + "' is not supported for device of type '" + deviceName() + "'");
625 : }
626 :
627 :
628 : void
629 0 : MSDevice_GLOSA::setParameter(const std::string& key, const std::string& value) {
630 : double doubleValue;
631 : try {
632 0 : doubleValue = StringUtils::toDouble(value);
633 0 : } catch (NumberFormatException&) {
634 0 : throw InvalidArgument("Setting parameter '" + key + "' requires a number for device of type '" + deviceName() + "'");
635 0 : }
636 0 : if (key == "minSpeed") {
637 0 : myMinSpeed = doubleValue;
638 : } else {
639 0 : throw InvalidArgument("Setting parameter '" + key + "' is not supported for device of type '" + deviceName() + "'");
640 : }
641 0 : }
642 :
643 :
644 : /****************************************************************************/
|
