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 42501 : MSAbstractLaneChangeModel::initGlobalOptions(const OptionsCont& oc) {
74 42501 : myAllowOvertakingRight = oc.getBool("lanechange.overtake-right");
75 42501 : myLCOutput = oc.isSet("lanechange-output");
76 42501 : myLCStartedOutput = oc.getBool("lanechange-output.started");
77 42501 : myLCEndedOutput = oc.getBool("lanechange-output.ended");
78 42501 : myLCXYOutput = oc.getBool("lanechange-output.xy");
79 42501 : }
80 :
81 :
82 : MSAbstractLaneChangeModel*
83 4504610 : MSAbstractLaneChangeModel::build(LaneChangeModel lcm, MSVehicle& v) {
84 4504610 : 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 4504588 : 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 4502716 : case LaneChangeModel::DEFAULT:
97 4502716 : if (MSGlobals::gLateralResolution <= 0) {
98 3670671 : return new MSLCM_LC2013(v);
99 : } else {
100 832045 : 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 4504588 : MSAbstractLaneChangeModel::MSAbstractLaneChangeModel(MSVehicle& v, const LaneChangeModel model) :
109 4504588 : myVehicle(v),
110 4504588 : myOwnState(0),
111 4504588 : myPreviousState(0),
112 4504588 : myPreviousState2(0),
113 4504588 : myCanceledStateRight(LCA_NONE),
114 4504588 : myCanceledStateCenter(LCA_NONE),
115 4504588 : myCanceledStateLeft(LCA_NONE),
116 4504588 : mySpeedLat(0),
117 4504588 : myAccelerationLat(0),
118 4504588 : myAngleOffset(0),
119 4504588 : myPreviousAngleOffset(0),
120 4504588 : myCommittedSpeed(0),
121 4504588 : myLaneChangeCompletion(1.0),
122 4504588 : myLaneChangeDirection(0),
123 4504588 : myAlreadyChanged(false),
124 4504588 : myShadowLane(nullptr),
125 4504588 : myTargetLane(nullptr),
126 4504588 : myModel(model),
127 4504588 : myLastLateralGapLeft(0.),
128 4504588 : myLastLateralGapRight(0.),
129 4504588 : myLastLeaderGap(0.),
130 4504588 : myLastFollowerGap(0.),
131 4504588 : myLastLeaderSecureGap(0.),
132 4504588 : myLastFollowerSecureGap(0.),
133 4504588 : myLastOrigLeaderGap(0.),
134 4504588 : myLastOrigLeaderSecureGap(0.),
135 4504588 : myLastLeaderSpeed(0),
136 4504588 : myLastFollowerSpeed(0),
137 4504588 : myLastOrigLeaderSpeed(0),
138 4504588 : myDontResetLCGaps(false),
139 4504588 : myStrategicLookahead(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_STRATEGIC_LOOKAHEAD, UNDEFINED_LOOKAHEAD)),
140 4504588 : myMaxSpeedLatStanding(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_MAXSPEEDLATSTANDING, v.getVehicleType().getMaxSpeedLat())),
141 4504588 : myMaxSpeedLatFactor(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_MAXSPEEDLATFACTOR, 1)),
142 9009176 : 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 4504588 : (v.getVClass() & (SVC_BICYCLE | SVC_MOTORCYCLE | SVC_MOPED)) != 0 ? std::numeric_limits<double>::max() : 1.6)),
145 4504588 : mySigma(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SIGMA, 0.0)),
146 4504588 : myOvertakeRightParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_OVERTAKE_RIGHT, 0)),
147 4504588 : myAssertive(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_ASSERTIVE, 1)),
148 4504588 : myCooperativeHelpTime(TIME2STEPS(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_COOPERATIVE_HELPTIME, 60))),
149 4504588 : myHaveBlueLight(v.getDevice(typeid(MSDevice_Bluelight)) != nullptr), // see MSVehicle::initDevices
150 4504588 : myLastLaneChangeOffset(0),
151 4504588 : myAmOpposite(false),
152 4504588 : myManeuverDist(0.),
153 9009176 : myPreviousManeuverDist(0.) {
154 : saveLCState(-1, LCA_UNKNOWN, LCA_UNKNOWN);
155 : saveLCState(0, LCA_UNKNOWN, LCA_UNKNOWN);
156 : saveLCState(1, LCA_UNKNOWN, LCA_UNKNOWN);
157 4504588 : }
158 :
159 :
160 4504512 : MSAbstractLaneChangeModel::~MSAbstractLaneChangeModel() {
161 4504512 : }
162 :
163 : void
164 294191315 : MSAbstractLaneChangeModel::setOwnState(const int state) {
165 294191315 : myPreviousState2 = myPreviousState;
166 294191315 : myOwnState = state;
167 294191315 : myPreviousState = state; // myOwnState is modified in prepareStep so we make a backup
168 294191315 : }
169 :
170 : void
171 0 : MSAbstractLaneChangeModel::updateSafeLatDist(const double travelledLatDist) {
172 : UNUSED_PARAMETER(travelledLatDist);
173 0 : }
174 :
175 :
176 : void
177 82222225 : 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 82222225 : myManeuverDist = fabs(dist) < NUMERICAL_EPS ? 0. : dist;
187 : // store value which may be modified by the model during the next step
188 82222225 : myPreviousManeuverDist = myManeuverDist;
189 82222225 : }
190 :
191 :
192 : double
193 701281940 : MSAbstractLaneChangeModel::getManeuverDist() const {
194 701281940 : return myManeuverDist;
195 : }
196 :
197 : double
198 14462160 : MSAbstractLaneChangeModel::getPreviousManeuverDist() const {
199 14462160 : return myPreviousManeuverDist;
200 : }
201 :
202 : void
203 36492672 : MSAbstractLaneChangeModel::saveNeighbors(const int dir, const MSLeaderDistanceInfo& followers, const MSLeaderDistanceInfo& leaders) {
204 36492672 : if (dir == -1) {
205 16839572 : myLeftFollowers = std::make_shared<MSLeaderDistanceInfo>(followers);
206 16839572 : myLeftLeaders = std::make_shared<MSLeaderDistanceInfo>(leaders);
207 19653100 : } else if (dir == 1) {
208 19653100 : myRightFollowers = std::make_shared<MSLeaderDistanceInfo>(followers);
209 19653100 : myRightLeaders = std::make_shared<MSLeaderDistanceInfo>(leaders);
210 : } else {
211 : // dir \in {-1,1} !
212 : assert(false);
213 : }
214 36492672 : }
215 :
216 :
217 : void
218 242578570 : MSAbstractLaneChangeModel::saveNeighbors(const int dir, const std::pair<MSVehicle* const, double>& follower, const std::pair<MSVehicle* const, double>& leader) {
219 242578570 : if (dir == -1) {
220 116199384 : myLeftFollowers = std::make_shared<MSLeaderDistanceInfo>(follower, myVehicle.getLane()->getWidth());
221 232398768 : myLeftLeaders = std::make_shared<MSLeaderDistanceInfo>(leader, myVehicle.getLane()->getWidth());
222 126379186 : } else if (dir == 1) {
223 126379186 : myRightFollowers = std::make_shared<MSLeaderDistanceInfo>(follower, myVehicle.getLane()->getWidth());
224 252758372 : myRightLeaders = std::make_shared<MSLeaderDistanceInfo>(leader, myVehicle.getLane()->getWidth());
225 : } else {
226 : // dir \in {-1,1} !
227 : assert(false);
228 : }
229 242578570 : }
230 :
231 :
232 : void
233 369406731 : MSAbstractLaneChangeModel::clearNeighbors() {
234 : myLeftFollowers = nullptr;
235 : myLeftLeaders = nullptr;
236 : myRightFollowers = nullptr;
237 : myRightLeaders = nullptr;
238 369406731 : }
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 299235314 : MSAbstractLaneChangeModel::avoidOvertakeRight(const MSVehicle* const neighLeader, const bool allowProb) const {
289 299235314 : return (!myAllowOvertakingRight // the highway case
290 299229863 : && !myVehicle.congested()
291 39258121 : && myVehicle.getVehicleType().getVehicleClass() != SVC_EMERGENCY
292 598465177 : && (!allowProb || myOvertakeRightParam == 0 || myOvertakeRightParam < RandHelper::rand(myVehicle.getRNG()))) ||
293 186783837 : (neighLeader != nullptr && neighLeader->isStopped() // the bus stop case
294 342056 : && neighLeader->getStops().front().busstop != nullptr
295 299266701 : && !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 1136370 : MSAbstractLaneChangeModel::startLaneChangeManeuver(MSLane* source, MSLane* target, int direction) {
313 1136370 : if (MSGlobals::gLaneChangeDuration > DELTA_T) {
314 43070 : myLaneChangeCompletion = 0;
315 43070 : myLaneChangeDirection = direction;
316 43070 : setManeuverDist((target->getWidth() + source->getWidth()) * 0.5 * direction);
317 43070 : myVehicle.switchOffSignal(MSVehicle::VEH_SIGNAL_BLINKER_RIGHT | MSVehicle::VEH_SIGNAL_BLINKER_LEFT);
318 43070 : myVehicle.switchOnSignal(((direction == 1) != MSGlobals::gLefthand) ? MSVehicle::VEH_SIGNAL_BLINKER_LEFT : MSVehicle::VEH_SIGNAL_BLINKER_RIGHT);
319 43070 : if (myLCOutput) {
320 830 : memorizeGapsAtLCInit();
321 : }
322 43070 : return true;
323 : } else {
324 1093300 : primaryLaneChanged(source, target, direction);
325 1093300 : 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 1136082 : MSAbstractLaneChangeModel::primaryLaneChanged(MSLane* source, MSLane* target, int direction) {
341 1136082 : initLastLaneChangeOffset(direction);
342 1136082 : myVehicle.leaveLane(MSMoveReminder::NOTIFICATION_LANE_CHANGE, target);
343 1136082 : source->leftByLaneChange(&myVehicle);
344 2272164 : laneChangeOutput("change", source, target, direction); // record position on the source edge in case of opposite change
345 1136082 : if (&source->getEdge() != &target->getEdge()) {
346 44140 : 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 44140 : myVehicle.setTentativeLaneAndPosition(target, source->getOppositePos(myVehicle.getPositionOnLane()), -myVehicle.getLateralPositionOnLane());
353 44140 : target->forceVehicleInsertion(&myVehicle, myVehicle.getPositionOnLane(), MSMoveReminder::NOTIFICATION_LANE_CHANGE, myVehicle.getLateralPositionOnLane());
354 1091942 : } 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 1091712 : myVehicle.enterLaneAtLaneChange(target);
369 1091712 : 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 1136082 : myVehicle.updateDriveItems();
374 1136082 : changed();
375 1136082 : }
376 :
377 : void
378 1136344 : MSAbstractLaneChangeModel::laneChangeOutput(const std::string& tag, MSLane* source, MSLane* target, int direction, double maneuverDist) {
379 1136344 : if (myLCOutput) {
380 17127 : OutputDevice& of = OutputDevice::getDeviceByOption("lanechange-output");
381 17127 : of.openTag(tag);
382 17127 : of.writeAttr(SUMO_ATTR_ID, myVehicle.getID());
383 17127 : of.writeAttr(SUMO_ATTR_TYPE, myVehicle.getVehicleType().getID());
384 17127 : of.writeAttr(SUMO_ATTR_TIME, time2string(MSNet::getInstance()->getCurrentTimeStep()));
385 17127 : of.writeAttr(SUMO_ATTR_FROM, source->getID());
386 17127 : of.writeAttr(SUMO_ATTR_TO, target->getID());
387 17127 : of.writeAttr(SUMO_ATTR_DIR, direction);
388 17127 : of.writeAttr(SUMO_ATTR_SPEED, myVehicle.getSpeed());
389 17127 : of.writeAttr(SUMO_ATTR_POSITION, myVehicle.getPositionOnLane());
390 51381 : of.writeAttr("reason", toString((LaneChangeAction)(myOwnState & ~(
391 : LCA_RIGHT | LCA_LEFT
392 : | LCA_AMBLOCKINGLEADER | LCA_AMBLOCKINGFOLLOWER
393 : | LCA_MRIGHT | LCA_MLEFT
394 34254 : | LCA_AMBACKBLOCKER | LCA_AMBACKBLOCKER_STANDING))) + myVehicle.getParameter().getParameter("lcReason"));
395 17127 : of.writeAttr("leaderGap", myLastLeaderGap == NO_NEIGHBOR ? "None" : toString(myLastLeaderGap), myLastLeaderGap == NO_NEIGHBOR);
396 17127 : of.writeAttr("leaderSecureGap", myLastLeaderSecureGap == NO_NEIGHBOR ? "None" : toString(myLastLeaderSecureGap), myLastLeaderSecureGap == NO_NEIGHBOR);
397 17127 : of.writeAttr("leaderSpeed", myLastLeaderSpeed == NO_NEIGHBOR ? "None" : toString(myLastLeaderSpeed), myLastLeaderSpeed == NO_NEIGHBOR);
398 17127 : of.writeAttr("followerGap", myLastFollowerGap == NO_NEIGHBOR ? "None" : toString(myLastFollowerGap), myLastFollowerGap == NO_NEIGHBOR);
399 17127 : of.writeAttr("followerSecureGap", myLastFollowerSecureGap == NO_NEIGHBOR ? "None" : toString(myLastFollowerSecureGap), myLastFollowerSecureGap == NO_NEIGHBOR);
400 17127 : of.writeAttr("followerSpeed", myLastFollowerSpeed == NO_NEIGHBOR ? "None" : toString(myLastFollowerSpeed), myLastFollowerSpeed == NO_NEIGHBOR);
401 17127 : of.writeAttr("origLeaderGap", myLastOrigLeaderGap == NO_NEIGHBOR ? "None" : toString(myLastOrigLeaderGap), myLastOrigLeaderGap == NO_NEIGHBOR);
402 17127 : of.writeAttr("origLeaderSecureGap", myLastOrigLeaderSecureGap == NO_NEIGHBOR ? "None" : toString(myLastOrigLeaderSecureGap), myLastOrigLeaderSecureGap == NO_NEIGHBOR);
403 17127 : of.writeAttr("origLeaderSpeed", myLastOrigLeaderSpeed == NO_NEIGHBOR ? "None" : toString(myLastOrigLeaderSpeed), myLastOrigLeaderSpeed == NO_NEIGHBOR);
404 17127 : if (MSGlobals::gLateralResolution > 0) {
405 7859 : const double latGap = direction < 0 ? myLastLateralGapRight : myLastLateralGapLeft;
406 7859 : of.writeAttr("latGap", latGap == NO_NEIGHBOR ? "None" : toString(latGap), latGap == NO_NEIGHBOR);
407 7859 : if (maneuverDist != 0) {
408 372 : of.writeAttr("maneuverDistance", toString(maneuverDist));
409 : }
410 : }
411 17127 : if (myLCXYOutput) {
412 62 : of.writeAttr(SUMO_ATTR_X, myVehicle.getPosition().x());
413 62 : of.writeAttr(SUMO_ATTR_Y, myVehicle.getPosition().y());
414 : }
415 17127 : of.closeTag();
416 17127 : if (MSGlobals::gLaneChangeDuration > DELTA_T) {
417 829 : clearGapsAtLCInit();
418 : }
419 : }
420 1136344 : }
421 :
422 :
423 : double
424 619955 : MSAbstractLaneChangeModel::computeSpeedLat(double /*latDist*/, double& maneuverDist, bool /*urgent*/) const {
425 619955 : 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 619269 : return maneuverDist / STEPS2TIME(MSGlobals::gLaneChangeDuration);
430 : }
431 : }
432 :
433 :
434 : double
435 80833 : MSAbstractLaneChangeModel::getAssumedDecelForLaneChangeDuration() const {
436 80833 : return MAX2(LC_ASSUMED_DECEL, -myVehicle.getAcceleration());
437 : }
438 :
439 : void
440 85443799 : MSAbstractLaneChangeModel::setSpeedLat(double speedLat) {
441 85443799 : myAccelerationLat = SPEED2ACCEL(speedLat - mySpeedLat);
442 85443799 : mySpeedLat = speedLat;
443 85443799 : }
444 :
445 :
446 : void
447 614771272 : MSAbstractLaneChangeModel::resetSpeedLat() {
448 614771272 : if (MSGlobals::gLaneChangeDuration > 0 && !isChangingLanes()) {
449 2562943 : setSpeedLat(0);
450 : }
451 614771272 : }
452 :
453 :
454 : bool
455 593482 : 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 593482 : double maneuverDist = getManeuverDist();
459 593482 : setSpeedLat(computeSpeedLat(0, maneuverDist, (myOwnState & LCA_URGENT) != 0));
460 593482 : myLaneChangeCompletion += (SPEED2DIST(mySpeedLat) / myManeuverDist);
461 593482 : return !pastBefore && pastMidpoint();
462 : }
463 :
464 :
465 : void
466 71272 : MSAbstractLaneChangeModel::endLaneChangeManeuver(const MSMoveReminder::Notification reason) {
467 : UNUSED_PARAMETER(reason);
468 71272 : myLaneChangeCompletion = 1;
469 71272 : cleanupShadowLane();
470 71272 : cleanupTargetLane();
471 : myNoPartiallyOccupatedByShadow.clear();
472 71272 : myVehicle.switchOffSignal(MSVehicle::VEH_SIGNAL_BLINKER_RIGHT | MSVehicle::VEH_SIGNAL_BLINKER_LEFT);
473 71272 : myVehicle.fixPosition();
474 71272 : 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 71272 : }
488 :
489 :
490 : MSLane*
491 18541582 : MSAbstractLaneChangeModel::getShadowLane(const MSLane* lane, double posLat) const {
492 18541582 : if (std::find(myNoPartiallyOccupatedByShadow.begin(), myNoPartiallyOccupatedByShadow.end(), lane) == myNoPartiallyOccupatedByShadow.end()) {
493 : // initialize shadow lane
494 18541498 : 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 18541498 : if (myAmOpposite) {
501 : // return the neigh-lane in forward direction
502 539908 : return lane->getParallelLane(1);
503 18001590 : } else if (overlap > NUMERICAL_EPS) {
504 10589500 : const int shadowDirection = posLat < 0 ? -1 : 1;
505 10589500 : return lane->getParallelLane(shadowDirection);
506 7412090 : } 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 18402220 : MSAbstractLaneChangeModel::getShadowLane(const MSLane* lane) const {
520 18402220 : return getShadowLane(lane, myVehicle.getLateralPositionOnLane());
521 : }
522 :
523 :
524 : void
525 4576346 : MSAbstractLaneChangeModel::cleanupShadowLane() {
526 4576346 : if (myShadowLane != nullptr) {
527 22444 : if (debugVehicle()) {
528 0 : std::cout << SIMTIME << " cleanupShadowLane\n";
529 : }
530 22444 : myShadowLane->resetPartialOccupation(&myVehicle);
531 22444 : myShadowLane = nullptr;
532 : }
533 4577409 : for (MSLane* further : myShadowFurtherLanes) {
534 1063 : if (debugVehicle()) {
535 0 : std::cout << SIMTIME << " cleanupShadowLane2\n";
536 : }
537 1063 : further->resetPartialOccupation(&myVehicle);
538 1063 : if (further->getBidiLane() != nullptr) {
539 30 : further->getBidiLane()->resetPartialOccupation(&myVehicle);
540 : }
541 : }
542 : myShadowFurtherLanes.clear();
543 : myNoPartiallyOccupatedByShadow.clear();
544 4576346 : }
545 :
546 : void
547 4576346 : MSAbstractLaneChangeModel::cleanupTargetLane() {
548 4576346 : if (myTargetLane != nullptr) {
549 618 : if (debugVehicle()) {
550 0 : std::cout << SIMTIME << " cleanupTargetLane\n";
551 : }
552 618 : myTargetLane->resetManeuverReservation(&myVehicle);
553 618 : myTargetLane = nullptr;
554 : }
555 4576356 : for (std::vector<MSLane*>::const_iterator it = myFurtherTargetLanes.begin(); it != myFurtherTargetLanes.end(); ++it) {
556 10 : if (debugVehicle()) {
557 0 : std::cout << SIMTIME << " cleanupTargetLane\n";
558 : }
559 10 : if (*it != nullptr) {
560 10 : (*it)->resetManeuverReservation(&myVehicle);
561 : }
562 : }
563 : myFurtherTargetLanes.clear();
564 : // myNoPartiallyOccupatedByShadow.clear();
565 4576346 : }
566 :
567 :
568 : bool
569 29117921 : MSAbstractLaneChangeModel::cancelRequest(int state, int laneOffset) {
570 : // store request before canceling
571 29117921 : getCanceledState(laneOffset) |= state;
572 29117921 : int ret = myVehicle.influenceChangeDecision(state);
573 29117921 : return ret != state;
574 : }
575 :
576 : double
577 20148619 : MSAbstractLaneChangeModel::getMaxSpeedLat2() const {
578 20148619 : return MAX2(myVehicle.getVehicleType().getMaxSpeedLat(), myMaxSpeedLatStanding);
579 : }
580 :
581 : void
582 1136082 : MSAbstractLaneChangeModel::initLastLaneChangeOffset(int dir) {
583 1136082 : if (dir > 0) {
584 625507 : myLastLaneChangeOffset = 1;
585 510575 : } else if (dir < 0) {
586 510575 : myLastLaneChangeOffset = -1;
587 : }
588 1136082 : }
589 :
590 : void
591 6138685 : MSAbstractLaneChangeModel::updateShadowLane() {
592 6138685 : if (!MSGlobals::gSublane) {
593 : // assume each vehicle drives at the center of its lane and act as if it fits
594 639934 : return;
595 : }
596 5498751 : if (myShadowLane != nullptr) {
597 : #ifdef DEBUG_SHADOWLANE
598 : if (debugVehicle()) {
599 : std::cout << SIMTIME << " updateShadowLane()\n";
600 : }
601 : #endif
602 1913093 : myShadowLane->resetPartialOccupation(&myVehicle);
603 : }
604 5498751 : myShadowLane = getShadowLane(myVehicle.getLane());
605 : std::vector<MSLane*> passed;
606 5498751 : if (myShadowLane != nullptr) {
607 1935537 : myShadowLane->setPartialOccupation(&myVehicle);
608 1935537 : const std::vector<MSLane*>& further = myVehicle.getFurtherLanes();
609 1935537 : if (myAmOpposite) {
610 : assert(further.size() == 0);
611 : } else {
612 : const std::vector<double>& furtherPosLat = myVehicle.getFurtherLanesPosLat();
613 : assert(further.size() == furtherPosLat.size());
614 1717924 : passed.push_back(myShadowLane);
615 1857286 : for (int i = 0; i < (int)further.size(); ++i) {
616 139362 : 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 139362 : if (shadowFurther != nullptr && shadowFurther->getLinkTo(passed.back()) != nullptr) {
623 60693 : passed.push_back(shadowFurther);
624 : }
625 : }
626 : std::reverse(passed.begin(), passed.end());
627 : }
628 : } else {
629 3563214 : 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 5498751 : myVehicle.updateFurtherLanes(myShadowFurtherLanes, myShadowFurtherLanesPosLat, passed);
644 : #ifdef DEBUG_SHADOWLANE
645 : if (debugVehicle()) std::cout
646 : << "\n after:" << " myShadowFurtherLanes=" << toString(myShadowFurtherLanes) << "\n";
647 : #endif
648 5498751 : }
649 :
650 :
651 : int
652 9942898 : MSAbstractLaneChangeModel::getShadowDirection() const {
653 9942898 : if (isChangingLanes()) {
654 694051 : if (pastMidpoint()) {
655 253489 : return -myLaneChangeDirection;
656 : } else {
657 440562 : return myLaneChangeDirection;
658 : }
659 9248847 : } else if (myShadowLane == nullptr) {
660 : return 0;
661 9248847 : } else if (myAmOpposite) {
662 : // return neigh-lane in forward direction
663 : return 1;
664 8741820 : } else if (&myShadowLane->getEdge() == &myVehicle.getLane()->getEdge()) {
665 8640006 : return myShadowLane->getIndex() - myVehicle.getLane()->getIndex();
666 : } else {
667 : // overlap with opposite direction lane
668 : return 1;
669 : }
670 : }
671 :
672 :
673 : MSLane*
674 87114751 : 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 87114751 : if (myTargetLane != nullptr) {
685 222261 : myTargetLane->resetManeuverReservation(&myVehicle);
686 : }
687 : // Clear old further target lanes
688 87128900 : for (MSLane* oldTargetLane : myFurtherTargetLanes) {
689 14149 : if (oldTargetLane != nullptr) {
690 9012 : oldTargetLane->resetManeuverReservation(&myVehicle);
691 : }
692 : }
693 : myFurtherTargetLanes.clear();
694 :
695 : // Get new target lanes and issue a maneuver reservation.
696 : int targetDir;
697 87114751 : myTargetLane = determineTargetLane(targetDir);
698 87114751 : if (myTargetLane != nullptr) {
699 222879 : 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 237038 : for (MSLane* furtherLane : myVehicle.getFurtherLanes()) {
703 14159 : MSLane* furtherTargetLane = furtherLane->getParallelLane(targetDir);
704 14159 : myFurtherTargetLanes.push_back(furtherTargetLane);
705 14159 : if (furtherTargetLane != nullptr) {
706 9022 : 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 87114751 : return myTargetLane;
718 : }
719 :
720 :
721 : MSLane*
722 87114751 : MSAbstractLaneChangeModel::determineTargetLane(int& targetDir) const {
723 87114751 : targetDir = 0;
724 87114751 : if (myManeuverDist == 0) {
725 : return nullptr;
726 : }
727 : // Current lateral boundaries of the vehicle
728 2488578 : const double vehRight = myVehicle.getLateralPositionOnLane() - 0.5 * myVehicle.getWidth();
729 2488578 : const double vehLeft = myVehicle.getLateralPositionOnLane() + 0.5 * myVehicle.getWidth();
730 2488578 : const double halfLaneWidth = 0.5 * myVehicle.getLane()->getWidth();
731 :
732 2488578 : if (vehRight + myManeuverDist < -halfLaneWidth) {
733 : // Vehicle intends to traverse the right lane boundary
734 333081 : targetDir = -1;
735 2155497 : } else if (vehLeft + myManeuverDist > halfLaneWidth) {
736 : // Vehicle intends to traverse the left lane boundary
737 523832 : targetDir = 1;
738 : }
739 2488578 : if (targetDir == 0) {
740 : // Presently, no maneuvering into another lane is begun.
741 : return nullptr;
742 : }
743 856913 : MSLane* target = myVehicle.getLane()->getParallelLane(targetDir);
744 856913 : if (target == nullptr || target == myShadowLane) {
745 : return nullptr;
746 : } else {
747 : return target;
748 : }
749 : }
750 :
751 :
752 :
753 : double
754 707914266 : MSAbstractLaneChangeModel::calcAngleOffset() {
755 : double result = 0.;
756 707914266 : if (!(fabs(mySpeedLat) < NUMERICAL_EPS && fabs(myPreviousAngleOffset * 180 / M_PI) < NUMERICAL_EPS)) {
757 20678382 : if (myVehicle.getLength() < sqrt(SPEED2DIST(mySpeedLat) * SPEED2DIST(mySpeedLat) + SPEED2DIST(myVehicle.getSpeed()) * SPEED2DIST(myVehicle.getSpeed()))) {
758 5713631 : result = atan2(mySpeedLat, myVehicle.getSpeed());
759 : } else {
760 14964751 : result = myPreviousAngleOffset + asin((sin(M_PI / 2 - myPreviousAngleOffset) * (SPEED2DIST(mySpeedLat) - tan(myPreviousAngleOffset) * SPEED2DIST(myVehicle.getSpeed()))) / myVehicle.getLength());
761 : }
762 : }
763 :
764 707914266 : myAngleOffset = result;
765 707914266 : return result;
766 : }
767 :
768 :
769 : double
770 81555 : MSAbstractLaneChangeModel::estimateLCDuration(const double speed, const double remainingManeuverDist, const double decel, bool urgent) const {
771 :
772 81555 : const SUMOVTypeParameter::SubParams& lcParams = myVehicle.getVehicleType().getParameter().getLCParams();
773 136206 : if (lcParams.find(SUMO_ATTR_LCA_MAXSPEEDLATSTANDING) == lcParams.end() && lcParams.find(SUMO_ATTR_LCA_MAXSPEEDLATFACTOR) == lcParams.end()) {
774 54651 : 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 54625 : 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 54636 : MSAbstractLaneChangeModel::remainingTime() const {
878 : assert(isChangingLanes()); // Only to be called during ongoing lane change
879 54636 : const SUMOVTypeParameter::SubParams& lcParams = myVehicle.getVehicleType().getParameter().getLCParams();
880 108911 : if (lcParams.find(SUMO_ATTR_LCA_MAXSPEEDLATSTANDING) == lcParams.end() && lcParams.find(SUMO_ATTR_LCA_MAXSPEEDLATFACTOR) == lcParams.end()) {
881 54275 : if (myVehicle.getVehicleType().wasSet(VTYPEPARS_MAXSPEED_LAT_SET)) {
882 0 : return TIME2STEPS((1. - myLaneChangeCompletion) * myManeuverDist / myVehicle.getVehicleType().getMaxSpeedLat());
883 : } else {
884 54275 : 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 2450135 : MSAbstractLaneChangeModel::setShadowApproachingInformation(MSLink* link) const {
897 : //std::cout << SIMTIME << " veh=" << myVehicle.getID() << " @=" << &myVehicle << " set shadow approaching=" << link->getViaLaneOrLane()->getID() << "\n";
898 2450135 : myApproachedByShadow.push_back(link);
899 2450135 : }
900 :
901 : void
902 640555229 : MSAbstractLaneChangeModel::removeShadowApproachingInformation() const {
903 643005364 : 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 2450135 : (*it)->removeApproaching(&myVehicle);
906 : }
907 : myApproachedByShadow.clear();
908 640555229 : }
909 :
910 :
911 :
912 : void
913 41336561 : MSAbstractLaneChangeModel::checkTraCICommands() {
914 41336561 : int newstate = myVehicle.influenceChangeDecision(myOwnState);
915 41336561 : int oldstate = myVehicle.getLaneChangeModel().getOwnState();
916 41336561 : 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 41336545 : 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 41336561 : if (gDebugFlag2) {
954 0 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " stateAfterTraCI=" << toString((LaneChangeAction)newstate) << " original=" << toString((LaneChangeAction)oldstate) << "\n";
955 : }
956 41336561 : }
957 :
958 : void
959 44298 : MSAbstractLaneChangeModel::changedToOpposite() {
960 44298 : myAmOpposite = !myAmOpposite;
961 44298 : myAlreadyChanged = true;
962 44298 : }
963 :
964 : void
965 746641 : MSAbstractLaneChangeModel::setFollowerGaps(CLeaderDist follower, double secGap) {
966 746641 : if (follower.first != 0) {
967 419176 : myLastFollowerGap = follower.second + follower.first->getVehicleType().getMinGap();
968 419176 : myLastFollowerSecureGap = secGap;
969 419176 : myLastFollowerSpeed = follower.first->getSpeed();
970 : }
971 746641 : }
972 :
973 : void
974 746641 : MSAbstractLaneChangeModel::setLeaderGaps(CLeaderDist leader, double secGap) {
975 746641 : if (leader.first != 0) {
976 547958 : myLastLeaderGap = leader.second + myVehicle.getVehicleType().getMinGap();
977 547958 : myLastLeaderSecureGap = secGap;
978 547958 : myLastLeaderSpeed = leader.first->getSpeed();
979 : }
980 746641 : }
981 :
982 : void
983 746641 : MSAbstractLaneChangeModel::setOrigLeaderGaps(CLeaderDist leader, double secGap) {
984 746641 : if (leader.first != 0) {
985 443515 : myLastOrigLeaderGap = leader.second + myVehicle.getVehicleType().getMinGap();
986 443515 : myLastOrigLeaderSecureGap = secGap;
987 443515 : myLastOrigLeaderSpeed = leader.first->getSpeed();
988 : }
989 746641 : }
990 :
991 : void
992 625572612 : MSAbstractLaneChangeModel::prepareStep() {
993 625572612 : getCanceledState(-1) = LCA_NONE;
994 625572612 : getCanceledState(0) = LCA_NONE;
995 625572612 : 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 625572612 : myLastLateralGapRight = NO_NEIGHBOR;
1000 625572612 : myLastLateralGapLeft = NO_NEIGHBOR;
1001 625572612 : if (!myDontResetLCGaps) {
1002 625567529 : myLastLeaderGap = NO_NEIGHBOR;
1003 625567529 : myLastLeaderSecureGap = NO_NEIGHBOR;
1004 625567529 : myLastFollowerGap = NO_NEIGHBOR;
1005 625567529 : myLastFollowerSecureGap = NO_NEIGHBOR;
1006 625567529 : myLastOrigLeaderGap = NO_NEIGHBOR;
1007 625567529 : myLastOrigLeaderSecureGap = NO_NEIGHBOR;
1008 625567529 : myLastLeaderSpeed = NO_NEIGHBOR;
1009 625567529 : myLastFollowerSpeed = NO_NEIGHBOR;
1010 625567529 : myLastOrigLeaderSpeed = NO_NEIGHBOR;
1011 : }
1012 625572612 : myCommittedSpeed = 0;
1013 625572612 : }
1014 :
1015 : void
1016 7859 : MSAbstractLaneChangeModel::setFollowerGaps(const MSLeaderDistanceInfo& vehicles) {
1017 : int rightmost;
1018 : int leftmost;
1019 7859 : vehicles.getSubLanes(&myVehicle, 0, rightmost, leftmost);
1020 31794 : for (int i = rightmost; i <= leftmost; ++i) {
1021 23935 : CLeaderDist vehDist = vehicles[i];
1022 23935 : 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 7859 : }
1034 :
1035 : void
1036 7859 : MSAbstractLaneChangeModel::setLeaderGaps(const MSLeaderDistanceInfo& vehicles) {
1037 : int rightmost;
1038 : int leftmost;
1039 7859 : vehicles.getSubLanes(&myVehicle, 0, rightmost, leftmost);
1040 31794 : for (int i = rightmost; i <= leftmost; ++i) {
1041 23935 : CLeaderDist vehDist = vehicles[i];
1042 23935 : 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 7859 : }
1054 :
1055 : void
1056 7859 : MSAbstractLaneChangeModel::setOrigLeaderGaps(const MSLeaderDistanceInfo& vehicles) {
1057 : int rightmost;
1058 : int leftmost;
1059 7859 : vehicles.getSubLanes(&myVehicle, 0, rightmost, leftmost);
1060 31688 : for (int i = rightmost; i <= leftmost; ++i) {
1061 23829 : CLeaderDist vehDist = vehicles[i];
1062 23829 : 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 7859 : }
1074 :
1075 :
1076 : bool
1077 27786464 : MSAbstractLaneChangeModel::isStrategicBlocked() const {
1078 27786464 : const int stateRight = mySavedStateRight.second;
1079 27786464 : if (
1080 : (stateRight & LCA_STRATEGIC) != 0
1081 : && (stateRight & LCA_RIGHT) != 0
1082 841016 : && (stateRight & LCA_BLOCKED) != 0) {
1083 : return true;
1084 : }
1085 27233867 : const int stateLeft = mySavedStateLeft.second;
1086 27233867 : if (
1087 : (stateLeft & LCA_STRATEGIC) != 0
1088 : && (stateLeft & LCA_LEFT) != 0
1089 213583 : && (stateLeft & LCA_BLOCKED) != 0) {
1090 142699 : return true;
1091 : }
1092 : return false;
1093 : }
1094 :
1095 : double
1096 362119537 : MSAbstractLaneChangeModel::getForwardPos() const {
1097 362119537 : 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 51663623 : MSAbstractLaneChangeModel::addLCSpeedAdvice(const double vSafe, bool ownAdvice) {
1113 51663623 : const double accel = SPEED2ACCEL(vSafe - myVehicle.getSpeed());
1114 51663623 : myLCAccelerationAdvices.push_back({accel, ownAdvice});
1115 51663623 : }
1116 :
1117 :
1118 : bool
1119 14874244 : MSAbstractLaneChangeModel::canOvertakeRight(const MSVehicle* const nv, const double dist, const double maxSpeedDiff, const double helpOvertakeSpeed, double& vSafe, double& deltaV) const {
1120 14874244 : deltaV = MAX2(maxSpeedDiff, myVehicle.getSpeed() - nv->getSpeed());
1121 14874244 : if (deltaV > 0) {
1122 2869896 : const double vMaxDecel = getCarFollowModel().getSpeedAfterMaxDecel(myVehicle.getSpeed());
1123 2869896 : const double vSafeFollow = getCarFollowModel().followSpeed(
1124 2869896 : &myVehicle, myVehicle.getSpeed(), dist, nv->getSpeed(), nv->getCarFollowModel().getMaxDecel());
1125 2869896 : const double vStayBehind = nv->getSpeed() - helpOvertakeSpeed;
1126 2869896 : if (vSafeFollow >= vMaxDecel) {
1127 2570361 : vSafe = vSafeFollow;
1128 : } else {
1129 299535 : vSafe = MAX2(vMaxDecel, vStayBehind);
1130 : }
1131 2869896 : return true;
1132 : }
1133 : return false;
1134 : }
1135 :
1136 :
1137 : void
1138 2615 : MSAbstractLaneChangeModel::saveState(OutputDevice& out) const {
1139 : std::vector<double> lcState;
1140 2615 : lcState.push_back((double)myOwnState);
1141 2615 : for (const auto& item : myLCAccelerationAdvices) {
1142 0 : lcState.push_back(item.first);
1143 0 : lcState.push_back((double)item.second);
1144 : }
1145 2615 : out.writeAttr(SUMO_ATTR_LCSTATE_BASE, lcState);
1146 :
1147 2615 : if (MSGlobals::gLaneChangeDuration > 0) {
1148 1 : out.writeAttr(SUMO_ATTR_LCSTATE, std::vector<double> {mySpeedLat, myLaneChangeCompletion, (double)myLaneChangeDirection});
1149 : }
1150 2615 : }
1151 :
1152 : void
1153 3472 : MSAbstractLaneChangeModel::loadState(const SUMOSAXAttributes& attrs) {
1154 3472 : if (attrs.hasAttribute(SUMO_ATTR_LCSTATE_BASE)) {
1155 6944 : std::istringstream bis(attrs.getString(SUMO_ATTR_LCSTATE_BASE));
1156 : double token;
1157 : bis >> token;
1158 3472 : myOwnState = (int)token; // double is suffciently precise
1159 : double prev = std::numeric_limits<double>::max();
1160 6944 : 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 3472 : }
1168 3472 : 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 3472 : }
1175 :
1176 :
1177 : double
1178 3916279 : MSAbstractLaneChangeModel::getExtraReservation(int bestLaneOffset, double neighExtraDist) const {
1179 3916279 : if (neighExtraDist > myVehicle.getVehicleType().getLengthWithGap()) {
1180 : return 0;
1181 : }
1182 3916009 : if (bestLaneOffset < -1) {
1183 : return 20;
1184 3873380 : } else if (bestLaneOffset > 1) {
1185 406928 : return 40;
1186 : }
1187 : return 0;
1188 : }
1189 :
1190 :
1191 : double
1192 592702737 : MSAbstractLaneChangeModel::getCooperativeHelpSpeed(const MSLane* lane, double distToLaneEnd) const {
1193 592702737 : if (myCooperativeHelpTime >= 0) {
1194 592702737 : std::pair<double, SUMOTime> backAndWaiting = lane->getEdge().getLastBlocked(lane->getIndex());
1195 592702737 : if (backAndWaiting.second >= myCooperativeHelpTime) {
1196 2232762 : double gap = distToLaneEnd - lane->getLength() + backAndWaiting.first - myVehicle.getVehicleType().getMinGap() - NUMERICAL_EPS;
1197 2232762 : if (backAndWaiting.first < 0) {
1198 370366 : if (myVehicle.getLane()->getToJunction() == lane->getFromJunction()) {
1199 205328 : 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 187908 : gap = myVehicle.getLane()->getLength() - myVehicle.getPositionOnLane();
1205 : }
1206 : }
1207 : }
1208 2215342 : if (gap > 0) {
1209 1428981 : 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 1428981 : if (stopSpeed >= myVehicle.getCarFollowModel().minNextSpeed(myVehicle.getSpeed(), &myVehicle)) {
1214 : // regular braking is helpful
1215 1426798 : return stopSpeed;
1216 : }
1217 : }
1218 : }
1219 : }
1220 : // do not restrict speed
1221 : return std::numeric_limits<double>::max();
1222 : }
|
