Eclipse SUMO - Simulation of Urban MObility
MSDevice_Bluelight.cpp
Go to the documentation of this file.
1 /****************************************************************************/
2 // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3 // Copyright (C) 2013-2020 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 /****************************************************************************/
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 
30 #include <microsim/MSNet.h>
31 #include <microsim/MSLane.h>
32 #include <microsim/MSEdge.h>
33 #include <microsim/MSVehicle.h>
36 #include <microsim/MSVehicleType.h>
37 #include "MSDevice_Tripinfo.h"
38 #include "MSDevice_Bluelight.h"
39 
40 //#define DEBUG_BLUELIGHT
41 
42 // ===========================================================================
43 // method definitions
44 // ===========================================================================
45 // ---------------------------------------------------------------------------
46 // static initialisation methods
47 // ---------------------------------------------------------------------------
48 void
50  oc.addOptionSubTopic("Bluelight Device");
51  insertDefaultAssignmentOptions("bluelight", "Bluelight Device", oc);
52 
53  oc.doRegister("device.bluelight.reactiondist", new Option_Float(25.0));
54  oc.addDescription("device.bluelight.reactiondist", "Bluelight Device", "Set the distance at which other drivers react to the blue light and siren sound");
55 }
56 
57 
58 void
59 MSDevice_Bluelight::buildVehicleDevices(SUMOVehicle& v, std::vector<MSVehicleDevice*>& into) {
61  if (equippedByDefaultAssignmentOptions(oc, "bluelight", v, false)) {
62  MSDevice_Bluelight* device = new MSDevice_Bluelight(v, "bluelight_" + v.getID(),
63  getFloatParam(v, oc, "bluelight.reactiondist", oc.getFloat("device.bluelight.reactiondist"), false));
64  into.push_back(device);
65  }
66 }
67 
68 
69 // ---------------------------------------------------------------------------
70 // MSDevice_Bluelight-methods
71 // ---------------------------------------------------------------------------
72 MSDevice_Bluelight::MSDevice_Bluelight(SUMOVehicle& holder, const std::string& id,
73  double reactionDist) :
74  MSVehicleDevice(holder, id),
75  myReactionDist(reactionDist) {
76 #ifdef DEBUG_BLUELIGHT
77  std::cout << "initialized device '" << id << "' with myReactionDist=" << myReactionDist << "\n";
78 #endif
79 }
80 
81 
83 }
84 
85 
86 bool
88  double /* newPos */, double newSpeed) {
89 #ifdef DEBUG_BLUELIGHT
90  std::cout << "device '" << getID() << "' notifyMove: newSpeed=" << newSpeed << "\n";
91 #else
92  UNUSED_PARAMETER(newSpeed);
93 #endif
94  // check whether another device is present on the vehicle:
95  /*MSDevice_Tripinfo* otherDevice = static_cast<MSDevice_Tripinfo*>(veh.getDevice(typeid(MSDevice_Tripinfo)));
96  if (otherDevice != 0) {
97  std::cout << " veh '" << veh.getID() << " has device '" << otherDevice->getID() << "'\n";
98  }*/
99  //violate red lights this only need to be done once so shift it todo
100  MSVehicle& ego = dynamic_cast<MSVehicle&>(veh);
101  MSVehicle::Influencer& redLight = ego.getInfluencer();
102  redLight.setSpeedMode(7);
103  if (veh.getWaitingTime() > TIME2STEPS(1)) {
104  // advance as far as possible (assume vehicles will keep moving out of the way)
107  } else {
108  // restore defaults
113  }
114  // build a rescue lane for all vehicles on the route of the emergency vehicle within the range of the siren
118  //std::string currentEdgeID = veh.getEdge()->getID();
119  //use edges on the way of the emergency vehicle
120  std::vector<const MSLane*> myUpcomingLanes = ego.getUpcomingLanesUntil(myReactionDist);
121  std::vector<const MSEdge*> myUpcomingEdges;
122  //get edgeIDs from Lanes
123  for (const MSLane* const l : myUpcomingLanes) {
124  myUpcomingEdges.push_back(&l->getEdge());
125  }
126  for (MSVehicleControl::constVehIt vit = vc.loadedVehBegin(); vit != vc.loadedVehEnd(); ++vit) {
127  MSVehicle* veh2 = dynamic_cast<MSVehicle*>(vit->second);
128  assert(veh2 != nullptr);
129  if (veh2->getLane() == nullptr) {
130  continue;
131  }
132  //Vehicle only from edge should react
133  if (std::find(myUpcomingEdges.begin(), myUpcomingEdges.end(), &veh2->getLane()->getEdge()) != myUpcomingEdges.end()) { //currentEdgeID == veh2->getEdge()->getID())
134  if (veh2->getDevice(typeid(MSDevice_Bluelight)) != nullptr) {
135  // emergency vehicles should not react
136  continue;
137  }
138  const int numLanes = (int)veh2->getEdge()->getLanes().size();
139  //make sure that vehicle are still building the a rescue lane
140  if (influencedVehicles.count(veh2->getID()) > 0) {
141  //Vehicle gets a new Vehicletype to change the alignment and the lanechange options
142  MSVehicleType& t = veh2->getSingularType();
143  //Setting the lateral alignment to build a rescue lane
144  if (veh2->getLane()->getIndex() == numLanes - 1) {
146  // the alignement is changet to left for the vehicle std::cout << "New alignment to left for vehicle: " << veh2->getID() << " " << veh2->getVehicleType().getPreferredLateralAlignment() << "\n";
147  } else {
149  // the alignement is changet to right for the vehicle std::cout << "New alignment to right for vehicle: " << veh2->getID() << " " << veh2->getVehicleType().getPreferredLateralAlignment() << "\n";
150  }
151  }
152 
153  double distanceDelta = veh.getPosition().distanceTo(veh2->getPosition());
154  //emergency vehicle has to slow down when entering the resuce lane
155  if (distanceDelta <= 10 && veh.getID() != veh2->getID() && influencedVehicles.count(veh2->getID()) > 0 && veh2->getSpeed() < 1) {
156  // set ev speed to 20 km/h 0 5.56 m/s
157  std::vector<std::pair<SUMOTime, double> > speedTimeLine;
158  speedTimeLine.push_back(std::make_pair(MSNet::getInstance()->getCurrentTimeStep(), veh.getSpeed()));
159  speedTimeLine.push_back(std::make_pair(MSNet::getInstance()->getCurrentTimeStep() + TIME2STEPS(2), 5.56));
160  redLight.setSpeedTimeLine(speedTimeLine);
161  }
162 
163  // the perception of the sound of the siren should be around 25 meters
164  // todo only vehicles in front of the emergency vehicle should react
165  if (distanceDelta <= myReactionDist && veh.getID() != veh2->getID() && influencedVehicles.count(veh2->getID()) == 0) {
166  //online a percentage of vehicles should react to the emergency vehicle to make the behaviour more realistic
167  double reaction = RandHelper::rand();
168  MSVehicle::Influencer& lanechange = veh2->getInfluencer();
169 
170  //other vehicle should not use the rescue lane so they should not make any lane changes
171  lanechange.setLaneChangeMode(1605);//todo change lane back
172  //const int numLanes = (int)veh2->getEdge()->getLanes().size();
173  // the vehicles should react according to the distance to the emergency vehicle taken from real world data
174  double reactionProb = 0.189; // todo works only for one second steps
175  if (distanceDelta < 12.5) {
176  reactionProb = 0.577;
177  }
178  if (reaction < reactionProb) {
179  influencedVehicles.insert(veh2->getID());
180  influencedTypes.insert(std::make_pair(veh2->getID(), veh2->getVehicleType().getID()));
181 
182  //Vehicle gets a new Vehicletype to change the alignment and the lanechange options
183  MSVehicleType& t = veh2->getSingularType();
184  //Setting the lateral alignment to build a rescue lane
185  if (veh2->getLane()->getIndex() == numLanes - 1) {
187  // the alignement is changet to left for the vehicle std::cout << "New alignment to left for vehicle: " << veh2->getID() << " " << veh2->getVehicleType().getPreferredLateralAlignment() << "\n";
188  } else {
190  // the alignement is changet to right for the vehicle std::cout << "New alignment to right for vehicle: " << veh2->getID() << " " << veh2->getVehicleType().getPreferredLateralAlignment() << "\n";
191  }
192  // disable strategic lane-changing
194  }
195  }
196 
197  } else { //if vehicle is passed all vehicles which had to react should get their state back after they leave the communication range
198  if (influencedVehicles.count(veh2->getID()) > 0) {
199  double distanceDelta = veh.getPosition().distanceTo(veh2->getPosition());
200  if (distanceDelta > myReactionDist && veh.getID() != veh2->getID()) {
201  influencedVehicles.erase(veh2->getID());
202  std::map<std::string, std::string>::iterator it = influencedTypes.find(veh2->getID());
203  if (it != influencedTypes.end()) {
204  // The vehicle gets back its old VehicleType after the emergency vehicle have passed them
205  MSVehicleType* targetType = MSNet::getInstance()->getVehicleControl().getVType(it->second);
206  //targetType is nullptr if the vehicle type has already changed to its old vehicleType
207  if (targetType != nullptr) {
208  veh2->replaceVehicleType(targetType);
211  }
212  }
213  }
214  }
215  }
216  }
217  // ego is at the end of its current lane and cannot continue
218  if (ego.getBestLanesContinuation().size() == 1 && ego.getLane()->getLength() - ego.getPositionOnLane() <= POSITION_EPS
219  // route continues
220  && myUpcomingEdges.size() > 1) {
221  const MSEdge* currentEdge = &ego.getLane()->getEdge();
222  // move onto the intersection as if there was a connection from the current lane
223  const MSEdge* next = currentEdge->getInternalFollowingEdge(myUpcomingEdges[1]);
224  if (next == nullptr) {
225  next = myUpcomingEdges[1];
226  }
227  // pick the lane that causes the minimizes lateral jump
228  const std::vector<MSLane*>* allowed = next->allowedLanes(ego.getVClass());
229  MSLane* nextLane = next->getLanes().front();
230  double bestJump = std::numeric_limits<double>::max();
231  double newPosLat = 0;
232  if (allowed != nullptr) {
233  for (MSLane* nextCand : *allowed) {
234  for (auto ili : nextCand->getIncomingLanes()) {
235  if (&ili.lane->getEdge() == currentEdge) {
236  double jump = fabs(ego.getLatOffset(ili.lane) + ego.getLateralPositionOnLane());
237  if (jump < bestJump) {
238  //std::cout << SIMTIME << " nextCand=" << nextCand->getID() << " from=" << ili.lane->getID() << " jump=" << jump << "\n";
239  bestJump = jump;
240  nextLane = nextCand;
241  // stay within newLane
242  const double maxVehOffset = MAX2(0.0, nextLane->getWidth() - ego.getVehicleType().getWidth()) * 0.5;
243  newPosLat = ego.getLatOffset(ili.lane) + ego.getLateralPositionOnLane();
244  newPosLat = MAX2(-maxVehOffset, newPosLat);
245  newPosLat = MIN2(maxVehOffset, newPosLat);
246  }
247  }
248  }
249  }
250  }
251  ego.leaveLane(NOTIFICATION_JUNCTION, nextLane);
254  ego.setTentativeLaneAndPosition(nextLane, 0, newPosLat); // update position
255  ego.enterLaneAtMove(nextLane);
256  // sublane model must adapt state to the new lane
258  }
259  return true; // keep the device
260 }
261 
262 
263 bool
265 #ifdef DEBUG_BLUELIGHT
266  std::cout << "device '" << getID() << "' notifyEnter: reason=" << reason << " currentEdge=" << veh.getEdge()->getID() << "\n";
267 #else
268  UNUSED_PARAMETER(veh);
269  UNUSED_PARAMETER(reason);
270 #endif
271  return true; // keep the device
272 }
273 
274 
275 bool
276 MSDevice_Bluelight::notifyLeave(SUMOTrafficObject& veh, double /*lastPos*/, MSMoveReminder::Notification reason, const MSLane* /* enteredLane */) {
277 #ifdef DEBUG_BLUELIGHT
278  std::cout << "device '" << getID() << "' notifyLeave: reason=" << reason << " currentEdge=" << veh.getEdge()->getID() << "\n";
279 #else
280  UNUSED_PARAMETER(veh);
281  UNUSED_PARAMETER(reason);
282 #endif
283  return true; // keep the device
284 }
285 
286 
287 void
289  if (tripinfoOut != nullptr) {
290  tripinfoOut->openTag("bluelight");
291  tripinfoOut->closeTag();
292  }
293 }
294 
295 std::string
296 MSDevice_Bluelight::getParameter(const std::string& key) const {
297  if (key == "reactiondist") {
298  return toString(myReactionDist);
299  }
300  throw InvalidArgument("Parameter '" + key + "' is not supported for device of type '" + deviceName() + "'");
301 }
302 
303 
304 void
305 MSDevice_Bluelight::setParameter(const std::string& key, const std::string& value) {
306  double doubleValue;
307  try {
308  doubleValue = StringUtils::toDouble(value);
309  } catch (NumberFormatException&) {
310  throw InvalidArgument("Setting parameter '" + key + "' requires a number for device of type '" + deviceName() + "'");
311  }
312  if (key == "reactiondist") {
313  myReactionDist = doubleValue;
314  } else {
315  throw InvalidArgument("Setting parameter '" + key + "' is not supported for device of type '" + deviceName() + "'");
316  }
317 }
318 
319 
320 /****************************************************************************/
#define TIME2STEPS(x)
Definition: SUMOTime.h:55
@ LATALIGN_RIGHT
drive on the right side
@ LATALIGN_LEFT
drive on the left side
@ LATALIGN_ARBITRARY
maintain the current alignment
@ SUMO_ATTR_LCA_SPEEDGAIN_LOOKAHEAD
@ SUMO_ATTR_LCA_STRATEGIC_PARAM
#define UNUSED_PARAMETER(x)
Definition: StdDefs.h:29
T MIN2(T a, T b)
Definition: StdDefs.h:73
T MAX2(T a, T b)
Definition: StdDefs.h:79
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:44
virtual void setParameter(const std::string &key, const std::string &value)
try to set the given parameter for this laneChangeModel. Throw exception for unsupported key
MSVehicleDevice * getDevice(const std::type_info &type) const
Returns a device of the given type if it exists or 0.
MSVehicleType & getSingularType()
Replaces the current vehicle type with a new one used by this vehicle only.
void replaceVehicleType(MSVehicleType *type)
Replaces the current vehicle type by the one given.
const MSEdge * getEdge() const
Returns the edge the vehicle is currently at.
const MSVehicleType & getVehicleType() const
Returns the vehicle's type definition.
SUMOVehicleClass getVClass() const
Returns the vehicle's access class.
A device which collects info on the vehicle trip (mainly on departure and arrival)
bool notifyMove(SUMOTrafficObject &veh, double oldPos, double newPos, double newSpeed)
Checks for waiting steps when the vehicle moves.
void setParameter(const std::string &key, const std::string &value)
try to set the given parameter for this device. Throw exception for unsupported key
std::string getParameter(const std::string &key) const
try to retrieve the given parameter from this device. Throw exception for unsupported key
static void insertOptions(OptionsCont &oc)
Inserts MSDevice_Bluelight-options.
std::map< std::string, std::string > influencedTypes
static void buildVehicleDevices(SUMOVehicle &v, std::vector< MSVehicleDevice * > &into)
Build devices for the given vehicle, if needed.
double myReactionDist
reaction distance of other vehicle (i.e. due to different noise levels of the siren)
~MSDevice_Bluelight()
Destructor.
bool notifyEnter(SUMOTrafficObject &veh, MSMoveReminder::Notification reason, const MSLane *enteredLane=0)
Saves departure info on insertion.
const std::string deviceName() const
return the name for this type of device
void generateOutput(OutputDevice *tripinfoOut) const
Called on writing tripinfo output.
MSDevice_Bluelight(SUMOVehicle &holder, const std::string &id, double reactionDist)
Constructor.
std::set< std::string > influencedVehicles
bool notifyLeave(SUMOTrafficObject &veh, double lastPos, MSMoveReminder::Notification reason, const MSLane *enteredLane=0)
Saves arrival info.
static double getFloatParam(const SUMOVehicle &v, const OptionsCont &oc, std::string paramName, double deflt, bool required)
Definition: MSDevice.cpp:185
static void insertDefaultAssignmentOptions(const std::string &deviceName, const std::string &optionsTopic, OptionsCont &oc, const bool isPerson=false)
Adds common command options that allow to assign devices to vehicles.
Definition: MSDevice.cpp:134
static bool equippedByDefaultAssignmentOptions(const OptionsCont &oc, const std::string &deviceName, DEVICEHOLDER &v, bool outputOptionSet, const bool isPerson=false)
Determines whether a vehicle should get a certain device.
Definition: MSDevice.h:204
A road/street connecting two junctions.
Definition: MSEdge.h:77
const MSEdge * getInternalFollowingEdge(const MSEdge *followerAfterInternal) const
Definition: MSEdge.cpp:690
const std::vector< MSLane * > * allowedLanes(const MSEdge &destination, SUMOVehicleClass vclass=SVC_IGNORING) const
Get the allowed lanes to reach the destination-edge.
Definition: MSEdge.cpp:366
const std::vector< MSLane * > & getLanes() const
Returns this edge's lanes.
Definition: MSEdge.h:166
Representation of a lane in the micro simulation.
Definition: MSLane.h:82
double getLength() const
Returns the lane's length.
Definition: MSLane.h:539
int getIndex() const
Returns the lane's index.
Definition: MSLane.h:562
MSEdge & getEdge() const
Returns the lane's edge.
Definition: MSLane.h:673
double getWidth() const
Returns the lane's width.
Definition: MSLane.h:555
Notification
Definition of a vehicle state.
@ NOTIFICATION_JUNCTION
The vehicle arrived at a junction.
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
Definition: MSNet.cpp:171
MSVehicleControl & getVehicleControl()
Returns the vehicle control.
Definition: MSNet.h:371
Changes the wished vehicle speed / lanes.
Definition: MSVehicle.h:1341
void setLaneChangeMode(int value)
Sets lane changing behavior.
Definition: MSVehicle.cpp:772
void setSpeedMode(int speedMode)
Sets speed-constraining behaviors.
Definition: MSVehicle.cpp:762
void setSpeedTimeLine(const std::vector< std::pair< SUMOTime, double > > &speedTimeLine)
Sets a new velocity timeline.
Definition: MSVehicle.cpp:391
The class responsible for building and deletion of vehicles.
std::map< std::string, SUMOVehicle * >::const_iterator constVehIt
Definition of the internal vehicles map iterator.
MSVehicleType * getVType(const std::string &id=DEFAULT_VTYPE_ID, std::mt19937 *rng=nullptr)
Returns the named vehicle type or a sample from the named distribution.
constVehIt loadedVehBegin() const
Returns the begin of the internal vehicle map.
constVehIt loadedVehEnd() const
Returns the end of the internal vehicle map.
Abstract in-vehicle device.
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:77
bool enterLaneAtMove(MSLane *enteredLane, bool onTeleporting=false)
Update when the vehicle enters a new lane in the move step.
Definition: MSVehicle.cpp:4452
const std::vector< const MSLane * > getUpcomingLanesUntil(double distance) const
Returns the upcoming (best followed by default 0) sequence of lanes to continue the route starting at...
Definition: MSVehicle.cpp:5134
void setTentativeLaneAndPosition(MSLane *lane, double pos, double posLat=0)
set tentative lane and position during insertion to ensure that all cfmodels work (some of them requi...
Definition: MSVehicle.cpp:5463
MSAbstractLaneChangeModel & getLaneChangeModel()
Definition: MSVehicle.cpp:4699
Position getPosition(const double offset=0) const
Return current position (x/y, cartesian)
Definition: MSVehicle.cpp:1122
const std::vector< MSLane * > & getBestLanesContinuation() const
Returns the best sequence of lanes to continue the route starting at myLane.
Definition: MSVehicle.cpp:5106
void leaveLane(const MSMoveReminder::Notification reason, const MSLane *approachedLane=0)
Update of members if vehicle leaves a new lane in the lane change step or at arrival.
Definition: MSVehicle.cpp:4634
double getLatOffset(const MSLane *lane) const
Get the offset that that must be added to interpret myState.myPosLat for the given lane.
Definition: MSVehicle.cpp:5523
Influencer & getInfluencer()
Definition: MSVehicle.cpp:6059
double getLateralPositionOnLane() const
Get the vehicle's lateral position on the lane.
Definition: MSVehicle.h:411
double getSpeed() const
Returns the vehicle's current speed.
Definition: MSVehicle.h:458
double getPositionOnLane() const
Get the vehicle's position along the lane.
Definition: MSVehicle.h:374
const MSLane * getLane() const
Returns the lane the vehicle is on.
Definition: MSVehicle.h:550
The car-following model and parameter.
Definition: MSVehicleType.h:62
double getWidth() const
Get the width which vehicles of this class shall have when being drawn.
void setPreferredLateralAlignment(LateralAlignment latAlignment)
Set vehicle's preferred lateral alignment.
const std::string & getID() const
Returns the name of the vehicle type.
Definition: MSVehicleType.h:90
const SUMOVTypeParameter & getParameter() const
const std::string & getID() const
Returns the id.
Definition: Named.h:73
A storage for options typed value containers)
Definition: OptionsCont.h:89
void addDescription(const std::string &name, const std::string &subtopic, const std::string &description)
Adds a description for an option.
void doRegister(const std::string &name, Option *v)
Adds an option under the given name.
Definition: OptionsCont.cpp:75
double getFloat(const std::string &name) const
Returns the double-value of the named option (only for Option_Float)
void addOptionSubTopic(const std::string &topic)
Adds an option subtopic.
static OptionsCont & getOptions()
Retrieves the options.
Definition: OptionsCont.cpp:58
Static storage of an output device and its base (abstract) implementation.
Definition: OutputDevice.h:60
OutputDevice & openTag(const std::string &xmlElement)
Opens an XML tag.
bool closeTag(const std::string &comment="")
Closes the most recently opened tag and optionally adds a comment.
double distanceTo(const Position &p2) const
returns the euclidean distance in 3 dimension
Definition: Position.h:231
static double rand(std::mt19937 *rng=nullptr)
Returns a random real number in [0, 1)
Definition: RandHelper.h:51
Representation of a vehicle, person, or container.
virtual SUMOTime getWaitingTime() const =0
virtual double getSpeed() const =0
Returns the object's current speed.
virtual const MSVehicleType & getVehicleType() const =0
Returns the object's "vehicle" type.
virtual Position getPosition(const double offset=0) const =0
Return current position (x/y, cartesian)
virtual const MSEdge * getEdge() const =0
Returns the edge the object is currently at.
std::string getLCParamString(const SumoXMLAttr attr, const std::string &defaultValue) const
Returns the named value from the map, or the default if it is not contained there.
Representation of a vehicle.
Definition: SUMOVehicle.h:58
static double toDouble(const std::string &sData)
converts a string into the double value described by it by calling the char-type converter