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