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