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_Bluelight.cpp
15 : /// @author Daniel Krajzewicz
16 : /// @author Michael Behrisch
17 : /// @author Jakob Erdmann
18 : /// @author Laura Bieker
19 : /// @date 01.06.2017
20 : ///
21 : // A device for emergency vehicle. The behaviour of other traffic participants will be triggered with this device.
22 : // For example building a rescue lane.
23 : /****************************************************************************/
24 : #include <config.h>
25 :
26 : #include <utils/common/StringUtils.h>
27 : #include <utils/common/StringTokenizer.h>
28 : #include <utils/options/OptionsCont.h>
29 : #include <utils/iodevices/OutputDevice.h>
30 : #include <utils/vehicle/SUMOVehicle.h>
31 : #include <microsim/MSNet.h>
32 : #include <microsim/MSLane.h>
33 : #include <microsim/MSEdge.h>
34 : #include <microsim/MSLink.h>
35 : #include <microsim/MSVehicle.h>
36 : #include <microsim/lcmodels/MSAbstractLaneChangeModel.h>
37 : #include <microsim/MSVehicleControl.h>
38 : #include <microsim/MSVehicleType.h>
39 : #include "MSDevice_Tripinfo.h"
40 : #include "MSDevice_Bluelight.h"
41 :
42 : //#define DEBUG_BLUELIGHT
43 : //#define DEBUG_BLUELIGHT_RESCUELANE
44 :
45 : #define INFLUENCED_BY "rescueLane"
46 :
47 : // ===========================================================================
48 : // method definitions
49 : // ===========================================================================
50 : // ---------------------------------------------------------------------------
51 : // static initialisation methods
52 : // ---------------------------------------------------------------------------
53 : void
54 39784 : MSDevice_Bluelight::insertOptions(OptionsCont& oc) {
55 39784 : oc.addOptionSubTopic("Bluelight Device");
56 79568 : insertDefaultAssignmentOptions("bluelight", "Bluelight Device", oc);
57 :
58 39784 : oc.doRegister("device.bluelight.reactiondist", new Option_Float(25.0));
59 79568 : oc.addDescription("device.bluelight.reactiondist", "Bluelight Device", TL("Set the distance at which other drivers react to the blue light and siren sound"));
60 39784 : oc.doRegister("device.bluelight.mingapfactor", new Option_Float(1.));
61 79568 : oc.addDescription("device.bluelight.mingapfactor", "Bluelight Device", TL("Reduce the minGap for reacting vehicles by the given factor"));
62 39784 : }
63 :
64 :
65 : void
66 5371069 : MSDevice_Bluelight::buildVehicleDevices(SUMOVehicle& v, std::vector<MSVehicleDevice*>& into) {
67 5371069 : OptionsCont& oc = OptionsCont::getOptions();
68 10742138 : if (equippedByDefaultAssignmentOptions(oc, "bluelight", v, false)) {
69 208 : if (MSGlobals::gUseMesoSim) {
70 0 : WRITE_WARNINGF(TL("bluelight device is not compatible with mesosim (ignored for vehicle '%')"), v.getID());
71 : } else {
72 416 : MSDevice_Bluelight* device = new MSDevice_Bluelight(v, "bluelight_" + v.getID(),
73 : v.getFloatParam("device.bluelight.reactiondist"),
74 624 : v.getFloatParam("device.bluelight.mingapfactor"));
75 208 : into.push_back(device);
76 : }
77 : }
78 5371069 : }
79 :
80 :
81 : // ---------------------------------------------------------------------------
82 : // MSDevice_Bluelight-methods
83 : // ---------------------------------------------------------------------------
84 208 : MSDevice_Bluelight::MSDevice_Bluelight(SUMOVehicle& holder, const std::string& id,
85 208 : const double reactionDist, const double minGapFactor) :
86 : MSVehicleDevice(holder, id),
87 208 : myReactionDist(reactionDist),
88 208 : myMinGapFactor(minGapFactor) {
89 : #ifdef DEBUG_BLUELIGHT
90 : std::cout << SIMTIME << " initialized device '" << id << "' with myReactionDist=" << myReactionDist << "\n";
91 : #endif
92 : // set only once to permit TraCI override
93 208 : MSVehicle& veh = dynamic_cast<MSVehicle&>(holder);
94 208 : veh.getInfluencer().setSpeedMode(7);
95 208 : }
96 :
97 :
98 416 : MSDevice_Bluelight::~MSDevice_Bluelight() {
99 416 : }
100 :
101 :
102 : bool
103 65189 : MSDevice_Bluelight::notifyMove(SUMOTrafficObject& veh, double /* oldPos */,
104 : double /* newPos */, double newSpeed) {
105 : #ifdef DEBUG_BLUELIGHT
106 : std::cout << SIMTIME << " device '" << getID() << "' notifyMove: newSpeed=" << newSpeed << "\n";
107 : #else
108 : UNUSED_PARAMETER(newSpeed);
109 : #endif
110 : //violate red lights this only need to be done once so shift it todo
111 65189 : MSVehicle& ego = dynamic_cast<MSVehicle&>(veh);
112 65189 : MSVehicle::Influencer& redLight = ego.getInfluencer();
113 65189 : const double vMax = ego.getLane()->getVehicleMaxSpeed(&ego);
114 65189 : if (ego.getSpeed() < 0.5 * vMax) {
115 : // advance as far as possible (assume vehicles will keep moving out of the way)
116 63196 : ego.getLaneChangeModel().setParameter(toString(SUMO_ATTR_LCA_STRATEGIC_PARAM), "-1");
117 63196 : ego.getLaneChangeModel().setParameter(toString(SUMO_ATTR_LCA_SPEEDGAIN_LOOKAHEAD), "0");
118 63196 : ego.getLaneChangeModel().setParameter(toString(SUMO_ATTR_LCA_SPEEDGAIN_REMAIN_TIME), "0");
119 : try {
120 63196 : ego.getLaneChangeModel().setParameter(toString(SUMO_ATTR_MINGAP_LAT), "0");
121 426 : } catch (InvalidArgument&) {
122 : // not supported by the current laneChangeModel
123 426 : }
124 : } else {
125 : // restore defaults
126 100773 : ego.getLaneChangeModel().setParameter(toString(SUMO_ATTR_LCA_STRATEGIC_PARAM),
127 33591 : ego.getVehicleType().getParameter().getLCParamString(SUMO_ATTR_LCA_STRATEGIC_PARAM, "1"));
128 100773 : ego.getLaneChangeModel().setParameter(toString(SUMO_ATTR_LCA_SPEEDGAIN_LOOKAHEAD),
129 33591 : ego.getVehicleType().getParameter().getLCParamString(SUMO_ATTR_LCA_SPEEDGAIN_LOOKAHEAD, "5"));
130 100773 : ego.getLaneChangeModel().setParameter(toString(SUMO_ATTR_LCA_SPEEDGAIN_REMAIN_TIME),
131 33591 : ego.getVehicleType().getParameter().getLCParamString(SUMO_ATTR_LCA_SPEEDGAIN_REMAIN_TIME, "20"));
132 : try {
133 100773 : ego.getLaneChangeModel().setParameter(toString(SUMO_ATTR_MINGAP_LAT),
134 64662 : toString(ego.getVehicleType().getMinGapLat()));
135 2520 : } catch (InvalidArgument&) {
136 : // not supported by the current laneChangeModel
137 2520 : }
138 : }
139 : // build a rescue lane for all vehicles on the route of the emergency vehicle within the range of the siren
140 65189 : MSVehicleControl& vc = MSNet::getInstance()->getVehicleControl();
141 : // use edges on the way of the emergency vehicle
142 : std::vector<const MSEdge*> upcomingEdges;
143 : std::set<MSVehicle*, ComparatorIdLess> upcomingVehicles;
144 : std::set<std::string> lastStepInfluencedVehicles = myInfluencedVehicles;
145 : std::vector<MSLink*> upcomingLinks;
146 65189 : double affectedJunctionDist = ego.getPositionOnLane() + myReactionDist;
147 188949 : for (const MSLane* const l : ego.getUpcomingLanesUntil(myReactionDist)) {
148 123760 : upcomingEdges.push_back(&l->getEdge());
149 :
150 123760 : affectedJunctionDist -= l->getLength();
151 123760 : if (affectedJunctionDist > 0 && l->isInternal()) {
152 29453 : upcomingLinks.push_back(l->getIncomingLanes()[0].viaLink);
153 : }
154 65189 : }
155 :
156 188949 : for (const MSEdge* const e : upcomingEdges) {
157 : //inform all vehicles on upcomingEdges
158 668546 : for (const SUMOVehicle* v : e->getVehicles()) {
159 544786 : upcomingVehicles.insert(dynamic_cast<MSVehicle*>(const_cast<SUMOVehicle*>(v)));
160 : if (lastStepInfluencedVehicles.count(v->getID()) > 0) {
161 : lastStepInfluencedVehicles.erase(v->getID());
162 : }
163 123760 : }
164 : }
165 : // reset all vehicles that were in myInfluencedVehicles in the previous step but not in the current step todo refactor
166 66626 : for (std::string vehID : lastStepInfluencedVehicles) {
167 : myInfluencedVehicles.erase(vehID);
168 : Parameterised::Map::iterator it = myInfluencedTypes.find(vehID);
169 1437 : MSVehicle* veh2 = dynamic_cast<MSVehicle*>(vc.getVehicle(vehID));
170 1384 : if (veh2 != nullptr && it != myInfluencedTypes.end()) {
171 : // The vehicle gets back its old VehicleType after the emergency vehicle have passed them
172 1384 : resetVehicle(veh2, it->second);
173 : }
174 : }
175 :
176 609975 : for (MSVehicle* veh2 : upcomingVehicles) {
177 : assert(veh2 != nullptr);
178 544786 : if (veh2->getLane() == nullptr) {
179 0 : continue;
180 : }
181 544786 : if (std::find(upcomingEdges.begin(), upcomingEdges.end(), &veh2->getLane()->getEdge()) != upcomingEdges.end()) {
182 544782 : if (veh2->getDevice(typeid(MSDevice_Bluelight)) != nullptr) {
183 : // emergency vehicles should not react
184 277082 : continue;
185 : }
186 267700 : const int numLanes = (int)veh2->getLane()->getEdge().getNumLanes();
187 : // make sure that vehicles are still building the rescue lane as they might have moved to a new edge or changed lanes
188 : if (myInfluencedVehicles.count(veh2->getID()) > 0) {
189 : // Vehicle gets a new Vehicletype to change the alignment and the lanechange options
190 124004 : MSVehicleType& t = veh2->getSingularType();
191 : // Setting the lateral alignment to build a rescue lane
192 124004 : LatAlignmentDefinition align = LatAlignmentDefinition::RIGHT;
193 124004 : if (veh2->getLane()->getIndex() == numLanes - 1) {
194 52484 : align = LatAlignmentDefinition::LEFT;
195 : }
196 124004 : t.setPreferredLateralAlignment(align);
197 : #ifdef DEBUG_BLUELIGHT_RESCUELANE
198 : std::cout << "Refresh alignment for vehicle: " << veh2->getID()
199 : << " laneIndex=" << veh2->getLane()->getIndex() << " numLanes=" << numLanes
200 : << " alignment=" << toString(align) << "\n";
201 : #endif
202 : }
203 :
204 267700 : double distanceDelta = veh.getPosition().distanceTo(veh2->getPosition());
205 : //emergency vehicle has to slow down when entering the rescue lane
206 297678 : if (distanceDelta <= 10 && veh.getID() != veh2->getID() && myInfluencedVehicles.count(veh2->getID()) > 0 && veh2->getSpeed() < 1) {
207 : // set ev speed to 20 km/h 0 5.56 m/s
208 28590 : const double redSpeed = ego.getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_DRIVE_RED_SPEED, 5.56);
209 : std::vector<std::pair<SUMOTime, double> > speedTimeLine;
210 28590 : speedTimeLine.push_back(std::make_pair(MSNet::getInstance()->getCurrentTimeStep(), veh.getSpeed()));
211 28590 : speedTimeLine.push_back(std::make_pair(MSNet::getInstance()->getCurrentTimeStep() + TIME2STEPS(2), redSpeed));
212 28590 : redLight.setSpeedTimeLine(speedTimeLine);
213 28590 : }
214 :
215 : // the perception of the sound of the siren should be around 25 meters
216 : // todo only vehicles in front of the emergency vehicle should react
217 267700 : if (distanceDelta <= myReactionDist && veh.getID() != veh2->getID() && myInfluencedVehicles.count(veh2->getID()) == 0) {
218 : // only a percentage of vehicles should react to the emergency vehicle to make the behaviour more realistic
219 18088 : double reaction = RandHelper::rand();
220 18088 : MSVehicle::Influencer& lanechange = veh2->getInfluencer();
221 :
222 : //other vehicle should not use the rescue lane so they should not make any lane changes
223 18088 : lanechange.setLaneChangeMode(1605);//todo change lane back
224 : // the vehicles should react according to the distance to the emergency vehicle taken from real world data
225 : double reactionProb = (
226 18088 : distanceDelta < myHolder.getFloatParam("device.bluelight.near-dist", false, 12.5)
227 36176 : ? myHolder.getFloatParam("device.bluelight.reaction-prob-near", false, 0.577)
228 35473 : : myHolder.getFloatParam("device.bluelight.reaction-prob-far", false, 0.189));
229 : // todo works only for one second steps
230 : //std::cout << SIMTIME << " veh2=" << veh2->getID() << " distanceDelta=" << distanceDelta << " reaction=" << reaction << " reactionProb=" << reactionProb << "\n";
231 18088 : if (veh2->isActionStep(SIMSTEP) && reaction < reactionProb * veh2->getActionStepLengthSecs()) {
232 : myInfluencedVehicles.insert(veh2->getID());
233 2874 : myInfluencedTypes.insert(std::make_pair(veh2->getID(), veh2->getVehicleType().getID()));
234 1437 : if (myMinGapFactor != 1.) {
235 : // TODO this is a permanent change to the vtype!
236 12 : MSNet::getInstance()->getVehicleControl().getVType(veh2->getVehicleType().getID())->getCarFollowModel().setCollisionMinGapFactor(myMinGapFactor);
237 : }
238 :
239 : // Vehicle gets a new Vehicletype to change the alignment and the lanechange options
240 1437 : MSVehicleType& t = veh2->getSingularType();
241 : // Setting the lateral alignment to build a rescue lane
242 1437 : LatAlignmentDefinition align = LatAlignmentDefinition::RIGHT;
243 1437 : if (veh2->getLane()->getIndex() == numLanes - 1) {
244 676 : align = LatAlignmentDefinition::LEFT;
245 : }
246 1437 : t.setPreferredLateralAlignment(align);
247 1437 : t.setMinGap(t.getMinGap() * myMinGapFactor);
248 1437 : const_cast<SUMOVTypeParameter&>(t.getParameter()).jmParameter[SUMO_ATTR_JM_STOPLINE_GAP] = toString(myMinGapFactor);
249 : // disable strategic lane-changing
250 : #ifdef DEBUG_BLUELIGHT_RESCUELANE
251 : std::cout << SIMTIME << " device=" << getID() << " formingRescueLane=" << veh2->getID()
252 : << " laneIndex=" << veh2->getLane()->getIndex() << " numLanes=" << numLanes
253 : << " alignment=" << toString(align) << "\n";
254 : #endif
255 2874 : std::vector<std::string> influencedBy = StringTokenizer(veh2->getParameter().getParameter(INFLUENCED_BY, "")).getVector();
256 1437 : if (std::find(influencedBy.begin(), influencedBy.end(), myHolder.getID()) == influencedBy.end()) {
257 1437 : influencedBy.push_back(myHolder.getID());
258 2874 : const_cast<SUMOVehicleParameter&>(veh2->getParameter()).setParameter(INFLUENCED_BY, toString(influencedBy));
259 : }
260 2874 : veh2->getLaneChangeModel().setParameter(toString(SUMO_ATTR_LCA_STRATEGIC_PARAM), "-1");
261 1437 : }
262 : }
263 :
264 : } else { //if vehicle is passed all vehicles which had to react should get their state back after they leave the communication range
265 : if (myInfluencedVehicles.count(veh2->getID()) > 0) {
266 2 : double distanceDelta = veh.getPosition().distanceTo(veh2->getPosition());
267 2 : if (distanceDelta > myReactionDist && veh.getID() != veh2->getID()) {
268 : myInfluencedVehicles.erase(veh2->getID());
269 : Parameterised::Map::iterator it = myInfluencedTypes.find(veh2->getID());
270 0 : if (it != myInfluencedTypes.end()) {
271 : // The vehicle gets back its old VehicleType after the emergency vehicle have passed them
272 0 : resetVehicle(veh2, it->second);
273 : }
274 : }
275 : }
276 : }
277 : }
278 : // make upcoming junction foes slow down
279 94642 : for (MSLink* link : upcomingLinks) {
280 29453 : auto avi = link->getApproaching(&ego);
281 : MSLink::BlockingFoes blockingFoes;
282 29453 : link->opened(avi.arrivalTime, avi.arrivalSpeed, avi.arrivalSpeed, ego.getLength(),
283 29453 : 0, ego.getCarFollowModel().getMaxDecel(), ego.getWaitingTime(), ego.getLateralPositionOnLane(), &blockingFoes, true, &ego);
284 29453 : const SUMOTime timeToArrival = avi.arrivalTime - SIMSTEP;
285 36860 : for (const SUMOTrafficObject* foe : blockingFoes) {
286 7407 : if (!foe->isVehicle()) {
287 0 : continue;
288 : }
289 7407 : const double dist = ego.getPosition().distanceTo2D(foe->getPosition());
290 7407 : if (dist < myReactionDist) {
291 1933 : MSVehicle* microFoe = dynamic_cast<MSVehicle*>(const_cast<SUMOTrafficObject*>(foe));
292 1933 : if (microFoe->getDevice(typeid(MSDevice_Bluelight)) != nullptr) {
293 : // emergency vehicles should not react
294 0 : continue;
295 : }
296 1933 : const double timeToBrake = foe->getSpeed() / SUMOVTypeParameter::getDefaultDecel();
297 1933 : if (timeToArrival < TIME2STEPS(timeToBrake + 1)) {
298 : // trigger emergency braking
299 793 : const double decel = 0.5 * foe->getSpeed() * foe->getSpeed() / avi.dist;
300 : std::vector<std::pair<SUMOTime, double> > speedTimeLine;
301 793 : speedTimeLine.push_back(std::make_pair(SIMSTEP, foe->getSpeed()));
302 803 : speedTimeLine.push_back(std::make_pair(SIMSTEP + TIME2STEPS(foe->getSpeed() / decel), 0));
303 793 : microFoe->getInfluencer().setSpeedTimeLine(speedTimeLine);
304 : //std::cout << SIMTIME << " foe=" << foe->getID() << " timeToBrake=" << timeToBrake << " timeToArrival=" << STEPS2TIME(timeToArrival) << " decel=" << decel << "\n";
305 793 : }
306 : }
307 : }
308 29453 : }
309 65189 : return true; // keep the device
310 130378 : }
311 :
312 :
313 : void
314 1384 : MSDevice_Bluelight::resetVehicle(MSVehicle* veh2, const std::string& targetTypeID) {
315 1384 : MSVehicleType* targetType = MSNet::getInstance()->getVehicleControl().getVType(targetTypeID);
316 : //targetType is nullptr if the vehicle type has already changed to its old vehicleType
317 1384 : if (targetType != nullptr) {
318 : #ifdef DEBUG_BLUELIGHT_RESCUELANE
319 : std::cout << SIMTIME << " device=" << getID() << " reset " << veh2->getID() << "\n";
320 : #endif
321 :
322 2768 : std::vector<std::string> influencedBy = StringTokenizer(veh2->getParameter().getParameter(INFLUENCED_BY, "")).getVector();
323 1384 : auto it = std::find(influencedBy.begin(), influencedBy.end(), myHolder.getID());
324 1384 : if (it != influencedBy.end()) {
325 : influencedBy.erase(it);
326 2768 : const_cast<SUMOVehicleParameter&>(veh2->getParameter()).setParameter(INFLUENCED_BY, toString(influencedBy));
327 : }
328 1384 : if (influencedBy.size() == 0) {
329 1035 : veh2->replaceVehicleType(targetType);
330 3105 : veh2->getLaneChangeModel().setParameter(toString(SUMO_ATTR_LCA_STRATEGIC_PARAM),
331 2070 : targetType->getParameter().getLCParamString(SUMO_ATTR_LCA_STRATEGIC_PARAM, "1"));
332 : }
333 1384 : }
334 1384 : }
335 :
336 :
337 :
338 : bool
339 1072 : MSDevice_Bluelight::notifyEnter(SUMOTrafficObject& veh, MSMoveReminder::Notification reason, const MSLane* enteredLane) {
340 : UNUSED_PARAMETER(veh);
341 : #ifdef DEBUG_BLUELIGHT
342 : std::cout << SIMTIME << " device '" << getID() << "' notifyEnter: reason=" << toString(reason) << " enteredLane=" << Named::getIDSecure(enteredLane) << "\n";
343 : #else
344 : UNUSED_PARAMETER(reason);
345 : UNUSED_PARAMETER(enteredLane);
346 : #endif
347 1072 : return true; // keep the device
348 : }
349 :
350 :
351 : bool
352 1072 : MSDevice_Bluelight::notifyLeave(SUMOTrafficObject& veh, double /*lastPos*/, MSMoveReminder::Notification reason, const MSLane* enteredLane) {
353 : UNUSED_PARAMETER(veh);
354 : #ifdef DEBUG_BLUELIGHT
355 : std::cout << SIMTIME << " device '" << getID() << "' notifyLeave: reason=" << toString(reason) << " approachedLane=" << Named::getIDSecure(enteredLane) << "\n";
356 : #else
357 : UNUSED_PARAMETER(reason);
358 : UNUSED_PARAMETER(enteredLane);
359 : #endif
360 1072 : return true; // keep the device
361 : }
362 :
363 :
364 : void
365 208 : MSDevice_Bluelight::generateOutput(OutputDevice* tripinfoOut) const {
366 208 : if (tripinfoOut != nullptr) {
367 164 : tripinfoOut->openTag("bluelight");
368 328 : tripinfoOut->closeTag();
369 : }
370 208 : }
371 :
372 : std::string
373 0 : MSDevice_Bluelight::getParameter(const std::string& key) const {
374 0 : if (key == "reactiondist") {
375 0 : return toString(myReactionDist);
376 : }
377 0 : throw InvalidArgument("Parameter '" + key + "' is not supported for device of type '" + deviceName() + "'");
378 : }
379 :
380 :
381 : void
382 0 : MSDevice_Bluelight::setParameter(const std::string& key, const std::string& value) {
383 : double doubleValue;
384 : try {
385 0 : doubleValue = StringUtils::toDouble(value);
386 0 : } catch (NumberFormatException&) {
387 0 : throw InvalidArgument("Setting parameter '" + key + "' requires a number for device of type '" + deviceName() + "'");
388 0 : }
389 0 : if (key == "reactiondist") {
390 0 : myReactionDist = doubleValue;
391 : } else {
392 0 : throw InvalidArgument("Setting parameter '" + key + "' is not supported for device of type '" + deviceName() + "'");
393 : }
394 0 : }
395 :
396 :
397 : /****************************************************************************/
|
