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