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