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