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 
static double gLateralResolution
Definition: MSGlobals.h:85
saves leader/follower vehicles and their distances relative to an ego vehicle
Definition: MSLeaderInfo.h:133
MSLeaderDistanceInfo(const MSLane *lane, const MSVehicle *ego, double latOffset)
Constructor.
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:80
virtual int addLeader(const MSVehicle *veh, bool beyond, double latOffset=0)
virtual std::string toString() const
print a debugging representation
virtual ~MSCriticalFollowerDistanceInfo()
Destructor.
int myFreeSublanes
the number of free sublanes
Definition: MSLeaderInfo.h:121
double getManeuverDist() const
Returns the remaining unblocked distance for the current maneuver. (only used by sublane model) ...
virtual void clear()
discard all information
T MAX2(T a, T b)
Definition: StdDefs.h:80
SUMOTime DELTA_T
Definition: SUMOTime.cpp:35
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
int egoRightMost
borders of the ego vehicle for filtering of free sublanes
Definition: MSLeaderInfo.h:124
double myWidth
the width of the lane to which this instance applies
Definition: MSLeaderInfo.h:114
MSAbstractLaneChangeModel & getLaneChangeModel()
Definition: MSVehicle.cpp:4609
std::vector< double > myDistances
Definition: MSLeaderInfo.h:176
virtual int addLeader(const MSVehicle *veh, double gap, double latOffset=0, int sublane=-1)
std::vector< double > myMissingGaps
Definition: MSLeaderInfo.h:226
const MSCFModel & getCarFollowModel() const
Returns the vehicle&#39;s car following model definition.
Definition: MSVehicle.h:894
virtual ~MSLeaderInfo()
Destructor.
virtual void clear()
discard all information
double getActionStepLengthSecs() const
Returns the vehicle&#39;s action step length in secs, i.e. the interval between two action points...
Definition: MSVehicle.h:513
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
SUMOTime getActionStepLength() const
Returns the vehicle&#39;s action step length in millisecs, i.e. the interval between two action points...
Definition: MSVehicle.h:505
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
std::string toString() const
print a debugging representation
T MIN2(T a, T b)
Definition: StdDefs.h:74
void clear()
discard all information
virtual ~MSLeaderDistanceInfo()
Destructor.
double getMaxDecel() const
Get the vehicle type&#39;s maximal comfortable deceleration [m/s^2].
Definition: MSCFModel.h:218
std::vector< const MSVehicle * > myVehicles
Definition: MSLeaderInfo.h:116
double getLateralPositionOnLane() const
Get the vehicle&#39;s lateral position on the lane.
Definition: MSVehicle.h:434
bool hasStoppedVehicle() const
whether a stopped vehicle is leader
double getMaxSpeedLat() const
Get vehicle&#39;s maximum lateral speed [m/s].
double getWidth() const
Get the width which vehicles of this class shall have when being drawn.
virtual std::string toString() const
print a debugging representation
bool myHasVehicles
Definition: MSLeaderInfo.h:127
std::pair< const MSVehicle *, double > CLeaderDist
Definition: MSLeaderInfo.h:35
int addFollower(const MSVehicle *veh, const MSVehicle *ego, double gap, double latOffset=0, int sublane=-1)
const MSVehicleType & getVehicleType() const
Returns the vehicle&#39;s type definition.
void getSublaneBorders(int sublane, double latOffset, double &rightSide, double &leftSide) const
CLeaderDist operator[](int sublane) const
return the vehicle and its distance for the given sublane
#define NUMERICAL_EPS
Definition: config.h:145
double getSpeed() const
Returns the vehicle&#39;s current speed.
Definition: MSVehicle.h:477
MSLeaderInfo(const MSLane *lane, const MSVehicle *ego=0, double latOffset=0)
Constructor.
Representation of a lane in the micro simulation.
Definition: MSLane.h:83
MSCriticalFollowerDistanceInfo(const MSLane *lane, const MSVehicle *ego, double latOffset)
Constructor.
double getSpeedLat() const
return the lateral speed of the current lane change maneuver