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-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 /****************************************************************************/
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 "MSLeaderInfo.h"
31 
32 
33 // ===========================================================================
34 // static member variables
35 // ===========================================================================
36 
37 
38 // ===========================================================================
39 // MSLeaderInfo member method definitions
40 // ===========================================================================
41 MSLeaderInfo::MSLeaderInfo(const MSLane* lane, const MSVehicle* ego, double latOffset) :
42  myWidth(lane->getWidth()),
43  myVehicles(MAX2(1, int(ceil(myWidth / MSGlobals::gLateralResolution))), (MSVehicle*)nullptr),
44  myFreeSublanes((int)myVehicles.size()),
45  egoRightMost(-1),
46  egoLeftMost(-1),
47  myHasVehicles(false) {
48  if (ego != nullptr) {
49  getSubLanes(ego, latOffset, egoRightMost, egoLeftMost);
50  // filter out sublanes not of interest to ego
52  myFreeSublanes -= (int)myVehicles.size() - 1 - egoLeftMost;
53  }
54 }
55 
56 
58 
59 
60 int
61 MSLeaderInfo::addLeader(const MSVehicle* veh, bool beyond, double latOffset) {
62  if (veh == nullptr) {
63  return myFreeSublanes;
64  }
65  if (myVehicles.size() == 1) {
66  // speedup for the simple case
67  if (!beyond || myVehicles[0] == 0) {
68  myVehicles[0] = veh;
69  myFreeSublanes = 0;
70  myHasVehicles = true;
71  }
72  return myFreeSublanes;
73  }
74  // map center-line based coordinates into [0, myWidth] coordinates
75  int rightmost, leftmost;
76  getSubLanes(veh, latOffset, rightmost, leftmost);
77  //if (gDebugFlag1) std::cout << " addLeader veh=" << veh->getID() << " beyond=" << beyond << " latOffset=" << latOffset << " rightmost=" << rightmost << " leftmost=" << leftmost << " myFreeSublanes=" << myFreeSublanes << "\n";
78  for (int sublane = rightmost; sublane <= leftmost; ++sublane) {
79  if ((egoRightMost < 0 || (egoRightMost <= sublane && sublane <= egoLeftMost))
80  && (!beyond || myVehicles[sublane] == 0)) {
81  if (myVehicles[sublane] == 0) {
83  }
84  myVehicles[sublane] = veh;
85  myHasVehicles = true;
86  }
87  }
88  return myFreeSublanes;
89 }
90 
91 
92 void
94  myVehicles.assign(myVehicles.size(), (MSVehicle*)nullptr);
95  myFreeSublanes = (int)myVehicles.size();
96  if (egoRightMost >= 0) {
98  myFreeSublanes -= (int)myVehicles.size() - 1 - egoLeftMost;
99  }
100 }
101 
102 
103 void
104 MSLeaderInfo::getSubLanes(const MSVehicle* veh, double latOffset, int& rightmost, int& leftmost) const {
105  if (myVehicles.size() == 1) {
106  // speedup for the simple case
107  rightmost = 0;
108  leftmost = 0;
109  return;
110  }
111  // map center-line based coordinates into [0, myWidth] coordinates
112  const double vehCenter = veh->getLateralPositionOnLane() + 0.5 * myWidth + latOffset;
113  const double vehHalfWidth = 0.5 * veh->getVehicleType().getWidth();
114  double rightVehSide = MAX2(0., vehCenter - vehHalfWidth);
115  double leftVehSide = MIN2(myWidth, vehCenter + vehHalfWidth);
116  // Reserve some additional space if the vehicle is performing a maneuver continuation.
117  if (veh->getActionStepLength() != DELTA_T) {
118  if (veh->getLaneChangeModel().getManeuverDist() < 0. || veh->getLaneChangeModel().getSpeedLat() < 0.) {
119  const double maneuverDist = MIN2(veh->getVehicleType().getMaxSpeedLat() * veh->getActionStepLengthSecs(), -MIN2(0., veh->getLaneChangeModel().getManeuverDist()));
120  rightVehSide -= maneuverDist;
121  }
122  if (veh->getLaneChangeModel().getManeuverDist() > 0. || veh->getLaneChangeModel().getSpeedLat() > 0.) {
123  const double maneuverDist = MIN2(veh->getVehicleType().getMaxSpeedLat() * veh->getActionStepLengthSecs(), MAX2(0., veh->getLaneChangeModel().getManeuverDist()));
124  leftVehSide += maneuverDist;
125  }
126  }
127 
128  rightmost = MAX2(0, (int)floor((rightVehSide + NUMERICAL_EPS) / MSGlobals::gLateralResolution));
129  leftmost = MIN2((int)myVehicles.size() - 1, (int)floor((leftVehSide - NUMERICAL_EPS) / MSGlobals::gLateralResolution));
130  //if (veh->getID() == "Pepoli_11_41") std::cout << SIMTIME << " veh=" << veh->getID()
131  // << std::setprecision(10)
132  // << " posLat=" << veh->getLateralPositionOnLane()
133  // << " latOffset=" << latOffset
134  // << " rightVehSide=" << rightVehSide
135  // << " leftVehSide=" << leftVehSide
136  // << " rightmost=" << rightmost
137  // << " leftmost=" << leftmost
138  // << std::setprecision(2)
139  // << "\n";
140 }
141 
142 
143 void
144 MSLeaderInfo::getSublaneBorders(int sublane, double latOffset, double& rightSide, double& leftSide) const {
145  assert(sublane >= 0);
146  assert(sublane < (int)myVehicles.size());
148  rightSide = sublane * res + latOffset;
149  leftSide = MIN2((sublane + 1) * res, myWidth) + latOffset;
150 }
151 
152 
153 const MSVehicle*
154 MSLeaderInfo::operator[](int sublane) const {
155  assert(sublane >= 0);
156  assert(sublane < (int)myVehicles.size());
157  return myVehicles[sublane];
158 }
159 
160 
161 std::string
163  std::ostringstream oss;
164  oss.setf(std::ios::fixed, std::ios::floatfield);
165  oss << std::setprecision(2);
166  for (int i = 0; i < (int)myVehicles.size(); ++i) {
167  oss << Named::getIDSecure(myVehicles[i]);
168  if (i < (int)myVehicles.size() - 1) {
169  oss << ", ";
170  }
171  }
172  oss << " free=" << myFreeSublanes;
173  return oss.str();
174 }
175 
176 
177 bool
179  if (!myHasVehicles) {
180  return false;
181  }
182  for (int i = 0; i < (int)myVehicles.size(); ++i) {
183  if (myVehicles[0] != 0 && myVehicles[0]->isStopped()) {
184  return true;
185  }
186  }
187  return false;
188 }
189 
190 // ===========================================================================
191 // MSLeaderDistanceInfo member method definitions
192 // ===========================================================================
193 
194 
195 MSLeaderDistanceInfo::MSLeaderDistanceInfo(const MSLane* lane, const MSVehicle* ego, double latOffset) :
196  MSLeaderInfo(lane, ego, latOffset),
197  myDistances(myVehicles.size(), std::numeric_limits<double>::max()) {
198 }
199 
200 
202  MSLeaderInfo(dummy, nullptr, 0),
203  myDistances(1, cLeaderDist.second) {
204  assert(myVehicles.size() == 1);
205  myVehicles[0] = cLeaderDist.first;
206  myHasVehicles = cLeaderDist.first != nullptr;
207 }
208 
210 
211 
212 int
213 MSLeaderDistanceInfo::addLeader(const MSVehicle* veh, double gap, double latOffset, int sublane) {
214  //if (SIMTIME == 31 && gDebugFlag1 && veh != 0 && veh->getID() == "cars.8") {
215  // std::cout << " BREAKPOINT\n";
216  //}
217  if (veh == nullptr) {
218  return myFreeSublanes;
219  }
220  if (myVehicles.size() == 1) {
221  // speedup for the simple case
222  sublane = 0;
223  }
224  if (sublane >= 0 && sublane < (int)myVehicles.size()) {
225  // sublane is already given
226  if (gap < myDistances[sublane]) {
227  if (myVehicles[sublane] == 0) {
228  myFreeSublanes--;
229  }
230  myVehicles[sublane] = veh;
231  myDistances[sublane] = gap;
232  myHasVehicles = true;
233  }
234  return myFreeSublanes;
235  }
236  int rightmost, leftmost;
237  getSubLanes(veh, latOffset, rightmost, leftmost);
238  for (int sublaneIdx = rightmost; sublaneIdx <= leftmost; ++sublaneIdx) {
239  if ((egoRightMost < 0 || (egoRightMost <= sublaneIdx && sublaneIdx <= egoLeftMost))
240  && gap < myDistances[sublaneIdx]) {
241  if (myVehicles[sublaneIdx] == 0) {
242  myFreeSublanes--;
243  }
244  myVehicles[sublaneIdx] = veh;
245  myDistances[sublaneIdx] = gap;
246  myHasVehicles = true;
247  }
248  }
249  return myFreeSublanes;
250 }
251 
252 
253 void
256  myDistances.assign(myVehicles.size(), std::numeric_limits<double>::max());
257 }
258 
259 
262  assert(sublane >= 0);
263  assert(sublane < (int)myVehicles.size());
264  return std::make_pair(myVehicles[sublane], myDistances[sublane]);
265 }
266 
267 
268 std::string
270  std::ostringstream oss;
271  oss.setf(std::ios::fixed, std::ios::floatfield);
272  oss << std::setprecision(2);
273  for (int i = 0; i < (int)myVehicles.size(); ++i) {
274  oss << Named::getIDSecure(myVehicles[i]) << ":";
275  if (myVehicles[i] == 0) {
276  oss << "inf";
277  } else {
278  oss << myDistances[i];
279  }
280  if (i < (int)myVehicles.size() - 1) {
281  oss << ", ";
282  }
283  }
284  oss << " free=" << myFreeSublanes;
285  return oss.str();
286 }
287 
288 
289 // ===========================================================================
290 // MSCriticalFollowerDistanceInfo member method definitions
291 // ===========================================================================
292 
293 
295  MSLeaderDistanceInfo(lane, ego, latOffset),
296  myMissingGaps(myVehicles.size(), -std::numeric_limits<double>::max()) {
297 }
298 
299 
301 
302 
303 int
304 MSCriticalFollowerDistanceInfo::addFollower(const MSVehicle* veh, const MSVehicle* ego, double gap, double latOffset, int sublane) {
305  if (veh == nullptr) {
306  return myFreeSublanes;
307  }
308  const double requiredGap = veh->getCarFollowModel().getSecureGap(veh, ego, veh->getSpeed(), ego->getSpeed(), ego->getCarFollowModel().getMaxDecel());
309  const double missingGap = requiredGap - gap;
310  /*
311  if (ego->getID() == "disabled" || gDebugFlag1) {
312  std::cout << " addFollower veh=" << veh->getID()
313  << " ego=" << ego->getID()
314  << " gap=" << gap
315  << " reqGap=" << requiredGap
316  << " missingGap=" << missingGap
317  << " latOffset=" << latOffset
318  << " sublane=" << sublane
319  << "\n";
320  if (sublane > 0) {
321  std::cout
322  << " dists[s]=" << myDistances[sublane]
323  << " gaps[s]=" << myMissingGaps[sublane]
324  << "\n";
325  } else {
326  std::cout << toString() << "\n";
327  }
328  }
329  */
330  if (myVehicles.size() == 1) {
331  // speedup for the simple case
332  sublane = 0;
333  }
334  if (sublane >= 0 && sublane < (int)myVehicles.size()) {
335  // sublane is already given
336  // overlapping vehicles are stored preferably
337  // among those vehicles with missing gap, closer ones are preferred
338  if ((missingGap > myMissingGaps[sublane]
339  || (missingGap > 0 && gap < myDistances[sublane])
340  || (gap < 0 && myDistances[sublane] > 0))
341  && !(gap > 0 && myDistances[sublane] < 0)
342  && !(myMissingGaps[sublane] > 0 && myDistances[sublane] < gap)
343  ) {
344  if (myVehicles[sublane] == 0) {
345  myFreeSublanes--;
346  }
347  myVehicles[sublane] = veh;
348  myDistances[sublane] = gap;
349  myMissingGaps[sublane] = missingGap;
350  myHasVehicles = true;
351  }
352  return myFreeSublanes;
353  }
354  int rightmost, leftmost;
355  getSubLanes(veh, latOffset, rightmost, leftmost);
356  for (int sublaneIdx = rightmost; sublaneIdx <= leftmost; ++sublaneIdx) {
357  if ((egoRightMost < 0 || (egoRightMost <= sublaneIdx && sublaneIdx <= egoLeftMost))
358  // overlapping vehicles are stored preferably
359  // among those vehicles with missing gap, closer ones are preferred
360  && (missingGap > myMissingGaps[sublaneIdx]
361  || (missingGap > 0 && gap < myDistances[sublaneIdx])
362  || (gap < 0 && myDistances[sublaneIdx] > 0))
363  && !(gap > 0 && myDistances[sublaneIdx] < 0)
364  && !(myMissingGaps[sublaneIdx] > 0 && myDistances[sublaneIdx] < gap)
365  ) {
366  if (myVehicles[sublaneIdx] == 0) {
367  myFreeSublanes--;
368  }
369  myVehicles[sublaneIdx] = veh;
370  myDistances[sublaneIdx] = gap;
371  myMissingGaps[sublaneIdx] = missingGap;
372  myHasVehicles = true;
373  }
374  }
375  return myFreeSublanes;
376 }
377 
378 
379 void
382  myMissingGaps.assign(myVehicles.size(), -std::numeric_limits<double>::max());
383 }
384 
385 
386 std::string
388  std::ostringstream oss;
389  oss.setf(std::ios::fixed, std::ios::floatfield);
390  oss << std::setprecision(2);
391  for (int i = 0; i < (int)myVehicles.size(); ++i) {
392  oss << Named::getIDSecure(myVehicles[i]) << ":";
393  if (myVehicles[i] == 0) {
394  oss << "inf:-inf";
395  } else {
396  oss << myDistances[i] << ":" << myMissingGaps[i];
397  }
398  if (i < (int)myVehicles.size() - 1) {
399  oss << ", ";
400  }
401  }
402  oss << " free=" << myFreeSublanes;
403  return oss.str();
404 }
405 
406 
407 /****************************************************************************/
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:73
T MAX2(T a, T b)
Definition: StdDefs.h:79
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:328
double getMaxDecel() const
Get the vehicle type's maximal comfortable deceleration [m/s^2].
Definition: MSCFModel.h:216
MSCriticalFollowerDistanceInfo(const MSLane *lane, const MSVehicle *ego, double latOffset)
Constructor.
std::vector< double > myMissingGaps
Definition: MSLeaderInfo.h:223
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:82
Representation of a lane in the micro simulation.
Definition: MSLane.h:82
saves leader/follower vehicles and their distances relative to an ego vehicle
Definition: MSLeaderInfo.h:130
virtual std::string toString() const
print a debugging representation
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:173
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:113
int myFreeSublanes
the number of free sublanes
Definition: MSLeaderInfo.h:118
bool hasStoppedVehicle() const
whether a stopped vehicle is leader
int egoRightMost
borders of the ego vehicle for filtering of free sublanes
Definition: MSLeaderInfo.h:121
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
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:111
bool myHasVehicles
Definition: MSLeaderInfo.h:124
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:77
MSAbstractLaneChangeModel & getLaneChangeModel()
Definition: MSVehicle.cpp:4699
double getActionStepLengthSecs() const
Returns the vehicle's action step length in secs, i.e. the interval between two action points.
Definition: MSVehicle.h:502
SUMOTime getActionStepLength() const
Returns the vehicle's action step length in millisecs, i.e. the interval between two action points.
Definition: MSVehicle.h:494
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
const MSCFModel & getCarFollowModel() const
Returns the vehicle's car following model definition.
Definition: MSVehicle.h:930
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:66