Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2001-2026 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 : /****************************************************************************/
14 : /// @file MSAbstractLaneChangeModel.cpp
15 : /// @author Daniel Krajzewicz
16 : /// @author Friedemann Wesner
17 : /// @author Sascha Krieg
18 : /// @author Michael Behrisch
19 : /// @author Jakob Erdmann
20 : /// @author Leonhard Luecken
21 : /// @date Fri, 29.04.2005
22 : ///
23 : // Interface for lane-change models
24 : /****************************************************************************/
25 :
26 : // ===========================================================================
27 : // DEBUG
28 : // ===========================================================================
29 : //#define DEBUG_TARGET_LANE
30 : //#define DEBUG_SHADOWLANE
31 : //#define DEBUG_OPPOSITE
32 : //#define DEBUG_MANEUVER
33 : #define DEBUG_COND (myVehicle.isSelected())
34 :
35 : #include <config.h>
36 :
37 : #include <utils/options/OptionsCont.h>
38 : #include <utils/xml/SUMOSAXAttributes.h>
39 : #include <utils/geom/GeomHelper.h>
40 : #include <microsim/MSNet.h>
41 : #include <microsim/MSEdge.h>
42 : #include <microsim/MSLane.h>
43 : #include <microsim/MSLink.h>
44 : #include <microsim/MSStop.h>
45 : #include <microsim/MSDriverState.h>
46 : #include <microsim/MSGlobals.h>
47 : #include <microsim/devices/MSDevice_Bluelight.h>
48 : #include "MSLCM_DK2008.h"
49 : #include "MSLCM_LC2013.h"
50 : #include "MSLCM_LC2013_CC.h"
51 : #include "MSLCM_SL2015.h"
52 : #include "MSAbstractLaneChangeModel.h"
53 :
54 : /* -------------------------------------------------------------------------
55 : * static members
56 : * ----------------------------------------------------------------------- */
57 : bool MSAbstractLaneChangeModel::myAllowOvertakingRight(false);
58 : bool MSAbstractLaneChangeModel::myLCOutput(false);
59 : bool MSAbstractLaneChangeModel::myLCStartedOutput(false);
60 : bool MSAbstractLaneChangeModel::myLCEndedOutput(false);
61 : bool MSAbstractLaneChangeModel::myLCXYOutput(false);
62 : const double MSAbstractLaneChangeModel::NO_NEIGHBOR(std::numeric_limits<double>::max());
63 : const double MSAbstractLaneChangeModel::UNDEFINED_LOOKAHEAD(-1);
64 :
65 : #define LC_ASSUMED_DECEL 1.0 // the minimal constant deceleration assumed to estimate the duration of a continuous lane-change at its initiation.
66 :
67 : /* -------------------------------------------------------------------------
68 : * MSAbstractLaneChangeModel-methods
69 : * ----------------------------------------------------------------------- */
70 :
71 : void
72 41523 : MSAbstractLaneChangeModel::initGlobalOptions(const OptionsCont& oc) {
73 41523 : myAllowOvertakingRight = oc.getBool("lanechange.overtake-right");
74 41523 : myLCOutput = oc.isSet("lanechange-output");
75 41523 : myLCStartedOutput = oc.getBool("lanechange-output.started");
76 41523 : myLCEndedOutput = oc.getBool("lanechange-output.ended");
77 41523 : myLCXYOutput = oc.getBool("lanechange-output.xy");
78 41523 : }
79 :
80 :
81 : MSAbstractLaneChangeModel*
82 4791757 : MSAbstractLaneChangeModel::build(LaneChangeModel lcm, MSVehicle& v) {
83 4791757 : if (MSGlobals::gLateralResolution > 0 && lcm != LaneChangeModel::SL2015 && lcm != LaneChangeModel::DEFAULT) {
84 44 : throw ProcessError(TLF("Lane change model '%' is not compatible with sublane simulation", toString(lcm)));
85 : }
86 4791735 : switch (lcm) {
87 8 : case LaneChangeModel::DK2008:
88 8 : return new MSLCM_DK2008(v);
89 1290 : case LaneChangeModel::LC2013:
90 1290 : return new MSLCM_LC2013(v);
91 0 : case LaneChangeModel::LC2013_CC:
92 0 : return new MSLCM_LC2013_CC(v);
93 574 : case LaneChangeModel::SL2015:
94 574 : return new MSLCM_SL2015(v);
95 4789863 : case LaneChangeModel::DEFAULT:
96 4789863 : if (MSGlobals::gLateralResolution <= 0) {
97 3970187 : return new MSLCM_LC2013(v);
98 : } else {
99 819676 : return new MSLCM_SL2015(v);
100 : }
101 0 : default:
102 0 : throw ProcessError(TLF("Lane change model '%' not implemented", toString(lcm)));
103 : }
104 : }
105 :
106 :
107 4791735 : MSAbstractLaneChangeModel::MSAbstractLaneChangeModel(MSVehicle& v, const LaneChangeModel model) :
108 4791735 : myVehicle(v),
109 4791735 : myOwnState(0),
110 4791735 : myPreviousState(0),
111 4791735 : myPreviousState2(0),
112 4791735 : myCanceledStateRight(LCA_NONE),
113 4791735 : myCanceledStateCenter(LCA_NONE),
114 4791735 : myCanceledStateLeft(LCA_NONE),
115 4791735 : mySpeedLat(0),
116 4791735 : myAccelerationLat(0),
117 4791735 : myAngleOffset(0),
118 4791735 : myPreviousAngleOffset(0),
119 4791735 : myCommittedSpeed(0),
120 4791735 : myLaneChangeCompletion(1.0),
121 4791735 : myLaneChangeDirection(0),
122 4791735 : myAlreadyChanged(false),
123 4791735 : myShadowLane(nullptr),
124 4791735 : myTargetLane(nullptr),
125 4791735 : myModel(model),
126 4791735 : myLastLateralGapLeft(0.),
127 4791735 : myLastLateralGapRight(0.),
128 4791735 : myLastLeaderGap(0.),
129 4791735 : myLastFollowerGap(0.),
130 4791735 : myLastLeaderSecureGap(0.),
131 4791735 : myLastFollowerSecureGap(0.),
132 4791735 : myLastOrigLeaderGap(0.),
133 4791735 : myLastOrigLeaderSecureGap(0.),
134 4791735 : myLastLeaderSpeed(0),
135 4791735 : myLastFollowerSpeed(0),
136 4791735 : myLastOrigLeaderSpeed(0),
137 4791735 : myDontResetLCGaps(false),
138 4791735 : myStrategicLookahead(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_STRATEGIC_LOOKAHEAD, UNDEFINED_LOOKAHEAD)),
139 4791735 : myMaxSpeedLatStanding(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_MAXSPEEDLATSTANDING, v.getVehicleType().getMaxSpeedLat())),
140 4791735 : myMaxSpeedLatFactor(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_MAXSPEEDLATFACTOR, 1)),
141 9583470 : myMaxDistLatStanding(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_MAXDISTLATSTANDING,
142 : // prevent lateral sliding for cars but permit for two-wheelers due to better maneuverability
143 4791735 : (v.getVClass() & (SVC_BICYCLE | SVC_MOTORCYCLE | SVC_MOPED)) != 0 ? std::numeric_limits<double>::max() : 1.6)),
144 4791735 : mySigma(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SIGMA, 0.0)),
145 4791735 : myOvertakeRightParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_OVERTAKE_RIGHT, 0)),
146 4791735 : myAssertive(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_ASSERTIVE, 1)),
147 4791735 : myCooperativeHelpTime(TIME2STEPS(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_COOPERATIVE_HELPTIME, 60))),
148 4791735 : myHaveBlueLight(v.getDevice(typeid(MSDevice_Bluelight)) != nullptr), // see MSVehicle::initDevices
149 4791735 : myLastLaneChangeOffset(0),
150 4791735 : myAmOpposite(false),
151 4791735 : myManeuverDist(0.),
152 9583470 : myPreviousManeuverDist(0.) {
153 : saveLCState(-1, LCA_UNKNOWN, LCA_UNKNOWN);
154 : saveLCState(0, LCA_UNKNOWN, LCA_UNKNOWN);
155 : saveLCState(1, LCA_UNKNOWN, LCA_UNKNOWN);
156 4791735 : }
157 :
158 :
159 4791660 : MSAbstractLaneChangeModel::~MSAbstractLaneChangeModel() {
160 4791660 : }
161 :
162 : void
163 292097625 : MSAbstractLaneChangeModel::setOwnState(const int state) {
164 292097625 : myPreviousState2 = myPreviousState;
165 292097625 : myOwnState = state;
166 292097625 : myPreviousState = state; // myOwnState is modified in prepareStep so we make a backup
167 292097625 : }
168 :
169 : void
170 0 : MSAbstractLaneChangeModel::updateSafeLatDist(const double travelledLatDist) {
171 : UNUSED_PARAMETER(travelledLatDist);
172 0 : }
173 :
174 :
175 : void
176 80770594 : MSAbstractLaneChangeModel::setManeuverDist(const double dist) {
177 : #ifdef DEBUG_MANEUVER
178 : if (DEBUG_COND) {
179 : std::cout << SIMTIME
180 : << " veh=" << myVehicle.getID()
181 : << " setManeuverDist() old=" << myManeuverDist << " new=" << dist
182 : << std::endl;
183 : }
184 : #endif
185 80770594 : myManeuverDist = fabs(dist) < NUMERICAL_EPS ? 0. : dist;
186 : // store value which may be modified by the model during the next step
187 80770594 : myPreviousManeuverDist = myManeuverDist;
188 80770594 : }
189 :
190 :
191 : double
192 699218041 : MSAbstractLaneChangeModel::getManeuverDist() const {
193 699218041 : return myManeuverDist;
194 : }
195 :
196 : double
197 14279628 : MSAbstractLaneChangeModel::getPreviousManeuverDist() const {
198 14279628 : return myPreviousManeuverDist;
199 : }
200 :
201 : void
202 35212361 : MSAbstractLaneChangeModel::saveNeighbors(const int dir, const MSLeaderDistanceInfo& followers, const MSLeaderDistanceInfo& leaders) {
203 35212361 : if (dir == -1) {
204 15943677 : myLeftFollowers = std::make_shared<MSLeaderDistanceInfo>(followers);
205 15943677 : myLeftLeaders = std::make_shared<MSLeaderDistanceInfo>(leaders);
206 19268684 : } else if (dir == 1) {
207 19268684 : myRightFollowers = std::make_shared<MSLeaderDistanceInfo>(followers);
208 19268684 : myRightLeaders = std::make_shared<MSLeaderDistanceInfo>(leaders);
209 : } else {
210 : // dir \in {-1,1} !
211 : assert(false);
212 : }
213 35212361 : }
214 :
215 :
216 : void
217 241559810 : MSAbstractLaneChangeModel::saveNeighbors(const int dir, const std::pair<MSVehicle* const, double>& follower, const std::pair<MSVehicle* const, double>& leader) {
218 241559810 : if (dir == -1) {
219 115586431 : myLeftFollowers = std::make_shared<MSLeaderDistanceInfo>(follower, myVehicle.getLane()->getWidth());
220 231172862 : myLeftLeaders = std::make_shared<MSLeaderDistanceInfo>(leader, myVehicle.getLane()->getWidth());
221 125973379 : } else if (dir == 1) {
222 125973379 : myRightFollowers = std::make_shared<MSLeaderDistanceInfo>(follower, myVehicle.getLane()->getWidth());
223 251946758 : myRightLeaders = std::make_shared<MSLeaderDistanceInfo>(leader, myVehicle.getLane()->getWidth());
224 : } else {
225 : // dir \in {-1,1} !
226 : assert(false);
227 : }
228 241559810 : }
229 :
230 :
231 : void
232 371512146 : MSAbstractLaneChangeModel::clearNeighbors() {
233 : myLeftFollowers = nullptr;
234 : myLeftLeaders = nullptr;
235 : myRightFollowers = nullptr;
236 : myRightLeaders = nullptr;
237 371512146 : }
238 :
239 :
240 : const std::shared_ptr<MSLeaderDistanceInfo>
241 0 : MSAbstractLaneChangeModel::getFollowers(const int dir) {
242 0 : if (dir == -1) {
243 : return myLeftFollowers;
244 0 : } else if (dir == 1) {
245 : return myRightFollowers;
246 : } else {
247 : // dir \in {-1,1} !
248 : assert(false);
249 : }
250 : return nullptr;
251 : }
252 :
253 : const std::shared_ptr<MSLeaderDistanceInfo>
254 0 : MSAbstractLaneChangeModel::getLeaders(const int dir) {
255 0 : if (dir == -1) {
256 : return myLeftLeaders;
257 0 : } else if (dir == 1) {
258 : return myRightLeaders;
259 : } else {
260 : // dir \in {-1,1} !
261 : assert(false);
262 : }
263 : return nullptr;
264 : }
265 :
266 :
267 : bool
268 109 : MSAbstractLaneChangeModel::congested(const MSVehicle* const neighLeader) {
269 109 : if (neighLeader == nullptr) {
270 : return false;
271 : }
272 : // Congested situation are relevant only on highways (maxSpeed > 70km/h)
273 : // and congested on German Highways means that the vehicles have speeds
274 : // below 60km/h. Overtaking on the right is allowed then.
275 0 : if ((myVehicle.getLane()->getSpeedLimit() <= 70.0 / 3.6) || (neighLeader->getLane()->getSpeedLimit() <= 70.0 / 3.6)) {
276 :
277 0 : return false;
278 : }
279 0 : if (myVehicle.congested() && neighLeader->congested()) {
280 : return true;
281 : }
282 : return false;
283 : }
284 :
285 :
286 : bool
287 294927684 : MSAbstractLaneChangeModel::avoidOvertakeRight() const {
288 294927684 : return (!myAllowOvertakingRight
289 294922233 : && !myVehicle.congested()
290 37875586 : && myVehicle.getVehicleType().getVehicleClass() != SVC_EMERGENCY
291 332801229 : && (myOvertakeRightParam == 0 || myOvertakeRightParam < RandHelper::rand(myVehicle.getRNG())));
292 : }
293 :
294 : bool
295 109 : MSAbstractLaneChangeModel::predInteraction(const std::pair<MSVehicle*, double>& leader) {
296 109 : if (leader.first == 0) {
297 : return false;
298 : }
299 : // let's check it on highways only
300 0 : if (leader.first->getSpeed() < (80.0 / 3.6)) {
301 : return false;
302 : }
303 0 : return leader.second < getCarFollowModel().interactionGap(&myVehicle, leader.first->getSpeed());
304 : }
305 :
306 :
307 : bool
308 1126547 : MSAbstractLaneChangeModel::startLaneChangeManeuver(MSLane* source, MSLane* target, int direction) {
309 1126547 : if (MSGlobals::gLaneChangeDuration > DELTA_T) {
310 37120 : myLaneChangeCompletion = 0;
311 37120 : myLaneChangeDirection = direction;
312 37120 : setManeuverDist((target->getWidth() + source->getWidth()) * 0.5 * direction);
313 37120 : myVehicle.switchOffSignal(MSVehicle::VEH_SIGNAL_BLINKER_RIGHT | MSVehicle::VEH_SIGNAL_BLINKER_LEFT);
314 37120 : myVehicle.switchOnSignal(((direction == 1) != MSGlobals::gLefthand) ? MSVehicle::VEH_SIGNAL_BLINKER_LEFT : MSVehicle::VEH_SIGNAL_BLINKER_RIGHT);
315 37120 : if (myLCOutput) {
316 670 : memorizeGapsAtLCInit();
317 : }
318 37120 : return true;
319 : } else {
320 1089427 : primaryLaneChanged(source, target, direction);
321 1089427 : return false;
322 : }
323 : }
324 :
325 : void
326 670 : MSAbstractLaneChangeModel::memorizeGapsAtLCInit() {
327 670 : myDontResetLCGaps = true;
328 670 : }
329 :
330 : void
331 669 : MSAbstractLaneChangeModel::clearGapsAtLCInit() {
332 669 : myDontResetLCGaps = false;
333 669 : }
334 :
335 : void
336 1126259 : MSAbstractLaneChangeModel::primaryLaneChanged(MSLane* source, MSLane* target, int direction) {
337 1126259 : initLastLaneChangeOffset(direction);
338 1126259 : myVehicle.leaveLane(MSMoveReminder::NOTIFICATION_LANE_CHANGE, target);
339 1126259 : source->leftByLaneChange(&myVehicle);
340 2252518 : laneChangeOutput("change", source, target, direction); // record position on the source edge in case of opposite change
341 1126259 : if (&source->getEdge() != &target->getEdge()) {
342 43960 : changedToOpposite();
343 : #ifdef DEBUG_OPPOSITE
344 : if (debugVehicle()) {
345 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " primaryLaneChanged source=" << source->getID() << " target=" << target->getID() << " nowOpposite=" << myAmOpposite << "\n";
346 : }
347 : #endif
348 43960 : myVehicle.setTentativeLaneAndPosition(target, source->getOppositePos(myVehicle.getPositionOnLane()), -myVehicle.getLateralPositionOnLane());
349 43960 : target->forceVehicleInsertion(&myVehicle, myVehicle.getPositionOnLane(), MSMoveReminder::NOTIFICATION_LANE_CHANGE, myVehicle.getLateralPositionOnLane());
350 1082299 : } else if (myAmOpposite) {
351 : #ifdef DEBUG_OPPOSITE
352 : if (debugVehicle()) {
353 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " primaryLaneChanged source=" << source->getID() << " target=" << target->getID() << " stayOpposite\n";
354 : }
355 : #endif
356 230 : myAlreadyChanged = true;
357 230 : myVehicle.setTentativeLaneAndPosition(target, myVehicle.getPositionOnLane(), myVehicle.getLateralPositionOnLane());
358 230 : if (!MSGlobals::gSublane) {
359 : // in the continous case, the vehicle is added to the target lane via MSLaneChanger::continueChange / registerHop
360 : // in the sublane case, the vehicle is added to the target lane via MSLaneChangerSublane::checkChangeOppositeSublane / MSLane::myTmpVehicles
361 80 : target->forceVehicleInsertion(&myVehicle, myVehicle.getPositionOnLane(), MSMoveReminder::NOTIFICATION_LANE_CHANGE, myVehicle.getLateralPositionOnLane());
362 : }
363 : } else {
364 1082069 : myVehicle.enterLaneAtLaneChange(target);
365 1082069 : target->enteredByLaneChange(&myVehicle);
366 : }
367 : // Assure that the drive items are up to date (even if the following step is no actionstep for the vehicle).
368 : // This is necessary because the lane advance uses the target lane from the corresponding drive item.
369 1126259 : myVehicle.updateDriveItems();
370 1126259 : changed();
371 1126259 : }
372 :
373 : void
374 1126518 : MSAbstractLaneChangeModel::laneChangeOutput(const std::string& tag, MSLane* source, MSLane* target, int direction, double maneuverDist) {
375 1126518 : if (myLCOutput) {
376 16637 : OutputDevice& of = OutputDevice::getDeviceByOption("lanechange-output");
377 16637 : of.openTag(tag);
378 16637 : of.writeAttr(SUMO_ATTR_ID, myVehicle.getID());
379 16637 : of.writeAttr(SUMO_ATTR_TYPE, myVehicle.getVehicleType().getID());
380 16637 : of.writeAttr(SUMO_ATTR_TIME, time2string(MSNet::getInstance()->getCurrentTimeStep()));
381 16637 : of.writeAttr(SUMO_ATTR_FROM, source->getID());
382 16637 : of.writeAttr(SUMO_ATTR_TO, target->getID());
383 16637 : of.writeAttr(SUMO_ATTR_DIR, direction);
384 16637 : of.writeAttr(SUMO_ATTR_SPEED, myVehicle.getSpeed());
385 16637 : of.writeAttr(SUMO_ATTR_POSITION, myVehicle.getPositionOnLane());
386 49911 : of.writeAttr("reason", toString((LaneChangeAction)(myOwnState & ~(
387 : LCA_RIGHT | LCA_LEFT
388 : | LCA_AMBLOCKINGLEADER | LCA_AMBLOCKINGFOLLOWER
389 : | LCA_MRIGHT | LCA_MLEFT
390 33274 : | LCA_AMBACKBLOCKER | LCA_AMBACKBLOCKER_STANDING))) + myVehicle.getParameter().getParameter("lcReason"));
391 16637 : of.writeAttr("leaderGap", myLastLeaderGap == NO_NEIGHBOR ? "None" : toString(myLastLeaderGap), myLastLeaderGap == NO_NEIGHBOR);
392 16637 : of.writeAttr("leaderSecureGap", myLastLeaderSecureGap == NO_NEIGHBOR ? "None" : toString(myLastLeaderSecureGap), myLastLeaderSecureGap == NO_NEIGHBOR);
393 16637 : of.writeAttr("leaderSpeed", myLastLeaderSpeed == NO_NEIGHBOR ? "None" : toString(myLastLeaderSpeed), myLastLeaderSpeed == NO_NEIGHBOR);
394 16637 : of.writeAttr("followerGap", myLastFollowerGap == NO_NEIGHBOR ? "None" : toString(myLastFollowerGap), myLastFollowerGap == NO_NEIGHBOR);
395 16637 : of.writeAttr("followerSecureGap", myLastFollowerSecureGap == NO_NEIGHBOR ? "None" : toString(myLastFollowerSecureGap), myLastFollowerSecureGap == NO_NEIGHBOR);
396 16637 : of.writeAttr("followerSpeed", myLastFollowerSpeed == NO_NEIGHBOR ? "None" : toString(myLastFollowerSpeed), myLastFollowerSpeed == NO_NEIGHBOR);
397 16637 : of.writeAttr("origLeaderGap", myLastOrigLeaderGap == NO_NEIGHBOR ? "None" : toString(myLastOrigLeaderGap), myLastOrigLeaderGap == NO_NEIGHBOR);
398 16637 : of.writeAttr("origLeaderSecureGap", myLastOrigLeaderSecureGap == NO_NEIGHBOR ? "None" : toString(myLastOrigLeaderSecureGap), myLastOrigLeaderSecureGap == NO_NEIGHBOR);
399 16637 : of.writeAttr("origLeaderSpeed", myLastOrigLeaderSpeed == NO_NEIGHBOR ? "None" : toString(myLastOrigLeaderSpeed), myLastOrigLeaderSpeed == NO_NEIGHBOR);
400 16637 : if (MSGlobals::gLateralResolution > 0) {
401 7653 : const double latGap = direction < 0 ? myLastLateralGapRight : myLastLateralGapLeft;
402 7653 : of.writeAttr("latGap", latGap == NO_NEIGHBOR ? "None" : toString(latGap), latGap == NO_NEIGHBOR);
403 7653 : if (maneuverDist != 0) {
404 366 : of.writeAttr("maneuverDistance", toString(maneuverDist));
405 : }
406 : }
407 16637 : if (myLCXYOutput) {
408 62 : of.writeAttr(SUMO_ATTR_X, myVehicle.getPosition().x());
409 62 : of.writeAttr(SUMO_ATTR_Y, myVehicle.getPosition().y());
410 : }
411 16637 : of.closeTag();
412 16637 : if (MSGlobals::gLaneChangeDuration > DELTA_T) {
413 669 : clearGapsAtLCInit();
414 : }
415 : }
416 1126518 : }
417 :
418 :
419 : double
420 606898 : MSAbstractLaneChangeModel::computeSpeedLat(double /*latDist*/, double& maneuverDist, bool /*urgent*/) const {
421 606898 : if (myVehicle.getVehicleType().wasSet(VTYPEPARS_MAXSPEED_LAT_SET)) {
422 686 : int stepsToChange = (int)ceil(fabs(maneuverDist) / SPEED2DIST(myVehicle.getVehicleType().getMaxSpeedLat()));
423 686 : return DIST2SPEED(maneuverDist / stepsToChange);
424 : } else {
425 606212 : return maneuverDist / STEPS2TIME(MSGlobals::gLaneChangeDuration);
426 : }
427 : }
428 :
429 :
430 : double
431 74617 : MSAbstractLaneChangeModel::getAssumedDecelForLaneChangeDuration() const {
432 74617 : return MAX2(LC_ASSUMED_DECEL, -myVehicle.getAcceleration());
433 : }
434 :
435 : void
436 83800245 : MSAbstractLaneChangeModel::setSpeedLat(double speedLat) {
437 83800245 : myAccelerationLat = SPEED2ACCEL(speedLat - mySpeedLat);
438 83800245 : mySpeedLat = speedLat;
439 83800245 : }
440 :
441 :
442 : void
443 618465569 : MSAbstractLaneChangeModel::resetSpeedLat() {
444 618465569 : if (MSGlobals::gLaneChangeDuration > 0 && !isChangingLanes()) {
445 2377952 : setSpeedLat(0);
446 : }
447 618465569 : }
448 :
449 :
450 : bool
451 580425 : MSAbstractLaneChangeModel::updateCompletion() {
452 : const bool pastBefore = pastMidpoint();
453 : // maneuverDist is not updated in the context of continuous lane changing but represents the full LC distance
454 580425 : double maneuverDist = getManeuverDist();
455 580425 : setSpeedLat(computeSpeedLat(0, maneuverDist, (myOwnState & LCA_URGENT) != 0));
456 580425 : myLaneChangeCompletion += (SPEED2DIST(mySpeedLat) / myManeuverDist);
457 580425 : return !pastBefore && pastMidpoint();
458 : }
459 :
460 :
461 : void
462 63724 : MSAbstractLaneChangeModel::endLaneChangeManeuver(const MSMoveReminder::Notification reason) {
463 : UNUSED_PARAMETER(reason);
464 63724 : myLaneChangeCompletion = 1;
465 63724 : cleanupShadowLane();
466 63724 : cleanupTargetLane();
467 : myNoPartiallyOccupatedByShadow.clear();
468 63724 : myVehicle.switchOffSignal(MSVehicle::VEH_SIGNAL_BLINKER_RIGHT | MSVehicle::VEH_SIGNAL_BLINKER_LEFT);
469 63724 : myVehicle.fixPosition();
470 63724 : if (myAmOpposite && reason != MSMoveReminder::NOTIFICATION_LANE_CHANGE) {
471 133 : if (reason == MSMoveReminder::NOTIFICATION_PARKING && myVehicle.getNextStop().isOpposite) {
472 : // opposite driving continues after parking
473 : } else {
474 : // aborted maneuver
475 : #ifdef DEBUG_OPPOSITE
476 : if (debugVehicle()) {
477 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " aborted maneuver (no longer opposite)\n";
478 : }
479 : #endif
480 108 : changedToOpposite();
481 : }
482 : }
483 63724 : }
484 :
485 :
486 : MSLane*
487 18599859 : MSAbstractLaneChangeModel::getShadowLane(const MSLane* lane, double posLat) const {
488 18599859 : if (std::find(myNoPartiallyOccupatedByShadow.begin(), myNoPartiallyOccupatedByShadow.end(), lane) == myNoPartiallyOccupatedByShadow.end()) {
489 : // initialize shadow lane
490 18599859 : const double overlap = myVehicle.getLateralOverlap(posLat, lane);
491 : #ifdef DEBUG_SHADOWLANE
492 : if (debugVehicle()) {
493 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " posLat=" << posLat << " overlap=" << overlap << "\n";
494 : }
495 : #endif
496 18599859 : if (myAmOpposite) {
497 : // return the neigh-lane in forward direction
498 539325 : return lane->getParallelLane(1);
499 18060534 : } else if (overlap > NUMERICAL_EPS) {
500 10515461 : const int shadowDirection = posLat < 0 ? -1 : 1;
501 10515461 : return lane->getParallelLane(shadowDirection);
502 7545073 : } else if (isChangingLanes() && myLaneChangeCompletion < 0.5) {
503 : // "reserve" target lane even when there is no overlap yet
504 254322 : return lane->getParallelLane(myLaneChangeDirection);
505 : } else {
506 : return nullptr;
507 : }
508 : } else {
509 : return nullptr;
510 : }
511 : }
512 :
513 :
514 : MSLane*
515 18470001 : MSAbstractLaneChangeModel::getShadowLane(const MSLane* lane) const {
516 18470001 : return getShadowLane(lane, myVehicle.getLateralPositionOnLane());
517 : }
518 :
519 :
520 : void
521 4855946 : MSAbstractLaneChangeModel::cleanupShadowLane() {
522 4855946 : if (myShadowLane != nullptr) {
523 17301 : if (debugVehicle()) {
524 0 : std::cout << SIMTIME << " cleanupShadowLane\n";
525 : }
526 17301 : myShadowLane->resetPartialOccupation(&myVehicle);
527 17301 : myShadowLane = nullptr;
528 : }
529 4856320 : for (std::vector<MSLane*>::const_iterator it = myShadowFurtherLanes.begin(); it != myShadowFurtherLanes.end(); ++it) {
530 374 : if (debugVehicle()) {
531 0 : std::cout << SIMTIME << " cleanupShadowLane2\n";
532 : }
533 374 : (*it)->resetPartialOccupation(&myVehicle);
534 : }
535 : myShadowFurtherLanes.clear();
536 : myNoPartiallyOccupatedByShadow.clear();
537 4855946 : }
538 :
539 : void
540 4855946 : MSAbstractLaneChangeModel::cleanupTargetLane() {
541 4855946 : if (myTargetLane != nullptr) {
542 576 : if (debugVehicle()) {
543 0 : std::cout << SIMTIME << " cleanupTargetLane\n";
544 : }
545 576 : myTargetLane->resetManeuverReservation(&myVehicle);
546 576 : myTargetLane = nullptr;
547 : }
548 4855954 : for (std::vector<MSLane*>::const_iterator it = myFurtherTargetLanes.begin(); it != myFurtherTargetLanes.end(); ++it) {
549 8 : if (debugVehicle()) {
550 0 : std::cout << SIMTIME << " cleanupTargetLane\n";
551 : }
552 8 : if (*it != nullptr) {
553 8 : (*it)->resetManeuverReservation(&myVehicle);
554 : }
555 : }
556 : myFurtherTargetLanes.clear();
557 : // myNoPartiallyOccupatedByShadow.clear();
558 4855946 : }
559 :
560 :
561 : bool
562 29247908 : MSAbstractLaneChangeModel::cancelRequest(int state, int laneOffset) {
563 : // store request before canceling
564 29247908 : getCanceledState(laneOffset) |= state;
565 29247908 : int ret = myVehicle.influenceChangeDecision(state);
566 29247908 : return ret != state;
567 : }
568 :
569 : double
570 20051827 : MSAbstractLaneChangeModel::getMaxSpeedLat2() const {
571 20051827 : return MAX2(myVehicle.getVehicleType().getMaxSpeedLat(), myMaxSpeedLatStanding);
572 : }
573 :
574 : void
575 1126259 : MSAbstractLaneChangeModel::initLastLaneChangeOffset(int dir) {
576 1126259 : if (dir > 0) {
577 616691 : myLastLaneChangeOffset = 1;
578 509568 : } else if (dir < 0) {
579 509568 : myLastLaneChangeOffset = -1;
580 : }
581 1126259 : }
582 :
583 : void
584 6168437 : MSAbstractLaneChangeModel::updateShadowLane() {
585 6168437 : if (!MSGlobals::gSublane) {
586 : // assume each vehicle drives at the center of its lane and act as if it fits
587 813543 : return;
588 : }
589 5354894 : if (myShadowLane != nullptr) {
590 : #ifdef DEBUG_SHADOWLANE
591 : if (debugVehicle()) {
592 : std::cout << SIMTIME << " updateShadowLane()\n";
593 : }
594 : #endif
595 1874726 : myShadowLane->resetPartialOccupation(&myVehicle);
596 : }
597 5354894 : myShadowLane = getShadowLane(myVehicle.getLane());
598 : std::vector<MSLane*> passed;
599 5354894 : if (myShadowLane != nullptr) {
600 1892027 : myShadowLane->setPartialOccupation(&myVehicle);
601 1892027 : const std::vector<MSLane*>& further = myVehicle.getFurtherLanes();
602 1892027 : if (myAmOpposite) {
603 : assert(further.size() == 0);
604 : } else {
605 : const std::vector<double>& furtherPosLat = myVehicle.getFurtherLanesPosLat();
606 : assert(further.size() == furtherPosLat.size());
607 1674700 : passed.push_back(myShadowLane);
608 1804558 : for (int i = 0; i < (int)further.size(); ++i) {
609 129858 : MSLane* shadowFurther = getShadowLane(further[i], furtherPosLat[i]);
610 : #ifdef DEBUG_SHADOWLANE
611 : if (debugVehicle()) {
612 : std::cout << SIMTIME << " further=" << further[i]->getID() << " (posLat=" << furtherPosLat[i] << ") shadowFurther=" << Named::getIDSecure(shadowFurther) << "\n";
613 : }
614 : #endif
615 129858 : if (shadowFurther != nullptr && shadowFurther->getLinkTo(passed.back()) != nullptr) {
616 55794 : passed.push_back(shadowFurther);
617 : }
618 : }
619 : std::reverse(passed.begin(), passed.end());
620 : }
621 : } else {
622 3462867 : if (isChangingLanes() && myVehicle.getLateralOverlap() > NUMERICAL_EPS) {
623 4 : WRITE_WARNING("Vehicle '" + myVehicle.getID() + "' could not finish continuous lane change (lane disappeared) time=" +
624 : time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
625 1 : endLaneChangeManeuver();
626 : }
627 : }
628 : #ifdef DEBUG_SHADOWLANE
629 : if (debugVehicle()) {
630 : std::cout << SIMTIME << " updateShadowLane() veh=" << myVehicle.getID()
631 : << " newShadowLane=" << Named::getIDSecure(myShadowLane)
632 : << "\n before:" << " myShadowFurtherLanes=" << toString(myShadowFurtherLanes) << " further=" << toString(myVehicle.getFurtherLanes()) << " passed=" << toString(passed);
633 : std::cout << std::endl;
634 : }
635 : #endif
636 5354894 : myVehicle.updateFurtherLanes(myShadowFurtherLanes, myShadowFurtherLanesPosLat, passed);
637 : #ifdef DEBUG_SHADOWLANE
638 : if (debugVehicle()) std::cout
639 : << "\n after:" << " myShadowFurtherLanes=" << toString(myShadowFurtherLanes) << "\n";
640 : #endif
641 5354894 : }
642 :
643 :
644 : int
645 9783300 : MSAbstractLaneChangeModel::getShadowDirection() const {
646 9783300 : if (isChangingLanes()) {
647 640902 : if (pastMidpoint()) {
648 208730 : return -myLaneChangeDirection;
649 : } else {
650 432172 : return myLaneChangeDirection;
651 : }
652 9142398 : } else if (myShadowLane == nullptr) {
653 : return 0;
654 9142398 : } else if (myAmOpposite) {
655 : // return neigh-lane in forward direction
656 : return 1;
657 8636211 : } else if (&myShadowLane->getEdge() == &myVehicle.getLane()->getEdge()) {
658 8535147 : return myShadowLane->getIndex() - myVehicle.getLane()->getIndex();
659 : } else {
660 : // overlap with opposite direction lane
661 : return 1;
662 : }
663 : }
664 :
665 :
666 : MSLane*
667 85565578 : MSAbstractLaneChangeModel::updateTargetLane() {
668 : #ifdef DEBUG_TARGET_LANE
669 : MSLane* oldTarget = myTargetLane;
670 : std::vector<MSLane*> oldFurtherTargets = myFurtherTargetLanes;
671 : if (debugVehicle()) {
672 : std::cout << SIMTIME << " veh '" << myVehicle.getID() << "' (lane=" << myVehicle.getLane()->getID() << ") updateTargetLane()"
673 : << "\n oldTarget: " << (oldTarget == nullptr ? "NULL" : oldTarget->getID())
674 : << " oldFurtherTargets: " << toString(oldFurtherTargets);
675 : }
676 : #endif
677 85565578 : if (myTargetLane != nullptr) {
678 214983 : myTargetLane->resetManeuverReservation(&myVehicle);
679 : }
680 : // Clear old further target lanes
681 85576177 : for (MSLane* oldTargetLane : myFurtherTargetLanes) {
682 10599 : if (oldTargetLane != nullptr) {
683 8156 : oldTargetLane->resetManeuverReservation(&myVehicle);
684 : }
685 : }
686 : myFurtherTargetLanes.clear();
687 :
688 : // Get new target lanes and issue a maneuver reservation.
689 : int targetDir;
690 85565578 : myTargetLane = determineTargetLane(targetDir);
691 85565578 : if (myTargetLane != nullptr) {
692 215559 : myTargetLane->setManeuverReservation(&myVehicle);
693 : // further targets are just the target lanes corresponding to the vehicle's further lanes
694 : // @note In a neglectable amount of situations we might add a reservation for a shadow further lane.
695 226166 : for (MSLane* furtherLane : myVehicle.getFurtherLanes()) {
696 10607 : MSLane* furtherTargetLane = furtherLane->getParallelLane(targetDir);
697 10607 : myFurtherTargetLanes.push_back(furtherTargetLane);
698 10607 : if (furtherTargetLane != nullptr) {
699 8164 : furtherTargetLane->setManeuverReservation(&myVehicle);
700 : }
701 : }
702 : }
703 : #ifdef DEBUG_TARGET_LANE
704 : if (debugVehicle()) {
705 : std::cout << "\n newTarget (maneuverDist=" << myManeuverDist << " offset=" << targetDir << "): " << (myTargetLane == nullptr ? "NULL" : myTargetLane->getID())
706 : << " newFurtherTargets: " << toString(myFurtherTargetLanes)
707 : << std::endl;
708 : }
709 : #endif
710 85565578 : return myTargetLane;
711 : }
712 :
713 :
714 : MSLane*
715 85565578 : MSAbstractLaneChangeModel::determineTargetLane(int& targetDir) const {
716 85565578 : targetDir = 0;
717 85565578 : if (myManeuverDist == 0) {
718 : return nullptr;
719 : }
720 : // Current lateral boundaries of the vehicle
721 2412333 : const double vehRight = myVehicle.getLateralPositionOnLane() - 0.5 * myVehicle.getWidth();
722 2412333 : const double vehLeft = myVehicle.getLateralPositionOnLane() + 0.5 * myVehicle.getWidth();
723 2412333 : const double halfLaneWidth = 0.5 * myVehicle.getLane()->getWidth();
724 :
725 2412333 : if (vehRight + myManeuverDist < -halfLaneWidth) {
726 : // Vehicle intends to traverse the right lane boundary
727 333241 : targetDir = -1;
728 2079092 : } else if (vehLeft + myManeuverDist > halfLaneWidth) {
729 : // Vehicle intends to traverse the left lane boundary
730 498200 : targetDir = 1;
731 : }
732 2412333 : if (targetDir == 0) {
733 : // Presently, no maneuvering into another lane is begun.
734 : return nullptr;
735 : }
736 831441 : MSLane* target = myVehicle.getLane()->getParallelLane(targetDir);
737 831441 : if (target == nullptr || target == myShadowLane) {
738 : return nullptr;
739 : } else {
740 : return target;
741 : }
742 : }
743 :
744 :
745 :
746 : double
747 710261185 : MSAbstractLaneChangeModel::calcAngleOffset() {
748 : double result = 0.;
749 710261185 : if (!(fabs(mySpeedLat) < NUMERICAL_EPS && fabs(myPreviousAngleOffset * 180 / M_PI) < NUMERICAL_EPS)) {
750 19667259 : if (myVehicle.getLength() < sqrt(SPEED2DIST(mySpeedLat) * SPEED2DIST(mySpeedLat) + SPEED2DIST(myVehicle.getSpeed()) * SPEED2DIST(myVehicle.getSpeed()))) {
751 5631276 : result = atan2(mySpeedLat, myVehicle.getSpeed());
752 : } else {
753 14035983 : result = myPreviousAngleOffset + asin((sin(M_PI / 2 - myPreviousAngleOffset) * (SPEED2DIST(mySpeedLat) - tan(myPreviousAngleOffset) * SPEED2DIST(myVehicle.getSpeed()))) / myVehicle.getLength());
754 : }
755 : }
756 :
757 710261185 : myAngleOffset = result;
758 710261185 : return result;
759 : }
760 :
761 :
762 : double
763 75339 : MSAbstractLaneChangeModel::estimateLCDuration(const double speed, const double remainingManeuverDist, const double decel, bool urgent) const {
764 :
765 75339 : const SUMOVTypeParameter::SubParams& lcParams = myVehicle.getVehicleType().getParameter().getLCParams();
766 123774 : if (lcParams.find(SUMO_ATTR_LCA_MAXSPEEDLATSTANDING) == lcParams.end() && lcParams.find(SUMO_ATTR_LCA_MAXSPEEDLATFACTOR) == lcParams.end()) {
767 48435 : if (!myVehicle.getVehicleType().wasSet(VTYPEPARS_MAXSPEED_LAT_SET)) {
768 : // no dependency of lateral speed on longitudinal speed. (Only called prior to LC initialization to determine whether it could be completed)
769 48409 : return STEPS2TIME(MSGlobals::gLaneChangeDuration);
770 : } else {
771 26 : return remainingManeuverDist / myVehicle.getVehicleType().getMaxSpeedLat();
772 : }
773 : }
774 :
775 26904 : if (remainingManeuverDist == 0) {
776 : return 0;
777 : }
778 :
779 : // Check argument assumptions
780 : assert(speed >= 0);
781 : assert(remainingManeuverDist >= 0);
782 : assert(decel > 0);
783 : assert(myVehicle.getVehicleType().getMaxSpeedLat() > 0);
784 : assert(myMaxSpeedLatStanding <= myVehicle.getVehicleType().getMaxSpeedLat());
785 : assert(myMaxSpeedLatStanding >= 0);
786 :
787 : // for brevity
788 : const double v0 = speed;
789 : const double D = remainingManeuverDist;
790 : const double b = decel;
791 26904 : const double wmin = myMaxSpeedLatStanding;
792 26904 : const double f = myMaxSpeedLatFactor;
793 26904 : const double wmax = myVehicle.getVehicleType().getMaxSpeedLat();
794 :
795 : /* Here's the approach for the calculation of the required time for the LC:
796 : * To obtain the maximal LC-duration, for v(t) we assume that v(t)=max(0, v0-b*t),
797 : * Where v(t)=0 <=> t >= ts:=v0/b
798 : * For the lateral speed w(t) this gives:
799 : * w(t) = min(wmax, wmin + f*v(t))
800 : * The lateral distance covered until t is
801 : * d(t) = int_0^t w(s) ds
802 : * We distinguish three possibilities for the solution d(T)=D, where T is the time of the LC completion.
803 : * 1) w(T) = wmax, i.e. v(T)>(wmax-wmin)/f
804 : * 2) wmin < w(T) < wmax, i.e. (wmax-wmin)/f > v(T) > 0
805 : * 3) w(T) = wmin, i.e., v(T)=0
806 : */
807 26904 : const double vm = (wmax - wmin) / f;
808 : double distSoFar = 0.;
809 : double timeSoFar = 0.;
810 : double v = v0;
811 26904 : if (v > vm) {
812 1411 : const double wmaxTime = (v0 - vm) / b;
813 1411 : const double d1 = wmax * wmaxTime;
814 1411 : if (d1 >= D) {
815 420 : return D / wmax;
816 : } else {
817 991 : distSoFar += d1;
818 991 : timeSoFar += wmaxTime;
819 : v = vm;
820 : }
821 : }
822 26484 : if (v > 0) {
823 : /* Here, w(t1+t) = wmin + f*v(t1+t) = wmin + f*(v - b*t)
824 : * Thus, the additional lateral distance covered after time t is:
825 : * d2 = (wmin + f*v)*t - 0.5*f*b*t^2
826 : * and the additional lateral distance covered until v=0 at t=v/b is:
827 : * d2 = (wmin + 0.5*f*v)*t
828 : */
829 5655 : const double t = v / b; // stop time
830 5655 : const double d2 = (wmin + 0.5 * f * v) * t; // lateral distance covered until stop
831 : assert(d2 > 0);
832 5655 : if (distSoFar + d2 >= D) {
833 : // LC is completed during this phase
834 11 : const double x = 0.5 * f * b;
835 11 : const double y = wmin + f * v;
836 : /* Solve D - distSoFar = y*t - x*t^2.
837 : * 0 = x*t^2 - y*t/x + (D - distSoFar)/x
838 : */
839 11 : const double p = 0.5 * y / x;
840 11 : const double q = (D - distSoFar) / x;
841 : assert(p * p - q > 0);
842 11 : const double t2 = p + sqrt(p * p - q);
843 11 : return timeSoFar + t2;
844 : } else {
845 : distSoFar += d2;
846 5644 : timeSoFar += t;
847 : //v = 0;
848 : }
849 : }
850 : // If we didn't return yet this means the LC was not completed until the vehicle stops (if braking with rate b)
851 26473 : if (wmin == 0) {
852 : // LC won't be completed if vehicle stands
853 26473 : double maneuverDist = remainingManeuverDist;
854 26473 : const double vModel = computeSpeedLat(maneuverDist, maneuverDist, urgent);
855 26473 : double result = D / vModel;
856 : // make sure that the vehicle isn't braking to a stop during the manuever
857 26473 : if (vModel > SUMO_const_haltingSpeed && (vModel + myVehicle.getAcceleration() * result) > SUMO_const_haltingSpeed) {
858 : // unless the model tells us something different
859 : return result;
860 : } else {
861 25725 : return -1;
862 : }
863 : } else {
864 : // complete LC with lateral speed wmin
865 0 : return timeSoFar + (D - distSoFar) / wmin;
866 : }
867 : }
868 :
869 : SUMOTime
870 46945 : MSAbstractLaneChangeModel::remainingTime() const {
871 : assert(isChangingLanes()); // Only to be called during ongoing lane change
872 46945 : const SUMOVTypeParameter::SubParams& lcParams = myVehicle.getVehicleType().getParameter().getLCParams();
873 93529 : if (lcParams.find(SUMO_ATTR_LCA_MAXSPEEDLATSTANDING) == lcParams.end() && lcParams.find(SUMO_ATTR_LCA_MAXSPEEDLATFACTOR) == lcParams.end()) {
874 46584 : if (myVehicle.getVehicleType().wasSet(VTYPEPARS_MAXSPEED_LAT_SET)) {
875 0 : return TIME2STEPS((1. - myLaneChangeCompletion) * myManeuverDist / myVehicle.getVehicleType().getMaxSpeedLat());
876 : } else {
877 46584 : return (SUMOTime)((1. - myLaneChangeCompletion) * (double)MSGlobals::gLaneChangeDuration);
878 : }
879 : }
880 : // Using maxSpeedLat(Factor/Standing)
881 361 : const bool urgent = (myOwnState & LCA_URGENT) != 0;
882 406 : return TIME2STEPS(estimateLCDuration(myVehicle.getSpeed(),
883 : fabs(myManeuverDist * (1 - myLaneChangeCompletion)),
884 : myVehicle.getCarFollowModel().getMaxDecel(), urgent));
885 : }
886 :
887 :
888 : void
889 2431055 : MSAbstractLaneChangeModel::setShadowApproachingInformation(MSLink* link) const {
890 : //std::cout << SIMTIME << " veh=" << myVehicle.getID() << " @=" << &myVehicle << " set shadow approaching=" << link->getViaLaneOrLane()->getID() << "\n";
891 2431055 : myApproachedByShadow.push_back(link);
892 2431055 : }
893 :
894 : void
895 642598540 : MSAbstractLaneChangeModel::removeShadowApproachingInformation() const {
896 645029595 : for (std::vector<MSLink*>::iterator it = myApproachedByShadow.begin(); it != myApproachedByShadow.end(); ++it) {
897 : //std::cout << SIMTIME << " veh=" << myVehicle.getID() << " @=" << &myVehicle << " remove shadow approaching=" << (*it)->getViaLaneOrLane()->getID() << "\n";
898 2431055 : (*it)->removeApproaching(&myVehicle);
899 : }
900 : myApproachedByShadow.clear();
901 642598540 : }
902 :
903 :
904 :
905 : void
906 41435142 : MSAbstractLaneChangeModel::checkTraCICommands() {
907 41435142 : int newstate = myVehicle.influenceChangeDecision(myOwnState);
908 41435142 : int oldstate = myVehicle.getLaneChangeModel().getOwnState();
909 41435142 : if (myOwnState != newstate) {
910 16 : if (MSGlobals::gLateralResolution > 0.) {
911 : // Calculate and set the lateral maneuver distance corresponding to the change request
912 : // to induce a corresponding sublane change.
913 12 : const int dir = (newstate & LCA_RIGHT) != 0 ? -1 : ((newstate & LCA_LEFT) != 0 ? 1 : 0);
914 : // minimum distance to move the vehicle fully onto the lane at offset dir
915 12 : const double latLaneDist = myVehicle.lateralDistanceToLane(dir);
916 12 : if ((newstate & LCA_TRACI) != 0) {
917 12 : if ((newstate & LCA_STAY) != 0) {
918 0 : setManeuverDist(0.);
919 12 : } else if (((newstate & LCA_RIGHT) != 0 && dir < 0)
920 8 : || ((newstate & LCA_LEFT) != 0 && dir > 0)) {
921 4 : setManeuverDist(latLaneDist);
922 : }
923 : }
924 12 : if (myVehicle.hasInfluencer()) {
925 : // lane change requests override sublane change requests
926 12 : myVehicle.getInfluencer().resetLatDist();
927 : }
928 :
929 : }
930 16 : setOwnState(newstate);
931 : } else {
932 : // Check for sublane change requests
933 41435126 : if (myVehicle.hasInfluencer() && myVehicle.getInfluencer().getLatDist() != 0) {
934 304 : const double maneuverDist = myVehicle.getInfluencer().getLatDist();
935 304 : myVehicle.getLaneChangeModel().setManeuverDist(maneuverDist);
936 304 : myVehicle.getInfluencer().resetLatDist();
937 304 : newstate |= LCA_TRACI;
938 304 : if (myOwnState != newstate) {
939 4 : setOwnState(newstate);
940 : }
941 304 : if (gDebugFlag2) {
942 0 : std::cout << " traci influenced maneuverDist=" << maneuverDist << "\n";
943 : }
944 : }
945 : }
946 41435142 : if (gDebugFlag2) {
947 0 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " stateAfterTraCI=" << toString((LaneChangeAction)newstate) << " original=" << toString((LaneChangeAction)oldstate) << "\n";
948 : }
949 41435142 : }
950 :
951 : void
952 44118 : MSAbstractLaneChangeModel::changedToOpposite() {
953 44118 : myAmOpposite = !myAmOpposite;
954 44118 : myAlreadyChanged = true;
955 44118 : }
956 :
957 : void
958 747084 : MSAbstractLaneChangeModel::setFollowerGaps(CLeaderDist follower, double secGap) {
959 747084 : if (follower.first != 0) {
960 436099 : myLastFollowerGap = follower.second + follower.first->getVehicleType().getMinGap();
961 436099 : myLastFollowerSecureGap = secGap;
962 436099 : myLastFollowerSpeed = follower.first->getSpeed();
963 : }
964 747084 : }
965 :
966 : void
967 747084 : MSAbstractLaneChangeModel::setLeaderGaps(CLeaderDist leader, double secGap) {
968 747084 : if (leader.first != 0) {
969 544132 : myLastLeaderGap = leader.second + myVehicle.getVehicleType().getMinGap();
970 544132 : myLastLeaderSecureGap = secGap;
971 544132 : myLastLeaderSpeed = leader.first->getSpeed();
972 : }
973 747084 : }
974 :
975 : void
976 747084 : MSAbstractLaneChangeModel::setOrigLeaderGaps(CLeaderDist leader, double secGap) {
977 747084 : if (leader.first != 0) {
978 443639 : myLastOrigLeaderGap = leader.second + myVehicle.getVehicleType().getMinGap();
979 443639 : myLastOrigLeaderSecureGap = secGap;
980 443639 : myLastOrigLeaderSpeed = leader.first->getSpeed();
981 : }
982 747084 : }
983 :
984 : void
985 627713547 : MSAbstractLaneChangeModel::prepareStep() {
986 627713547 : getCanceledState(-1) = LCA_NONE;
987 627713547 : getCanceledState(0) = LCA_NONE;
988 627713547 : getCanceledState(1) = LCA_NONE;
989 : saveLCState(-1, LCA_UNKNOWN, LCA_UNKNOWN);
990 : saveLCState(0, LCA_UNKNOWN, LCA_UNKNOWN);
991 : saveLCState(1, LCA_UNKNOWN, LCA_UNKNOWN);
992 627713547 : myLastLateralGapRight = NO_NEIGHBOR;
993 627713547 : myLastLateralGapLeft = NO_NEIGHBOR;
994 627713547 : if (!myDontResetLCGaps) {
995 627708504 : myLastLeaderGap = NO_NEIGHBOR;
996 627708504 : myLastLeaderSecureGap = NO_NEIGHBOR;
997 627708504 : myLastFollowerGap = NO_NEIGHBOR;
998 627708504 : myLastFollowerSecureGap = NO_NEIGHBOR;
999 627708504 : myLastOrigLeaderGap = NO_NEIGHBOR;
1000 627708504 : myLastOrigLeaderSecureGap = NO_NEIGHBOR;
1001 627708504 : myLastLeaderSpeed = NO_NEIGHBOR;
1002 627708504 : myLastFollowerSpeed = NO_NEIGHBOR;
1003 627708504 : myLastOrigLeaderSpeed = NO_NEIGHBOR;
1004 : }
1005 627713547 : myCommittedSpeed = 0;
1006 627713547 : }
1007 :
1008 : void
1009 7653 : MSAbstractLaneChangeModel::setFollowerGaps(const MSLeaderDistanceInfo& vehicles) {
1010 : int rightmost;
1011 : int leftmost;
1012 7653 : vehicles.getSubLanes(&myVehicle, 0, rightmost, leftmost);
1013 31187 : for (int i = rightmost; i <= leftmost; ++i) {
1014 23534 : CLeaderDist vehDist = vehicles[i];
1015 23534 : if (vehDist.first != 0) {
1016 16254 : const MSVehicle* leader = &myVehicle;
1017 : const MSVehicle* follower = vehDist.first;
1018 16254 : const double netGap = vehDist.second + follower->getVehicleType().getMinGap();
1019 16254 : if (netGap < myLastFollowerGap && netGap >= 0) {
1020 6912 : myLastFollowerGap = netGap;
1021 6912 : myLastFollowerSecureGap = follower->getCarFollowModel().getSecureGap(follower, leader, follower->getSpeed(), leader->getSpeed(), leader->getCarFollowModel().getMaxDecel());
1022 6912 : myLastFollowerSpeed = follower->getSpeed();
1023 : }
1024 : }
1025 : }
1026 7653 : }
1027 :
1028 : void
1029 7653 : MSAbstractLaneChangeModel::setLeaderGaps(const MSLeaderDistanceInfo& vehicles) {
1030 : int rightmost;
1031 : int leftmost;
1032 7653 : vehicles.getSubLanes(&myVehicle, 0, rightmost, leftmost);
1033 31187 : for (int i = rightmost; i <= leftmost; ++i) {
1034 23534 : CLeaderDist vehDist = vehicles[i];
1035 23534 : if (vehDist.first != 0) {
1036 : const MSVehicle* leader = vehDist.first;
1037 18286 : const MSVehicle* follower = &myVehicle;
1038 18286 : const double netGap = vehDist.second + follower->getVehicleType().getMinGap();
1039 18286 : if (netGap < myLastLeaderGap && netGap >= 0) {
1040 7144 : myLastLeaderGap = netGap;
1041 7144 : myLastLeaderSecureGap = follower->getCarFollowModel().getSecureGap(follower, leader, follower->getSpeed(), leader->getSpeed(), leader->getCarFollowModel().getMaxDecel());
1042 7144 : myLastLeaderSpeed = leader->getSpeed();
1043 : }
1044 : }
1045 : }
1046 7653 : }
1047 :
1048 : void
1049 7653 : MSAbstractLaneChangeModel::setOrigLeaderGaps(const MSLeaderDistanceInfo& vehicles) {
1050 : int rightmost;
1051 : int leftmost;
1052 7653 : vehicles.getSubLanes(&myVehicle, 0, rightmost, leftmost);
1053 31081 : for (int i = rightmost; i <= leftmost; ++i) {
1054 23428 : CLeaderDist vehDist = vehicles[i];
1055 23428 : if (vehDist.first != 0) {
1056 : const MSVehicle* leader = vehDist.first;
1057 12286 : const MSVehicle* follower = &myVehicle;
1058 12286 : const double netGap = vehDist.second + follower->getVehicleType().getMinGap();
1059 12286 : if (netGap < myLastOrigLeaderGap && netGap >= 0) {
1060 4727 : myLastOrigLeaderGap = netGap;
1061 4727 : myLastOrigLeaderSecureGap = follower->getCarFollowModel().getSecureGap(follower, leader, follower->getSpeed(), leader->getSpeed(), leader->getCarFollowModel().getMaxDecel());
1062 4727 : myLastOrigLeaderSpeed = leader->getSpeed();
1063 : }
1064 : }
1065 : }
1066 7653 : }
1067 :
1068 :
1069 : bool
1070 26962683 : MSAbstractLaneChangeModel::isStrategicBlocked() const {
1071 26962683 : const int stateRight = mySavedStateRight.second;
1072 26962683 : if (
1073 : (stateRight & LCA_STRATEGIC) != 0
1074 : && (stateRight & LCA_RIGHT) != 0
1075 822022 : && (stateRight & LCA_BLOCKED) != 0) {
1076 : return true;
1077 : }
1078 26413126 : const int stateLeft = mySavedStateLeft.second;
1079 26413126 : if (
1080 : (stateLeft & LCA_STRATEGIC) != 0
1081 : && (stateLeft & LCA_LEFT) != 0
1082 142263 : && (stateLeft & LCA_BLOCKED) != 0) {
1083 72721 : return true;
1084 : }
1085 : return false;
1086 : }
1087 :
1088 : double
1089 358564012 : MSAbstractLaneChangeModel::getForwardPos() const {
1090 358564012 : return myAmOpposite ? myVehicle.getLane()->getLength() - myVehicle.getPositionOnLane() : myVehicle.getPositionOnLane();
1091 : }
1092 :
1093 :
1094 : int
1095 1425 : MSAbstractLaneChangeModel::getNormalizedLaneIndex() {
1096 1425 : const int i = myVehicle.getLane()->getIndex();
1097 1425 : if (myAmOpposite) {
1098 789 : return myVehicle.getLane()->getParallelOpposite()->getEdge().getNumLanes() + myVehicle.getLane()->getEdge().getNumLanes() - 1 - i;
1099 : } else {
1100 : return i;
1101 : }
1102 : }
1103 :
1104 : void
1105 48278763 : MSAbstractLaneChangeModel::addLCSpeedAdvice(const double vSafe, bool ownAdvice) {
1106 48278763 : const double accel = SPEED2ACCEL(vSafe - myVehicle.getSpeed());
1107 48278763 : myLCAccelerationAdvices.push_back({accel, ownAdvice});
1108 48278763 : }
1109 :
1110 :
1111 : void
1112 2456 : MSAbstractLaneChangeModel::saveState(OutputDevice& out) const {
1113 : std::vector<std::string> lcState;
1114 2456 : if (MSGlobals::gLaneChangeDuration > 0) {
1115 1 : lcState.push_back(toString(mySpeedLat));
1116 1 : lcState.push_back(toString(myLaneChangeCompletion));
1117 2 : lcState.push_back(toString(myLaneChangeDirection));
1118 : }
1119 2456 : if (lcState.size() > 0) {
1120 1 : out.writeAttr(SUMO_ATTR_LCSTATE, lcState);
1121 : }
1122 2456 : }
1123 :
1124 : void
1125 3541 : MSAbstractLaneChangeModel::loadState(const SUMOSAXAttributes& attrs) {
1126 3541 : if (attrs.hasAttribute(SUMO_ATTR_LCSTATE)) {
1127 1 : std::istringstream bis(attrs.getString(SUMO_ATTR_LCSTATE));
1128 1 : bis >> mySpeedLat;
1129 1 : bis >> myLaneChangeCompletion;
1130 1 : bis >> myLaneChangeDirection;
1131 1 : }
1132 3541 : }
1133 :
1134 :
1135 : double
1136 4169711 : MSAbstractLaneChangeModel::getExtraReservation(int bestLaneOffset, double neighExtraDist) const {
1137 4169711 : if (neighExtraDist > myVehicle.getVehicleType().getLengthWithGap()) {
1138 : return 0;
1139 : }
1140 4169411 : if (bestLaneOffset < -1) {
1141 : return 20;
1142 4126896 : } else if (bestLaneOffset > 1) {
1143 402477 : return 40;
1144 : }
1145 : return 0;
1146 : }
1147 :
1148 :
1149 : double
1150 595076294 : MSAbstractLaneChangeModel::getCooperativeHelpSpeed(const MSLane* lane, double distToLaneEnd) const {
1151 595076294 : if (myCooperativeHelpTime >= 0) {
1152 595076294 : std::pair<double, SUMOTime> backAndWaiting = lane->getEdge().getLastBlocked(lane->getIndex());
1153 595076294 : if (backAndWaiting.second >= myCooperativeHelpTime) {
1154 1790829 : double gap = distToLaneEnd - lane->getLength() + backAndWaiting.first - myVehicle.getVehicleType().getMinGap() - NUMERICAL_EPS;
1155 1790829 : if (backAndWaiting.first < 0) {
1156 292151 : if (myVehicle.getLane()->getToJunction() == lane->getFromJunction()) {
1157 124443 : if (myVehicle.getLane()->isInternal()) {
1158 : // already on the junction, potentially blocking lane change, do not stop
1159 : gap = -1;
1160 : } else {
1161 : // stop before entering the junction
1162 119711 : gap = myVehicle.getLane()->getLength() - myVehicle.getPositionOnLane();
1163 : }
1164 : }
1165 : }
1166 1786097 : if (gap > 0) {
1167 1122907 : double stopSpeed = myVehicle.getCarFollowModel().stopSpeed(&myVehicle, myVehicle.getSpeed(), gap);
1168 : //if (myVehicle.isSelected() && stopSpeed < myVehicle.getSpeed()) {
1169 : // std::cout << SIMTIME << " veh=" << myVehicle.getID() << " lane=" << lane->getID() << " dte=" << distToLaneEnd << " gap=" << gap << " backPos=" << backAndWaiting.first << " waiting=" << backAndWaiting.second << " helpTime=" << myCooperativeHelpTime << " stopSpeed=" << stopSpeed << " minNext=" << myVehicle.getCarFollowModel().minNextSpeed(myVehicle.getSpeed(), &myVehicle) << "\n";
1170 : //}
1171 1122907 : if (stopSpeed >= myVehicle.getCarFollowModel().minNextSpeed(myVehicle.getSpeed(), &myVehicle)) {
1172 : // regular braking is helpful
1173 1120590 : return stopSpeed;
1174 : }
1175 : }
1176 : }
1177 : }
1178 : // do not restrict speed
1179 : return std::numeric_limits<double>::max();
1180 : }
|
