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-2022 German Aerospace Center (DLR) and others.
4// This program and the accompanying materials are made available under the
5// terms of the Eclipse Public License 2.0 which is available at
6// https://www.eclipse.org/legal/epl-2.0/
7// This Source Code may also be made available under the following Secondary
8// Licenses when the conditions for such availability set forth in the Eclipse
9// Public License 2.0 are satisfied: GNU General Public License, version 2
10// or later which is available at
11// https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12// SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13/****************************************************************************/
18// Information about vehicles ahead (may be multiple vehicles if
19// lateral-resolution is active)
20/****************************************************************************/
21#include <config.h>
22
23#include <cassert>
24#include <cmath>
26#include <microsim/MSGlobals.h>
27#include <microsim/MSVehicle.h>
29#include <microsim/MSNet.h>
30#include <microsim/MSLane.h>
31#include "MSLeaderInfo.h"
32
33
34// ===========================================================================
35// MSLeaderInfo member method definitions
36// ===========================================================================
37MSLeaderInfo::MSLeaderInfo(const double laneWidth, const MSVehicle* ego, const double latOffset) :
38 myWidth(laneWidth),
39 myOffset(0),
40 myVehicles(MAX2(1, int(ceil(laneWidth / MSGlobals::gLateralResolution))), (MSVehicle*)nullptr),
41 myFreeSublanes((int)myVehicles.size()),
42 egoRightMost(-1),
43 egoLeftMost(-1),
44 myHasVehicles(false) {
45 if (ego != nullptr) {
46 getSubLanes(ego, latOffset, egoRightMost, egoLeftMost);
47 // filter out sublanes not of interest to ego
49 myFreeSublanes -= (int)myVehicles.size() - 1 - egoLeftMost;
50 }
51}
52
53
55
56
57int
58MSLeaderInfo::addLeader(const MSVehicle* veh, bool beyond, double latOffset) {
59 if (veh == nullptr) {
60 return myFreeSublanes;
61 }
62 if (myVehicles.size() == 1) {
63 // speedup for the simple case
64 if (!beyond || myVehicles[0] == 0) {
65 myVehicles[0] = veh;
67 myHasVehicles = true;
68 }
69 return myFreeSublanes;
70 }
71 // map center-line based coordinates into [0, myWidth] coordinates
72 int rightmost, leftmost;
73 getSubLanes(veh, latOffset, rightmost, leftmost);
74 //if (gDebugFlag1) std::cout << " addLeader veh=" << veh->getID() << " beyond=" << beyond << " latOffset=" << latOffset << " rightmost=" << rightmost << " leftmost=" << leftmost << " myFreeSublanes=" << myFreeSublanes << "\n";
75 for (int sublane = rightmost; sublane <= leftmost; ++sublane) {
76 if ((egoRightMost < 0 || (egoRightMost <= sublane && sublane <= egoLeftMost))
77 && (!beyond || myVehicles[sublane] == 0)) {
78 if (myVehicles[sublane] == 0) {
80 }
81 myVehicles[sublane] = veh;
82 myHasVehicles = true;
83 }
84 }
85 return myFreeSublanes;
86}
87
88
89void
91 myVehicles.assign(myVehicles.size(), (MSVehicle*)nullptr);
92 myFreeSublanes = (int)myVehicles.size();
93 if (egoRightMost >= 0) {
95 myFreeSublanes -= (int)myVehicles.size() - 1 - egoLeftMost;
96 }
97}
98
99
100void
101MSLeaderInfo::getSubLanes(const MSVehicle* veh, double latOffset, int& rightmost, int& leftmost) const {
102 if (myVehicles.size() == 1) {
103 // speedup for the simple case
104 rightmost = 0;
105 leftmost = 0;
106 return;
107 }
108 // map center-line based coordinates into [0, myWidth] coordinates
109 const double vehCenter = veh->getLateralPositionOnLane() + 0.5 * myWidth + latOffset + myOffset * MSGlobals::gLateralResolution;
110 const double vehHalfWidth = 0.5 * veh->getVehicleType().getWidth();
111 double rightVehSide = vehCenter - vehHalfWidth;
112 double leftVehSide = vehCenter + vehHalfWidth;
113 // Reserve some additional space if the vehicle is performing a maneuver continuation.
114 if (veh->getActionStepLength() != DELTA_T) {
115 if (veh->getLaneChangeModel().getManeuverDist() < 0. || veh->getLaneChangeModel().getSpeedLat() < 0.) {
116 const double maneuverDist = MIN2(veh->getVehicleType().getMaxSpeedLat() * veh->getActionStepLengthSecs(), -MIN2(0., veh->getLaneChangeModel().getManeuverDist()));
117 rightVehSide -= maneuverDist;
118 }
119 if (veh->getLaneChangeModel().getManeuverDist() > 0. || veh->getLaneChangeModel().getSpeedLat() > 0.) {
120 const double maneuverDist = MIN2(veh->getVehicleType().getMaxSpeedLat() * veh->getActionStepLengthSecs(), MAX2(0., veh->getLaneChangeModel().getManeuverDist()));
121 leftVehSide += maneuverDist;
122 }
123 }
124 if (rightVehSide > myWidth || leftVehSide < 0) {
125 // vehicle does not touch this lane
126 // set the values so that an iteration
127 // for (i = rightmost; i <= leftmost; i++) stops immediately
128 rightmost = -1000;
129 leftmost = -2000;
130 } else {
131 rightmost = MAX2(0, (int)floor((rightVehSide + NUMERICAL_EPS) / MSGlobals::gLateralResolution));
132 leftmost = MIN2((int)myVehicles.size() - 1, (int)floor(MAX2(0.0, leftVehSide - NUMERICAL_EPS) / MSGlobals::gLateralResolution));
133 }
134 //if (veh->isSelected()) std::cout << SIMTIME << " veh=" << veh->getID()
135 // << std::setprecision(2)
136 // << " posLat=" << veh->getLateralPositionOnLane()
137 // << " latOffset=" << latOffset
138 // << " vehCenter=" << vehCenter
139 // << " rightVehSide=" << rightVehSide
140 // << " leftVehSide=" << leftVehSide
141 // << " rightmost=" << rightmost
142 // << " leftmost=" << leftmost
143 // << " myOffset=" << myOffset
144 // << std::setprecision(2)
145 // << "\n";
146}
147
148
149void
150MSLeaderInfo::getSublaneBorders(int sublane, double latOffset, double& rightSide, double& leftSide) const {
151 assert(sublane >= 0);
152 assert(sublane < (int)myVehicles.size());
154 rightSide = sublane * res + latOffset - myOffset * MSGlobals::gLateralResolution;
155 leftSide = MIN2((sublane + 1) * res, myWidth) + latOffset - myOffset * MSGlobals::gLateralResolution;
156}
157
158
159const MSVehicle*
160MSLeaderInfo::operator[](int sublane) const {
161 assert(sublane >= 0);
162 assert(sublane < (int)myVehicles.size());
163 return myVehicles[sublane];
164}
165
166
167std::string
169 std::ostringstream oss;
170 oss.setf(std::ios::fixed, std::ios::floatfield);
171 oss << std::setprecision(2);
172 for (int i = 0; i < (int)myVehicles.size(); ++i) {
174 if (i < (int)myVehicles.size() - 1) {
175 oss << ", ";
176 }
177 }
178 oss << " free=" << myFreeSublanes;
179 return oss.str();
180}
181
182
183void
186 myOffset = offset;
187}
188
189
190bool
192 if (!myHasVehicles) {
193 return false;
194 }
195 for (int i = 0; i < (int)myVehicles.size(); ++i) {
196 if (myVehicles[i] != 0 && myVehicles[i]->isStopped()) {
197 return true;
198 }
199 }
200 return false;
201}
202
203void
205 for (int i = 0; i < (int)myVehicles.size(); ++i) {
206 const MSVehicle* veh = myVehicles[i];
207 if (veh != 0 &&
209 || &lane->getEdge() != &veh->getLane()->getEdge())) {
210 myVehicles[i] = nullptr;
211 }
212 }
213}
214
215
216// ===========================================================================
217// MSLeaderDistanceInfo member method definitions
218// ===========================================================================
219MSLeaderDistanceInfo::MSLeaderDistanceInfo(const double laneWidth, const MSVehicle* ego, const double latOffset) :
220 MSLeaderInfo(laneWidth, ego, latOffset),
221 myDistances(myVehicles.size(), std::numeric_limits<double>::max()) {
222}
223
224
225MSLeaderDistanceInfo::MSLeaderDistanceInfo(const CLeaderDist& cLeaderDist, const double laneWidth) :
226 MSLeaderInfo(laneWidth, nullptr, 0.),
227 myDistances(1, cLeaderDist.second) {
228 assert(myVehicles.size() == 1);
229 myVehicles[0] = cLeaderDist.first;
230 myHasVehicles = cLeaderDist.first != nullptr;
231}
232
234
235
236int
237MSLeaderDistanceInfo::addLeader(const MSVehicle* veh, double gap, double latOffset, int sublane) {
238 //if (SIMTIME == 31 && gDebugFlag1 && veh != 0 && veh->getID() == "cars.8") {
239 // std::cout << " BREAKPOINT\n";
240 //}
241 if (veh == nullptr) {
242 return myFreeSublanes;
243 }
244 if (myVehicles.size() == 1) {
245 // speedup for the simple case
246 sublane = 0;
247 }
248 if (sublane >= 0 && sublane < (int)myVehicles.size()) {
249 // sublane is already given
250 if (gap < myDistances[sublane]) {
251 if (myVehicles[sublane] == 0) {
253 }
254 myVehicles[sublane] = veh;
255 myDistances[sublane] = gap;
256 myHasVehicles = true;
257 }
258 return myFreeSublanes;
259 }
260 int rightmost, leftmost;
261 getSubLanes(veh, latOffset, rightmost, leftmost);
262 for (int sublaneIdx = rightmost; sublaneIdx <= leftmost; ++sublaneIdx) {
263 if ((egoRightMost < 0 || (egoRightMost <= sublaneIdx && sublaneIdx <= egoLeftMost))
264 && gap < myDistances[sublaneIdx]) {
265 if (myVehicles[sublaneIdx] == 0) {
267 }
268 myVehicles[sublaneIdx] = veh;
269 myDistances[sublaneIdx] = gap;
270 myHasVehicles = true;
271 }
272 }
273 return myFreeSublanes;
274}
275
276
277void
280 myDistances.assign(myVehicles.size(), std::numeric_limits<double>::max());
281}
282
283
286 assert(sublane >= 0);
287 assert(sublane < (int)myVehicles.size());
288 return std::make_pair(myVehicles[sublane], myDistances[sublane]);
289}
290
291
292std::string
294 std::ostringstream oss;
295 oss.setf(std::ios::fixed, std::ios::floatfield);
296 oss << std::setprecision(2);
297 for (int i = 0; i < (int)myVehicles.size(); ++i) {
298 oss << Named::getIDSecure(myVehicles[i]) << ":";
299 if (myVehicles[i] == 0) {
300 oss << "inf";
301 } else {
302 oss << myDistances[i];
303 }
304 if (i < (int)myVehicles.size() - 1) {
305 oss << ", ";
306 }
307 }
308 oss << " free=" << myFreeSublanes;
309 return oss.str();
310}
311
312void
314 for (int i = 0; i < (int)myVehicles.size(); ++i) {
315 if (myVehicles[i] != nullptr) {
316 if (myVehicles[i]->getLaneChangeModel().isOpposite()) {
317 myDistances[i] -= myVehicles[i]->getVehicleType().getLength();
318 } else if (isFollower && myDistances[i] > POSITION_EPS) {
319 // can ignore oncoming followers once they are past
320 myVehicles[i] = nullptr;
321 myDistances[i] = -1;
322 }
323 }
324 }
325}
326
327
328void
330 for (int i = 0; i < (int)myVehicles.size(); ++i) {
331 if (myVehicles[i] != nullptr) {
332 myDistances[i] += amount;
333 }
334 }
335}
336
339 double minGap = -1;
340 const MSVehicle* veh = nullptr;
341 if (hasVehicles()) {
342 minGap = std::numeric_limits<double>::max();
343 for (int i = 0; i < (int)myVehicles.size(); ++i) {
344 if (myVehicles[i] != nullptr && myDistances[i] < minGap) {
345 minGap = myDistances[i];
346 veh = myVehicles[i];
347 }
348 }
349 }
350 return std::make_pair(veh, minGap);
351}
352
353
354void
356 const double pos = ego->getPositionOnLane();
357 for (int i = 0; i < (int)myVehicles.size(); ++i) {
358 if (myVehicles[i] != nullptr && myDistances[i] < 0 && myVehicles[i]->getPositionOnLane() == pos
359 && &myVehicles[i]->getLane()->getEdge() == &ego->getLane()->getEdge()) {
360 other.myVehicles[i] = myVehicles[i];
361 other.myDistances[i] = myDistances[i];
362 myVehicles[i] = nullptr;
363 myDistances[i] = -1;
364 }
365 }
366}
367
368// ===========================================================================
369// MSCriticalFollowerDistanceInfo member method definitions
370// ===========================================================================
371
372
373MSCriticalFollowerDistanceInfo::MSCriticalFollowerDistanceInfo(const double laneWidth, const MSVehicle* ego, const double latOffset, const bool haveOppositeLeaders) :
374 MSLeaderDistanceInfo(laneWidth, ego, latOffset),
375 myMissingGaps(myVehicles.size(), -std::numeric_limits<double>::max()),
376 myHaveOppositeLeaders(haveOppositeLeaders)
377{ }
378
379
381
382
383int
384MSCriticalFollowerDistanceInfo::addFollower(const MSVehicle* veh, const MSVehicle* ego, double gap, double latOffset, int sublane) {
385 if (veh == nullptr) {
386 return myFreeSublanes;
387 }
388 const double requiredGap = (myHaveOppositeLeaders ? 0
389 : veh->getCarFollowModel().getSecureGap(veh, ego, veh->getSpeed(), ego->getSpeed(), ego->getCarFollowModel().getMaxDecel()));
390 const double missingGap = requiredGap - gap;
391 /*
392 if (ego->getID() == "disabled" || gDebugFlag1) {
393 std::cout << " addFollower veh=" << veh->getID()
394 << " ego=" << ego->getID()
395 << " gap=" << gap
396 << " reqGap=" << requiredGap
397 << " missingGap=" << missingGap
398 << " latOffset=" << latOffset
399 << " sublane=" << sublane
400 << "\n";
401 if (sublane > 0) {
402 std::cout
403 << " dists[s]=" << myDistances[sublane]
404 << " gaps[s]=" << myMissingGaps[sublane]
405 << "\n";
406 } else {
407 std::cout << toString() << "\n";
408 }
409 }
410 */
411 if (myVehicles.size() == 1) {
412 // speedup for the simple case
413 sublane = 0;
414 }
415 if (sublane >= 0 && sublane < (int)myVehicles.size()) {
416 // sublane is already given
417 // overlapping vehicles are stored preferably
418 // among those vehicles with missing gap, closer ones are preferred
419 if ((missingGap > myMissingGaps[sublane]
420 || (missingGap > 0 && gap < myDistances[sublane])
421 || (gap < 0 && myDistances[sublane] > 0))
422 && !(gap > 0 && myDistances[sublane] < 0)
423 && !(myMissingGaps[sublane] > 0 && myDistances[sublane] < gap)
424 ) {
425 if (myVehicles[sublane] == 0) {
427 }
428 myVehicles[sublane] = veh;
429 myDistances[sublane] = gap;
430 myMissingGaps[sublane] = missingGap;
431 myHasVehicles = true;
432 }
433 return myFreeSublanes;
434 }
435 int rightmost, leftmost;
436 getSubLanes(veh, latOffset, rightmost, leftmost);
437 for (int sublaneIdx = rightmost; sublaneIdx <= leftmost; ++sublaneIdx) {
438 if ((egoRightMost < 0 || (egoRightMost <= sublaneIdx && sublaneIdx <= egoLeftMost))
439 // overlapping vehicles are stored preferably
440 // among those vehicles with missing gap, closer ones are preferred
441 && (missingGap > myMissingGaps[sublaneIdx]
442 || (missingGap > 0 && gap < myDistances[sublaneIdx])
443 || (gap < 0 && myDistances[sublaneIdx] > 0))
444 && !(gap > 0 && myDistances[sublaneIdx] < 0)
445 && !(myMissingGaps[sublaneIdx] > 0 && myDistances[sublaneIdx] < gap)
446 ) {
447 if (myVehicles[sublaneIdx] == 0) {
449 }
450 myVehicles[sublaneIdx] = veh;
451 myDistances[sublaneIdx] = gap;
452 myMissingGaps[sublaneIdx] = missingGap;
453 myHasVehicles = true;
454 }
455 }
456 return myFreeSublanes;
457}
458
459
460void
463 myMissingGaps.assign(myVehicles.size(), -std::numeric_limits<double>::max());
464}
465
466
467std::string
469 std::ostringstream oss;
470 oss.setf(std::ios::fixed, std::ios::floatfield);
471 oss << std::setprecision(2);
472 for (int i = 0; i < (int)myVehicles.size(); ++i) {
473 oss << Named::getIDSecure(myVehicles[i]) << ":";
474 if (myVehicles[i] == 0) {
475 oss << "inf:-inf";
476 } else {
477 oss << myDistances[i] << ":" << myMissingGaps[i];
478 }
479 if (i < (int)myVehicles.size() - 1) {
480 oss << ", ";
481 }
482 }
483 oss << " free=" << myFreeSublanes;
484 return oss.str();
485}
486
487
488/****************************************************************************/
std::pair< const MSVehicle *, double > CLeaderDist
Definition: MSLeaderInfo.h:38
SUMOTime DELTA_T
Definition: SUMOTime.cpp:37
T MIN2(T a, T b)
Definition: StdDefs.h:71
T MAX2(T a, T b)
Definition: StdDefs.h:77
double getManeuverDist() const
Returns the remaining unblocked distance for the current maneuver. (only used by sublane model)
double getSpeedLat() const
return the lateral speed of the current lane change maneuver
const MSVehicleType & getVehicleType() const
Returns the vehicle's type definition.
virtual double getSecureGap(const MSVehicle *const, const MSVehicle *const, 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:390
double getMaxDecel() const
Get the vehicle type's maximal comfortable deceleration [m/s^2].
Definition: MSCFModel.h:262
std::vector< double > myMissingGaps
Definition: MSLeaderInfo.h:248
virtual ~MSCriticalFollowerDistanceInfo()
Destructor.
MSCriticalFollowerDistanceInfo(const double laneWidth, const MSVehicle *ego, const double latOffset, const bool haveOppositeLeaders=false)
Constructor.
std::string toString() const
print a debugging representation
void clear()
discard all information
int addFollower(const MSVehicle *veh, const MSVehicle *ego, double gap, double latOffset=0, int sublane=-1)
static double gLateralResolution
Definition: MSGlobals.h:97
Representation of a lane in the micro simulation.
Definition: MSLane.h:84
MSEdge & getEdge() const
Returns the lane's edge.
Definition: MSLane.h:713
saves leader/follower vehicles and their distances relative to an ego vehicle
Definition: MSLeaderInfo.h:144
virtual std::string toString() const
print a debugging representation
CLeaderDist getClosest() const
return vehicle with the smalles gap
virtual ~MSLeaderDistanceInfo()
Destructor.
MSLeaderDistanceInfo(const double laneWidth, const MSVehicle *ego, const double latOffset)
Constructor.
CLeaderDist operator[](int sublane) const
return the vehicle and its distance for the given sublane
virtual void clear()
discard all information
std::vector< double > myDistances
Definition: MSLeaderInfo.h:198
void fixOppositeGaps(bool isFollower)
subtract vehicle length from all gaps if the leader vehicle is driving in the opposite direction
void patchGaps(double amount)
add given value to all gaps
virtual int addLeader(const MSVehicle *veh, double gap, double latOffset=0, int sublane=-1)
void moveSamePosTo(const MSVehicle *ego, MSLeaderDistanceInfo &other)
std::vector< const MSVehicle * > myVehicles
Definition: MSLeaderInfo.h:127
int myFreeSublanes
the number of free sublanes
Definition: MSLeaderInfo.h:132
int myOffset
an extra offset for shifting the interpretation of sublane borders (default [0,myWidth])
Definition: MSLeaderInfo.h:125
void setSublaneOffset(int offset)
set number of sublanes by which to shift positions
bool hasStoppedVehicle() const
whether a stopped vehicle is leader
void removeOpposite(const MSLane *lane)
remove vehicles that are driving in the opposite direction (fully or partially) on the given lane
int egoRightMost
borders of the ego vehicle for filtering of free sublanes
Definition: MSLeaderInfo.h:135
void getSublaneBorders(int sublane, double latOffset, double &rightSide, double &leftSide) const
virtual int addLeader(const MSVehicle *veh, bool beyond, double latOffset=0.)
virtual std::string toString() const
print a debugging representation
virtual void clear()
discard all information
MSLeaderInfo(const double laneWidth, const MSVehicle *ego=nullptr, const double latOffset=0.)
Constructor.
virtual ~MSLeaderInfo()
Destructor.
const MSVehicle * operator[](int sublane) const
return the vehicle for the given sublane
bool hasVehicles() const
Definition: MSLeaderInfo.h:94
void getSubLanes(const MSVehicle *veh, double latOffset, int &rightmost, int &leftmost) const
double myWidth
the width of the lane to which this instance applies
Definition: MSLeaderInfo.h:122
bool myHasVehicles
Definition: MSLeaderInfo.h:138
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:77
MSAbstractLaneChangeModel & getLaneChangeModel()
Definition: MSVehicle.cpp:5367
double getActionStepLengthSecs() const
Returns the vehicle's action step length in secs, i.e. the interval between two action points.
Definition: MSVehicle.h:529
SUMOTime getActionStepLength() const
Returns the vehicle's action step length in millisecs, i.e. the interval between two action points.
Definition: MSVehicle.h:521
const MSLane * getLane() const
Returns the lane the vehicle is on.
Definition: MSVehicle.h:577
double getLateralPositionOnLane() const
Get the vehicle's lateral position on the lane.
Definition: MSVehicle.h:416
double getSpeed() const
Returns the vehicle's current speed.
Definition: MSVehicle.h:486
const MSCFModel & getCarFollowModel() const
Returns the vehicle's car following model definition.
Definition: MSVehicle.h:966
double getPositionOnLane() const
Get the vehicle's position along the lane.
Definition: MSVehicle.h:377
double getWidth() const
Get the width which vehicles of this class shall have when being drawn.
double getMaxSpeedLat() const
Get vehicle's maximum lateral speed [m/s].
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
Definition: json.hpp:4471