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-2022 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 /****************************************************************************/
18 // Information about vehicles ahead (may be multiple vehicles if
19 // lateral-resolution is active)
20 /****************************************************************************/
21 #include <config.h>
22 
23 #include <cassert>
24 #include <cmath>
25 #include <utils/common/ToString.h>
26 #include <microsim/MSGlobals.h>
27 #include <microsim/MSVehicle.h>
29 #include <microsim/MSNet.h>
30 #include <microsim/MSLane.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 = vehCenter - vehHalfWidth;
116  double leftVehSide = 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  if (rightVehSide > myWidth || leftVehSide < 0) {
129  // vehicle does not touch this lane
130  // set the values so that an iteration
131  // for (i = rightmost; i <= leftmost; i++) stops immediately
132  rightmost = -1000;
133  leftmost = -2000;
134  } else {
135  rightmost = MAX2(0, (int)floor((rightVehSide + NUMERICAL_EPS) / MSGlobals::gLateralResolution));
136  leftmost = MIN2((int)myVehicles.size() - 1, (int)floor((leftVehSide - NUMERICAL_EPS) / MSGlobals::gLateralResolution));
137  }
138  //if (veh->getID() == "flow_0.33" && SIMTIME == 264) std::cout << SIMTIME << " veh=" << veh->getID()
139  // << std::setprecision(10)
140  // << " posLat=" << veh->getLateralPositionOnLane()
141  // << " latOffset=" << latOffset
142  // << " vehCenter=" << vehCenter
143  // << " rightVehSide=" << rightVehSide
144  // << " leftVehSide=" << leftVehSide
145  // << " rightmost=" << rightmost
146  // << " leftmost=" << leftmost
147  // << std::setprecision(2)
148  // << "\n";
149 }
150 
151 
152 void
153 MSLeaderInfo::getSublaneBorders(int sublane, double latOffset, double& rightSide, double& leftSide) const {
154  assert(sublane >= 0);
155  assert(sublane < (int)myVehicles.size());
157  rightSide = sublane * res + latOffset;
158  leftSide = MIN2((sublane + 1) * res, myWidth) + latOffset;
159 }
160 
161 
162 const MSVehicle*
163 MSLeaderInfo::operator[](int sublane) const {
164  assert(sublane >= 0);
165  assert(sublane < (int)myVehicles.size());
166  return myVehicles[sublane];
167 }
168 
169 
170 std::string
172  std::ostringstream oss;
173  oss.setf(std::ios::fixed, std::ios::floatfield);
174  oss << std::setprecision(2);
175  for (int i = 0; i < (int)myVehicles.size(); ++i) {
176  oss << Named::getIDSecure(myVehicles[i]);
177  if (i < (int)myVehicles.size() - 1) {
178  oss << ", ";
179  }
180  }
181  oss << " free=" << myFreeSublanes;
182  return oss.str();
183 }
184 
185 
186 bool
188  if (!myHasVehicles) {
189  return false;
190  }
191  for (int i = 0; i < (int)myVehicles.size(); ++i) {
192  if (myVehicles[i] != 0 && myVehicles[i]->isStopped()) {
193  return true;
194  }
195  }
196  return false;
197 }
198 
199 void
201  for (int i = 0; i < (int)myVehicles.size(); ++i) {
202  const MSVehicle* veh = myVehicles[i];
203  if (veh != 0 &&
205  || &lane->getEdge() != &veh->getLane()->getEdge())) {
206  myVehicles[i] = nullptr;
207  }
208  }
209 }
210 
211 // ===========================================================================
212 // MSLeaderDistanceInfo member method definitions
213 // ===========================================================================
214 
215 
216 MSLeaderDistanceInfo::MSLeaderDistanceInfo(const MSLane* lane, const MSVehicle* ego, double latOffset) :
217  MSLeaderInfo(lane, ego, latOffset),
218  myDistances(myVehicles.size(), std::numeric_limits<double>::max()) {
219 }
220 
221 
223  MSLeaderInfo(dummy, nullptr, 0),
224  myDistances(1, cLeaderDist.second) {
225  assert(myVehicles.size() == 1);
226  myVehicles[0] = cLeaderDist.first;
227  myHasVehicles = cLeaderDist.first != nullptr;
228 }
229 
231 
232 
233 int
234 MSLeaderDistanceInfo::addLeader(const MSVehicle* veh, double gap, double latOffset, int sublane) {
235  //if (SIMTIME == 31 && gDebugFlag1 && veh != 0 && veh->getID() == "cars.8") {
236  // std::cout << " BREAKPOINT\n";
237  //}
238  if (veh == nullptr) {
239  return myFreeSublanes;
240  }
241  if (myVehicles.size() == 1) {
242  // speedup for the simple case
243  sublane = 0;
244  }
245  if (sublane >= 0 && sublane < (int)myVehicles.size()) {
246  // sublane is already given
247  if (gap < myDistances[sublane]) {
248  if (myVehicles[sublane] == 0) {
249  myFreeSublanes--;
250  }
251  myVehicles[sublane] = veh;
252  myDistances[sublane] = gap;
253  myHasVehicles = true;
254  }
255  return myFreeSublanes;
256  }
257  int rightmost, leftmost;
258  getSubLanes(veh, latOffset, rightmost, leftmost);
259  for (int sublaneIdx = rightmost; sublaneIdx <= leftmost; ++sublaneIdx) {
260  if ((egoRightMost < 0 || (egoRightMost <= sublaneIdx && sublaneIdx <= egoLeftMost))
261  && gap < myDistances[sublaneIdx]) {
262  if (myVehicles[sublaneIdx] == 0) {
263  myFreeSublanes--;
264  }
265  myVehicles[sublaneIdx] = veh;
266  myDistances[sublaneIdx] = gap;
267  myHasVehicles = true;
268  }
269  }
270  return myFreeSublanes;
271 }
272 
273 
274 void
277  myDistances.assign(myVehicles.size(), std::numeric_limits<double>::max());
278 }
279 
280 
283  assert(sublane >= 0);
284  assert(sublane < (int)myVehicles.size());
285  return std::make_pair(myVehicles[sublane], myDistances[sublane]);
286 }
287 
288 
289 std::string
291  std::ostringstream oss;
292  oss.setf(std::ios::fixed, std::ios::floatfield);
293  oss << std::setprecision(2);
294  for (int i = 0; i < (int)myVehicles.size(); ++i) {
295  oss << Named::getIDSecure(myVehicles[i]) << ":";
296  if (myVehicles[i] == 0) {
297  oss << "inf";
298  } else {
299  oss << myDistances[i];
300  }
301  if (i < (int)myVehicles.size() - 1) {
302  oss << ", ";
303  }
304  }
305  oss << " free=" << myFreeSublanes;
306  return oss.str();
307 }
308 
309 void
311  for (int i = 0; i < (int)myVehicles.size(); ++i) {
312  if (myVehicles[i] != nullptr) {
313  if (myVehicles[i]->getLaneChangeModel().isOpposite()) {
314  myDistances[i] -= myVehicles[i]->getVehicleType().getLength();
315  } else if (isFollower && myDistances[i] > POSITION_EPS) {
316  // can ignore oncoming followers once they are past
317  myVehicles[i] = nullptr;
318  myDistances[i] = -1;
319  }
320  }
321  }
322 }
323 
326  double minGap = -1;
327  const MSVehicle* veh = nullptr;
328  if (hasVehicles()) {
329  minGap = std::numeric_limits<double>::max();
330  for (int i = 0; i < (int)myVehicles.size(); ++i) {
331  if (myVehicles[i] != nullptr && myDistances[i] < minGap) {
332  minGap = myDistances[i];
333  veh = myVehicles[i];
334  }
335  }
336  }
337  return std::make_pair(veh, minGap);
338 }
339 
340 // ===========================================================================
341 // MSCriticalFollowerDistanceInfo member method definitions
342 // ===========================================================================
343 
344 
345 MSCriticalFollowerDistanceInfo::MSCriticalFollowerDistanceInfo(const MSLane* lane, const MSVehicle* ego, double latOffset, bool haveOppositeLeaders) :
346  MSLeaderDistanceInfo(lane, ego, latOffset),
347  myMissingGaps(myVehicles.size(), -std::numeric_limits<double>::max()),
348  myHaveOppositeLeaders(haveOppositeLeaders)
349 { }
350 
351 
353 
354 
355 int
356 MSCriticalFollowerDistanceInfo::addFollower(const MSVehicle* veh, const MSVehicle* ego, double gap, double latOffset, int sublane) {
357  if (veh == nullptr) {
358  return myFreeSublanes;
359  }
360  const double requiredGap = (myHaveOppositeLeaders ? 0
361  : veh->getCarFollowModel().getSecureGap(veh, ego, veh->getSpeed(), ego->getSpeed(), ego->getCarFollowModel().getMaxDecel()));
362  const double missingGap = requiredGap - gap;
363  /*
364  if (ego->getID() == "disabled" || gDebugFlag1) {
365  std::cout << " addFollower veh=" << veh->getID()
366  << " ego=" << ego->getID()
367  << " gap=" << gap
368  << " reqGap=" << requiredGap
369  << " missingGap=" << missingGap
370  << " latOffset=" << latOffset
371  << " sublane=" << sublane
372  << "\n";
373  if (sublane > 0) {
374  std::cout
375  << " dists[s]=" << myDistances[sublane]
376  << " gaps[s]=" << myMissingGaps[sublane]
377  << "\n";
378  } else {
379  std::cout << toString() << "\n";
380  }
381  }
382  */
383  if (myVehicles.size() == 1) {
384  // speedup for the simple case
385  sublane = 0;
386  }
387  if (sublane >= 0 && sublane < (int)myVehicles.size()) {
388  // sublane is already given
389  // overlapping vehicles are stored preferably
390  // among those vehicles with missing gap, closer ones are preferred
391  if ((missingGap > myMissingGaps[sublane]
392  || (missingGap > 0 && gap < myDistances[sublane])
393  || (gap < 0 && myDistances[sublane] > 0))
394  && !(gap > 0 && myDistances[sublane] < 0)
395  && !(myMissingGaps[sublane] > 0 && myDistances[sublane] < gap)
396  ) {
397  if (myVehicles[sublane] == 0) {
398  myFreeSublanes--;
399  }
400  myVehicles[sublane] = veh;
401  myDistances[sublane] = gap;
402  myMissingGaps[sublane] = missingGap;
403  myHasVehicles = true;
404  }
405  return myFreeSublanes;
406  }
407  int rightmost, leftmost;
408  getSubLanes(veh, latOffset, rightmost, leftmost);
409  for (int sublaneIdx = rightmost; sublaneIdx <= leftmost; ++sublaneIdx) {
410  if ((egoRightMost < 0 || (egoRightMost <= sublaneIdx && sublaneIdx <= egoLeftMost))
411  // overlapping vehicles are stored preferably
412  // among those vehicles with missing gap, closer ones are preferred
413  && (missingGap > myMissingGaps[sublaneIdx]
414  || (missingGap > 0 && gap < myDistances[sublaneIdx])
415  || (gap < 0 && myDistances[sublaneIdx] > 0))
416  && !(gap > 0 && myDistances[sublaneIdx] < 0)
417  && !(myMissingGaps[sublaneIdx] > 0 && myDistances[sublaneIdx] < gap)
418  ) {
419  if (myVehicles[sublaneIdx] == 0) {
420  myFreeSublanes--;
421  }
422  myVehicles[sublaneIdx] = veh;
423  myDistances[sublaneIdx] = gap;
424  myMissingGaps[sublaneIdx] = missingGap;
425  myHasVehicles = true;
426  }
427  }
428  return myFreeSublanes;
429 }
430 
431 
432 void
435  myMissingGaps.assign(myVehicles.size(), -std::numeric_limits<double>::max());
436 }
437 
438 
439 std::string
441  std::ostringstream oss;
442  oss.setf(std::ios::fixed, std::ios::floatfield);
443  oss << std::setprecision(2);
444  for (int i = 0; i < (int)myVehicles.size(); ++i) {
445  oss << Named::getIDSecure(myVehicles[i]) << ":";
446  if (myVehicles[i] == 0) {
447  oss << "inf:-inf";
448  } else {
449  oss << myDistances[i] << ":" << myMissingGaps[i];
450  }
451  if (i < (int)myVehicles.size() - 1) {
452  oss << ", ";
453  }
454  }
455  oss << " free=" << myFreeSublanes;
456  return oss.str();
457 }
458 
459 
460 /****************************************************************************/
std::pair< const MSVehicle *, double > CLeaderDist
Definition: MSLeaderInfo.h:32
SUMOTime DELTA_T
Definition: SUMOTime.cpp:37
T MIN2(T a, T b)
Definition: StdDefs.h:74
T MAX2(T a, T b)
Definition: StdDefs.h:80
double getManeuverDist() const
Returns the remaining unblocked distance for the current maneuver. (only used by sublane model)
double getSpeedLat() const
return the lateral speed of the current lane change maneuver
const MSVehicleType & getVehicleType() const
Returns the vehicle's type definition.
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:351
double getMaxDecel() const
Get the vehicle type's maximal comfortable deceleration [m/s^2].
Definition: MSCFModel.h:239
std::vector< double > myMissingGaps
Definition: MSLeaderInfo.h:232
MSCriticalFollowerDistanceInfo(const MSLane *lane, const MSVehicle *ego, double latOffset, bool haveOppositeLeaders=false)
Constructor.
virtual ~MSCriticalFollowerDistanceInfo()
Destructor.
std::string toString() const
print a debugging representation
void clear()
discard all information
int addFollower(const MSVehicle *veh, const MSVehicle *ego, double gap, double latOffset=0, int sublane=-1)
static double gLateralResolution
Definition: MSGlobals.h:88
Representation of a lane in the micro simulation.
Definition: MSLane.h:82
MSEdge & getEdge() const
Returns the lane's edge.
Definition: MSLane.h:674
saves leader/follower vehicles and their distances relative to an ego vehicle
Definition: MSLeaderInfo.h:133
virtual std::string toString() const
print a debugging representation
CLeaderDist getClosest() const
return vehicle with the smalles gap
virtual ~MSLeaderDistanceInfo()
Destructor.
CLeaderDist operator[](int sublane) const
return the vehicle and its distance for the given sublane
virtual void clear()
discard all information
std::vector< double > myDistances
Definition: MSLeaderInfo.h:182
void fixOppositeGaps(bool isFollower)
subtract vehicle length from all gaps if the leader vehicle is driving in the opposite direction
virtual int addLeader(const MSVehicle *veh, double gap, double latOffset=0, int sublane=-1)
MSLeaderDistanceInfo(const MSLane *lane, const MSVehicle *ego, double latOffset)
Constructor.
virtual int addLeader(const MSVehicle *veh, bool beyond, double latOffset=0)
std::vector< const MSVehicle * > myVehicles
Definition: MSLeaderInfo.h:116
int myFreeSublanes
the number of free sublanes
Definition: MSLeaderInfo.h:121
bool hasStoppedVehicle() const
whether a stopped vehicle is leader
void removeOpposite(const MSLane *lane)
remove vehicles that are driving in the opposite direction (fully or partially) on the given lane
int egoRightMost
borders of the ego vehicle for filtering of free sublanes
Definition: MSLeaderInfo.h:124
void getSublaneBorders(int sublane, double latOffset, double &rightSide, double &leftSide) const
MSLeaderInfo(const MSLane *lane, const MSVehicle *ego=0, double latOffset=0)
Constructor.
virtual std::string toString() const
print a debugging representation
virtual void clear()
discard all information
virtual ~MSLeaderInfo()
Destructor.
const MSVehicle * operator[](int sublane) const
return the vehicle for the given sublane
bool hasVehicles() const
Definition: MSLeaderInfo.h:93
void getSubLanes(const MSVehicle *veh, double latOffset, int &rightmost, int &leftmost) const
double myWidth
the width of the lane to which this instance applies
Definition: MSLeaderInfo.h:114
bool myHasVehicles
Definition: MSLeaderInfo.h:127
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:75
MSAbstractLaneChangeModel & getLaneChangeModel()
Definition: MSVehicle.cpp:5051
double getActionStepLengthSecs() const
Returns the vehicle's action step length in secs, i.e. the interval between two action points.
Definition: MSVehicle.h:504
SUMOTime getActionStepLength() const
Returns the vehicle's action step length in millisecs, i.e. the interval between two action points.
Definition: MSVehicle.h:496
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:462
const MSLane * getLane() const
Returns the lane the vehicle is on.
Definition: MSVehicle.h:552
const MSCFModel & getCarFollowModel() const
Returns the vehicle's car following model definition.
Definition: MSVehicle.h:917
double getWidth() const
Get the width which vehicles of this class shall have when being drawn.
double getMaxSpeedLat() const
Get vehicle's maximum lateral speed [m/s].
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:67