Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2013-2026 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 44303 : MSDevice_Bluelight::insertOptions(OptionsCont& oc) {
55 44303 : oc.addOptionSubTopic("Bluelight Device");
56 88606 : insertDefaultAssignmentOptions("bluelight", "Bluelight Device", oc);
57 :
58 44303 : oc.doRegister("device.bluelight.reactiondist", new Option_Float(25.0));
59 88606 : oc.addDescription("device.bluelight.reactiondist", "Bluelight Device", TL("Set the distance at which other drivers react to the blue light and siren sound"));
60 44303 : oc.doRegister("device.bluelight.mingapfactor", new Option_Float(1.));
61 88606 : oc.addDescription("device.bluelight.mingapfactor", "Bluelight Device", TL("Reduce the minGap for reacting vehicles by the given factor"));
62 44303 : }
63 :
64 :
65 : void
66 5554736 : MSDevice_Bluelight::buildVehicleDevices(SUMOVehicle& v, std::vector<MSVehicleDevice*>& into) {
67 5554736 : OptionsCont& oc = OptionsCont::getOptions();
68 11109472 : 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 5554736 : }
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 71964 : 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 71964 : MSVehicle& ego = dynamic_cast<MSVehicle&>(veh);
112 71964 : MSVehicle::Influencer& redLight = ego.getInfluencer();
113 71964 : const double vMax = ego.getLane()->getVehicleMaxSpeed(&ego);
114 71964 : if (ego.getSpeed() < 0.5 * vMax) {
115 : // advance as far as possible (assume vehicles will keep moving out of the way)
116 76794 : ego.getLaneChangeModel().setParameter(toString(SUMO_ATTR_LCA_STRATEGIC_PARAM), "-1");
117 76794 : ego.getLaneChangeModel().setParameter(toString(SUMO_ATTR_LCA_SPEEDGAIN_LOOKAHEAD), "0");
118 76794 : ego.getLaneChangeModel().setParameter(toString(SUMO_ATTR_LCA_SPEEDGAIN_REMAIN_TIME), "0");
119 : try {
120 76794 : 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 100701 : ego.getLaneChangeModel().setParameter(toString(SUMO_ATTR_LCA_STRATEGIC_PARAM),
127 33567 : ego.getVehicleType().getParameter().getLCParamString(SUMO_ATTR_LCA_STRATEGIC_PARAM, "1"));
128 100701 : ego.getLaneChangeModel().setParameter(toString(SUMO_ATTR_LCA_SPEEDGAIN_LOOKAHEAD),
129 33567 : ego.getVehicleType().getParameter().getLCParamString(SUMO_ATTR_LCA_SPEEDGAIN_LOOKAHEAD, "5"));
130 100701 : ego.getLaneChangeModel().setParameter(toString(SUMO_ATTR_LCA_SPEEDGAIN_REMAIN_TIME),
131 33567 : ego.getVehicleType().getParameter().getLCParamString(SUMO_ATTR_LCA_SPEEDGAIN_REMAIN_TIME, "20"));
132 : try {
133 100701 : ego.getLaneChangeModel().setParameter(toString(SUMO_ATTR_MINGAP_LAT),
134 64614 : 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 71964 : 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 71964 : double affectedJunctionDist = ego.getPositionOnLane() + myReactionDist;
147 214372 : for (const MSLane* const l : ego.getUpcomingLanesUntil(myReactionDist)) {
148 142408 : upcomingEdges.push_back(&l->getEdge());
149 :
150 142408 : affectedJunctionDist -= l->getLength();
151 142408 : if (affectedJunctionDist > 0 && l->isInternal()) {
152 36611 : upcomingLinks.push_back(l->getIncomingLanes()[0].viaLink);
153 : }
154 71964 : }
155 :
156 214372 : for (const MSEdge* const e : upcomingEdges) {
157 : //inform all vehicles on upcomingEdges
158 729671 : for (const SUMOVehicle* v : e->getVehicles()) {
159 587263 : upcomingVehicles.insert(dynamic_cast<MSVehicle*>(const_cast<SUMOVehicle*>(v)));
160 : if (lastStepInfluencedVehicles.count(v->getID()) > 0) {
161 : lastStepInfluencedVehicles.erase(v->getID());
162 : }
163 142408 : }
164 : }
165 : // reset all vehicles that were in myInfluencedVehicles in the previous step but not in the current step todo refactor
166 73404 : for (std::string vehID : lastStepInfluencedVehicles) {
167 : myInfluencedVehicles.erase(vehID);
168 : Parameterised::Map::iterator it = myInfluencedTypes.find(vehID);
169 1440 : MSVehicle* veh2 = dynamic_cast<MSVehicle*>(vc.getVehicle(vehID));
170 1389 : if (veh2 != nullptr && it != myInfluencedTypes.end()) {
171 : // The vehicle gets back its old VehicleType after the emergency vehicle have passed them
172 1389 : resetVehicle(veh2, it->second);
173 : }
174 : }
175 :
176 659227 : for (MSVehicle* veh2 : upcomingVehicles) {
177 : assert(veh2 != nullptr);
178 587263 : if (veh2->getLane() == nullptr) {
179 0 : continue;
180 : }
181 587263 : if (std::find(upcomingEdges.begin(), upcomingEdges.end(), &veh2->getLane()->getEdge()) != upcomingEdges.end()) {
182 587259 : if (veh2->getDevice(typeid(MSDevice_Bluelight)) != nullptr) {
183 : // emergency vehicles should not react
184 300580 : continue;
185 : }
186 286679 : 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 142843 : MSVehicleType& t = veh2->getSingularType();
191 : // Setting the lateral alignment to build a rescue lane
192 142843 : LatAlignmentDefinition align = LatAlignmentDefinition::RIGHT;
193 142843 : if (veh2->getLane()->getIndex() == numLanes - 1) {
194 61898 : align = LatAlignmentDefinition::LEFT;
195 : }
196 142843 : 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 286679 : double distanceDelta = veh.getPosition().distanceTo(veh2->getPosition());
205 : //emergency vehicle has to slow down when entering the rescue lane
206 324065 : 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 36022 : const double redSpeed = ego.getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_DRIVE_RED_SPEED, 5.56);
209 : std::vector<std::pair<SUMOTime, double> > speedTimeLine;
210 36022 : speedTimeLine.push_back(std::make_pair(MSNet::getInstance()->getCurrentTimeStep(), veh.getSpeed()));
211 36022 : speedTimeLine.push_back(std::make_pair(MSNet::getInstance()->getCurrentTimeStep() + TIME2STEPS(2), redSpeed));
212 36022 : redLight.setSpeedTimeLine(speedTimeLine);
213 36022 : }
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 286679 : 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 18123 : double reaction = RandHelper::rand();
220 18123 : MSVehicle::Influencer& lanechange = veh2->getInfluencer();
221 :
222 : //other vehicle should not use the rescue lane so they should not make any lane changes
223 18123 : 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 18123 : distanceDelta < myHolder.getFloatParam("device.bluelight.near-dist", false, 12.5)
227 36246 : ? myHolder.getFloatParam("device.bluelight.reaction-prob-near", false, 0.577)
228 35549 : : 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 18123 : if (veh2->isActionStep(SIMSTEP) && reaction < reactionProb * veh2->getActionStepLengthSecs()) {
232 : myInfluencedVehicles.insert(veh2->getID());
233 2880 : myInfluencedTypes.insert(std::make_pair(veh2->getID(), veh2->getVehicleType().getID()));
234 1440 : 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 1440 : MSVehicleType& t = veh2->getSingularType();
241 : // Setting the lateral alignment to build a rescue lane
242 1440 : LatAlignmentDefinition align = LatAlignmentDefinition::RIGHT;
243 1440 : if (veh2->getLane()->getIndex() == numLanes - 1) {
244 675 : align = LatAlignmentDefinition::LEFT;
245 : }
246 1440 : t.setPreferredLateralAlignment(align);
247 1440 : t.setMinGap(t.getMinGap() * myMinGapFactor);
248 1440 : 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 2880 : std::vector<std::string> influencedBy = StringTokenizer(veh2->getParameter().getParameter(INFLUENCED_BY, "")).getVector();
256 1440 : if (std::find(influencedBy.begin(), influencedBy.end(), myHolder.getID()) == influencedBy.end()) {
257 1440 : influencedBy.push_back(myHolder.getID());
258 2880 : const_cast<SUMOVehicleParameter&>(veh2->getParameter()).setParameter(INFLUENCED_BY, toString(influencedBy));
259 : }
260 2880 : veh2->getLaneChangeModel().setParameter(toString(SUMO_ATTR_LCA_STRATEGIC_PARAM), "-1");
261 1440 : }
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 1 : double distanceDelta = veh.getPosition().distanceTo(veh2->getPosition());
267 1 : 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 108575 : for (MSLink* link : upcomingLinks) {
280 36611 : auto avi = link->getApproaching(&ego);
281 : MSLink::BlockingFoes blockingFoes;
282 36611 : link->opened(avi.arrivalTime, avi.arrivalSpeed, avi.arrivalSpeed, ego.getLength(),
283 36611 : 0, ego.getCarFollowModel().getMaxDecel(), ego.getWaitingTime(), ego.getLateralPositionOnLane(), &blockingFoes, true, &ego);
284 36611 : const SUMOTime timeToArrival = avi.arrivalTime - SIMSTEP;
285 43980 : for (const SUMOTrafficObject* foe : blockingFoes) {
286 7369 : if (!foe->isVehicle()) {
287 0 : continue;
288 : }
289 7369 : const double dist = ego.getPosition().distanceTo2D(foe->getPosition());
290 7369 : if (dist < myReactionDist) {
291 1902 : MSVehicle* microFoe = dynamic_cast<MSVehicle*>(const_cast<SUMOTrafficObject*>(foe));
292 1902 : if (microFoe->getDevice(typeid(MSDevice_Bluelight)) != nullptr) {
293 : // emergency vehicles should not react
294 0 : continue;
295 : }
296 1902 : const double timeToBrake = foe->getSpeed() / SUMOVTypeParameter::getDefaultDecel();
297 1902 : if (timeToArrival < TIME2STEPS(timeToBrake + 1)) {
298 : // trigger emergency braking
299 752 : const double decel = 0.5 * foe->getSpeed() * foe->getSpeed() / avi.dist;
300 : std::vector<std::pair<SUMOTime, double> > speedTimeLine;
301 752 : speedTimeLine.push_back(std::make_pair(SIMSTEP, foe->getSpeed()));
302 762 : speedTimeLine.push_back(std::make_pair(SIMSTEP + TIME2STEPS(foe->getSpeed() / decel), 0));
303 752 : microFoe->getInfluencer().setSpeedTimeLine(speedTimeLine);
304 : //std::cout << SIMTIME << " foe=" << foe->getID() << " timeToBrake=" << timeToBrake << " timeToArrival=" << STEPS2TIME(timeToArrival) << " decel=" << decel << "\n";
305 752 : }
306 : }
307 : }
308 36611 : }
309 71964 : return true; // keep the device
310 143928 : }
311 :
312 :
313 : void
314 1389 : MSDevice_Bluelight::resetVehicle(MSVehicle* veh2, const std::string& targetTypeID) {
315 1389 : MSVehicleType* targetType = MSNet::getInstance()->getVehicleControl().getVType(targetTypeID);
316 : //targetType is nullptr if the vehicle type has already changed to its old vehicleType
317 1389 : if (targetType != nullptr) {
318 : #ifdef DEBUG_BLUELIGHT_RESCUELANE
319 : std::cout << SIMTIME << " device=" << getID() << " reset " << veh2->getID() << "\n";
320 : #endif
321 :
322 2778 : std::vector<std::string> influencedBy = StringTokenizer(veh2->getParameter().getParameter(INFLUENCED_BY, "")).getVector();
323 1389 : auto it = std::find(influencedBy.begin(), influencedBy.end(), myHolder.getID());
324 1389 : if (it != influencedBy.end()) {
325 : influencedBy.erase(it);
326 2778 : const_cast<SUMOVehicleParameter&>(veh2->getParameter()).setParameter(INFLUENCED_BY, toString(influencedBy));
327 : }
328 1389 : if (influencedBy.size() == 0) {
329 1041 : veh2->replaceVehicleType(targetType);
330 3123 : veh2->getLaneChangeModel().setParameter(toString(SUMO_ATTR_LCA_STRATEGIC_PARAM),
331 2082 : targetType->getParameter().getLCParamString(SUMO_ATTR_LCA_STRATEGIC_PARAM, "1"));
332 : }
333 1389 : }
334 1389 : }
335 :
336 :
337 :
338 : bool
339 1075 : 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 1075 : return true; // keep the device
348 : }
349 :
350 :
351 : bool
352 1075 : 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 1075 : 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 : /****************************************************************************/
|
