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-2016 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 #ifdef CHECK_MEMORY_LEAKS
41 #include <foreign/nvwa/debug_new.h>
42 #endif // CHECK_MEMORY_LEAKS
43 
44 
45 // ===========================================================================
46 // static member variables
47 // ===========================================================================
48 
49 
50 // ===========================================================================
51 // MSLeaderInfo member method definitions
52 // ===========================================================================
53 MSLeaderInfo::MSLeaderInfo(const MSLane* lane, const MSVehicle* ego, SUMOReal latOffset) :
54  myWidth(lane->getWidth()),
55  myVehicles(MAX2(1, int(ceil(myWidth / MSGlobals::gLateralResolution))), (MSVehicle*)0),
56  myFreeSublanes((int)myVehicles.size()),
57  egoRightMost(-1),
58  egoLeftMost(-1),
59  myHasVehicles(false) {
60  if (ego != 0) {
61  getSubLanes(ego, latOffset, egoRightMost, egoLeftMost);
62  // filter out sublanes not of interest to ego
64  myFreeSublanes -= (int)myVehicles.size() - 1 - egoLeftMost;
65  }
66 }
67 
68 
70 
71 
72 int
73 MSLeaderInfo::addLeader(const MSVehicle* veh, bool beyond, SUMOReal latOffset) {
74  if (veh == 0) {
75  return myFreeSublanes;
76  }
77  if (myVehicles.size() == 1) {
78  // speedup for the simple case
79  if (!beyond || myVehicles[0] == 0) {
80  myVehicles[0] = veh;
81  myFreeSublanes = 0;
82  myHasVehicles = true;
83  }
84  return myFreeSublanes;
85  }
86  // map center-line based coordinates into [0, myWidth] coordinates
87  int rightmost, leftmost;
88  getSubLanes(veh, latOffset, rightmost, leftmost);
89  for (int sublane = rightmost; sublane <= leftmost; ++sublane) {
90  if ((egoRightMost < 0 || (egoRightMost <= sublane && sublane <= egoLeftMost))
91  && (!beyond || myVehicles[sublane] == 0)) {
92  if (myVehicles[sublane] == 0) {
94  }
95  myVehicles[sublane] = veh;
96  myHasVehicles = true;
97  }
98  }
99  return myFreeSublanes;
100 }
101 
102 
103 void
105  myVehicles.assign(myVehicles.size(), (MSVehicle*)0);
106  myFreeSublanes = (int)myVehicles.size();
107  if (egoRightMost >= 0) {
109  myFreeSublanes -= (int)myVehicles.size() - 1 - egoLeftMost;
110  }
111 }
112 
113 
114 void
115 MSLeaderInfo::getSubLanes(const MSVehicle* veh, SUMOReal latOffset, int& rightmost, int& leftmost) const {
116  if (myVehicles.size() == 1) {
117  // speedup for the simple case
118  rightmost = 0;
119  leftmost = 0;
120  return;
121  }
122  // map center-line based coordinates into [0, myWidth] coordinates
123  const SUMOReal vehCenter = veh->getLateralPositionOnLane() + 0.5 * myWidth + latOffset;
124  const SUMOReal vehHalfWidth = 0.5 * veh->getVehicleType().getWidth();
125  const SUMOReal rightVehSide = MAX2((SUMOReal)0, vehCenter - vehHalfWidth);
126  const SUMOReal leftVehSide = MIN2(myWidth, vehCenter + vehHalfWidth);
127  rightmost = (int)floor((rightVehSide + NUMERICAL_EPS) / MSGlobals::gLateralResolution);
128  leftmost = MIN2((int)myVehicles.size() - 1, (int)floor(leftVehSide / MSGlobals::gLateralResolution));
129  //if (veh->getID() == "car2") std::cout << SIMTIME << " veh=" << veh->getID()
130  // << std::setprecision(10)
131  // << " posLat=" << veh->getLateralPositionOnLane()
132  // << " rightVehSide=" << rightVehSide
133  // << " leftVehSide=" << leftVehSide
134  // << " rightmost=" << rightmost
135  // << " leftmost=" << leftmost
136  // << std::setprecision(2)
137  // << "\n";
138 }
139 
140 
141 const MSVehicle*
142 MSLeaderInfo::operator[](int sublane) const {
143  assert(sublane >= 0);
144  assert(sublane < (int)myVehicles.size());
145  return myVehicles[sublane];
146 }
147 
148 
149 std::string
151  std::ostringstream oss;
152  oss.setf(std::ios::fixed , std::ios::floatfield);
153  oss << std::setprecision(2);
154  for (int i = 0; i < (int)myVehicles.size(); ++i) {
155  oss << Named::getIDSecure(myVehicles[i]);
156  if (i < (int)myVehicles.size() - 1) {
157  oss << ", ";
158  }
159  }
160  oss << " free=" << myFreeSublanes;
161  return oss.str();
162 }
163 
164 
165 bool
167  if (!myHasVehicles) {
168  return false;
169  }
170  for (int i = 0; i < (int)myVehicles.size(); ++i) {
171  if (myVehicles[0] != 0 && myVehicles[0]->isStopped()) {
172  return true;
173  }
174  }
175  return false;
176 }
177 
178 // ===========================================================================
179 // MSLeaderDistanceInfo member method definitions
180 // ===========================================================================
181 
182 
184  MSLeaderInfo(lane, ego, latOffset),
185  myDistances(myVehicles.size(), std::numeric_limits<SUMOReal>::max()) {
186 }
187 
188 
190 
191 
192 int
193 MSLeaderDistanceInfo::addLeader(const MSVehicle* veh, SUMOReal gap, SUMOReal latOffset, int sublane) {
194  //if (SIMTIME == 31 && gDebugFlag1 && veh != 0 && veh->getID() == "cars.8") {
195  // std::cout << " BREAKPOINT\n";
196  //}
197  if (veh == 0) {
198  return myFreeSublanes;
199  }
200  if (myVehicles.size() == 1) {
201  // speedup for the simple case
202  sublane = 0;
203  }
204  if (sublane >= 0) {
205  // sublane is already given
206  if (gap < myDistances[sublane]) {
207  if (myVehicles[sublane] == 0) {
208  myFreeSublanes--;
209  }
210  myVehicles[sublane] = veh;
211  myDistances[sublane] = gap;
212  myHasVehicles = true;
213  }
214  return myFreeSublanes;
215  }
216  int rightmost, leftmost;
217  getSubLanes(veh, latOffset, rightmost, leftmost);
218  for (int sublane = rightmost; sublane <= leftmost; ++sublane) {
219  if ((egoRightMost < 0 || (egoRightMost <= sublane && sublane <= egoLeftMost))
220  && gap < myDistances[sublane]) {
221  if (myVehicles[sublane] == 0) {
222  myFreeSublanes--;
223  }
224  myVehicles[sublane] = veh;
225  myDistances[sublane] = gap;
226  myHasVehicles = true;
227  }
228  }
229  return myFreeSublanes;
230 }
231 
232 
233 void
237 }
238 
239 
242  assert(sublane >= 0);
243  assert(sublane < (int)myVehicles.size());
244  return std::make_pair(myVehicles[sublane], myDistances[sublane]);
245 }
246 
247 
248 std::string
250  std::ostringstream oss;
251  oss.setf(std::ios::fixed , std::ios::floatfield);
252  oss << std::setprecision(2);
253  for (int i = 0; i < (int)myVehicles.size(); ++i) {
254  oss << Named::getIDSecure(myVehicles[i]) << ":";
255  if (myVehicles[i] == 0) {
256  oss << "inf";
257  } else {
258  oss << myDistances[i];
259  }
260  if (i < (int)myVehicles.size() - 1) {
261  oss << ", ";
262  }
263  }
264  oss << " free=" << myFreeSublanes;
265  return oss.str();
266 }
267 
268 
269 // ===========================================================================
270 // MSCriticalFollowerDistanceInfo member method definitions
271 // ===========================================================================
272 
273 
275  MSLeaderDistanceInfo(lane, ego, latOffset),
276  myMissingGaps(myVehicles.size(), -std::numeric_limits<SUMOReal>::max()) {
277 }
278 
279 
281 
282 
283 int
284 MSCriticalFollowerDistanceInfo::addFollower(const MSVehicle* veh, const MSVehicle* ego, SUMOReal gap, SUMOReal latOffset, int sublane) {
285  if (veh == 0) {
286  return myFreeSublanes;
287  }
288  const SUMOReal requiredGap = veh->getCarFollowModel().getSecureGap(veh->getSpeed(), ego->getSpeed(), ego->getCarFollowModel().getMaxDecel());
289  const SUMOReal missingGap = requiredGap - gap;
290  //if (gDebugFlag1) {
291  // std::cout << " addFollower veh=" << veh->getID()
292  // << " ego=" << ego->getID()
293  // << " gap=" << gap
294  // << " reqGap=" << requiredGap
295  // << " missingGap=" << missingGap
296  // << " latOffset=" << latOffset
297  // << " sublane=" << sublane
298  // << "\n";
299  // if (sublane > 0) {
300  // std::cout
301  // << " dists[s]=" << myDistances[sublane]
302  // << " gaps[s]=" << myMissingGaps[sublane]
303  // << "\n";
304  // }
305  //}
306  if (myVehicles.size() == 1) {
307  // speedup for the simple case
308  sublane = 0;
309  }
310  if (sublane >= 0) {
311  // sublane is already given
312  if (missingGap > myMissingGaps[sublane]) {
313  if (myVehicles[sublane] == 0) {
314  myFreeSublanes--;
315  }
316  myVehicles[sublane] = veh;
317  myDistances[sublane] = gap;
318  myMissingGaps[sublane] = missingGap;
319  myHasVehicles = true;
320  }
321  return myFreeSublanes;
322  }
323  int rightmost, leftmost;
324  getSubLanes(veh, latOffset, rightmost, leftmost);
325  for (int sublane = rightmost; sublane <= leftmost; ++sublane) {
326  if ((egoRightMost < 0 || (egoRightMost <= sublane && sublane <= egoLeftMost))
327  && missingGap > myMissingGaps[sublane]) {
328  if (myVehicles[sublane] == 0) {
329  myFreeSublanes--;
330  }
331  myVehicles[sublane] = veh;
332  myDistances[sublane] = gap;
333  myMissingGaps[sublane] = missingGap;
334  myHasVehicles = true;
335  }
336  }
337  return myFreeSublanes;
338 }
339 
340 
341 void
345 }
346 
347 
348 std::string
350  std::ostringstream oss;
351  oss.setf(std::ios::fixed , std::ios::floatfield);
352  oss << std::setprecision(2);
353  for (int i = 0; i < (int)myVehicles.size(); ++i) {
354  oss << Named::getIDSecure(myVehicles[i]) << ":";
355  if (myVehicles[i] == 0) {
356  oss << "inf:-inf";
357  } else {
358  oss << myDistances[i] << ":" << myMissingGaps[i];
359  }
360  if (i < (int)myVehicles.size() - 1) {
361  oss << ", ";
362  }
363  }
364  oss << " free=" << myFreeSublanes;
365  return oss.str();
366 }
367 /****************************************************************************/
368 
std::vector< SUMOReal > myDistances
Definition: MSLeaderInfo.h:163
saves leader/follower vehicles and their distances relative to an ego vehicle
Definition: MSLeaderInfo.h:127
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:81
virtual std::string toString() const
print a debugging representation
const MSCFModel & getCarFollowModel() const
Returns the vehicle&#39;s car following model definition.
Definition: MSVehicle.h:700
virtual ~MSCriticalFollowerDistanceInfo()
Destructor.
virtual std::string toString() const
print a debugging representation
int myFreeSublanes
the number of free sublanes
Definition: MSLeaderInfo.h:115
virtual void clear()
discard all information
MSLeaderInfo(const MSLane *lane, const MSVehicle *ego=0, SUMOReal latOffset=0)
Constructor.
std::pair< const MSVehicle *, SUMOReal > CLeaderDist
Definition: MSLeaderInfo.h:42
T MAX2(T a, T b)
Definition: StdDefs.h:75
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
SUMOReal getSecureGap(const SUMOReal speed, const SUMOReal leaderSpeed, const SUMOReal leaderMaxDecel) const
Returns the minimum gap to reserve if the leader is braking at maximum.
Definition: MSCFModel.h:272
int egoRightMost
borders of the ego vehicle for filtering of free sublanes
Definition: MSLeaderInfo.h:118
bool hasStoppedVehicle() const
whether a stopped vehicle is leader
SUMOReal getLateralPositionOnLane() const
Get the vehicle&#39;s lateral position on the lane.
Definition: MSVehicle.h:375
CLeaderDist operator[](int sublane) const
return the vehicle and its distance for the given sublane
std::vector< SUMOReal > myMissingGaps
Definition: MSLeaderInfo.h:213
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
virtual int addLeader(const MSVehicle *veh, bool beyond, SUMOReal latOffset=0)
T MIN2(T a, T b)
Definition: StdDefs.h:69
void clear()
discard all information
virtual int addLeader(const MSVehicle *veh, SUMOReal gap, SUMOReal latOffset=0, int sublane=-1)
virtual ~MSLeaderDistanceInfo()
Destructor.
SUMOReal getMaxDecel() const
Get the vehicle type&#39;s maximum deceleration [m/s^2].
Definition: MSCFModel.h:186
void getSubLanes(const MSVehicle *veh, SUMOReal latOffset, int &rightmost, int &leftmost) const
std::vector< const MSVehicle * > myVehicles
Definition: MSLeaderInfo.h:110
int addFollower(const MSVehicle *veh, const MSVehicle *ego, SUMOReal gap, SUMOReal latOffset=0, int sublane=-1)
MSCriticalFollowerDistanceInfo(const MSLane *lane, const MSVehicle *ego, SUMOReal latOffset)
Constructor.
SUMOReal getWidth() const
Get the width which vehicles of this class shall have when being drawn.
bool myHasVehicles
Definition: MSLeaderInfo.h:121
SUMOReal myWidth
the width of the lane to which this instance applies
Definition: MSLeaderInfo.h:108
const MSVehicleType & getVehicleType() const
Returns the vehicle&#39;s type definition.
Definition: MSBaseVehicle.h:97
static SUMOReal gLateralResolution
Definition: MSGlobals.h:84
SUMOReal getSpeed() const
Returns the vehicle&#39;s current speed.
Definition: MSVehicle.h:410
#define SUMOReal
Definition: config.h:213
#define NUMERICAL_EPS
Definition: config.h:160
std::string toString() const
print a debugging representation
Representation of a lane in the micro simulation.
Definition: MSLane.h:79
MSLeaderDistanceInfo(const MSLane *lane, const MSVehicle *ego, SUMOReal latOffset)
Constructor.