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-2018 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());
149  rightSide = sublane * MSGlobals::gLateralResolution + latOffset;
150  leftSide = MIN2((sublane + 1) * MSGlobals::gLateralResolution, myWidth) + latOffset;
151 }
152 
153 
154 const MSVehicle*
155 MSLeaderInfo::operator[](int sublane) const {
156  assert(sublane >= 0);
157  assert(sublane < (int)myVehicles.size());
158  return myVehicles[sublane];
159 }
160 
161 
162 std::string
164  std::ostringstream oss;
165  oss.setf(std::ios::fixed , std::ios::floatfield);
166  oss << std::setprecision(2);
167  for (int i = 0; i < (int)myVehicles.size(); ++i) {
168  oss << Named::getIDSecure(myVehicles[i]);
169  if (i < (int)myVehicles.size() - 1) {
170  oss << ", ";
171  }
172  }
173  oss << " free=" << myFreeSublanes;
174  return oss.str();
175 }
176 
177 
178 bool
180  if (!myHasVehicles) {
181  return false;
182  }
183  for (int i = 0; i < (int)myVehicles.size(); ++i) {
184  if (myVehicles[0] != 0 && myVehicles[0]->isStopped()) {
185  return true;
186  }
187  }
188  return false;
189 }
190 
191 // ===========================================================================
192 // MSLeaderDistanceInfo member method definitions
193 // ===========================================================================
194 
195 
196 MSLeaderDistanceInfo::MSLeaderDistanceInfo(const MSLane* lane, const MSVehicle* ego, double latOffset) :
197  MSLeaderInfo(lane, ego, latOffset),
198  myDistances(myVehicles.size(), std::numeric_limits<double>::max()) {
199 }
200 
201 
203  MSLeaderInfo(dummy, nullptr, 0),
204  myDistances(1, cLeaderDist.second) {
205  assert(myVehicles.size() == 1);
206  myVehicles[0] = cLeaderDist.first;
207 }
208 
210 
211 
212 int
213 MSLeaderDistanceInfo::addLeader(const MSVehicle* veh, double gap, double latOffset, int sublane) {
214  //if (SIMTIME == 31 && gDebugFlag1 && veh != 0 && veh->getID() == "cars.8") {
215  // std::cout << " BREAKPOINT\n";
216  //}
217  if (veh == nullptr) {
218  return myFreeSublanes;
219  }
220  if (myVehicles.size() == 1) {
221  // speedup for the simple case
222  sublane = 0;
223  }
224  if (sublane >= 0 && sublane < (int)myVehicles.size()) {
225  // sublane is already given
226  if (gap < myDistances[sublane]) {
227  if (myVehicles[sublane] == 0) {
228  myFreeSublanes--;
229  }
230  myVehicles[sublane] = veh;
231  myDistances[sublane] = gap;
232  myHasVehicles = true;
233  }
234  return myFreeSublanes;
235  }
236  int rightmost, leftmost;
237  getSubLanes(veh, latOffset, rightmost, leftmost);
238  for (int sublane = rightmost; sublane <= leftmost; ++sublane) {
239  if ((egoRightMost < 0 || (egoRightMost <= sublane && sublane <= egoLeftMost))
240  && gap < myDistances[sublane]) {
241  if (myVehicles[sublane] == 0) {
242  myFreeSublanes--;
243  }
244  myVehicles[sublane] = veh;
245  myDistances[sublane] = gap;
246  myHasVehicles = true;
247  }
248  }
249  return myFreeSublanes;
250 }
251 
252 
253 void
256  myDistances.assign(myVehicles.size(), std::numeric_limits<double>::max());
257 }
258 
259 
262  assert(sublane >= 0);
263  assert(sublane < (int)myVehicles.size());
264  return std::make_pair(myVehicles[sublane], myDistances[sublane]);
265 }
266 
267 
268 std::string
270  std::ostringstream oss;
271  oss.setf(std::ios::fixed , std::ios::floatfield);
272  oss << std::setprecision(2);
273  for (int i = 0; i < (int)myVehicles.size(); ++i) {
274  oss << Named::getIDSecure(myVehicles[i]) << ":";
275  if (myVehicles[i] == 0) {
276  oss << "inf";
277  } else {
278  oss << myDistances[i];
279  }
280  if (i < (int)myVehicles.size() - 1) {
281  oss << ", ";
282  }
283  }
284  oss << " free=" << myFreeSublanes;
285  return oss.str();
286 }
287 
288 
289 // ===========================================================================
290 // MSCriticalFollowerDistanceInfo member method definitions
291 // ===========================================================================
292 
293 
295  MSLeaderDistanceInfo(lane, ego, latOffset),
296  myMissingGaps(myVehicles.size(), -std::numeric_limits<double>::max()) {
297 }
298 
299 
301 
302 
303 int
304 MSCriticalFollowerDistanceInfo::addFollower(const MSVehicle* veh, const MSVehicle* ego, double gap, double latOffset, int sublane) {
305  if (veh == nullptr) {
306  return myFreeSublanes;
307  }
308  const double requiredGap = veh->getCarFollowModel().getSecureGap(veh->getSpeed(), ego->getSpeed(), ego->getCarFollowModel().getMaxDecel());
309  const double missingGap = requiredGap - gap;
310  /*
311  if (ego->getID() == "disabled" || gDebugFlag1) {
312  std::cout << " addFollower veh=" << veh->getID()
313  << " ego=" << ego->getID()
314  << " gap=" << gap
315  << " reqGap=" << requiredGap
316  << " missingGap=" << missingGap
317  << " latOffset=" << latOffset
318  << " sublane=" << sublane
319  << "\n";
320  if (sublane > 0) {
321  std::cout
322  << " dists[s]=" << myDistances[sublane]
323  << " gaps[s]=" << myMissingGaps[sublane]
324  << "\n";
325  } else {
326  std::cout << toString() << "\n";
327  }
328  }
329  */
330  if (myVehicles.size() == 1) {
331  // speedup for the simple case
332  sublane = 0;
333  }
334  if (sublane >= 0 && sublane < (int)myVehicles.size()) {
335  // sublane is already given
336  // overlapping vehicles are stored preferably
337  // among those vehicles with missing gap, closer ones are preferred
338  if ((missingGap > myMissingGaps[sublane]
339  || (missingGap > 0 && gap < myDistances[sublane])
340  || (gap < 0 && myDistances[sublane] > 0))
341  && !(gap > 0 && myDistances[sublane] < 0)
342  && !(myMissingGaps[sublane] > 0 && myDistances[sublane] < gap)
343  ) {
344  if (myVehicles[sublane] == 0) {
345  myFreeSublanes--;
346  }
347  myVehicles[sublane] = veh;
348  myDistances[sublane] = gap;
349  myMissingGaps[sublane] = missingGap;
350  myHasVehicles = true;
351  }
352  return myFreeSublanes;
353  }
354  int rightmost, leftmost;
355  getSubLanes(veh, latOffset, rightmost, leftmost);
356  for (int sublane = rightmost; sublane <= leftmost; ++sublane) {
357  if ((egoRightMost < 0 || (egoRightMost <= sublane && sublane <= egoLeftMost))
358  // overlapping vehicles are stored preferably
359  // among those vehicles with missing gap, closer ones are preferred
360  && (missingGap > myMissingGaps[sublane]
361  || (missingGap > 0 && gap < myDistances[sublane])
362  || (gap < 0 && myDistances[sublane] > 0))
363  && !(gap > 0 && myDistances[sublane] < 0)
364  && !(myMissingGaps[sublane] > 0 && myDistances[sublane] < gap)
365  ) {
366  if (myVehicles[sublane] == 0) {
367  myFreeSublanes--;
368  }
369  myVehicles[sublane] = veh;
370  myDistances[sublane] = gap;
371  myMissingGaps[sublane] = missingGap;
372  myHasVehicles = true;
373  }
374  }
375  return myFreeSublanes;
376 }
377 
378 
379 void
382  myMissingGaps.assign(myVehicles.size(), -std::numeric_limits<double>::max());
383 }
384 
385 
386 std::string
388  std::ostringstream oss;
389  oss.setf(std::ios::fixed , std::ios::floatfield);
390  oss << std::setprecision(2);
391  for (int i = 0; i < (int)myVehicles.size(); ++i) {
392  oss << Named::getIDSecure(myVehicles[i]) << ":";
393  if (myVehicles[i] == 0) {
394  oss << "inf:-inf";
395  } else {
396  oss << myDistances[i] << ":" << myMissingGaps[i];
397  }
398  if (i < (int)myVehicles.size() - 1) {
399  oss << ", ";
400  }
401  }
402  oss << " free=" << myFreeSublanes;
403  return oss.str();
404 }
405 /****************************************************************************/
406 
static double gLateralResolution
Definition: MSGlobals.h:85
saves leader/follower vehicles and their distances relative to an ego vehicle
Definition: MSLeaderInfo.h:129
MSLeaderDistanceInfo(const MSLane *lane, const MSVehicle *ego, double latOffset)
Constructor.
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:79
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:117
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:76
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:71
int egoRightMost
borders of the ego vehicle for filtering of free sublanes
Definition: MSLeaderInfo.h:120
double myWidth
the width of the lane to which this instance applies
Definition: MSLeaderInfo.h:110
MSAbstractLaneChangeModel & getLaneChangeModel()
Definition: MSVehicle.cpp:4307
std::vector< double > myDistances
Definition: MSLeaderInfo.h:168
virtual int addLeader(const MSVehicle *veh, double gap, double latOffset=0, int sublane=-1)
std::vector< double > myMissingGaps
Definition: MSLeaderInfo.h:218
const MSCFModel & getCarFollowModel() const
Returns the vehicle&#39;s car following model definition.
Definition: MSVehicle.h:891
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:517
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:509
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:70
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:112
double getLateralPositionOnLane() const
Get the vehicle&#39;s lateral position on the lane.
Definition: MSVehicle.h:440
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:123
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:148
double getSpeed() const
Returns the vehicle&#39;s current speed.
Definition: MSVehicle.h:483
MSLeaderInfo(const MSLane *lane, const MSVehicle *ego=0, double latOffset=0)
Constructor.
Representation of a lane in the micro simulation.
Definition: MSLane.h:78
MSCriticalFollowerDistanceInfo(const MSLane *lane, const MSVehicle *ego, double latOffset)
Constructor.
double getSpeedLat() const
return the lateral speed of the current lane change maneuver