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