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