Eclipse SUMO - Simulation of Urban MObility
MSLeaderInfo.cpp
Go to the documentation of this file.
1 /****************************************************************************/
2 // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3 // Copyright (C) 2002-2019 German Aerospace Center (DLR) and others.
4 // This program and the accompanying materials
5 // are made available under the terms of the Eclipse Public License v2.0
6 // which accompanies this distribution, and is available at
7 // http://www.eclipse.org/legal/epl-v20.html
8 // SPDX-License-Identifier: EPL-2.0
9 /****************************************************************************/
14 // Information about vehicles ahead (may be multiple vehicles if
15 // lateral-resolution is active)
16 /****************************************************************************/
17 
18 
19 // ===========================================================================
20 // included modules
21 // ===========================================================================
22 #include <config.h>
23 
24 #include <cassert>
25 #include <cmath>
26 #include <utils/common/ToString.h>
27 #include <microsim/MSGlobals.h>
28 #include <microsim/MSVehicle.h>
30 #include <microsim/MSNet.h>
31 #include "MSLeaderInfo.h"
32 
33 
34 // ===========================================================================
35 // static member variables
36 // ===========================================================================
37 
38 
39 // ===========================================================================
40 // MSLeaderInfo member method definitions
41 // ===========================================================================
42 MSLeaderInfo::MSLeaderInfo(const MSLane* lane, const MSVehicle* ego, double latOffset) :
43  myWidth(lane->getWidth()),
44  myVehicles(MAX2(1, int(ceil(myWidth / MSGlobals::gLateralResolution))), (MSVehicle*)nullptr),
45  myFreeSublanes((int)myVehicles.size()),
46  egoRightMost(-1),
47  egoLeftMost(-1),
48  myHasVehicles(false) {
49  if (ego != nullptr) {
50  getSubLanes(ego, latOffset, egoRightMost, egoLeftMost);
51  // filter out sublanes not of interest to ego
53  myFreeSublanes -= (int)myVehicles.size() - 1 - egoLeftMost;
54  }
55 }
56 
57 
59 
60 
61 int
62 MSLeaderInfo::addLeader(const MSVehicle* veh, bool beyond, double latOffset) {
63  if (veh == nullptr) {
64  return myFreeSublanes;
65  }
66  if (myVehicles.size() == 1) {
67  // speedup for the simple case
68  if (!beyond || myVehicles[0] == 0) {
69  myVehicles[0] = veh;
70  myFreeSublanes = 0;
71  myHasVehicles = true;
72  }
73  return myFreeSublanes;
74  }
75  // map center-line based coordinates into [0, myWidth] coordinates
76  int rightmost, leftmost;
77  getSubLanes(veh, latOffset, rightmost, leftmost);
78  //if (gDebugFlag1) std::cout << " addLeader veh=" << veh->getID() << " beyond=" << beyond << " latOffset=" << latOffset << " rightmost=" << rightmost << " leftmost=" << leftmost << " myFreeSublanes=" << myFreeSublanes << "\n";
79  for (int sublane = rightmost; sublane <= leftmost; ++sublane) {
80  if ((egoRightMost < 0 || (egoRightMost <= sublane && sublane <= egoLeftMost))
81  && (!beyond || myVehicles[sublane] == 0)) {
82  if (myVehicles[sublane] == 0) {
84  }
85  myVehicles[sublane] = veh;
86  myHasVehicles = true;
87  }
88  }
89  return myFreeSublanes;
90 }
91 
92 
93 void
95  myVehicles.assign(myVehicles.size(), (MSVehicle*)nullptr);
96  myFreeSublanes = (int)myVehicles.size();
97  if (egoRightMost >= 0) {
99  myFreeSublanes -= (int)myVehicles.size() - 1 - egoLeftMost;
100  }
101 }
102 
103 
104 void
105 MSLeaderInfo::getSubLanes(const MSVehicle* veh, double latOffset, int& rightmost, int& leftmost) const {
106  if (myVehicles.size() == 1) {
107  // speedup for the simple case
108  rightmost = 0;
109  leftmost = 0;
110  return;
111  }
112  // map center-line based coordinates into [0, myWidth] coordinates
113  const double vehCenter = veh->getLateralPositionOnLane() + 0.5 * myWidth + latOffset;
114  const double vehHalfWidth = 0.5 * veh->getVehicleType().getWidth();
115  double rightVehSide = MAX2(0., vehCenter - vehHalfWidth);
116  double leftVehSide = MIN2(myWidth, vehCenter + vehHalfWidth);
117  // Reserve some additional space if the vehicle is performing a maneuver continuation.
118  if (veh->getActionStepLength() != DELTA_T) {
119  if (veh->getLaneChangeModel().getManeuverDist() < 0. || veh->getLaneChangeModel().getSpeedLat() < 0.) {
120  const double maneuverDist = MIN2(veh->getVehicleType().getMaxSpeedLat() * veh->getActionStepLengthSecs(), -MIN2(0., veh->getLaneChangeModel().getManeuverDist()));
121  rightVehSide -= maneuverDist;
122  }
123  if (veh->getLaneChangeModel().getManeuverDist() > 0. || veh->getLaneChangeModel().getSpeedLat() > 0.) {
124  const double maneuverDist = MIN2(veh->getVehicleType().getMaxSpeedLat() * veh->getActionStepLengthSecs(), MAX2(0., veh->getLaneChangeModel().getManeuverDist()));
125  leftVehSide += maneuverDist;
126  }
127  }
128 
129  rightmost = MAX2(0, (int)floor((rightVehSide + NUMERICAL_EPS) / MSGlobals::gLateralResolution));
130  leftmost = MIN2((int)myVehicles.size() - 1, (int)floor((leftVehSide - NUMERICAL_EPS) / MSGlobals::gLateralResolution));
131  //if (veh->getID() == "Pepoli_11_41") std::cout << SIMTIME << " veh=" << veh->getID()
132  // << std::setprecision(10)
133  // << " posLat=" << veh->getLateralPositionOnLane()
134  // << " latOffset=" << latOffset
135  // << " rightVehSide=" << rightVehSide
136  // << " leftVehSide=" << leftVehSide
137  // << " rightmost=" << rightmost
138  // << " leftmost=" << leftmost
139  // << std::setprecision(2)
140  // << "\n";
141 }
142 
143 
144 void
145 MSLeaderInfo::getSublaneBorders(int sublane, double latOffset, double& rightSide, double& leftSide) const {
146  assert(sublane >= 0);
147  assert(sublane < (int)myVehicles.size());
149  rightSide = sublane * res + latOffset;
150  leftSide = MIN2((sublane + 1) * res, myWidth) + latOffset;
151 }
152 
153 
154 const MSVehicle*
155 MSLeaderInfo::operator[](int sublane) const {
156  assert(sublane >= 0);
157  assert(sublane < (int)myVehicles.size());
158  return myVehicles[sublane];
159 }
160 
161 
162 std::string
164  std::ostringstream oss;
165  oss.setf(std::ios::fixed, std::ios::floatfield);
166  oss << std::setprecision(2);
167  for (int i = 0; i < (int)myVehicles.size(); ++i) {
168  oss << Named::getIDSecure(myVehicles[i]);
169  if (i < (int)myVehicles.size() - 1) {
170  oss << ", ";
171  }
172  }
173  oss << " free=" << myFreeSublanes;
174  return oss.str();
175 }
176 
177 
178 bool
180  if (!myHasVehicles) {
181  return false;
182  }
183  for (int i = 0; i < (int)myVehicles.size(); ++i) {
184  if (myVehicles[0] != 0 && myVehicles[0]->isStopped()) {
185  return true;
186  }
187  }
188  return false;
189 }
190 
191 // ===========================================================================
192 // MSLeaderDistanceInfo member method definitions
193 // ===========================================================================
194 
195 
196 MSLeaderDistanceInfo::MSLeaderDistanceInfo(const MSLane* lane, const MSVehicle* ego, double latOffset) :
197  MSLeaderInfo(lane, ego, latOffset),
198  myDistances(myVehicles.size(), std::numeric_limits<double>::max()) {
199 }
200 
201 
203  MSLeaderInfo(dummy, nullptr, 0),
204  myDistances(1, cLeaderDist.second) {
205  assert(myVehicles.size() == 1);
206  myVehicles[0] = cLeaderDist.first;
207  myHasVehicles = cLeaderDist.first != nullptr;
208 }
209 
211 
212 
213 int
214 MSLeaderDistanceInfo::addLeader(const MSVehicle* veh, double gap, double latOffset, int sublane) {
215  //if (SIMTIME == 31 && gDebugFlag1 && veh != 0 && veh->getID() == "cars.8") {
216  // std::cout << " BREAKPOINT\n";
217  //}
218  if (veh == nullptr) {
219  return myFreeSublanes;
220  }
221  if (myVehicles.size() == 1) {
222  // speedup for the simple case
223  sublane = 0;
224  }
225  if (sublane >= 0 && sublane < (int)myVehicles.size()) {
226  // sublane is already given
227  if (gap < myDistances[sublane]) {
228  if (myVehicles[sublane] == 0) {
229  myFreeSublanes--;
230  }
231  myVehicles[sublane] = veh;
232  myDistances[sublane] = gap;
233  myHasVehicles = true;
234  }
235  return myFreeSublanes;
236  }
237  int rightmost, leftmost;
238  getSubLanes(veh, latOffset, rightmost, leftmost);
239  for (int sublane = rightmost; sublane <= leftmost; ++sublane) {
240  if ((egoRightMost < 0 || (egoRightMost <= sublane && sublane <= egoLeftMost))
241  && gap < myDistances[sublane]) {
242  if (myVehicles[sublane] == 0) {
243  myFreeSublanes--;
244  }
245  myVehicles[sublane] = veh;
246  myDistances[sublane] = gap;
247  myHasVehicles = true;
248  }
249  }
250  return myFreeSublanes;
251 }
252 
253 
254 void
257  myDistances.assign(myVehicles.size(), std::numeric_limits<double>::max());
258 }
259 
260 
263  assert(sublane >= 0);
264  assert(sublane < (int)myVehicles.size());
265  return std::make_pair(myVehicles[sublane], myDistances[sublane]);
266 }
267 
268 
269 std::string
271  std::ostringstream oss;
272  oss.setf(std::ios::fixed, std::ios::floatfield);
273  oss << std::setprecision(2);
274  for (int i = 0; i < (int)myVehicles.size(); ++i) {
275  oss << Named::getIDSecure(myVehicles[i]) << ":";
276  if (myVehicles[i] == 0) {
277  oss << "inf";
278  } else {
279  oss << myDistances[i];
280  }
281  if (i < (int)myVehicles.size() - 1) {
282  oss << ", ";
283  }
284  }
285  oss << " free=" << myFreeSublanes;
286  return oss.str();
287 }
288 
289 
290 // ===========================================================================
291 // MSCriticalFollowerDistanceInfo member method definitions
292 // ===========================================================================
293 
294 
296  MSLeaderDistanceInfo(lane, ego, latOffset),
297  myMissingGaps(myVehicles.size(), -std::numeric_limits<double>::max()) {
298 }
299 
300 
302 
303 
304 int
305 MSCriticalFollowerDistanceInfo::addFollower(const MSVehicle* veh, const MSVehicle* ego, double gap, double latOffset, int sublane) {
306  if (veh == nullptr) {
307  return myFreeSublanes;
308  }
309  const double requiredGap = veh->getCarFollowModel().getSecureGap(veh, ego, veh->getSpeed(), ego->getSpeed(), ego->getCarFollowModel().getMaxDecel());
310  const double missingGap = requiredGap - gap;
311  /*
312  if (ego->getID() == "disabled" || gDebugFlag1) {
313  std::cout << " addFollower veh=" << veh->getID()
314  << " ego=" << ego->getID()
315  << " gap=" << gap
316  << " reqGap=" << requiredGap
317  << " missingGap=" << missingGap
318  << " latOffset=" << latOffset
319  << " sublane=" << sublane
320  << "\n";
321  if (sublane > 0) {
322  std::cout
323  << " dists[s]=" << myDistances[sublane]
324  << " gaps[s]=" << myMissingGaps[sublane]
325  << "\n";
326  } else {
327  std::cout << toString() << "\n";
328  }
329  }
330  */
331  if (myVehicles.size() == 1) {
332  // speedup for the simple case
333  sublane = 0;
334  }
335  if (sublane >= 0 && sublane < (int)myVehicles.size()) {
336  // sublane is already given
337  // overlapping vehicles are stored preferably
338  // among those vehicles with missing gap, closer ones are preferred
339  if ((missingGap > myMissingGaps[sublane]
340  || (missingGap > 0 && gap < myDistances[sublane])
341  || (gap < 0 && myDistances[sublane] > 0))
342  && !(gap > 0 && myDistances[sublane] < 0)
343  && !(myMissingGaps[sublane] > 0 && myDistances[sublane] < gap)
344  ) {
345  if (myVehicles[sublane] == 0) {
346  myFreeSublanes--;
347  }
348  myVehicles[sublane] = veh;
349  myDistances[sublane] = gap;
350  myMissingGaps[sublane] = missingGap;
351  myHasVehicles = true;
352  }
353  return myFreeSublanes;
354  }
355  int rightmost, leftmost;
356  getSubLanes(veh, latOffset, rightmost, leftmost);
357  for (int sublane = rightmost; sublane <= leftmost; ++sublane) {
358  if ((egoRightMost < 0 || (egoRightMost <= sublane && sublane <= egoLeftMost))
359  // overlapping vehicles are stored preferably
360  // among those vehicles with missing gap, closer ones are preferred
361  && (missingGap > myMissingGaps[sublane]
362  || (missingGap > 0 && gap < myDistances[sublane])
363  || (gap < 0 && myDistances[sublane] > 0))
364  && !(gap > 0 && myDistances[sublane] < 0)
365  && !(myMissingGaps[sublane] > 0 && myDistances[sublane] < gap)
366  ) {
367  if (myVehicles[sublane] == 0) {
368  myFreeSublanes--;
369  }
370  myVehicles[sublane] = veh;
371  myDistances[sublane] = gap;
372  myMissingGaps[sublane] = missingGap;
373  myHasVehicles = true;
374  }
375  }
376  return myFreeSublanes;
377 }
378 
379 
380 void
383  myMissingGaps.assign(myVehicles.size(), -std::numeric_limits<double>::max());
384 }
385 
386 
387 std::string
389  std::ostringstream oss;
390  oss.setf(std::ios::fixed, std::ios::floatfield);
391  oss << std::setprecision(2);
392  for (int i = 0; i < (int)myVehicles.size(); ++i) {
393  oss << Named::getIDSecure(myVehicles[i]) << ":";
394  if (myVehicles[i] == 0) {
395  oss << "inf:-inf";
396  } else {
397  oss << myDistances[i] << ":" << myMissingGaps[i];
398  }
399  if (i < (int)myVehicles.size() - 1) {
400  oss << ", ";
401  }
402  }
403  oss << " free=" << myFreeSublanes;
404  return oss.str();
405 }
406 /****************************************************************************/
407 
MSLeaderInfo::getSublaneBorders
void getSublaneBorders(int sublane, double latOffset, double &rightSide, double &leftSide) const
Definition: MSLeaderInfo.cpp:145
MSLeaderInfo::hasStoppedVehicle
bool hasStoppedVehicle() const
whether a stopped vehicle is leader
Definition: MSLeaderInfo.cpp:179
ToString.h
MIN2
T MIN2(T a, T b)
Definition: StdDefs.h:73
MSCFModel::getMaxDecel
double getMaxDecel() const
Get the vehicle type's maximal comfortable deceleration [m/s^2].
Definition: MSCFModel.h:217
MSLeaderDistanceInfo::operator[]
CLeaderDist operator[](int sublane) const
return the vehicle and its distance for the given sublane
Definition: MSLeaderInfo.cpp:262
MSCriticalFollowerDistanceInfo::~MSCriticalFollowerDistanceInfo
virtual ~MSCriticalFollowerDistanceInfo()
Destructor.
Definition: MSLeaderInfo.cpp:301
MSCriticalFollowerDistanceInfo::clear
void clear()
discard all information
Definition: MSLeaderInfo.cpp:381
MSNet.h
MSLane
Representation of a lane in the micro simulation.
Definition: MSLane.h:82
MSLeaderDistanceInfo
saves leader/follower vehicles and their distances relative to an ego vehicle
Definition: MSLeaderInfo.h:132
MSLeaderInfo::egoLeftMost
int egoLeftMost
Definition: MSLeaderInfo.h:124
DELTA_T
SUMOTime DELTA_T
Definition: SUMOTime.cpp:36
NUMERICAL_EPS
#define NUMERICAL_EPS
Definition: config.h:148
MSLeaderInfo::MSLeaderInfo
MSLeaderInfo(const MSLane *lane, const MSVehicle *ego=0, double latOffset=0)
Constructor.
Definition: MSLeaderInfo.cpp:42
MSLeaderInfo.h
MSCFModel::getSecureGap
virtual double getSecureGap(const MSVehicle *const, const MSVehicle *const, const double speed, const double leaderSpeed, const double leaderMaxDecel) const
Returns the minimum gap to reserve if the leader is braking at maximum (>=0)
Definition: MSCFModel.h:329
MSLeaderInfo::myFreeSublanes
int myFreeSublanes
the number of free sublanes
Definition: MSLeaderInfo.h:120
MSVehicleType::getMaxSpeedLat
double getMaxSpeedLat() const
Get vehicle's maximum lateral speed [m/s].
Definition: MSVehicleType.h:313
MSVehicle::getActionStepLength
SUMOTime getActionStepLength() const
Returns the vehicle's action step length in millisecs, i.e. the interval between two action points.
Definition: MSVehicle.h:504
MSAbstractLaneChangeModel::getSpeedLat
double getSpeedLat() const
return the lateral speed of the current lane change maneuver
Definition: MSAbstractLaneChangeModel.h:544
MSGlobals
Definition: MSGlobals.h:48
MSLeaderInfo::~MSLeaderInfo
virtual ~MSLeaderInfo()
Destructor.
Definition: MSLeaderInfo.cpp:58
MSLeaderInfo::myWidth
double myWidth
the width of the lane to which this instance applies
Definition: MSLeaderInfo.h:113
MSLeaderInfo::addLeader
virtual int addLeader(const MSVehicle *veh, bool beyond, double latOffset=0)
Definition: MSLeaderInfo.cpp:62
MSVehicle::getCarFollowModel
const MSCFModel & getCarFollowModel() const
Returns the vehicle's car following model definition.
Definition: MSVehicle.h:893
MSVehicle::getLaneChangeModel
MSAbstractLaneChangeModel & getLaneChangeModel()
Definition: MSVehicle.cpp:4680
MSGlobals::gLateralResolution
static double gLateralResolution
Definition: MSGlobals.h:84
MSVehicle.h
MSVehicle::getLateralPositionOnLane
double getLateralPositionOnLane() const
Get the vehicle's lateral position on the lane.
Definition: MSVehicle.h:429
MAX2
T MAX2(T a, T b)
Definition: StdDefs.h:79
MSLeaderInfo::egoRightMost
int egoRightMost
borders of the ego vehicle for filtering of free sublanes
Definition: MSLeaderInfo.h:123
MSLeaderInfo
Definition: MSLeaderInfo.h:49
MSLeaderInfo::getSubLanes
void getSubLanes(const MSVehicle *veh, double latOffset, int &rightmost, int &leftmost) const
Definition: MSLeaderInfo.cpp:105
MSLeaderInfo::myVehicles
std::vector< const MSVehicle * > myVehicles
Definition: MSLeaderInfo.h:115
MSLeaderInfo::clear
virtual void clear()
discard all information
Definition: MSLeaderInfo.cpp:94
MSVehicle::getActionStepLengthSecs
double getActionStepLengthSecs() const
Returns the vehicle's action step length in secs, i.e. the interval between two action points.
Definition: MSVehicle.h:512
MSCriticalFollowerDistanceInfo::MSCriticalFollowerDistanceInfo
MSCriticalFollowerDistanceInfo(const MSLane *lane, const MSVehicle *ego, double latOffset)
Constructor.
Definition: MSLeaderInfo.cpp:295
MSVehicleType::getWidth
double getWidth() const
Get the width which vehicles of this class shall have when being drawn.
Definition: MSVehicleType.h:246
MSLeaderInfo::toString
virtual std::string toString() const
print a debugging representation
Definition: MSLeaderInfo.cpp:163
MSLeaderDistanceInfo::~MSLeaderDistanceInfo
virtual ~MSLeaderDistanceInfo()
Destructor.
Definition: MSLeaderInfo.cpp:210
MSGlobals.h
MSBaseVehicle::getVehicleType
const MSVehicleType & getVehicleType() const
Returns the vehicle's type definition.
Definition: MSBaseVehicle.h:123
MSAbstractLaneChangeModel::getManeuverDist
double getManeuverDist() const
Returns the remaining unblocked distance for the current maneuver. (only used by sublane model)
Definition: MSAbstractLaneChangeModel.cpp:169
MSLeaderInfo::operator[]
const MSVehicle * operator[](int sublane) const
return the vehicle for the given sublane
Definition: MSLeaderInfo.cpp:155
MSCriticalFollowerDistanceInfo::toString
std::string toString() const
print a debugging representation
Definition: MSLeaderInfo.cpp:388
MSCriticalFollowerDistanceInfo::addFollower
int addFollower(const MSVehicle *veh, const MSVehicle *ego, double gap, double latOffset=0, int sublane=-1)
Definition: MSLeaderInfo.cpp:305
MSLeaderInfo::myHasVehicles
bool myHasVehicles
Definition: MSLeaderInfo.h:126
MSLeaderDistanceInfo::addLeader
virtual int addLeader(const MSVehicle *veh, double gap, double latOffset=0, int sublane=-1)
Definition: MSLeaderInfo.cpp:214
MSLeaderDistanceInfo::toString
virtual std::string toString() const
print a debugging representation
Definition: MSLeaderInfo.cpp:270
config.h
Named::getIDSecure
static std::string getIDSecure(const T *obj, const std::string &fallBack="NULL")
get an identifier for Named-like object which may be Null
Definition: Named.h:69
MSVehicle::getSpeed
double getSpeed() const
Returns the vehicle's current speed.
Definition: MSVehicle.h:476
MSCriticalFollowerDistanceInfo::myMissingGaps
std::vector< double > myMissingGaps
Definition: MSLeaderInfo.h:225
MSLeaderDistanceInfo::myDistances
std::vector< double > myDistances
Definition: MSLeaderInfo.h:175
CLeaderDist
std::pair< const MSVehicle *, double > CLeaderDist
Definition: MSLeaderInfo.h:34
MSLeaderDistanceInfo::clear
virtual void clear()
discard all information
Definition: MSLeaderInfo.cpp:255
MSLeaderDistanceInfo::MSLeaderDistanceInfo
MSLeaderDistanceInfo(const MSLane *lane, const MSVehicle *ego, double latOffset)
Constructor.
Definition: MSLeaderInfo.cpp:196
MSAbstractLaneChangeModel.h
MSVehicle
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:79