SUMO - Simulation of Urban MObility
MSLeaderInfo.cpp
Go to the documentation of this file.
1 /****************************************************************************/
7 // Information about vehicles ahead (may be multiple vehicles if
8 // lateral-resolution is active)
9 /****************************************************************************/
10 // SUMO, Simulation of Urban MObility; see http://sumo.dlr.de/
11 // Copyright (C) 2002-2017 DLR (http://www.dlr.de/) and contributors
12 /****************************************************************************/
13 //
14 // This file is part of SUMO.
15 // SUMO is free software: you can redistribute it and/or modify
16 // it under the terms of the GNU General Public License as published by
17 // the Free Software Foundation, either version 3 of the License, or
18 // (at your option) any later version.
19 //
20 /****************************************************************************/
21 
22 
23 // ===========================================================================
24 // included modules
25 // ===========================================================================
26 #ifdef _MSC_VER
27 #include <windows_config.h>
28 #else
29 #include <config.h>
30 #endif
31 
32 #include <cassert>
33 #include <math.h>
34 #include <utils/common/ToString.h>
35 #include <microsim/MSGlobals.h>
36 #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  for (int sublane = rightmost; sublane <= leftmost; ++sublane) {
86  if ((egoRightMost < 0 || (egoRightMost <= sublane && sublane <= egoLeftMost))
87  && (!beyond || myVehicles[sublane] == 0)) {
88  if (myVehicles[sublane] == 0) {
90  }
91  myVehicles[sublane] = veh;
92  myHasVehicles = true;
93  }
94  }
95  return myFreeSublanes;
96 }
97 
98 
99 void
101  myVehicles.assign(myVehicles.size(), (MSVehicle*)0);
102  myFreeSublanes = (int)myVehicles.size();
103  if (egoRightMost >= 0) {
105  myFreeSublanes -= (int)myVehicles.size() - 1 - egoLeftMost;
106  }
107 }
108 
109 
110 void
111 MSLeaderInfo::getSubLanes(const MSVehicle* veh, double latOffset, int& rightmost, int& leftmost) const {
112  if (myVehicles.size() == 1) {
113  // speedup for the simple case
114  rightmost = 0;
115  leftmost = 0;
116  return;
117  }
118  // map center-line based coordinates into [0, myWidth] coordinates
119  const double vehCenter = veh->getLateralPositionOnLane() + 0.5 * myWidth + latOffset;
120  const double vehHalfWidth = 0.5 * veh->getVehicleType().getWidth();
121  const double rightVehSide = MAX2(0., vehCenter - vehHalfWidth);
122  const double leftVehSide = MIN2(myWidth, vehCenter + vehHalfWidth);
123  rightmost = (int)floor((rightVehSide + NUMERICAL_EPS) / MSGlobals::gLateralResolution);
124  leftmost = MIN2((int)myVehicles.size() - 1, (int)floor(leftVehSide / MSGlobals::gLateralResolution));
125  //if (veh->getID() == "car2") std::cout << SIMTIME << " veh=" << veh->getID()
126  // << std::setprecision(10)
127  // << " posLat=" << veh->getLateralPositionOnLane()
128  // << " rightVehSide=" << rightVehSide
129  // << " leftVehSide=" << leftVehSide
130  // << " rightmost=" << rightmost
131  // << " leftmost=" << leftmost
132  // << std::setprecision(2)
133  // << "\n";
134 }
135 
136 
137 void
138 MSLeaderInfo::getSublaneBorders(int sublane, double latOffset, double& rightSide, double& leftSide) const {
139  assert(sublane >= 0);
140  assert(sublane < (int)myVehicles.size());
141  rightSide = sublane * MSGlobals::gLateralResolution + latOffset;
142  leftSide = MIN2((sublane + 1) * MSGlobals::gLateralResolution, myWidth) + latOffset;
143 }
144 
145 
146 const MSVehicle*
147 MSLeaderInfo::operator[](int sublane) const {
148  assert(sublane >= 0);
149  assert(sublane < (int)myVehicles.size());
150  return myVehicles[sublane];
151 }
152 
153 
154 std::string
156  std::ostringstream oss;
157  oss.setf(std::ios::fixed , std::ios::floatfield);
158  oss << std::setprecision(2);
159  for (int i = 0; i < (int)myVehicles.size(); ++i) {
160  oss << Named::getIDSecure(myVehicles[i]);
161  if (i < (int)myVehicles.size() - 1) {
162  oss << ", ";
163  }
164  }
165  oss << " free=" << myFreeSublanes;
166  return oss.str();
167 }
168 
169 
170 bool
172  if (!myHasVehicles) {
173  return false;
174  }
175  for (int i = 0; i < (int)myVehicles.size(); ++i) {
176  if (myVehicles[0] != 0 && myVehicles[0]->isStopped()) {
177  return true;
178  }
179  }
180  return false;
181 }
182 
183 // ===========================================================================
184 // MSLeaderDistanceInfo member method definitions
185 // ===========================================================================
186 
187 
188 MSLeaderDistanceInfo::MSLeaderDistanceInfo(const MSLane* lane, const MSVehicle* ego, double latOffset) :
189  MSLeaderInfo(lane, ego, latOffset),
190  myDistances(myVehicles.size(), std::numeric_limits<double>::max()) {
191 }
192 
193 
195  MSLeaderInfo(dummy, 0, 0),
196  myDistances(1, cLeaderDist.second) {
197  assert(myVehicles.size() == 1);
198  myVehicles[0] = cLeaderDist.first;
199 }
200 
202 
203 
204 int
205 MSLeaderDistanceInfo::addLeader(const MSVehicle* veh, double gap, double latOffset, int sublane) {
206  //if (SIMTIME == 31 && gDebugFlag1 && veh != 0 && veh->getID() == "cars.8") {
207  // std::cout << " BREAKPOINT\n";
208  //}
209  if (veh == 0) {
210  return myFreeSublanes;
211  }
212  if (myVehicles.size() == 1) {
213  // speedup for the simple case
214  sublane = 0;
215  }
216  if (sublane >= 0 && sublane < (int)myVehicles.size()) {
217  // sublane is already given
218  if (gap < myDistances[sublane]) {
219  if (myVehicles[sublane] == 0) {
220  myFreeSublanes--;
221  }
222  myVehicles[sublane] = veh;
223  myDistances[sublane] = gap;
224  myHasVehicles = true;
225  }
226  return myFreeSublanes;
227  }
228  int rightmost, leftmost;
229  getSubLanes(veh, latOffset, rightmost, leftmost);
230  for (int sublane = rightmost; sublane <= leftmost; ++sublane) {
231  if ((egoRightMost < 0 || (egoRightMost <= sublane && sublane <= egoLeftMost))
232  && gap < myDistances[sublane]) {
233  if (myVehicles[sublane] == 0) {
234  myFreeSublanes--;
235  }
236  myVehicles[sublane] = veh;
237  myDistances[sublane] = gap;
238  myHasVehicles = true;
239  }
240  }
241  return myFreeSublanes;
242 }
243 
244 
245 void
249 }
250 
251 
254  assert(sublane >= 0);
255  assert(sublane < (int)myVehicles.size());
256  return std::make_pair(myVehicles[sublane], myDistances[sublane]);
257 }
258 
259 
260 std::string
262  std::ostringstream oss;
263  oss.setf(std::ios::fixed , std::ios::floatfield);
264  oss << std::setprecision(2);
265  for (int i = 0; i < (int)myVehicles.size(); ++i) {
266  oss << Named::getIDSecure(myVehicles[i]) << ":";
267  if (myVehicles[i] == 0) {
268  oss << "inf";
269  } else {
270  oss << myDistances[i];
271  }
272  if (i < (int)myVehicles.size() - 1) {
273  oss << ", ";
274  }
275  }
276  oss << " free=" << myFreeSublanes;
277  return oss.str();
278 }
279 
280 
281 // ===========================================================================
282 // MSCriticalFollowerDistanceInfo member method definitions
283 // ===========================================================================
284 
285 
287  MSLeaderDistanceInfo(lane, ego, latOffset),
288  myMissingGaps(myVehicles.size(), -std::numeric_limits<double>::max()) {
289 }
290 
291 
293 
294 
295 int
296 MSCriticalFollowerDistanceInfo::addFollower(const MSVehicle* veh, const MSVehicle* ego, double gap, double latOffset, int sublane) {
297  if (veh == 0) {
298  return myFreeSublanes;
299  }
300  const double requiredGap = veh->getCarFollowModel().getSecureGap(veh->getSpeed(), ego->getSpeed(), ego->getCarFollowModel().getMaxDecel());
301  const double missingGap = requiredGap - gap;
302  //if (gDebugFlag1) {
303  // std::cout << " addFollower veh=" << veh->getID()
304  // << " ego=" << ego->getID()
305  // << " gap=" << gap
306  // << " reqGap=" << requiredGap
307  // << " missingGap=" << missingGap
308  // << " latOffset=" << latOffset
309  // << " sublane=" << sublane
310  // << "\n";
311  // if (sublane > 0) {
312  // std::cout
313  // << " dists[s]=" << myDistances[sublane]
314  // << " gaps[s]=" << myMissingGaps[sublane]
315  // << "\n";
316  // }
317  //}
318  if (myVehicles.size() == 1) {
319  // speedup for the simple case
320  sublane = 0;
321  }
322  if (sublane >= 0 && sublane < (int)myVehicles.size()) {
323  // sublane is already given
324  if (missingGap > myMissingGaps[sublane]) {
325  if (myVehicles[sublane] == 0) {
326  myFreeSublanes--;
327  }
328  myVehicles[sublane] = veh;
329  myDistances[sublane] = gap;
330  myMissingGaps[sublane] = missingGap;
331  myHasVehicles = true;
332  }
333  return myFreeSublanes;
334  }
335  int rightmost, leftmost;
336  getSubLanes(veh, latOffset, rightmost, leftmost);
337  for (int sublane = rightmost; sublane <= leftmost; ++sublane) {
338  if ((egoRightMost < 0 || (egoRightMost <= sublane && sublane <= egoLeftMost))
339  && missingGap > myMissingGaps[sublane]) {
340  if (myVehicles[sublane] == 0) {
341  myFreeSublanes--;
342  }
343  myVehicles[sublane] = veh;
344  myDistances[sublane] = gap;
345  myMissingGaps[sublane] = missingGap;
346  myHasVehicles = true;
347  }
348  }
349  return myFreeSublanes;
350 }
351 
352 
353 void
357 }
358 
359 
360 std::string
362  std::ostringstream oss;
363  oss.setf(std::ios::fixed , std::ios::floatfield);
364  oss << std::setprecision(2);
365  for (int i = 0; i < (int)myVehicles.size(); ++i) {
366  oss << Named::getIDSecure(myVehicles[i]) << ":";
367  if (myVehicles[i] == 0) {
368  oss << "inf:-inf";
369  } else {
370  oss << myDistances[i] << ":" << myMissingGaps[i];
371  }
372  if (i < (int)myVehicles.size() - 1) {
373  oss << ", ";
374  }
375  }
376  oss << " free=" << myFreeSublanes;
377  return oss.str();
378 }
379 /****************************************************************************/
380 
static double gLateralResolution
Definition: MSGlobals.h:92
saves leader/follower vehicles and their distances relative to an ego vehicle
Definition: MSLeaderInfo.h:136
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:124
virtual void clear()
discard all information
T MAX2(T a, T b)
Definition: StdDefs.h:70
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:59
int egoRightMost
borders of the ego vehicle for filtering of free sublanes
Definition: MSLeaderInfo.h:127
double myWidth
the width of the lane to which this instance applies
Definition: MSLeaderInfo.h:117
std::vector< double > myDistances
Definition: MSLeaderInfo.h:175
virtual int addLeader(const MSVehicle *veh, double gap, double latOffset=0, int sublane=-1)
std::vector< double > myMissingGaps
Definition: MSLeaderInfo.h:225
const MSCFModel & getCarFollowModel() const
Returns the vehicle&#39;s car following model definition.
Definition: MSVehicle.h:796
virtual ~MSLeaderInfo()
Destructor.
#define max(a, b)
Definition: polyfonts.c:65
virtual void clear()
discard all information
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:64
void clear()
discard all information
virtual ~MSLeaderDistanceInfo()
Destructor.
double getMaxDecel() const
Get the vehicle type&#39;s maximum deceleration [m/s^2].
Definition: MSCFModel.h:201
std::vector< const MSVehicle * > myVehicles
Definition: MSLeaderInfo.h:119
double getLateralPositionOnLane() const
Get the vehicle&#39;s lateral position on the lane.
Definition: MSVehicle.h:407
bool hasStoppedVehicle() const
whether a stopped vehicle is leader
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:130
std::pair< const MSVehicle *, double > CLeaderDist
Definition: MSLeaderInfo.h:42
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:276
double getSpeed() const
Returns the vehicle&#39;s current speed.
Definition: MSVehicle.h:442
MSLeaderInfo(const MSLane *lane, const MSVehicle *ego=0, double latOffset=0)
Constructor.
Representation of a lane in the micro simulation.
Definition: MSLane.h:79
MSCriticalFollowerDistanceInfo(const MSLane *lane, const MSVehicle *ego, double latOffset)
Constructor.