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