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 : /// @date 21.04.2021
17 : ///
18 : // A device for Green Light Optimal Speed Advisory
19 : /****************************************************************************/
20 : #include <config.h>
21 :
22 : #include <utils/common/StringUtils.h>
23 : #include <utils/options/OptionsCont.h>
24 : #include <utils/iodevices/OutputDevice.h>
25 : #include <utils/vehicle/SUMOVehicle.h>
26 : #include <microsim/traffic_lights/MSTrafficLightLogic.h>
27 : #include <microsim/MSNet.h>
28 : #include <microsim/MSLane.h>
29 : #include <microsim/MSEdge.h>
30 : #include <microsim/MSLink.h>
31 : #include <microsim/MSVehicle.h>
32 : #include "MSDevice_GLOSA.h"
33 :
34 : //#define DEBUG_GLOSA
35 : #define DEBUG_COND (true)
36 :
37 : // ===========================================================================
38 : // method definitions
39 : // ===========================================================================
40 : // ---------------------------------------------------------------------------
41 : // static initialisation methods
42 : // ---------------------------------------------------------------------------
43 : void
44 36320 : MSDevice_GLOSA::insertOptions(OptionsCont& oc) {
45 36320 : oc.addOptionSubTopic("GLOSA Device");
46 72640 : insertDefaultAssignmentOptions("glosa", "GLOSA Device", oc);
47 :
48 36320 : oc.doRegister("device.glosa.range", new Option_Float(100.0));
49 72640 : oc.addDescription("device.glosa.range", "GLOSA Device", TL("The communication range to the traffic light"));
50 :
51 36320 : oc.doRegister("device.glosa.max-speedfactor", new Option_Float(1.1));
52 72640 : oc.addDescription("device.glosa.max-speedfactor", "GLOSA Device", TL("The maximum speed factor when approaching a green light"));
53 :
54 36320 : oc.doRegister("device.glosa.min-speed", new Option_Float(5.0));
55 72640 : oc.addDescription("device.glosa.min-speed", "GLOSA Device", TL("Minimum speed when coasting towards a red light"));
56 36320 : }
57 :
58 :
59 : void
60 4658932 : MSDevice_GLOSA::buildVehicleDevices(SUMOVehicle& v, std::vector<MSVehicleDevice*>& into) {
61 4658932 : OptionsCont& oc = OptionsCont::getOptions();
62 12288092 : if (!MSGlobals::gUseMesoSim && equippedByDefaultAssignmentOptions(oc, "glosa", v, false)) {
63 16 : MSDevice_GLOSA* device = new MSDevice_GLOSA(v, "glosa_" + v.getID(),
64 32 : getFloatParam(v, OptionsCont::getOptions(), "glosa.min-speed", 5, true),
65 32 : getFloatParam(v, OptionsCont::getOptions(), "glosa.range", 100, true),
66 32 : getFloatParam(v, OptionsCont::getOptions(), "glosa.max-speedfactor", 1.1, true));
67 16 : into.push_back(device);
68 : }
69 4658932 : }
70 :
71 : void
72 0 : MSDevice_GLOSA::cleanup() {
73 : // cleaning up global state (if any)
74 0 : }
75 :
76 : // ---------------------------------------------------------------------------
77 : // MSDevice_GLOSA-methods
78 : // ---------------------------------------------------------------------------
79 16 : MSDevice_GLOSA::MSDevice_GLOSA(SUMOVehicle& holder, const std::string& id, double minSpeed, double range, double maxSpeedFactor) :
80 : MSVehicleDevice(holder, id),
81 0 : myVeh(dynamic_cast<MSVehicle&>(holder)),
82 16 : myNextTLSLink(nullptr),
83 16 : myDistance(0),
84 16 : myMinSpeed(minSpeed),
85 16 : myRange(range),
86 16 : myMaxSpeedFactor(maxSpeedFactor)
87 :
88 : {
89 16 : myOriginalSpeedFactor = myVeh.getChosenSpeedFactor();
90 16 : }
91 :
92 :
93 32 : MSDevice_GLOSA::~MSDevice_GLOSA() {
94 32 : }
95 :
96 :
97 : bool
98 564 : MSDevice_GLOSA::notifyMove(SUMOTrafficObject& /*tObject*/, double oldPos,
99 : double newPos, double /*newSpeed*/) {
100 564 : myDistance -= (newPos - oldPos);
101 564 : if (myNextTLSLink != nullptr && myDistance <= myRange) {
102 268 : const double vMax = myVeh.getLane()->getVehicleMaxSpeed(&myVeh);
103 268 : const double timeToJunction = earliest_arrival(myDistance, vMax);
104 268 : const double timeToSwitch = getTimeToSwitch(myNextTLSLink);
105 : #ifdef DEBUG_GLOSA
106 : if (DEBUG_COND) {
107 : std::cout << SIMTIME << " veh=" << myVeh.getID() << " d=" << myDistance << " ttJ=" << timeToJunction << " ttS=" << timeToSwitch << "\n";
108 : }
109 : #endif
110 268 : if (myNextTLSLink->haveGreen()) {
111 48 : if (timeToJunction > timeToSwitch) {
112 44 : if (myMaxSpeedFactor > myVeh.getChosenSpeedFactor()) {
113 4 : const double vMax2 = vMax / myVeh.getChosenSpeedFactor() * myMaxSpeedFactor;
114 4 : const double timetoJunction2 = earliest_arrival(myDistance, vMax2);
115 : // reaching the signal at yellow might be sufficient
116 4 : const double yellowSlack = myVeh.getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_DRIVE_AFTER_YELLOW_TIME, 0);
117 : #ifdef DEBUG_GLOSA
118 : if (DEBUG_COND) {
119 : std::cout << " vMax2=" << vMax2 << " ttJ2=" << timetoJunction2 << " yellowSlack=" << yellowSlack << "\n";
120 : }
121 : #endif
122 4 : if (timetoJunction2 <= (timeToSwitch + yellowSlack)) {
123 : // increase speed factor up to a maximum if necessary and useful
124 : // XXX could compute optimal speed factor here
125 4 : myVeh.setChosenSpeedFactor(myMaxSpeedFactor);
126 : }
127 : }
128 : }
129 220 : } else if (myNextTLSLink->haveRed()) {
130 216 : adaptSpeed(myDistance, timeToJunction, timeToSwitch);
131 : }
132 : }
133 564 : return true; // keep the device
134 : }
135 :
136 :
137 : bool
138 48 : MSDevice_GLOSA::notifyEnter(SUMOTrafficObject& /*veh*/, MSMoveReminder::Notification /*reason*/, const MSLane* /* enteredLane */) {
139 48 : const MSLink* prevLink = myNextTLSLink;
140 48 : myNextTLSLink = nullptr;
141 48 : const MSLane* lane = myVeh.getLane();
142 48 : const std::vector<MSLane*>& bestLaneConts = myVeh.getBestLanesContinuation(lane);
143 48 : double seen = lane->getLength() - myVeh.getPositionOnLane();
144 : int view = 1;
145 48 : std::vector<MSLink*>::const_iterator linkIt = MSLane::succLinkSec(myVeh, view, *lane, bestLaneConts);
146 64 : while (!lane->isLinkEnd(linkIt)) {
147 32 : if (!lane->getEdge().isInternal()) {
148 16 : if ((*linkIt)->isTLSControlled()) {
149 16 : myNextTLSLink = *linkIt;
150 16 : myDistance = seen;
151 16 : break;
152 : }
153 : }
154 16 : lane = (*linkIt)->getViaLaneOrLane();
155 16 : if (!lane->getEdge().isInternal()) {
156 16 : view++;
157 : }
158 16 : seen += lane->getLength();
159 16 : linkIt = MSLane::succLinkSec(myVeh, view, *lane, bestLaneConts);
160 : }
161 48 : if (prevLink != nullptr && myNextTLSLink == nullptr) {
162 : // moved passt tls
163 16 : myVeh.setChosenSpeedFactor(myOriginalSpeedFactor);
164 32 : } else if (myNextTLSLink != nullptr && prevLink != myNextTLSLink) {
165 : // approaching new tls
166 : double tlsRange = 1e10;
167 32 : const std::string val = myNextTLSLink->getTLLogic()->getParameter("device.glosa.range", "1e10");
168 : try {
169 16 : tlsRange = StringUtils::toDouble(val);
170 0 : } catch (const NumberFormatException&) {
171 0 : WRITE_WARNINGF(TL("Invalid value '%' for parameter 'device.glosa.range' of traffic light '%'"),
172 : val, myNextTLSLink->getTLLogic()->getID());
173 0 : }
174 32 : myRange = MIN2(getFloatParam(myVeh, OptionsCont::getOptions(), "glosa.range", 100, true), tlsRange);
175 : }
176 :
177 : #ifdef DEBUG_GLOSA
178 : if (DEBUG_COND) std::cout << SIMTIME << " veh=" << myVeh.getID() << " enter=" << myVeh.getLane()->getID() << " hadTLS=" << hadTLS
179 : << " tls=" << (myNextTLSLink == nullptr ? "NULL" : myNextTLSLink->getTLLogic()->getID()) << " dist=" << myDistance << "\n";
180 : #endif
181 48 : return true; // keep the device
182 : }
183 :
184 :
185 : double
186 268 : MSDevice_GLOSA::getTimeToSwitch(const MSLink* tlsLink) {
187 : assert(tlsLink != nullptr);
188 : const MSTrafficLightLogic* const tl = tlsLink->getTLLogic();
189 : assert(tl != nullptr);
190 268 : const auto& phases = tl->getPhases();
191 268 : const int n = (int)phases.size();
192 268 : const int cur = tl->getCurrentPhaseIndex();
193 268 : SUMOTime result = tl->getNextSwitchTime() - SIMSTEP;
194 676 : for (int i = 1; i < n; i++) {
195 672 : const auto& phase = phases[(cur + i) % n];
196 672 : const char ls = phase->getState()[tlsLink->getTLIndex()];
197 596 : if ((tlsLink->haveRed() && (ls == 'g' || ls == 'G'))
198 1052 : || (tlsLink->haveGreen() && ls != 'g' && ls != 'G')) {
199 : break;
200 : }
201 408 : result += phase->duration;
202 : }
203 268 : return STEPS2TIME(result);
204 : }
205 :
206 :
207 : double
208 272 : MSDevice_GLOSA::earliest_arrival(double distance, double vMax) {
209 : // assume we keep acceleration until we hit maximum speed
210 272 : const double v = myVeh.getSpeed();
211 272 : const double a = myVeh.getCarFollowModel().getMaxAccel();
212 272 : const double accel_time = MIN2((vMax - v) / a, time_to_junction_at_continuous_accel(distance, v));
213 272 : const double remaining_dist = distance - distance_at_continuous_accel(v, accel_time);
214 272 : const double remaining_time = remaining_dist / vMax;
215 272 : return accel_time + remaining_time;
216 : }
217 :
218 :
219 : /*
220 : double
221 : MSDevice_GLOSA::latest_arrival(speed, distance, earliest) {
222 : // assume we keep current speed until within myRange and then decelerate to myMinSpeed
223 : speed = max(speed, GLOSA_MIN_SPEED)
224 : potential_decel_dist = min(distance, GLOSA_RANGE)
225 : decel_time = (speed - GLOSA_MIN_SPEED) / GLOSA_DECEL
226 : avg_decel_speed = (speed + GLOSA_MIN_SPEED) / 2.0
227 : decel_dist = decel_time * avg_decel_speed
228 : if decel_dist > potential_decel_dist:
229 : decel_dist = potential_decel_dist
230 : # XXX actually avg_decel_speed is higher in this case
231 : decel_time = decel_dist / avg_decel_speed
232 : slow_dist = potential_decel_dist - decel_dist
233 : fast_dist = distance - (decel_dist + slow_dist)
234 : result = fast_dist / speed + decel_time + slow_dist / GLOSA_MIN_SPEED
235 : if result < earliest:
236 : if (distance > 15):
237 : print("DEBUG: fixing latest arrival of %s to match earliest of %s" % (result, earliest))
238 : result = earliest
239 : return result
240 : return 0;
241 : }
242 : */
243 :
244 :
245 : double
246 272 : MSDevice_GLOSA::distance_at_continuous_accel(double speed, double time) {
247 : const double v = speed;
248 : const double t = time;
249 272 : const double a = myVeh.getCarFollowModel().getMaxAccel();
250 : // integrated area composed of a rectangle and a triangle
251 272 : return v * t + a * t * t / 2;
252 : }
253 :
254 :
255 : double
256 272 : MSDevice_GLOSA::time_to_junction_at_continuous_accel(double d, double v) {
257 : // see distance_at_continuous_accel
258 : // t^2 + (2v/a)t - 2d/a = 0
259 272 : const double a = myVeh.getCarFollowModel().getMaxAccel();
260 272 : const double p_half = v / a;
261 272 : const double t = -p_half + sqrt(p_half * p_half + 2 * d / a);
262 272 : return t;
263 : }
264 :
265 :
266 : void
267 216 : MSDevice_GLOSA::adaptSpeed(double distance, double timeToJunction, double timeToSwitch) {
268 : // ensure that myVehicle arrives at the
269 : // junction with maximum speed when it switches to green
270 : // car performs a slowDown at time z to speed x for duration y
271 : // there are two basic strategies
272 : // a) maximize z -> this saves road space but leads to low x and thus excessive braking
273 : // b) maximize x -> this saves fuel but wastes road
274 : // c) compromise: b) but only when distance to junction is below a threshold
275 :
276 216 : const double vMax = myVeh.getLane()->getVehicleMaxSpeed(&myVeh);
277 : if (timeToJunction < timeToSwitch
278 216 : && myVeh.getSpeed() > myMinSpeed) {
279 : // need to start/continue maneuver
280 : const double t = timeToSwitch;
281 160 : const double a = myVeh.getCarFollowModel().getMaxAccel();
282 : const double d = myVeh.getCarFollowModel().getMaxDecel();
283 : const double u = myMinSpeed;
284 : const double w = vMax;
285 : const double s = distance;
286 160 : const double v = myVeh.getSpeed();
287 : // x : target speed
288 : // y : slow down duration
289 : // s is composed of 1 trapezoid (decel), 1 rectangle (maintain), 1 trapezoid (accel)
290 : // s = (v^2-x^2)/2d + x*(y-(v-x)/d) + (w^2-x^2)/2a
291 : // y = t - (w-x)/d
292 : // solution for x curtesy of mathomatic.org
293 : const double sign0 = -1; // XXX hack
294 160 : 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))))));
295 160 : if (root_argument < 0) {
296 : #ifdef DEBUG_GLOSA
297 : WRITE_WARNINGF("GLOSA error 1 root_argument=% s=% t=% v=%", root_argument, s, t, v);
298 : #endif
299 84 : return;
300 : }
301 124 : const double x = (((a * (v - (d * t))) + (d * w) - sign0 * sqrt(root_argument)) / (d + a));
302 124 : const double y = t - (w - x) / d;
303 124 : if (!(x >= u && x <= w && y > 0 && y < t)) {
304 : #ifdef DEBUG_GLOSA
305 : WRITE_WARNINGF("GLOSA error 2 x=% y=% s=% t=% v=%", x, y, s, t, v);
306 : #endif
307 : return;
308 : }
309 : const double targetSpeed = x;
310 : const double duration = y;
311 : #ifdef DEBUG_GLOSA
312 : if (DEBUG_COND) {
313 : std::cout << " targetSpeed=" << targetSpeed << " duration=" << duration << "\n";
314 : }
315 : #endif
316 : std::vector<std::pair<SUMOTime, double> > speedTimeLine;
317 76 : speedTimeLine.push_back(std::make_pair(MSNet::getInstance()->getCurrentTimeStep(), myVeh.getSpeed()));
318 76 : speedTimeLine.push_back(std::make_pair(MSNet::getInstance()->getCurrentTimeStep() + TIME2STEPS(duration), targetSpeed));
319 76 : myVeh.getInfluencer().setSpeedTimeLine(speedTimeLine);
320 : } else {
321 : // end maneuver
322 : std::vector<std::pair<SUMOTime, double> > speedTimeLine;
323 56 : speedTimeLine.push_back(std::make_pair(MSNet::getInstance()->getCurrentTimeStep(), myVeh.getSpeed()));
324 56 : speedTimeLine.push_back(std::make_pair(MSNet::getInstance()->getCurrentTimeStep(), vMax));
325 56 : myVeh.getInfluencer().setSpeedTimeLine(speedTimeLine);
326 : }
327 : }
328 :
329 :
330 : void
331 16 : MSDevice_GLOSA::generateOutput(OutputDevice* /*tripinfoOut*/) const {
332 : /*
333 : if (tripinfoOut != nullptr) {
334 : tripinfoOut->openTag("glosa_device");
335 : tripinfoOut->closeTag();
336 : }
337 : */
338 16 : }
339 :
340 : std::string
341 0 : MSDevice_GLOSA::getParameter(const std::string& key) const {
342 0 : if (key == "minSpeed") {
343 0 : return toString(myMinSpeed);
344 : }
345 0 : throw InvalidArgument("Parameter '" + key + "' is not supported for device of type '" + deviceName() + "'");
346 : }
347 :
348 :
349 : void
350 0 : MSDevice_GLOSA::setParameter(const std::string& key, const std::string& value) {
351 : double doubleValue;
352 : try {
353 0 : doubleValue = StringUtils::toDouble(value);
354 0 : } catch (NumberFormatException&) {
355 0 : throw InvalidArgument("Setting parameter '" + key + "' requires a number for device of type '" + deviceName() + "'");
356 0 : }
357 0 : if (key == "minSpeed") {
358 0 : myMinSpeed = doubleValue;
359 : } else {
360 0 : throw InvalidArgument("Setting parameter '" + key + "' is not supported for device of type '" + deviceName() + "'");
361 : }
362 0 : }
363 :
364 :
365 : /****************************************************************************/
|
