Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2001-2024 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 :
64 : #define LC_ASSUMED_DECEL 1.0 // the minimal constant deceleration assumed to estimate the duration of a continuous lane-change at its initiation.
65 :
66 : /* -------------------------------------------------------------------------
67 : * MSAbstractLaneChangeModel-methods
68 : * ----------------------------------------------------------------------- */
69 :
70 : void
71 42782 : MSAbstractLaneChangeModel::initGlobalOptions(const OptionsCont& oc) {
72 42782 : myAllowOvertakingRight = oc.getBool("lanechange.overtake-right");
73 42782 : myLCOutput = oc.isSet("lanechange-output");
74 42782 : myLCStartedOutput = oc.getBool("lanechange-output.started");
75 42782 : myLCEndedOutput = oc.getBool("lanechange-output.ended");
76 42782 : myLCXYOutput = oc.getBool("lanechange-output.xy");
77 42782 : }
78 :
79 :
80 : MSAbstractLaneChangeModel*
81 4367627 : MSAbstractLaneChangeModel::build(LaneChangeModel lcm, MSVehicle& v) {
82 4367627 : if (MSGlobals::gLateralResolution > 0 && lcm != LaneChangeModel::SL2015 && lcm != LaneChangeModel::DEFAULT) {
83 44 : throw ProcessError(TLF("Lane change model '%' is not compatible with sublane simulation", toString(lcm)));
84 : }
85 4367605 : switch (lcm) {
86 8 : case LaneChangeModel::DK2008:
87 8 : return new MSLCM_DK2008(v);
88 1290 : case LaneChangeModel::LC2013:
89 1290 : return new MSLCM_LC2013(v);
90 0 : case LaneChangeModel::LC2013_CC:
91 0 : return new MSLCM_LC2013_CC(v);
92 574 : case LaneChangeModel::SL2015:
93 574 : return new MSLCM_SL2015(v);
94 4365733 : case LaneChangeModel::DEFAULT:
95 4365733 : if (MSGlobals::gLateralResolution <= 0) {
96 3587081 : return new MSLCM_LC2013(v);
97 : } else {
98 778652 : return new MSLCM_SL2015(v);
99 : }
100 0 : default:
101 0 : throw ProcessError(TLF("Lane change model '%' not implemented", toString(lcm)));
102 : }
103 : }
104 :
105 :
106 4367605 : MSAbstractLaneChangeModel::MSAbstractLaneChangeModel(MSVehicle& v, const LaneChangeModel model) :
107 4367605 : myVehicle(v),
108 4367605 : myOwnState(0),
109 4367605 : myPreviousState(0),
110 4367605 : myPreviousState2(0),
111 4367605 : myCanceledStateRight(LCA_NONE),
112 4367605 : myCanceledStateCenter(LCA_NONE),
113 4367605 : myCanceledStateLeft(LCA_NONE),
114 4367605 : mySpeedLat(0),
115 4367605 : myAccelerationLat(0),
116 4367605 : myAngleOffset(0),
117 4367605 : myPreviousAngleOffset(0),
118 4367605 : myCommittedSpeed(0),
119 4367605 : myLaneChangeCompletion(1.0),
120 4367605 : myLaneChangeDirection(0),
121 4367605 : myAlreadyChanged(false),
122 4367605 : myShadowLane(nullptr),
123 4367605 : myTargetLane(nullptr),
124 4367605 : myModel(model),
125 4367605 : myLastLateralGapLeft(0.),
126 4367605 : myLastLateralGapRight(0.),
127 4367605 : myLastLeaderGap(0.),
128 4367605 : myLastFollowerGap(0.),
129 4367605 : myLastLeaderSecureGap(0.),
130 4367605 : myLastFollowerSecureGap(0.),
131 4367605 : myLastOrigLeaderGap(0.),
132 4367605 : myLastOrigLeaderSecureGap(0.),
133 4367605 : myLastLeaderSpeed(0),
134 4367605 : myLastFollowerSpeed(0),
135 4367605 : myLastOrigLeaderSpeed(0),
136 4367605 : myDontResetLCGaps(false),
137 4367605 : myMaxSpeedLatStanding(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_MAXSPEEDLATSTANDING, v.getVehicleType().getMaxSpeedLat())),
138 4367605 : myMaxSpeedLatFactor(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_MAXSPEEDLATFACTOR, 1)),
139 8735210 : myMaxDistLatStanding(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_MAXDISTLATSTANDING,
140 : // prevent lateral sliding for cars but permit for two-wheelers due to better maneuverability
141 4367605 : (v.getVClass() & (SVC_BICYCLE | SVC_MOTORCYCLE | SVC_MOPED)) != 0 ? std::numeric_limits<double>::max() : 1.6)),
142 4367605 : mySigma(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SIGMA, 0.0)),
143 4367605 : myOvertakeRightParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_OVERTAKE_RIGHT, 0)),
144 4367605 : myHaveBlueLight(v.getDevice(typeid(MSDevice_Bluelight)) != nullptr), // see MSVehicle::initDevices
145 4367605 : myLastLaneChangeOffset(0),
146 4367605 : myAmOpposite(false),
147 4367605 : myManeuverDist(0.),
148 8735210 : myPreviousManeuverDist(0.) {
149 : saveLCState(-1, LCA_UNKNOWN, LCA_UNKNOWN);
150 : saveLCState(0, LCA_UNKNOWN, LCA_UNKNOWN);
151 : saveLCState(1, LCA_UNKNOWN, LCA_UNKNOWN);
152 4367605 : }
153 :
154 :
155 4367529 : MSAbstractLaneChangeModel::~MSAbstractLaneChangeModel() {
156 4367529 : }
157 :
158 : void
159 239356550 : MSAbstractLaneChangeModel::setOwnState(const int state) {
160 239356550 : myPreviousState2 = myPreviousState;
161 239356550 : myOwnState = state;
162 239356550 : myPreviousState = state; // myOwnState is modified in prepareStep so we make a backup
163 239356550 : }
164 :
165 : void
166 0 : MSAbstractLaneChangeModel::updateSafeLatDist(const double travelledLatDist) {
167 : UNUSED_PARAMETER(travelledLatDist);
168 0 : }
169 :
170 :
171 : void
172 73898066 : MSAbstractLaneChangeModel::setManeuverDist(const double dist) {
173 : #ifdef DEBUG_MANEUVER
174 : if (DEBUG_COND) {
175 : std::cout << SIMTIME
176 : << " veh=" << myVehicle.getID()
177 : << " setManeuverDist() old=" << myManeuverDist << " new=" << dist
178 : << std::endl;
179 : }
180 : #endif
181 73898066 : myManeuverDist = fabs(dist) < NUMERICAL_EPS ? 0. : dist;
182 : // store value which may be modified by the model during the next step
183 73898066 : myPreviousManeuverDist = myManeuverDist;
184 73898066 : }
185 :
186 :
187 : double
188 683807967 : MSAbstractLaneChangeModel::getManeuverDist() const {
189 683807967 : return myManeuverDist;
190 : }
191 :
192 : double
193 13052830 : MSAbstractLaneChangeModel::getPreviousManeuverDist() const {
194 13052830 : return myPreviousManeuverDist;
195 : }
196 :
197 : void
198 31449241 : MSAbstractLaneChangeModel::saveNeighbors(const int dir, const MSLeaderDistanceInfo& followers, const MSLeaderDistanceInfo& leaders) {
199 31449241 : if (dir == -1) {
200 14185209 : myLeftFollowers = std::make_shared<MSLeaderDistanceInfo>(followers);
201 14185209 : myLeftLeaders = std::make_shared<MSLeaderDistanceInfo>(leaders);
202 17264032 : } else if (dir == 1) {
203 17264032 : myRightFollowers = std::make_shared<MSLeaderDistanceInfo>(followers);
204 17264032 : myRightLeaders = std::make_shared<MSLeaderDistanceInfo>(leaders);
205 : } else {
206 : // dir \in {-1,1} !
207 : assert(false);
208 : }
209 31449241 : }
210 :
211 :
212 : void
213 185828441 : MSAbstractLaneChangeModel::saveNeighbors(const int dir, const std::pair<MSVehicle* const, double>& follower, const std::pair<MSVehicle* const, double>& leader) {
214 185828441 : if (dir == -1) {
215 88252676 : myLeftFollowers = std::make_shared<MSLeaderDistanceInfo>(follower, myVehicle.getLane()->getWidth());
216 176505352 : myLeftLeaders = std::make_shared<MSLeaderDistanceInfo>(leader, myVehicle.getLane()->getWidth());
217 97575765 : } else if (dir == 1) {
218 97575765 : myRightFollowers = std::make_shared<MSLeaderDistanceInfo>(follower, myVehicle.getLane()->getWidth());
219 195151530 : myRightLeaders = std::make_shared<MSLeaderDistanceInfo>(leader, myVehicle.getLane()->getWidth());
220 : } else {
221 : // dir \in {-1,1} !
222 : assert(false);
223 : }
224 185828441 : }
225 :
226 :
227 : void
228 306553397 : MSAbstractLaneChangeModel::clearNeighbors() {
229 : myLeftFollowers = nullptr;
230 : myLeftLeaders = nullptr;
231 : myRightFollowers = nullptr;
232 : myRightLeaders = nullptr;
233 306553397 : }
234 :
235 :
236 : const std::shared_ptr<MSLeaderDistanceInfo>
237 0 : MSAbstractLaneChangeModel::getFollowers(const int dir) {
238 0 : if (dir == -1) {
239 : return myLeftFollowers;
240 0 : } else if (dir == 1) {
241 : return myRightFollowers;
242 : } else {
243 : // dir \in {-1,1} !
244 : assert(false);
245 : }
246 : return nullptr;
247 : }
248 :
249 : const std::shared_ptr<MSLeaderDistanceInfo>
250 0 : MSAbstractLaneChangeModel::getLeaders(const int dir) {
251 0 : if (dir == -1) {
252 : return myLeftLeaders;
253 0 : } else if (dir == 1) {
254 : return myRightLeaders;
255 : } else {
256 : // dir \in {-1,1} !
257 : assert(false);
258 : }
259 : return nullptr;
260 : }
261 :
262 :
263 : bool
264 109 : MSAbstractLaneChangeModel::congested(const MSVehicle* const neighLeader) {
265 109 : if (neighLeader == nullptr) {
266 : return false;
267 : }
268 : // Congested situation are relevant only on highways (maxSpeed > 70km/h)
269 : // and congested on German Highways means that the vehicles have speeds
270 : // below 60km/h. Overtaking on the right is allowed then.
271 0 : if ((myVehicle.getLane()->getSpeedLimit() <= 70.0 / 3.6) || (neighLeader->getLane()->getSpeedLimit() <= 70.0 / 3.6)) {
272 :
273 0 : return false;
274 : }
275 0 : if (myVehicle.congested() && neighLeader->congested()) {
276 : return true;
277 : }
278 : return false;
279 : }
280 :
281 :
282 : bool
283 231921428 : MSAbstractLaneChangeModel::avoidOvertakeRight() const {
284 231921428 : return (!myAllowOvertakingRight
285 231915977 : && !myVehicle.congested()
286 35266229 : && myVehicle.getVehicleType().getVehicleClass() != SVC_EMERGENCY
287 267185616 : && (myOvertakeRightParam == 0 || myOvertakeRightParam < RandHelper::rand(myVehicle.getRNG())));
288 : }
289 :
290 : bool
291 109 : MSAbstractLaneChangeModel::predInteraction(const std::pair<MSVehicle*, double>& leader) {
292 109 : if (leader.first == 0) {
293 : return false;
294 : }
295 : // let's check it on highways only
296 0 : if (leader.first->getSpeed() < (80.0 / 3.6)) {
297 : return false;
298 : }
299 0 : return leader.second < getCarFollowModel().interactionGap(&myVehicle, leader.first->getSpeed());
300 : }
301 :
302 :
303 : bool
304 1009005 : MSAbstractLaneChangeModel::startLaneChangeManeuver(MSLane* source, MSLane* target, int direction) {
305 1009005 : if (MSGlobals::gLaneChangeDuration > DELTA_T) {
306 36365 : myLaneChangeCompletion = 0;
307 36365 : myLaneChangeDirection = direction;
308 36365 : setManeuverDist((target->getWidth() + source->getWidth()) * 0.5 * direction);
309 36365 : myVehicle.switchOffSignal(MSVehicle::VEH_SIGNAL_BLINKER_RIGHT | MSVehicle::VEH_SIGNAL_BLINKER_LEFT);
310 36365 : myVehicle.switchOnSignal(((direction == 1) != MSGlobals::gLefthand) ? MSVehicle::VEH_SIGNAL_BLINKER_LEFT : MSVehicle::VEH_SIGNAL_BLINKER_RIGHT);
311 36365 : if (myLCOutput) {
312 650 : memorizeGapsAtLCInit();
313 : }
314 36365 : return true;
315 : } else {
316 972640 : primaryLaneChanged(source, target, direction);
317 972640 : return false;
318 : }
319 : }
320 :
321 : void
322 650 : MSAbstractLaneChangeModel::memorizeGapsAtLCInit() {
323 650 : myDontResetLCGaps = true;
324 650 : }
325 :
326 : void
327 649 : MSAbstractLaneChangeModel::clearGapsAtLCInit() {
328 649 : myDontResetLCGaps = false;
329 649 : }
330 :
331 : void
332 1008789 : MSAbstractLaneChangeModel::primaryLaneChanged(MSLane* source, MSLane* target, int direction) {
333 1008789 : initLastLaneChangeOffset(direction);
334 1008789 : myVehicle.leaveLane(MSMoveReminder::NOTIFICATION_LANE_CHANGE, target);
335 1008789 : source->leftByLaneChange(&myVehicle);
336 2017578 : laneChangeOutput("change", source, target, direction); // record position on the source edge in case of opposite change
337 1008789 : if (&source->getEdge() != &target->getEdge()) {
338 42619 : changedToOpposite();
339 : #ifdef DEBUG_OPPOSITE
340 : if (debugVehicle()) {
341 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " primaryLaneChanged source=" << source->getID() << " target=" << target->getID() << " nowOpposite=" << myAmOpposite << "\n";
342 : }
343 : #endif
344 42619 : myVehicle.setTentativeLaneAndPosition(target, source->getOppositePos(myVehicle.getPositionOnLane()), -myVehicle.getLateralPositionOnLane());
345 42619 : target->forceVehicleInsertion(&myVehicle, myVehicle.getPositionOnLane(), MSMoveReminder::NOTIFICATION_LANE_CHANGE, myVehicle.getLateralPositionOnLane());
346 966170 : } else if (myAmOpposite) {
347 : #ifdef DEBUG_OPPOSITE
348 : if (debugVehicle()) {
349 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " primaryLaneChanged source=" << source->getID() << " target=" << target->getID() << " stayOpposite\n";
350 : }
351 : #endif
352 230 : myAlreadyChanged = true;
353 230 : myVehicle.setTentativeLaneAndPosition(target, myVehicle.getPositionOnLane(), myVehicle.getLateralPositionOnLane());
354 230 : if (!MSGlobals::gSublane) {
355 : // in the continous case, the vehicle is added to the target lane via MSLaneChanger::continueChange / registerHop
356 : // in the sublane case, the vehicle is added to the target lane via MSLaneChangerSublane::checkChangeOppositeSublane / MSLane::myTmpVehicles
357 80 : target->forceVehicleInsertion(&myVehicle, myVehicle.getPositionOnLane(), MSMoveReminder::NOTIFICATION_LANE_CHANGE, myVehicle.getLateralPositionOnLane());
358 : }
359 : } else {
360 965940 : myVehicle.enterLaneAtLaneChange(target);
361 965940 : target->enteredByLaneChange(&myVehicle);
362 : }
363 : // Assure that the drive items are up to date (even if the following step is no actionstep for the vehicle).
364 : // This is necessary because the lane advance uses the target lane from the corresponding drive item.
365 1008789 : myVehicle.updateDriveItems();
366 1008789 : changed();
367 1008789 : }
368 :
369 : void
370 1009073 : MSAbstractLaneChangeModel::laneChangeOutput(const std::string& tag, MSLane* source, MSLane* target, int direction, double maneuverDist) {
371 1009073 : if (myLCOutput) {
372 14839 : OutputDevice& of = OutputDevice::getDeviceByOption("lanechange-output");
373 14839 : of.openTag(tag);
374 14839 : of.writeAttr(SUMO_ATTR_ID, myVehicle.getID());
375 14839 : of.writeAttr(SUMO_ATTR_TYPE, myVehicle.getVehicleType().getID());
376 29678 : of.writeAttr(SUMO_ATTR_TIME, time2string(MSNet::getInstance()->getCurrentTimeStep()));
377 : of.writeAttr(SUMO_ATTR_FROM, source->getID());
378 : of.writeAttr(SUMO_ATTR_TO, target->getID());
379 : of.writeAttr(SUMO_ATTR_DIR, direction);
380 14839 : of.writeAttr(SUMO_ATTR_SPEED, myVehicle.getSpeed());
381 14839 : of.writeAttr(SUMO_ATTR_POSITION, myVehicle.getPositionOnLane());
382 59356 : of.writeAttr("reason", toString((LaneChangeAction)(myOwnState & ~(
383 : LCA_RIGHT | LCA_LEFT
384 : | LCA_AMBLOCKINGLEADER | LCA_AMBLOCKINGFOLLOWER
385 : | LCA_MRIGHT | LCA_MLEFT
386 29678 : | LCA_AMBACKBLOCKER | LCA_AMBACKBLOCKER_STANDING))) + myVehicle.getParameter().getParameter("lcReason"));
387 29678 : of.writeAttr("leaderGap", myLastLeaderGap == NO_NEIGHBOR ? "None" : toString(myLastLeaderGap));
388 29678 : of.writeAttr("leaderSecureGap", myLastLeaderSecureGap == NO_NEIGHBOR ? "None" : toString(myLastLeaderSecureGap));
389 29678 : of.writeAttr("leaderSpeed", myLastLeaderSpeed == NO_NEIGHBOR ? "None" : toString(myLastLeaderSpeed));
390 29678 : of.writeAttr("followerGap", myLastFollowerGap == NO_NEIGHBOR ? "None" : toString(myLastFollowerGap));
391 29678 : of.writeAttr("followerSecureGap", myLastFollowerSecureGap == NO_NEIGHBOR ? "None" : toString(myLastFollowerSecureGap));
392 29678 : of.writeAttr("followerSpeed", myLastFollowerSpeed == NO_NEIGHBOR ? "None" : toString(myLastFollowerSpeed));
393 29678 : of.writeAttr("origLeaderGap", myLastOrigLeaderGap == NO_NEIGHBOR ? "None" : toString(myLastOrigLeaderGap));
394 29678 : of.writeAttr("origLeaderSecureGap", myLastOrigLeaderSecureGap == NO_NEIGHBOR ? "None" : toString(myLastOrigLeaderSecureGap));
395 29678 : of.writeAttr("origLeaderSpeed", myLastOrigLeaderSpeed == NO_NEIGHBOR ? "None" : toString(myLastOrigLeaderSpeed));
396 14839 : if (MSGlobals::gLateralResolution > 0) {
397 6533 : const double latGap = direction < 0 ? myLastLateralGapRight : myLastLateralGapLeft;
398 13066 : of.writeAttr("latGap", latGap == NO_NEIGHBOR ? "None" : toString(latGap));
399 6533 : if (maneuverDist != 0) {
400 408 : of.writeAttr("maneuverDistance", toString(maneuverDist));
401 : }
402 : }
403 14839 : if (myLCXYOutput) {
404 62 : of.writeAttr(SUMO_ATTR_X, myVehicle.getPosition().x());
405 124 : of.writeAttr(SUMO_ATTR_Y, myVehicle.getPosition().y());
406 : }
407 14839 : of.closeTag();
408 14839 : if (MSGlobals::gLaneChangeDuration > DELTA_T) {
409 649 : clearGapsAtLCInit();
410 : }
411 : }
412 1009073 : }
413 :
414 :
415 : double
416 612632 : MSAbstractLaneChangeModel::computeSpeedLat(double /*latDist*/, double& maneuverDist, bool /*urgent*/) const {
417 612632 : if (myVehicle.getVehicleType().wasSet(VTYPEPARS_MAXSPEED_LAT_SET)) {
418 686 : int stepsToChange = (int)ceil(fabs(maneuverDist) / SPEED2DIST(myVehicle.getVehicleType().getMaxSpeedLat()));
419 686 : return DIST2SPEED(maneuverDist / stepsToChange);
420 : } else {
421 611946 : return maneuverDist / STEPS2TIME(MSGlobals::gLaneChangeDuration);
422 : }
423 : }
424 :
425 :
426 : double
427 93257 : MSAbstractLaneChangeModel::getAssumedDecelForLaneChangeDuration() const {
428 93257 : return MAX2(LC_ASSUMED_DECEL, -myVehicle.getAcceleration());
429 : }
430 :
431 : void
432 76935042 : MSAbstractLaneChangeModel::setSpeedLat(double speedLat) {
433 76935042 : myAccelerationLat = SPEED2ACCEL(speedLat - mySpeedLat);
434 76935042 : mySpeedLat = speedLat;
435 76935042 : }
436 :
437 :
438 : void
439 541172315 : MSAbstractLaneChangeModel::resetSpeedLat() {
440 541172315 : if (MSGlobals::gLaneChangeDuration > 0 && !isChangingLanes()) {
441 2396277 : setSpeedLat(0);
442 : }
443 541172315 : }
444 :
445 :
446 : bool
447 1131742 : MSAbstractLaneChangeModel::updateCompletion() {
448 : const bool pastBefore = pastMidpoint();
449 : // maneuverDist is not updated in the context of continuous lane changing but represents the full LC distance
450 565871 : double maneuverDist = getManeuverDist();
451 565871 : setSpeedLat(computeSpeedLat(0, maneuverDist, (myOwnState & LCA_URGENT) != 0));
452 565871 : myLaneChangeCompletion += (SPEED2DIST(mySpeedLat) / myManeuverDist);
453 565871 : return !pastBefore && pastMidpoint();
454 : }
455 :
456 :
457 : void
458 55082 : MSAbstractLaneChangeModel::endLaneChangeManeuver(const MSMoveReminder::Notification reason) {
459 : UNUSED_PARAMETER(reason);
460 55082 : myLaneChangeCompletion = 1;
461 55082 : cleanupShadowLane();
462 55082 : cleanupTargetLane();
463 : myNoPartiallyOccupatedByShadow.clear();
464 55082 : myVehicle.switchOffSignal(MSVehicle::VEH_SIGNAL_BLINKER_RIGHT | MSVehicle::VEH_SIGNAL_BLINKER_LEFT);
465 55082 : myVehicle.fixPosition();
466 55082 : if (myAmOpposite && reason != MSMoveReminder::NOTIFICATION_LANE_CHANGE) {
467 148 : if (reason == MSMoveReminder::NOTIFICATION_PARKING && myVehicle.getNextStop().isOpposite) {
468 : // opposite driving continues after parking
469 : } else {
470 : // aborted maneuver
471 : #ifdef DEBUG_OPPOSITE
472 : if (debugVehicle()) {
473 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " aborted maneuver (no longer opposite)\n";
474 : }
475 : #endif
476 123 : changedToOpposite();
477 : }
478 : }
479 55082 : }
480 :
481 :
482 : MSLane*
483 15440931 : MSAbstractLaneChangeModel::getShadowLane(const MSLane* lane, double posLat) const {
484 15440931 : if (std::find(myNoPartiallyOccupatedByShadow.begin(), myNoPartiallyOccupatedByShadow.end(), lane) == myNoPartiallyOccupatedByShadow.end()) {
485 : // initialize shadow lane
486 15440931 : const double overlap = myVehicle.getLateralOverlap(posLat, lane);
487 : #ifdef DEBUG_SHADOWLANE
488 : if (debugVehicle()) {
489 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " posLat=" << posLat << " overlap=" << overlap << "\n";
490 : }
491 : #endif
492 15440931 : if (myAmOpposite) {
493 : // return the neigh-lane in forward direction
494 542218 : return lane->getParallelLane(1);
495 14898713 : } else if (overlap > NUMERICAL_EPS) {
496 8467130 : const int shadowDirection = posLat < 0 ? -1 : 1;
497 8467130 : return lane->getParallelLane(shadowDirection);
498 6431583 : } else if (isChangingLanes() && myLaneChangeCompletion < 0.5) {
499 : // "reserve" target lane even when there is no overlap yet
500 247973 : return lane->getParallelLane(myLaneChangeDirection);
501 : } else {
502 : return nullptr;
503 : }
504 : } else {
505 : return nullptr;
506 : }
507 : }
508 :
509 :
510 : MSLane*
511 15309144 : MSAbstractLaneChangeModel::getShadowLane(const MSLane* lane) const {
512 15309144 : return getShadowLane(lane, myVehicle.getLateralPositionOnLane());
513 : }
514 :
515 :
516 : void
517 4423195 : MSAbstractLaneChangeModel::cleanupShadowLane() {
518 4423195 : if (myShadowLane != nullptr) {
519 16015 : if (debugVehicle()) {
520 0 : std::cout << SIMTIME << " cleanupShadowLane\n";
521 : }
522 16015 : myShadowLane->resetPartialOccupation(&myVehicle);
523 16015 : myShadowLane = nullptr;
524 : }
525 4423600 : for (std::vector<MSLane*>::const_iterator it = myShadowFurtherLanes.begin(); it != myShadowFurtherLanes.end(); ++it) {
526 405 : if (debugVehicle()) {
527 0 : std::cout << SIMTIME << " cleanupShadowLane2\n";
528 : }
529 405 : (*it)->resetPartialOccupation(&myVehicle);
530 : }
531 : myShadowFurtherLanes.clear();
532 : myNoPartiallyOccupatedByShadow.clear();
533 4423195 : }
534 :
535 : void
536 4423195 : MSAbstractLaneChangeModel::cleanupTargetLane() {
537 4423195 : if (myTargetLane != nullptr) {
538 632 : if (debugVehicle()) {
539 0 : std::cout << SIMTIME << " cleanupTargetLane\n";
540 : }
541 632 : myTargetLane->resetManeuverReservation(&myVehicle);
542 632 : myTargetLane = nullptr;
543 : }
544 4423195 : for (std::vector<MSLane*>::const_iterator it = myFurtherTargetLanes.begin(); it != myFurtherTargetLanes.end(); ++it) {
545 0 : if (debugVehicle()) {
546 0 : std::cout << SIMTIME << " cleanupTargetLane\n";
547 : }
548 0 : if (*it != nullptr) {
549 0 : (*it)->resetManeuverReservation(&myVehicle);
550 : }
551 : }
552 : myFurtherTargetLanes.clear();
553 : // myNoPartiallyOccupatedByShadow.clear();
554 4423195 : }
555 :
556 :
557 : bool
558 25451420 : MSAbstractLaneChangeModel::cancelRequest(int state, int laneOffset) {
559 : // store request before canceling
560 25451420 : getCanceledState(laneOffset) |= state;
561 25451420 : int ret = myVehicle.influenceChangeDecision(state);
562 25451420 : return ret != state;
563 : }
564 :
565 : double
566 17619283 : MSAbstractLaneChangeModel::getMaxSpeedLat2() const {
567 17619283 : return MAX2(myVehicle.getVehicleType().getMaxSpeedLat(), myMaxSpeedLatStanding);
568 : }
569 :
570 : void
571 1008789 : MSAbstractLaneChangeModel::initLastLaneChangeOffset(int dir) {
572 1008789 : if (dir > 0) {
573 549221 : myLastLaneChangeOffset = 1;
574 459568 : } else if (dir < 0) {
575 459568 : myLastLaneChangeOffset = -1;
576 : }
577 1008789 : }
578 :
579 : void
580 5502123 : MSAbstractLaneChangeModel::updateShadowLane() {
581 5502123 : if (!MSGlobals::gSublane) {
582 : // assume each vehicle drives at the center of its lane and act as if it fits
583 498563 : return;
584 : }
585 5003560 : if (myShadowLane != nullptr) {
586 : #ifdef DEBUG_SHADOWLANE
587 : if (debugVehicle()) {
588 : std::cout << SIMTIME << " updateShadowLane()\n";
589 : }
590 : #endif
591 1755266 : myShadowLane->resetPartialOccupation(&myVehicle);
592 : }
593 5003560 : myShadowLane = getShadowLane(myVehicle.getLane());
594 : std::vector<MSLane*> passed;
595 5003560 : if (myShadowLane != nullptr) {
596 1771281 : myShadowLane->setPartialOccupation(&myVehicle);
597 1771281 : const std::vector<MSLane*>& further = myVehicle.getFurtherLanes();
598 1771281 : if (myAmOpposite) {
599 : assert(further.size() == 0);
600 : } else {
601 : const std::vector<double>& furtherPosLat = myVehicle.getFurtherLanesPosLat();
602 : assert(further.size() == furtherPosLat.size());
603 1552684 : passed.push_back(myShadowLane);
604 1684471 : for (int i = 0; i < (int)further.size(); ++i) {
605 131787 : MSLane* shadowFurther = getShadowLane(further[i], furtherPosLat[i]);
606 : #ifdef DEBUG_SHADOWLANE
607 : if (debugVehicle()) {
608 : std::cout << SIMTIME << " further=" << further[i]->getID() << " (posLat=" << furtherPosLat[i] << ") shadowFurther=" << Named::getIDSecure(shadowFurther) << "\n";
609 : }
610 : #endif
611 131787 : if (shadowFurther != nullptr && shadowFurther->getLinkTo(passed.back()) != nullptr) {
612 55561 : passed.push_back(shadowFurther);
613 : }
614 : }
615 : std::reverse(passed.begin(), passed.end());
616 : }
617 : } else {
618 3232279 : if (isChangingLanes() && myVehicle.getLateralOverlap() > NUMERICAL_EPS) {
619 4 : WRITE_WARNING("Vehicle '" + myVehicle.getID() + "' could not finish continuous lane change (lane disappeared) time=" +
620 : time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
621 1 : endLaneChangeManeuver();
622 : }
623 : }
624 : #ifdef DEBUG_SHADOWLANE
625 : if (debugVehicle()) {
626 : std::cout << SIMTIME << " updateShadowLane() veh=" << myVehicle.getID()
627 : << " newShadowLane=" << Named::getIDSecure(myShadowLane)
628 : << "\n before:" << " myShadowFurtherLanes=" << toString(myShadowFurtherLanes) << " further=" << toString(myVehicle.getFurtherLanes()) << " passed=" << toString(passed);
629 : std::cout << std::endl;
630 : }
631 : #endif
632 5003560 : myVehicle.updateFurtherLanes(myShadowFurtherLanes, myShadowFurtherLanesPosLat, passed);
633 : #ifdef DEBUG_SHADOWLANE
634 : if (debugVehicle()) std::cout
635 : << "\n after:" << " myShadowFurtherLanes=" << toString(myShadowFurtherLanes) << "\n";
636 : #endif
637 5003560 : }
638 :
639 :
640 : int
641 9141145 : MSAbstractLaneChangeModel::getShadowDirection() const {
642 9141145 : if (isChangingLanes()) {
643 634945 : if (pastMidpoint()) {
644 206108 : return -myLaneChangeDirection;
645 : } else {
646 428837 : return myLaneChangeDirection;
647 : }
648 8506200 : } else if (myShadowLane == nullptr) {
649 : return 0;
650 8506200 : } else if (myAmOpposite) {
651 : // return neigh-lane in forward direction
652 : return 1;
653 7983192 : } else if (&myShadowLane->getEdge() == &myVehicle.getLane()->getEdge()) {
654 7881102 : return myShadowLane->getIndex() - myVehicle.getLane()->getIndex();
655 : } else {
656 : // overlap with opposite direction lane
657 : return 1;
658 : }
659 : }
660 :
661 :
662 : MSLane*
663 78441794 : MSAbstractLaneChangeModel::updateTargetLane() {
664 : #ifdef DEBUG_TARGET_LANE
665 : MSLane* oldTarget = myTargetLane;
666 : std::vector<MSLane*> oldFurtherTargets = myFurtherTargetLanes;
667 : if (debugVehicle()) {
668 : std::cout << SIMTIME << " veh '" << myVehicle.getID() << "' (lane=" << myVehicle.getLane()->getID() << ") updateTargetLane()"
669 : << "\n oldTarget: " << (oldTarget == nullptr ? "NULL" : oldTarget->getID())
670 : << " oldFurtherTargets: " << toString(oldFurtherTargets);
671 : }
672 : #endif
673 78441794 : if (myTargetLane != nullptr) {
674 196216 : myTargetLane->resetManeuverReservation(&myVehicle);
675 : }
676 : // Clear old further target lanes
677 78452141 : for (MSLane* oldTargetLane : myFurtherTargetLanes) {
678 10347 : if (oldTargetLane != nullptr) {
679 8481 : oldTargetLane->resetManeuverReservation(&myVehicle);
680 : }
681 : }
682 : myFurtherTargetLanes.clear();
683 :
684 : // Get new target lanes and issue a maneuver reservation.
685 : int targetDir;
686 78441794 : myTargetLane = determineTargetLane(targetDir);
687 78441794 : if (myTargetLane != nullptr) {
688 196848 : myTargetLane->setManeuverReservation(&myVehicle);
689 : // further targets are just the target lanes corresponding to the vehicle's further lanes
690 : // @note In a neglectable amount of situations we might add a reservation for a shadow further lane.
691 207195 : for (MSLane* furtherLane : myVehicle.getFurtherLanes()) {
692 10347 : MSLane* furtherTargetLane = furtherLane->getParallelLane(targetDir);
693 10347 : myFurtherTargetLanes.push_back(furtherTargetLane);
694 10347 : if (furtherTargetLane != nullptr) {
695 8481 : furtherTargetLane->setManeuverReservation(&myVehicle);
696 : }
697 : }
698 : }
699 : #ifdef DEBUG_TARGET_LANE
700 : if (debugVehicle()) {
701 : std::cout << "\n newTarget (maneuverDist=" << myManeuverDist << " offset=" << targetDir << "): " << (myTargetLane == nullptr ? "NULL" : myTargetLane->getID())
702 : << " newFurtherTargets: " << toString(myFurtherTargetLanes)
703 : << std::endl;
704 : }
705 : #endif
706 78441794 : return myTargetLane;
707 : }
708 :
709 :
710 : MSLane*
711 78441794 : MSAbstractLaneChangeModel::determineTargetLane(int& targetDir) const {
712 78441794 : targetDir = 0;
713 78441794 : if (myManeuverDist == 0) {
714 : return nullptr;
715 : }
716 : // Current lateral boundaries of the vehicle
717 2209701 : const double vehRight = myVehicle.getLateralPositionOnLane() - 0.5 * myVehicle.getWidth();
718 2209701 : const double vehLeft = myVehicle.getLateralPositionOnLane() + 0.5 * myVehicle.getWidth();
719 2209701 : const double halfLaneWidth = 0.5 * myVehicle.getLane()->getWidth();
720 :
721 2209701 : if (vehRight + myManeuverDist < -halfLaneWidth) {
722 : // Vehicle intends to traverse the right lane boundary
723 291758 : targetDir = -1;
724 1917943 : } else if (vehLeft + myManeuverDist > halfLaneWidth) {
725 : // Vehicle intends to traverse the left lane boundary
726 462345 : targetDir = 1;
727 : }
728 2209701 : if (targetDir == 0) {
729 : // Presently, no maneuvering into another lane is begun.
730 : return nullptr;
731 : }
732 754103 : MSLane* target = myVehicle.getLane()->getParallelLane(targetDir);
733 754103 : if (target == nullptr || target == myShadowLane) {
734 : return nullptr;
735 : } else {
736 : return target;
737 : }
738 : }
739 :
740 :
741 :
742 : double
743 624570533 : MSAbstractLaneChangeModel::calcAngleOffset() {
744 : double result = 0.;
745 624570533 : if (!(fabs(mySpeedLat) < NUMERICAL_EPS && fabs(myPreviousAngleOffset * 180 / M_PI) < NUMERICAL_EPS)) {
746 17817705 : if (myVehicle.getLength() < sqrt(SPEED2DIST(mySpeedLat) * SPEED2DIST(mySpeedLat) + SPEED2DIST(myVehicle.getSpeed()) * SPEED2DIST(myVehicle.getSpeed()))) {
747 5362605 : result = atan2(mySpeedLat, myVehicle.getSpeed());
748 : } else {
749 12455100 : result = myPreviousAngleOffset + asin((sin(M_PI / 2 - myPreviousAngleOffset) * (SPEED2DIST(mySpeedLat) - tan(myPreviousAngleOffset) * SPEED2DIST(myVehicle.getSpeed()))) / myVehicle.getLength());
750 : }
751 : }
752 :
753 624570533 : myAngleOffset = result;
754 624570533 : return result;
755 : }
756 :
757 :
758 : double
759 93955 : MSAbstractLaneChangeModel::estimateLCDuration(const double speed, const double remainingManeuverDist, const double decel, bool urgent) const {
760 :
761 93955 : const SUMOVTypeParameter::SubParams& lcParams = myVehicle.getVehicleType().getParameter().getLCParams();
762 140745 : if (lcParams.find(SUMO_ATTR_LCA_MAXSPEEDLATSTANDING) == lcParams.end() && lcParams.find(SUMO_ATTR_LCA_MAXSPEEDLATFACTOR) == lcParams.end()) {
763 46790 : if (!myVehicle.getVehicleType().wasSet(VTYPEPARS_MAXSPEED_LAT_SET)) {
764 : // no dependency of lateral speed on longitudinal speed. (Only called prior to LC initialization to determine whether it could be completed)
765 46764 : return STEPS2TIME(MSGlobals::gLaneChangeDuration);
766 : } else {
767 26 : return remainingManeuverDist / myVehicle.getVehicleType().getMaxSpeedLat();
768 : }
769 : }
770 :
771 47165 : if (remainingManeuverDist == 0) {
772 : return 0;
773 : }
774 :
775 : // Check argument assumptions
776 : assert(speed >= 0);
777 : assert(remainingManeuverDist >= 0);
778 : assert(decel > 0);
779 : assert(myVehicle.getVehicleType().getMaxSpeedLat() > 0);
780 : assert(myMaxSpeedLatStanding <= myVehicle.getVehicleType().getMaxSpeedLat());
781 : assert(myMaxSpeedLatStanding >= 0);
782 :
783 : // for brevity
784 : const double v0 = speed;
785 : const double D = remainingManeuverDist;
786 : const double b = decel;
787 47165 : const double wmin = myMaxSpeedLatStanding;
788 47165 : const double f = myMaxSpeedLatFactor;
789 47165 : const double wmax = myVehicle.getVehicleType().getMaxSpeedLat();
790 :
791 : /* Here's the approach for the calculation of the required time for the LC:
792 : * To obtain the maximal LC-duration, for v(t) we assume that v(t)=max(0, v0-b*t),
793 : * Where v(t)=0 <=> t >= ts:=v0/b
794 : * For the lateral speed w(t) this gives:
795 : * w(t) = min(wmax, wmin + f*v(t))
796 : * The lateral distance covered until t is
797 : * d(t) = int_0^t w(s) ds
798 : * We distinguish three possibilities for the solution d(T)=D, where T is the time of the LC completion.
799 : * 1) w(T) = wmax, i.e. v(T)>(wmax-wmin)/f
800 : * 2) wmin < w(T) < wmax, i.e. (wmax-wmin)/f > v(T) > 0
801 : * 3) w(T) = wmin, i.e., v(T)=0
802 : */
803 47165 : const double vm = (wmax - wmin) / f;
804 : double distSoFar = 0.;
805 : double timeSoFar = 0.;
806 : double v = v0;
807 47165 : if (v > vm) {
808 1056 : const double wmaxTime = (v0 - vm) / b;
809 1056 : const double d1 = wmax * wmaxTime;
810 1056 : if (d1 >= D) {
811 392 : return D / wmax;
812 : } else {
813 664 : distSoFar += d1;
814 664 : timeSoFar += wmaxTime;
815 : v = vm;
816 : }
817 : }
818 46773 : if (v > 0) {
819 : /* Here, w(t1+t) = wmin + f*v(t1+t) = wmin + f*(v - b*t)
820 : * Thus, the additional lateral distance covered after time t is:
821 : * d2 = (wmin + f*v)*t - 0.5*f*b*t^2
822 : * and the additional lateral distance covered until v=0 at t=v/b is:
823 : * d2 = (wmin + 0.5*f*v)*t
824 : */
825 4560 : const double t = v / b; // stop time
826 4560 : const double d2 = (wmin + 0.5 * f * v) * t; // lateral distance covered until stop
827 : assert(d2 > 0);
828 4560 : if (distSoFar + d2 >= D) {
829 : // LC is completed during this phase
830 12 : const double x = 0.5 * f * b;
831 12 : const double y = wmin + f * v;
832 : /* Solve D - distSoFar = y*t - x*t^2.
833 : * 0 = x*t^2 - y*t/x + (D - distSoFar)/x
834 : */
835 12 : const double p = 0.5 * y / x;
836 12 : const double q = (D - distSoFar) / x;
837 : assert(p * p - q > 0);
838 12 : const double t2 = p + sqrt(p * p - q);
839 12 : return timeSoFar + t2;
840 : } else {
841 : distSoFar += d2;
842 4548 : timeSoFar += t;
843 : //v = 0;
844 : }
845 : }
846 : // If we didn't return yet this means the LC was not completed until the vehicle stops (if braking with rate b)
847 46761 : if (wmin == 0) {
848 : // LC won't be completed if vehicle stands
849 46761 : double maneuverDist = remainingManeuverDist;
850 46761 : const double vModel = computeSpeedLat(maneuverDist, maneuverDist, urgent);
851 46761 : double result = D / vModel;
852 : // make sure that the vehicle isn't braking to a stop during the manuever
853 46761 : if (vModel > SUMO_const_haltingSpeed && (vModel + myVehicle.getAcceleration() * result) > SUMO_const_haltingSpeed) {
854 : // unless the model tells us something different
855 : return result;
856 : } else {
857 46030 : return -1;
858 : }
859 : } else {
860 : // complete LC with lateral speed wmin
861 0 : return timeSoFar + (D - distSoFar) / wmin;
862 : }
863 : }
864 :
865 : SUMOTime
866 46393 : MSAbstractLaneChangeModel::remainingTime() const {
867 : assert(isChangingLanes()); // Only to be called during ongoing lane change
868 46393 : const SUMOVTypeParameter::SubParams& lcParams = myVehicle.getVehicleType().getParameter().getLCParams();
869 92437 : if (lcParams.find(SUMO_ATTR_LCA_MAXSPEEDLATSTANDING) == lcParams.end() && lcParams.find(SUMO_ATTR_LCA_MAXSPEEDLATFACTOR) == lcParams.end()) {
870 46044 : if (myVehicle.getVehicleType().wasSet(VTYPEPARS_MAXSPEED_LAT_SET)) {
871 0 : return TIME2STEPS((1. - myLaneChangeCompletion) * myManeuverDist / myVehicle.getVehicleType().getMaxSpeedLat());
872 : } else {
873 46044 : return (SUMOTime)((1. - myLaneChangeCompletion) * (double)MSGlobals::gLaneChangeDuration);
874 : }
875 : }
876 : // Using maxSpeedLat(Factor/Standing)
877 349 : const bool urgent = (myOwnState & LCA_URGENT) != 0;
878 394 : return TIME2STEPS(estimateLCDuration(myVehicle.getSpeed(),
879 : fabs(myManeuverDist * (1 - myLaneChangeCompletion)),
880 : myVehicle.getCarFollowModel().getMaxDecel(), urgent));
881 : }
882 :
883 :
884 : void
885 2283626 : MSAbstractLaneChangeModel::setShadowApproachingInformation(MSLink* link) const {
886 : //std::cout << SIMTIME << " veh=" << myVehicle.getID() << " @=" << &myVehicle << " set shadow approaching=" << link->getViaLaneOrLane()->getID() << "\n";
887 2283626 : myApproachedByShadow.push_back(link);
888 2283626 : }
889 :
890 : void
891 552912470 : MSAbstractLaneChangeModel::removeShadowApproachingInformation() const {
892 555196096 : for (std::vector<MSLink*>::iterator it = myApproachedByShadow.begin(); it != myApproachedByShadow.end(); ++it) {
893 : //std::cout << SIMTIME << " veh=" << myVehicle.getID() << " @=" << &myVehicle << " remove shadow approaching=" << (*it)->getViaLaneOrLane()->getID() << "\n";
894 2283626 : (*it)->removeApproaching(&myVehicle);
895 : }
896 : myApproachedByShadow.clear();
897 552912470 : }
898 :
899 :
900 :
901 : void
902 41739800 : MSAbstractLaneChangeModel::checkTraCICommands() {
903 41739800 : int newstate = myVehicle.influenceChangeDecision(myOwnState);
904 41739800 : int oldstate = myVehicle.getLaneChangeModel().getOwnState();
905 41739800 : if (myOwnState != newstate) {
906 20 : if (MSGlobals::gLateralResolution > 0.) {
907 : // Calculate and set the lateral maneuver distance corresponding to the change request
908 : // to induce a corresponding sublane change.
909 15 : const int dir = (newstate & LCA_RIGHT) != 0 ? -1 : ((newstate & LCA_LEFT) != 0 ? 1 : 0);
910 : // minimum distance to move the vehicle fully onto the lane at offset dir
911 15 : const double latLaneDist = myVehicle.lateralDistanceToLane(dir);
912 15 : if ((newstate & LCA_TRACI) != 0) {
913 15 : if ((newstate & LCA_STAY) != 0) {
914 0 : setManeuverDist(0.);
915 15 : } else if (((newstate & LCA_RIGHT) != 0 && dir < 0)
916 10 : || ((newstate & LCA_LEFT) != 0 && dir > 0)) {
917 5 : setManeuverDist(latLaneDist);
918 : }
919 : }
920 15 : if (myVehicle.hasInfluencer()) {
921 : // lane change requests override sublane change requests
922 15 : myVehicle.getInfluencer().resetLatDist();
923 : }
924 :
925 : }
926 20 : setOwnState(newstate);
927 : } else {
928 : // Check for sublane change requests
929 41739780 : if (myVehicle.hasInfluencer() && myVehicle.getInfluencer().getLatDist() != 0) {
930 380 : const double maneuverDist = myVehicle.getInfluencer().getLatDist();
931 380 : myVehicle.getLaneChangeModel().setManeuverDist(maneuverDist);
932 380 : myVehicle.getInfluencer().resetLatDist();
933 380 : newstate |= LCA_TRACI;
934 380 : if (myOwnState != newstate) {
935 5 : setOwnState(newstate);
936 : }
937 380 : if (gDebugFlag2) {
938 0 : std::cout << " traci influenced maneuverDist=" << maneuverDist << "\n";
939 : }
940 : }
941 : }
942 41739800 : if (gDebugFlag2) {
943 0 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " stateAfterTraCI=" << toString((LaneChangeAction)newstate) << " original=" << toString((LaneChangeAction)oldstate) << "\n";
944 : }
945 41739800 : }
946 :
947 : void
948 42771 : MSAbstractLaneChangeModel::changedToOpposite() {
949 42771 : myAmOpposite = !myAmOpposite;
950 42771 : myAlreadyChanged = true;
951 42771 : }
952 :
953 : void
954 666259 : MSAbstractLaneChangeModel::setFollowerGaps(CLeaderDist follower, double secGap) {
955 666259 : if (follower.first != 0) {
956 384152 : myLastFollowerGap = follower.second + follower.first->getVehicleType().getMinGap();
957 384152 : myLastFollowerSecureGap = secGap;
958 384152 : myLastFollowerSpeed = follower.first->getSpeed();
959 : }
960 666259 : }
961 :
962 : void
963 666259 : MSAbstractLaneChangeModel::setLeaderGaps(CLeaderDist leader, double secGap) {
964 666259 : if (leader.first != 0) {
965 475727 : myLastLeaderGap = leader.second + myVehicle.getVehicleType().getMinGap();
966 475727 : myLastLeaderSecureGap = secGap;
967 475727 : myLastLeaderSpeed = leader.first->getSpeed();
968 : }
969 666259 : }
970 :
971 : void
972 666259 : MSAbstractLaneChangeModel::setOrigLeaderGaps(CLeaderDist leader, double secGap) {
973 666259 : if (leader.first != 0) {
974 396042 : myLastOrigLeaderGap = leader.second + myVehicle.getVehicleType().getMinGap();
975 396042 : myLastOrigLeaderSecureGap = secGap;
976 396042 : myLastOrigLeaderSpeed = leader.first->getSpeed();
977 : }
978 666259 : }
979 :
980 : void
981 543064920 : MSAbstractLaneChangeModel::prepareStep() {
982 543064920 : getCanceledState(-1) = LCA_NONE;
983 543064920 : getCanceledState(0) = LCA_NONE;
984 543064920 : getCanceledState(1) = LCA_NONE;
985 : saveLCState(-1, LCA_UNKNOWN, LCA_UNKNOWN);
986 : saveLCState(0, LCA_UNKNOWN, LCA_UNKNOWN);
987 : saveLCState(1, LCA_UNKNOWN, LCA_UNKNOWN);
988 543064920 : myLastLateralGapRight = NO_NEIGHBOR;
989 543064920 : myLastLateralGapLeft = NO_NEIGHBOR;
990 543064920 : if (!myDontResetLCGaps) {
991 543060053 : myLastLeaderGap = NO_NEIGHBOR;
992 543060053 : myLastLeaderSecureGap = NO_NEIGHBOR;
993 543060053 : myLastFollowerGap = NO_NEIGHBOR;
994 543060053 : myLastFollowerSecureGap = NO_NEIGHBOR;
995 543060053 : myLastOrigLeaderGap = NO_NEIGHBOR;
996 543060053 : myLastOrigLeaderSecureGap = NO_NEIGHBOR;
997 543060053 : myLastLeaderSpeed = NO_NEIGHBOR;
998 543060053 : myLastFollowerSpeed = NO_NEIGHBOR;
999 543060053 : myLastOrigLeaderSpeed = NO_NEIGHBOR;
1000 : }
1001 543064920 : myCommittedSpeed = 0;
1002 543064920 : }
1003 :
1004 : void
1005 6533 : MSAbstractLaneChangeModel::setFollowerGaps(const MSLeaderDistanceInfo& vehicles) {
1006 : int rightmost;
1007 : int leftmost;
1008 6533 : vehicles.getSubLanes(&myVehicle, 0, rightmost, leftmost);
1009 27207 : for (int i = rightmost; i <= leftmost; ++i) {
1010 20674 : CLeaderDist vehDist = vehicles[i];
1011 20674 : if (vehDist.first != 0) {
1012 12731 : const MSVehicle* leader = &myVehicle;
1013 : const MSVehicle* follower = vehDist.first;
1014 12731 : const double netGap = vehDist.second + follower->getVehicleType().getMinGap();
1015 12731 : if (netGap < myLastFollowerGap && netGap >= 0) {
1016 5465 : myLastFollowerGap = netGap;
1017 5465 : myLastFollowerSecureGap = follower->getCarFollowModel().getSecureGap(follower, leader, follower->getSpeed(), leader->getSpeed(), leader->getCarFollowModel().getMaxDecel());
1018 5465 : myLastFollowerSpeed = follower->getSpeed();
1019 : }
1020 : }
1021 : }
1022 6533 : }
1023 :
1024 : void
1025 6533 : MSAbstractLaneChangeModel::setLeaderGaps(const MSLeaderDistanceInfo& vehicles) {
1026 : int rightmost;
1027 : int leftmost;
1028 6533 : vehicles.getSubLanes(&myVehicle, 0, rightmost, leftmost);
1029 27207 : for (int i = rightmost; i <= leftmost; ++i) {
1030 20674 : CLeaderDist vehDist = vehicles[i];
1031 20674 : if (vehDist.first != 0) {
1032 : const MSVehicle* leader = vehDist.first;
1033 15512 : const MSVehicle* follower = &myVehicle;
1034 15512 : const double netGap = vehDist.second + follower->getVehicleType().getMinGap();
1035 15512 : if (netGap < myLastLeaderGap && netGap >= 0) {
1036 5884 : myLastLeaderGap = netGap;
1037 5884 : myLastLeaderSecureGap = follower->getCarFollowModel().getSecureGap(follower, leader, follower->getSpeed(), leader->getSpeed(), leader->getCarFollowModel().getMaxDecel());
1038 5884 : myLastLeaderSpeed = leader->getSpeed();
1039 : }
1040 : }
1041 : }
1042 6533 : }
1043 :
1044 : void
1045 6533 : MSAbstractLaneChangeModel::setOrigLeaderGaps(const MSLeaderDistanceInfo& vehicles) {
1046 : int rightmost;
1047 : int leftmost;
1048 6533 : vehicles.getSubLanes(&myVehicle, 0, rightmost, leftmost);
1049 27102 : for (int i = rightmost; i <= leftmost; ++i) {
1050 20569 : CLeaderDist vehDist = vehicles[i];
1051 20569 : if (vehDist.first != 0) {
1052 : const MSVehicle* leader = vehDist.first;
1053 8880 : const MSVehicle* follower = &myVehicle;
1054 8880 : const double netGap = vehDist.second + follower->getVehicleType().getMinGap();
1055 8880 : if (netGap < myLastOrigLeaderGap && netGap >= 0) {
1056 3481 : myLastOrigLeaderGap = netGap;
1057 3481 : myLastOrigLeaderSecureGap = follower->getCarFollowModel().getSecureGap(follower, leader, follower->getSpeed(), leader->getSpeed(), leader->getCarFollowModel().getMaxDecel());
1058 3481 : myLastOrigLeaderSpeed = leader->getSpeed();
1059 : }
1060 : }
1061 : }
1062 6533 : }
1063 :
1064 :
1065 : bool
1066 22881063 : MSAbstractLaneChangeModel::isStrategicBlocked() const {
1067 22881063 : const int stateRight = mySavedStateRight.second;
1068 22881063 : if (
1069 : (stateRight & LCA_STRATEGIC) != 0
1070 : && (stateRight & LCA_RIGHT) != 0
1071 923069 : && (stateRight & LCA_BLOCKED) != 0) {
1072 : return true;
1073 : }
1074 22323345 : const int stateLeft = mySavedStateLeft.second;
1075 22323345 : if (
1076 : (stateLeft & LCA_STRATEGIC) != 0
1077 : && (stateLeft & LCA_LEFT) != 0
1078 68592 : && (stateLeft & LCA_BLOCKED) != 0) {
1079 13898 : return true;
1080 : }
1081 : return false;
1082 : }
1083 :
1084 : double
1085 218307675 : MSAbstractLaneChangeModel::getForwardPos() const {
1086 218307675 : return myAmOpposite ? myVehicle.getLane()->getLength() - myVehicle.getPositionOnLane() : myVehicle.getPositionOnLane();
1087 : }
1088 :
1089 :
1090 : int
1091 1426 : MSAbstractLaneChangeModel::getNormalizedLaneIndex() {
1092 1426 : const int i = myVehicle.getLane()->getIndex();
1093 1426 : if (myAmOpposite) {
1094 790 : return myVehicle.getLane()->getParallelOpposite()->getEdge().getNumLanes() + myVehicle.getLane()->getEdge().getNumLanes() - 1 - i;
1095 : } else {
1096 : return i;
1097 : }
1098 : }
1099 :
1100 : void
1101 40923226 : MSAbstractLaneChangeModel::addLCSpeedAdvice(const double vSafe, bool ownAdvice) {
1102 40923226 : const double accel = SPEED2ACCEL(vSafe - myVehicle.getSpeed());
1103 40923226 : myLCAccelerationAdvices.push_back({accel, ownAdvice});
1104 40923226 : }
1105 :
1106 :
1107 : void
1108 2158 : MSAbstractLaneChangeModel::saveState(OutputDevice& out) const {
1109 : std::vector<std::string> lcState;
1110 2158 : if (MSGlobals::gLaneChangeDuration > 0) {
1111 1 : lcState.push_back(toString(mySpeedLat));
1112 1 : lcState.push_back(toString(myLaneChangeCompletion));
1113 2 : lcState.push_back(toString(myLaneChangeDirection));
1114 : }
1115 2158 : if (lcState.size() > 0) {
1116 : out.writeAttr(SUMO_ATTR_LCSTATE, lcState);
1117 : }
1118 2158 : }
1119 :
1120 : void
1121 3867 : MSAbstractLaneChangeModel::loadState(const SUMOSAXAttributes& attrs) {
1122 3867 : if (attrs.hasAttribute(SUMO_ATTR_LCSTATE)) {
1123 1 : std::istringstream bis(attrs.getString(SUMO_ATTR_LCSTATE));
1124 1 : bis >> mySpeedLat;
1125 1 : bis >> myLaneChangeCompletion;
1126 1 : bis >> myLaneChangeDirection;
1127 1 : }
1128 3867 : }
|
