1 /****************************************************************************/
2 // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3 // Copyright (C) 2001-2019 German Aerospace Center (DLR) and others.
4 // This program and the accompanying materials
5 // are made available under the terms of the Eclipse Public License v2.0
6 // which accompanies this distribution, and is available at
7 // http://www.eclipse.org/legal/epl-v20.html
8 // SPDX-License-Identifier: EPL-2.0
9 /****************************************************************************/
10 /// @file MSVehicle.cpp
11 /// @author Christian Roessel
12 /// @author Jakob Erdmann
13 /// @author Bjoern Hendriks
14 /// @author Daniel Krajzewicz
15 /// @author Thimor Bohn
16 /// @author Friedemann Wesner
17 /// @author Laura Bieker
18 /// @author Clemens Honomichl
19 /// @author Michael Behrisch
20 /// @author Axel Wegener
21 /// @author Christoph Sommer
22 /// @author Leonhard Luecken
23 /// @author Lara Codeca
24 /// @date Mon, 05 Mar 2001
25 /// @version $Id$
26 ///
27 // Representation of a vehicle in the micro simulation
28 /****************************************************************************/
29
30 // ===========================================================================
31 // included modules
32 // ===========================================================================
33 #include <config.h>
34
35 #include <iostream>
36 #include <cassert>
37 #include <cmath>
38 #include <cstdlib>
39 #include <algorithm>
40 #include <map>
41 #include <memory>
42 #include <utils/common/ToString.h>
43 #include <utils/common/FileHelpers.h>
44 #include <utils/router/DijkstraRouter.h>
45 #include <utils/common/RandHelper.h>
46 #include <utils/emissions/PollutantsInterface.h>
47 #include <utils/emissions/HelpersHarmonoise.h>
48 #include <utils/common/StringUtils.h>
49 #include <utils/common/StdDefs.h>
50 #include <utils/geom/GeomHelper.h>
51 #include <utils/iodevices/OutputDevice.h>
52 #include <utils/iodevices/BinaryInputDevice.h>
53 #include <utils/xml/SUMOSAXAttributes.h>
54 #include <utils/vehicle/SUMOVehicleParserHelper.h>
55 #include <microsim/lcmodels/MSAbstractLaneChangeModel.h>
56 #include <microsim/pedestrians/MSPerson.h>
57 #include <microsim/pedestrians/MSPModel.h>
58 #include <microsim/devices/MSDevice_Transportable.h>
59 #include <microsim/devices/MSDevice_DriverState.h>
60 #include <microsim/devices/MSRoutingEngine.h>
61 #include <microsim/devices/MSDevice_Vehroutes.h>
62 #include <microsim/output/MSStopOut.h>
63 #include <microsim/trigger/MSChargingStation.h>
64 #include <microsim/traffic_lights/MSTrafficLightLogic.h>
65 #include "MSEdgeControl.h"
66 #include "MSVehicleControl.h"
67 #include "MSVehicleTransfer.h"
68 #include "MSGlobals.h"
69 #include "MSJunctionLogic.h"
70 #include "MSStoppingPlace.h"
71 #include "MSParkingArea.h"
72 #include "devices/MSDevice_Transportable.h"
73 #include "MSEdgeWeightsStorage.h"
74 #include <microsim/lcmodels/MSAbstractLaneChangeModel.h>
75 #include "MSMoveReminder.h"
76 #include "MSTransportableControl.h"
77 #include "MSLane.h"
78 #include "MSJunction.h"
79 #include "MSVehicle.h"
80 #include "MSEdge.h"
81 #include "MSVehicleType.h"
82 #include "MSNet.h"
83 #include "MSRoute.h"
84 #include "MSLinkCont.h"
85 #include "MSLeaderInfo.h"
86 #include "MSDriverState.h"
87
88 //#define DEBUG_PLAN_MOVE
89 //#define DEBUG_PLAN_MOVE_LEADERINFO
90 //#define DEBUG_CHECKREWINDLINKLANES
91 //#define DEBUG_EXEC_MOVE
92 //#define DEBUG_FURTHER
93 //#define DEBUG_SETFURTHER
94 //#define DEBUG_TARGET_LANE
95 //#define DEBUG_STOPS
96 //#define DEBUG_BESTLANES
97 //#define DEBUG_IGNORE_RED
98 //#define DEBUG_ACTIONSTEPS
99 //#define DEBUG_NEXT_TURN
100 //#define DEBUG_TRACI
101 //#define DEBUG_REVERSE_BIDI
102 //#define DEBUG_REPLACE_ROUTE
103 //#define DEBUG_COND (getID() == "follower")
104 //#define DEBUG_COND (true)
105 #define DEBUG_COND (isSelected())
106 //#define DEBUG_COND2(obj) (obj->getID() == "follower")
107 #define DEBUG_COND2(obj) (obj->isSelected())
108
109
110 #define STOPPING_PLACE_OFFSET 0.5
111
112 #define CRLL_LOOK_AHEAD 5
113
114 #define JUNCTION_BLOCKAGE_TIME 5 // s
115
116 // @todo Calibrate with real-world values / make configurable
117 #define DIST_TO_STOPLINE_EXPECT_PRIORITY 1.0
118
119 #define NUMERICAL_EPS_SPEED (0.1 * NUMERICAL_EPS * TS)
120
121 // ===========================================================================
122 // static value definitions
123 // ===========================================================================
124 std::vector<MSLane*> MSVehicle::myEmptyLaneVector;
125
126
127 // ===========================================================================
128 // method definitions
129 // ===========================================================================
130 /* -------------------------------------------------------------------------
131 * methods of MSVehicle::State
132 * ----------------------------------------------------------------------- */
State(const State & state)133 MSVehicle::State::State(const State& state) {
134 myPos = state.myPos;
135 mySpeed = state.mySpeed;
136 myPosLat = state.myPosLat;
137 myBackPos = state.myBackPos;
138 myPreviousSpeed = state.myPreviousSpeed;
139 myLastCoveredDist = state.myLastCoveredDist;
140 }
141
142
143 MSVehicle::State&
operator =(const State & state)144 MSVehicle::State::operator=(const State& state) {
145 myPos = state.myPos;
146 mySpeed = state.mySpeed;
147 myPosLat = state.myPosLat;
148 myBackPos = state.myBackPos;
149 myPreviousSpeed = state.myPreviousSpeed;
150 myLastCoveredDist = state.myLastCoveredDist;
151 return *this;
152 }
153
154
155 bool
operator !=(const State & state)156 MSVehicle::State::operator!=(const State& state) {
157 return (myPos != state.myPos ||
158 mySpeed != state.mySpeed ||
159 myPosLat != state.myPosLat ||
160 myLastCoveredDist != state.myLastCoveredDist ||
161 myPreviousSpeed != state.myPreviousSpeed ||
162 myBackPos != state.myBackPos);
163 }
164
165
State(double pos,double speed,double posLat,double backPos)166 MSVehicle::State::State(double pos, double speed, double posLat, double backPos) :
167 myPos(pos), mySpeed(speed), myPosLat(posLat), myBackPos(backPos), myPreviousSpeed(speed), myLastCoveredDist(SPEED2DIST(speed)) {}
168
169
170
171 /* -------------------------------------------------------------------------
172 * methods of MSVehicle::WaitingTimeCollector
173 * ----------------------------------------------------------------------- */
174
WaitingTimeCollector(SUMOTime memory)175 MSVehicle::WaitingTimeCollector::WaitingTimeCollector(SUMOTime memory) : myMemorySize(memory) {}
176
WaitingTimeCollector(const WaitingTimeCollector & wt)177 MSVehicle::WaitingTimeCollector::WaitingTimeCollector(const WaitingTimeCollector& wt) : myMemorySize(wt.getMemorySize()), myWaitingIntervals(wt.getWaitingIntervals()) {}
178
179 MSVehicle::WaitingTimeCollector&
operator =(const WaitingTimeCollector & wt)180 MSVehicle::WaitingTimeCollector::operator=(const WaitingTimeCollector& wt) {
181 myMemorySize = wt.getMemorySize();
182 myWaitingIntervals = wt.getWaitingIntervals();
183 return *this;
184 }
185
186 MSVehicle::WaitingTimeCollector&
operator =(SUMOTime t)187 MSVehicle::WaitingTimeCollector::operator=(SUMOTime t) {
188 myWaitingIntervals.clear();
189 passTime(t, true);
190 return *this;
191 }
192
193 SUMOTime
cumulatedWaitingTime(SUMOTime memorySpan) const194 MSVehicle::WaitingTimeCollector::cumulatedWaitingTime(SUMOTime memorySpan) const {
195 assert(memorySpan <= myMemorySize);
196 if (memorySpan == -1) {
197 memorySpan = myMemorySize;
198 }
199 SUMOTime totalWaitingTime = 0;
200 for (waitingIntervalList::const_iterator i = myWaitingIntervals.begin(); i != myWaitingIntervals.end(); i++) {
201 if (i->second >= memorySpan) {
202 if (i->first >= memorySpan) {
203 break;
204 } else {
205 totalWaitingTime += memorySpan - i->first;
206 }
207 } else {
208 totalWaitingTime += i->second - i->first;
209 }
210 }
211 return totalWaitingTime;
212 }
213
214 void
passTime(SUMOTime dt,bool waiting)215 MSVehicle::WaitingTimeCollector::passTime(SUMOTime dt, bool waiting) {
216 waitingIntervalList::iterator i = myWaitingIntervals.begin();
217 waitingIntervalList::iterator end = myWaitingIntervals.end();
218 bool startNewInterval = i == end || (i->first != 0);
219 while (i != end) {
220 i->first += dt;
221 if (i->first >= myMemorySize) {
222 break;
223 }
224 i->second += dt;
225 i++;
226 }
227
228 // remove intervals beyond memorySize
229 waitingIntervalList::iterator::difference_type d = std::distance(i, end);
230 while (d > 0) {
231 myWaitingIntervals.pop_back();
232 d--;
233 }
234
235 if (!waiting) {
236 return;
237 } else if (!startNewInterval) {
238 myWaitingIntervals.begin()->first = 0;
239 } else {
240 myWaitingIntervals.push_front(std::make_pair(0, dt));
241 }
242 return;
243 }
244
245
246
247
248 /* -------------------------------------------------------------------------
249 * methods of MSVehicle::Influencer::GapControlState
250 * ----------------------------------------------------------------------- */
251 void
vehicleStateChanged(const SUMOVehicle * const vehicle,MSNet::VehicleState to,const std::string &)252 MSVehicle::Influencer::GapControlVehStateListener::vehicleStateChanged(const SUMOVehicle* const vehicle, MSNet::VehicleState to, const std::string& /*info*/) {
253 // std::cout << "GapControlVehStateListener::vehicleStateChanged() vehicle=" << vehicle->getID() << ", to=" << to << std::endl;
254 switch (to) {
255 case MSNet::VEHICLE_STATE_STARTING_TELEPORT:
256 case MSNet::VEHICLE_STATE_ARRIVED:
257 case MSNet::VEHICLE_STATE_STARTING_PARKING: {
258 // Vehicle left road
259 // Look up reference vehicle in refVehMap and in case deactivate corresponding gap control
260 const MSVehicle* msVeh = static_cast<const MSVehicle*>(vehicle);
261 // std::cout << "GapControlVehStateListener::vehicleStateChanged() vehicle=" << vehicle->getID() << " left the road." << std::endl;
262 if (GapControlState::refVehMap.find(msVeh) != end(GapControlState::refVehMap)) {
263 // std::cout << "GapControlVehStateListener::deactivating ref vehicle=" << vehicle->getID() << std::endl;
264 GapControlState::refVehMap[msVeh]->deactivate();
265 }
266 }
267 break;
268 default:
269 {};
270 // do nothing, vehicle still on road
271 }
272 }
273
274 std::map<const MSVehicle*, MSVehicle::Influencer::GapControlState*>
275 MSVehicle::Influencer::GapControlState::refVehMap;
276
277 MSVehicle::Influencer::GapControlVehStateListener
278 MSVehicle::Influencer::GapControlState::vehStateListener;
279
GapControlState()280 MSVehicle::Influencer::GapControlState::GapControlState() :
281 tauOriginal(-1), tauCurrent(-1), tauTarget(-1), addGapCurrent(-1), addGapTarget(-1),
282 remainingDuration(-1), changeRate(-1), maxDecel(-1), referenceVeh(nullptr), active(false), gapAttained(false), prevLeader(nullptr),
283 lastUpdate(-1), timeHeadwayIncrement(0.0), spaceHeadwayIncrement(0.0) {}
284
285
~GapControlState()286 MSVehicle::Influencer::GapControlState::~GapControlState() {
287 deactivate();
288 }
289
290 void
init()291 MSVehicle::Influencer::GapControlState::init() {
292 // std::cout << "GapControlState::init()" << std::endl;
293 if (MSNet::hasInstance()) {
294 MSNet::VehicleStateListener* vsl = dynamic_cast<MSNet::VehicleStateListener*>(&vehStateListener);
295 MSNet::getInstance()->addVehicleStateListener(vsl);
296 } else {
297 WRITE_ERROR("MSVehicle::Influencer::GapControlState::init(): No MSNet instance found!")
298 }
299 }
300
301 void
cleanup()302 MSVehicle::Influencer::GapControlState::cleanup() {
303 MSNet::VehicleStateListener* vsl = dynamic_cast<MSNet::VehicleStateListener*>(&vehStateListener);
304 MSNet::getInstance()->removeVehicleStateListener(vsl);
305 }
306
307 void
activate(double tauOrig,double tauNew,double additionalGap,double dur,double rate,double decel,const MSVehicle * refVeh)308 MSVehicle::Influencer::GapControlState::activate(double tauOrig, double tauNew, double additionalGap, double dur, double rate, double decel, const MSVehicle* refVeh) {
309 if (MSGlobals::gUseMesoSim) {
310 WRITE_ERROR("No gap control available for meso.")
311 } else {
312 // always deactivate control before activating (triggers clean-up of refVehMap)
313 // std::cout << "activate gap control with refVeh=" << (refVeh==nullptr? "NULL" : refVeh->getID()) << std::endl;
314 tauOriginal = tauOrig;
315 tauCurrent = tauOrig;
316 tauTarget = tauNew;
317 addGapCurrent = 0.0;
318 addGapTarget = additionalGap;
319 remainingDuration = dur;
320 changeRate = rate;
321 maxDecel = decel;
322 referenceVeh = refVeh;
323 active = true;
324 gapAttained = false;
325 prevLeader = nullptr;
326 lastUpdate = SIMSTEP - DELTA_T;
327 timeHeadwayIncrement = changeRate * TS * (tauTarget - tauOriginal);
328 spaceHeadwayIncrement = changeRate * TS * addGapTarget;
329
330 if (referenceVeh != nullptr) {
331 // Add refVeh to refVehMap
332 GapControlState::refVehMap[referenceVeh] = this;
333 }
334 }
335 }
336
337 void
deactivate()338 MSVehicle::Influencer::GapControlState::deactivate() {
339 active = false;
340 if (referenceVeh != nullptr) {
341 // Remove corresponding refVehMapEntry if appropriate
342 GapControlState::refVehMap.erase(referenceVeh);
343 referenceVeh = nullptr;
344 }
345 }
346
347
348 /* -------------------------------------------------------------------------
349 * methods of MSVehicle::Influencer
350 * ----------------------------------------------------------------------- */
Influencer()351 MSVehicle::Influencer::Influencer() :
352 myGapControlState(nullptr),
353 myLatDist(0),
354 mySpeedAdaptationStarted(true),
355 myConsiderSafeVelocity(true),
356 myConsiderMaxAcceleration(true),
357 myConsiderMaxDeceleration(true),
358 myRespectJunctionPriority(true),
359 myEmergencyBrakeRedLight(true),
360 myLastRemoteAccess(-TIME2STEPS(20)),
361 myStrategicLC(LC_NOCONFLICT),
362 myCooperativeLC(LC_NOCONFLICT),
363 mySpeedGainLC(LC_NOCONFLICT),
364 myRightDriveLC(LC_NOCONFLICT),
365 mySublaneLC(LC_NOCONFLICT),
366 myTraciLaneChangePriority(LCP_URGENT),
367 myTraCISignals(-1),
368 myRoutingMode(0)
369 {}
370
371
~Influencer()372 MSVehicle::Influencer::~Influencer() {}
373
374 void
init()375 MSVehicle::Influencer::init() {
376 GapControlState::init();
377 }
378
379 void
cleanup()380 MSVehicle::Influencer::cleanup() {
381 GapControlState::cleanup();
382 }
383
384 void
setSpeedTimeLine(const std::vector<std::pair<SUMOTime,double>> & speedTimeLine)385 MSVehicle::Influencer::setSpeedTimeLine(const std::vector<std::pair<SUMOTime, double> >& speedTimeLine) {
386 mySpeedAdaptationStarted = true;
387 mySpeedTimeLine = speedTimeLine;
388 }
389
390 void
activateGapController(double originalTau,double newTimeHeadway,double newSpaceHeadway,double duration,double changeRate,double maxDecel,MSVehicle * refVeh)391 MSVehicle::Influencer::activateGapController(double originalTau, double newTimeHeadway, double newSpaceHeadway, double duration, double changeRate, double maxDecel, MSVehicle* refVeh) {
392 if (myGapControlState == nullptr) {
393 myGapControlState = std::make_shared<GapControlState>();
394 }
395 myGapControlState->activate(originalTau, newTimeHeadway, newSpaceHeadway, duration, changeRate, maxDecel, refVeh);
396 }
397
398 void
deactivateGapController()399 MSVehicle::Influencer::deactivateGapController() {
400 if (myGapControlState != nullptr && myGapControlState->active) {
401 myGapControlState->deactivate();
402 }
403 }
404
405 void
setLaneTimeLine(const std::vector<std::pair<SUMOTime,int>> & laneTimeLine)406 MSVehicle::Influencer::setLaneTimeLine(const std::vector<std::pair<SUMOTime, int> >& laneTimeLine) {
407 myLaneTimeLine = laneTimeLine;
408 }
409
410
411 void
adaptLaneTimeLine(int indexShift)412 MSVehicle::Influencer::adaptLaneTimeLine(int indexShift) {
413 for (auto& item : myLaneTimeLine) {
414 item.second += indexShift;
415 }
416 }
417
418
419 void
setSublaneChange(double latDist)420 MSVehicle::Influencer::setSublaneChange(double latDist) {
421 myLatDist = latDist;
422 }
423
424 int
getSpeedMode() const425 MSVehicle::Influencer::getSpeedMode() const {
426 return (1 * myConsiderSafeVelocity +
427 2 * myConsiderMaxAcceleration +
428 4 * myConsiderMaxDeceleration +
429 8 * myRespectJunctionPriority +
430 16 * myEmergencyBrakeRedLight);
431 }
432
433
434 int
getLaneChangeMode() const435 MSVehicle::Influencer::getLaneChangeMode() const {
436 return (1 * myStrategicLC +
437 4 * myCooperativeLC +
438 16 * mySpeedGainLC +
439 64 * myRightDriveLC +
440 256 * myTraciLaneChangePriority +
441 1024 * mySublaneLC);
442 }
443
444 SUMOTime
getLaneTimeLineDuration()445 MSVehicle::Influencer::getLaneTimeLineDuration() {
446 SUMOTime duration = -1;
447 for (std::vector<std::pair<SUMOTime, int>>::iterator i = myLaneTimeLine.begin(); i != myLaneTimeLine.end(); ++i) {
448 if (duration < 0) {
449 duration = i->first;
450 } else {
451 duration -= i->first;
452 }
453 }
454 return -duration;
455 }
456
457 SUMOTime
getLaneTimeLineEnd()458 MSVehicle::Influencer::getLaneTimeLineEnd() {
459 if (!myLaneTimeLine.empty()) {
460 return myLaneTimeLine.back().first;
461 } else {
462 return -1;
463 }
464 }
465
466
467 double
influenceSpeed(SUMOTime currentTime,double speed,double vSafe,double vMin,double vMax)468 MSVehicle::Influencer::influenceSpeed(SUMOTime currentTime, double speed, double vSafe, double vMin, double vMax) {
469 // keep original speed
470 myOriginalSpeed = speed;
471
472 // remove leading commands which are no longer valid
473 while (mySpeedTimeLine.size() == 1 || (mySpeedTimeLine.size() > 1 && currentTime > mySpeedTimeLine[1].first)) {
474 mySpeedTimeLine.erase(mySpeedTimeLine.begin());
475 }
476
477 if (!(mySpeedTimeLine.size() < 2 || currentTime < mySpeedTimeLine[0].first)) {
478 // Speed advice is active -> compute new speed according to speedTimeLine
479 if (!mySpeedAdaptationStarted) {
480 mySpeedTimeLine[0].second = speed;
481 mySpeedAdaptationStarted = true;
482 }
483 currentTime += DELTA_T;
484 const double td = STEPS2TIME(currentTime - mySpeedTimeLine[0].first) / STEPS2TIME(mySpeedTimeLine[1].first + DELTA_T - mySpeedTimeLine[0].first);
485 speed = mySpeedTimeLine[0].second - (mySpeedTimeLine[0].second - mySpeedTimeLine[1].second) * td;
486 if (myConsiderSafeVelocity) {
487 speed = MIN2(speed, vSafe);
488 }
489 if (myConsiderMaxAcceleration) {
490 speed = MIN2(speed, vMax);
491 }
492 if (myConsiderMaxDeceleration) {
493 speed = MAX2(speed, vMin);
494 }
495 }
496 return speed;
497 }
498
499 double
gapControlSpeed(SUMOTime currentTime,const SUMOVehicle * veh,double speed,double vSafe,double vMin,double vMax)500 MSVehicle::Influencer::gapControlSpeed(SUMOTime currentTime, const SUMOVehicle* veh, double speed, double vSafe, double vMin, double vMax) {
501 #ifdef DEBUG_TRACI
502 if DEBUG_COND2(veh) {
503 std::cout << currentTime << " Influencer::gapControlSpeed(): speed=" << speed
504 << ", vSafe=" << vSafe
505 << ", vMin=" << vMin
506 << ", vMax=" << vMax
507 << std::endl;
508 }
509 #endif
510 double gapControlSpeed = speed;
511 if (myGapControlState != nullptr && myGapControlState->active) {
512 // Determine leader and the speed that would be chosen by the gap controller
513 const double currentSpeed = veh->getSpeed();
514 const MSVehicle* msVeh = dynamic_cast<const MSVehicle*>(veh);
515 assert(msVeh != nullptr);
516 const double desiredTargetTimeSpacing = myGapControlState->tauTarget * currentSpeed;
517 std::pair<const MSVehicle*, double> leaderInfo;
518 if (myGapControlState->referenceVeh == nullptr) {
519 // No reference vehicle specified -> use current leader as reference
520 leaderInfo = msVeh->getLeader(MAX2(desiredTargetTimeSpacing, myGapControlState->addGapCurrent) + 20.);
521 } else {
522 // Control gap wrt reference vehicle
523 const MSVehicle* leader = myGapControlState->referenceVeh;
524 double dist = msVeh->getDistanceToPosition(leader->getPositionOnLane(), leader->getEdge()) - leader->getLength();
525 if (dist > 100000) {
526 // Reference vehicle was not found downstream the ego's route
527 // Maybe, it is behind the ego vehicle
528 dist = - leader->getDistanceToPosition(msVeh->getPositionOnLane(), msVeh->getEdge()) - leader->getLength();
529 #ifdef DEBUG_TRACI
530 if DEBUG_COND2(veh) {
531 if (dist < -100000) {
532 // also the ego vehicle is not ahead of the reference vehicle -> no CF-relation
533 std::cout << " Ego and reference vehicle are not in CF relation..." << std::endl;
534 } else {
535 std::cout << " Reference vehicle is behind ego..." << std::endl;
536 }
537 }
538 #endif
539 }
540 leaderInfo = std::make_pair(leader, dist - msVeh->getVehicleType().getMinGap());
541 }
542 const double fakeDist = MAX2(0.0, leaderInfo.second - myGapControlState->addGapCurrent);
543 #ifdef DEBUG_TRACI
544 if DEBUG_COND2(veh) {
545 const double desiredCurrentSpacing = myGapControlState->tauCurrent * currentSpeed;
546 std::cout << " Gap control active:"
547 << " currentSpeed=" << currentSpeed
548 << ", desiredTargetTimeSpacing=" << desiredTargetTimeSpacing
549 << ", desiredCurrentSpacing=" << desiredCurrentSpacing
550 << ", leader=" << (leaderInfo.first == nullptr ? "NULL" : leaderInfo.first->getID())
551 << ", dist=" << leaderInfo.second
552 << ", fakeDist=" << fakeDist
553 << ",\n tauOriginal=" << myGapControlState->tauOriginal
554 << ", tauTarget=" << myGapControlState->tauTarget
555 << ", tauCurrent=" << myGapControlState->tauCurrent
556 << std::endl;
557 }
558 #endif
559 if (leaderInfo.first != nullptr) {
560 if (myGapControlState->prevLeader != nullptr && myGapControlState->prevLeader != leaderInfo.first) {
561 // TODO: The leader changed. What to do?
562 }
563 // Remember leader
564 myGapControlState->prevLeader = leaderInfo.first;
565
566 // Calculate desired following speed assuming the alternative headway time
567 MSCFModel* cfm = (MSCFModel*) & (msVeh->getVehicleType().getCarFollowModel());
568 const double origTau = cfm->getHeadwayTime();
569 cfm->setHeadwayTime(myGapControlState->tauCurrent);
570 gapControlSpeed = MIN2(gapControlSpeed, cfm->followSpeed(msVeh, currentSpeed, fakeDist, leaderInfo.first->getSpeed(),
571 leaderInfo.first->getVehicleType().getCarFollowModel().getApparentDecel(), nullptr));
572 cfm->setHeadwayTime(origTau);
573 #ifdef DEBUG_TRACI
574 if DEBUG_COND2(veh) {
575 std::cout << " -> gapControlSpeed=" << gapControlSpeed;
576 if (myGapControlState->maxDecel > 0) {
577 std::cout << ", with maxDecel bound: " << MAX2(gapControlSpeed, currentSpeed - TS * myGapControlState->maxDecel);
578 }
579 std::cout << std::endl;
580 }
581 #endif
582 if (myGapControlState->maxDecel > 0) {
583 gapControlSpeed = MAX2(gapControlSpeed, currentSpeed - TS * myGapControlState->maxDecel);
584 }
585 }
586
587 // Update gap controller
588 // Check (1) if the gap control has established the desired gap,
589 // and (2) if it has maintained active for the given duration afterwards
590 if (myGapControlState->lastUpdate < currentTime) {
591 #ifdef DEBUG_TRACI
592 if DEBUG_COND2(veh) {
593 std::cout << " Updating GapControlState." << std::endl;
594 }
595 #endif
596 if (myGapControlState->tauCurrent == myGapControlState->tauTarget && myGapControlState->addGapCurrent == myGapControlState->addGapTarget) {
597 if (!myGapControlState->gapAttained) {
598 // Check if the desired gap was established (add the POSITIONAL_EPS to avoid infinite asymptotic behavior without having established the gap)
599 myGapControlState->gapAttained = leaderInfo.first == nullptr || leaderInfo.second > MAX2(desiredTargetTimeSpacing, myGapControlState->addGapTarget) - POSITION_EPS;
600 #ifdef DEBUG_TRACI
601 if DEBUG_COND2(veh) {
602 if (myGapControlState->gapAttained) {
603 std::cout << " Target gap was established." << std::endl;
604 }
605 }
606 #endif
607 } else {
608 // Count down remaining time if desired gap was established
609 myGapControlState->remainingDuration -= TS;
610 #ifdef DEBUG_TRACI
611 if DEBUG_COND2(veh) {
612 std::cout << " Gap control remaining duration: " << myGapControlState->remainingDuration << std::endl;
613 }
614 #endif
615 if (myGapControlState->remainingDuration <= 0) {
616 #ifdef DEBUG_TRACI
617 if DEBUG_COND2(veh) {
618 std::cout << " Gap control duration expired, deactivating control." << std::endl;
619 }
620 #endif
621 // switch off gap control
622 myGapControlState->deactivate();
623 }
624 }
625 } else {
626 // Adjust current headway values
627 myGapControlState->tauCurrent = MIN2(myGapControlState->tauCurrent + myGapControlState->timeHeadwayIncrement, myGapControlState->tauTarget);
628 myGapControlState->addGapCurrent = MIN2(myGapControlState->addGapCurrent + myGapControlState->spaceHeadwayIncrement, myGapControlState->addGapTarget);
629 }
630 }
631 if (myConsiderSafeVelocity) {
632 gapControlSpeed = MIN2(gapControlSpeed, vSafe);
633 }
634 if (myConsiderMaxAcceleration) {
635 gapControlSpeed = MIN2(gapControlSpeed, vMax);
636 }
637 if (myConsiderMaxDeceleration) {
638 gapControlSpeed = MAX2(gapControlSpeed, vMin);
639 }
640 return MIN2(speed, gapControlSpeed);
641 } else {
642 return speed;
643 }
644 }
645
646 double
getOriginalSpeed() const647 MSVehicle::Influencer::getOriginalSpeed() const {
648 return mySpeedTimeLine.empty() ? -1 : myOriginalSpeed;
649 }
650
651
652 int
influenceChangeDecision(const SUMOTime currentTime,const MSEdge & currentEdge,const int currentLaneIndex,int state)653 MSVehicle::Influencer::influenceChangeDecision(const SUMOTime currentTime, const MSEdge& currentEdge, const int currentLaneIndex, int state) {
654 // remove leading commands which are no longer valid
655 while (myLaneTimeLine.size() == 1 || (myLaneTimeLine.size() > 1 && currentTime > myLaneTimeLine[1].first)) {
656 myLaneTimeLine.erase(myLaneTimeLine.begin());
657 }
658 ChangeRequest changeRequest = REQUEST_NONE;
659 // do nothing if the time line does not apply for the current time
660 if (myLaneTimeLine.size() >= 2 && currentTime >= myLaneTimeLine[0].first) {
661 const int destinationLaneIndex = myLaneTimeLine[1].second;
662 if (destinationLaneIndex < (int)currentEdge.getLanes().size()) {
663 if (currentLaneIndex > destinationLaneIndex) {
664 changeRequest = REQUEST_RIGHT;
665 } else if (currentLaneIndex < destinationLaneIndex) {
666 changeRequest = REQUEST_LEFT;
667 } else {
668 changeRequest = REQUEST_HOLD;
669 }
670 } else if (currentEdge.getLanes().back()->getOpposite() != nullptr) { // change to opposite direction driving
671 changeRequest = REQUEST_LEFT;
672 state = state | LCA_TRACI;
673 }
674 }
675 // check whether the current reason shall be canceled / overridden
676 if ((state & LCA_WANTS_LANECHANGE_OR_STAY) != 0) {
677 // flags for the current reason
678 LaneChangeMode mode = LC_NEVER;
679 if ((state & LCA_TRACI) != 0 && myLatDist != 0) {
680 // security checks
681 if ((myTraciLaneChangePriority == LCP_ALWAYS)
682 || (myTraciLaneChangePriority == LCP_NOOVERLAP && (state & LCA_OVERLAPPING) == 0)) {
683 state &= ~(LCA_BLOCKED | LCA_OVERLAPPING);
684 }
685 // continue sublane change manoeuvre
686 return state;
687 } else if ((state & LCA_STRATEGIC) != 0) {
688 mode = myStrategicLC;
689 } else if ((state & LCA_COOPERATIVE) != 0) {
690 mode = myCooperativeLC;
691 } else if ((state & LCA_SPEEDGAIN) != 0) {
692 mode = mySpeedGainLC;
693 } else if ((state & LCA_KEEPRIGHT) != 0) {
694 mode = myRightDriveLC;
695 } else if ((state & LCA_SUBLANE) != 0) {
696 mode = mySublaneLC;
697 } else if ((state & LCA_TRACI) != 0) {
698 mode = LC_NEVER;
699 } else {
700 WRITE_WARNING("Lane change model did not provide a reason for changing (state=" + toString(state) + ", time=" + time2string(currentTime) + "\n");
701 }
702 if (mode == LC_NEVER) {
703 // cancel all lcModel requests
704 state &= ~LCA_WANTS_LANECHANGE_OR_STAY;
705 state &= ~LCA_URGENT;
706 } else if (mode == LC_NOCONFLICT && changeRequest != REQUEST_NONE) {
707 if (
708 ((state & LCA_LEFT) != 0 && changeRequest != REQUEST_LEFT) ||
709 ((state & LCA_RIGHT) != 0 && changeRequest != REQUEST_RIGHT) ||
710 ((state & LCA_STAY) != 0 && changeRequest != REQUEST_HOLD)) {
711 // cancel conflicting lcModel request
712 state &= ~LCA_WANTS_LANECHANGE_OR_STAY;
713 state &= ~LCA_URGENT;
714 }
715 } else if (mode == LC_ALWAYS) {
716 // ignore any TraCI requests
717 return state;
718 }
719 }
720 // apply traci requests
721 if (changeRequest == REQUEST_NONE) {
722 return state;
723 } else {
724 state |= LCA_TRACI;
725 // security checks
726 if ((myTraciLaneChangePriority == LCP_ALWAYS)
727 || (myTraciLaneChangePriority == LCP_NOOVERLAP && (state & LCA_OVERLAPPING) == 0)) {
728 state &= ~(LCA_BLOCKED | LCA_OVERLAPPING);
729 }
730 if (changeRequest != REQUEST_HOLD && myTraciLaneChangePriority != LCP_OPPORTUNISTIC) {
731 state |= LCA_URGENT;
732 }
733 switch (changeRequest) {
734 case REQUEST_HOLD:
735 return state | LCA_STAY;
736 case REQUEST_LEFT:
737 return state | LCA_LEFT;
738 case REQUEST_RIGHT:
739 return state | LCA_RIGHT;
740 default:
741 throw ProcessError("should not happen");
742 }
743 }
744 }
745
746
747 double
changeRequestRemainingSeconds(const SUMOTime currentTime) const748 MSVehicle::Influencer::changeRequestRemainingSeconds(const SUMOTime currentTime) const {
749 assert(myLaneTimeLine.size() >= 2);
750 assert(currentTime >= myLaneTimeLine[0].first);
751 return STEPS2TIME(myLaneTimeLine[1].first - currentTime);
752 }
753
754
755 void
setSpeedMode(int speedMode)756 MSVehicle::Influencer::setSpeedMode(int speedMode) {
757 myConsiderSafeVelocity = ((speedMode & 1) != 0);
758 myConsiderMaxAcceleration = ((speedMode & 2) != 0);
759 myConsiderMaxDeceleration = ((speedMode & 4) != 0);
760 myRespectJunctionPriority = ((speedMode & 8) != 0);
761 myEmergencyBrakeRedLight = ((speedMode & 16) != 0);
762 }
763
764
765 void
setLaneChangeMode(int value)766 MSVehicle::Influencer::setLaneChangeMode(int value) {
767 myStrategicLC = (LaneChangeMode)(value & (1 + 2));
768 myCooperativeLC = (LaneChangeMode)((value & (4 + 8)) >> 2);
769 mySpeedGainLC = (LaneChangeMode)((value & (16 + 32)) >> 4);
770 myRightDriveLC = (LaneChangeMode)((value & (64 + 128)) >> 6);
771 myTraciLaneChangePriority = (TraciLaneChangePriority)((value & (256 + 512)) >> 8);
772 mySublaneLC = (LaneChangeMode)((value & (1024 + 2048)) >> 10);
773 }
774
775
776 void
setRemoteControlled(Position xyPos,MSLane * l,double pos,double posLat,double angle,int edgeOffset,const ConstMSEdgeVector & route,SUMOTime t)777 MSVehicle::Influencer::setRemoteControlled(Position xyPos, MSLane* l, double pos, double posLat, double angle, int edgeOffset, const ConstMSEdgeVector& route, SUMOTime t) {
778 myRemoteXYPos = xyPos;
779 myRemoteLane = l;
780 myRemotePos = pos;
781 myRemotePosLat = posLat;
782 myRemoteAngle = angle;
783 myRemoteEdgeOffset = edgeOffset;
784 myRemoteRoute = route;
785 myLastRemoteAccess = t;
786 }
787
788
789 bool
isRemoteControlled() const790 MSVehicle::Influencer::isRemoteControlled() const {
791 return myLastRemoteAccess == MSNet::getInstance()->getCurrentTimeStep();
792 }
793
794
795 bool
isRemoteAffected(SUMOTime t) const796 MSVehicle::Influencer::isRemoteAffected(SUMOTime t) const {
797 return myLastRemoteAccess >= t - TIME2STEPS(10);
798 }
799
800 void
postProcessRemoteControl(MSVehicle * v)801 MSVehicle::Influencer::postProcessRemoteControl(MSVehicle* v) {
802 const bool wasOnRoad = v->isOnRoad();
803 if (v->isOnRoad()) {
804 v->onRemovalFromNet(MSMoveReminder::NOTIFICATION_TELEPORT);
805 v->getLane()->removeVehicle(v, MSMoveReminder::NOTIFICATION_TELEPORT);
806 }
807 if (myRemoteRoute.size() != 0) {
808 v->replaceRouteEdges(myRemoteRoute, -1, 0, "traci:moveToXY", true);
809 }
810 v->myCurrEdge = v->getRoute().begin() + myRemoteEdgeOffset;
811 if (myRemoteLane != nullptr && myRemotePos > myRemoteLane->getLength()) {
812 myRemotePos = myRemoteLane->getLength();
813 }
814 if (myRemoteLane != nullptr && fabs(myRemotePosLat) < 0.5 * (myRemoteLane->getWidth() + v->getVehicleType().getWidth())) {
815 MSMoveReminder::Notification notify = v->getDeparture() == NOT_YET_DEPARTED
816 ? MSMoveReminder::NOTIFICATION_DEPARTED
817 : MSMoveReminder::NOTIFICATION_TELEPORT_ARRIVED;
818 myRemoteLane->forceVehicleInsertion(v, myRemotePos, notify, myRemotePosLat);
819 v->updateBestLanes();
820 if (!wasOnRoad) {
821 v->drawOutsideNetwork(false);
822 }
823 //std::cout << "on road network p=" << myRemoteXYPos << " a=" << myRemoteAngle << " l=" << Named::getIDSecure(myRemoteLane) << " pos=" << myRemotePos << " posLat=" << myRemotePosLat << "\n";
824 } else {
825 if (v->getDeparture() == NOT_YET_DEPARTED) {
826 v->onDepart();
827 }
828 v->drawOutsideNetwork(true);
829 // see updateState
830 double vNext = v->processTraCISpeedControl(
831 v->getVehicleType().getMaxSpeed(), v->getSpeed());
832 v->setBrakingSignals(vNext);
833 v->updateWaitingTime(vNext);
834 v->myState.myPreviousSpeed = v->getSpeed();
835 v->myAcceleration = SPEED2ACCEL(vNext - v->getSpeed());
836 v->myState.mySpeed = vNext;
837 //std::cout << "outside network p=" << myRemoteXYPos << " a=" << myRemoteAngle << " l=" << Named::getIDSecure(myRemoteLane) << "\n";
838 }
839 // ensure that the position is correct (i.e. when the lanePosition is ambiguous at corners)
840 v->setRemoteState(myRemoteXYPos);
841 v->setAngle(GeomHelper::fromNaviDegree(myRemoteAngle));
842 }
843
844
845 double
implicitSpeedRemote(const MSVehicle * veh,double oldSpeed)846 MSVehicle::Influencer::implicitSpeedRemote(const MSVehicle* veh, double oldSpeed) {
847 if (veh->getPosition() == Position::INVALID) {
848 return oldSpeed;
849 }
850 double dist = veh->getPosition().distanceTo2D(myRemoteXYPos);
851 if (myRemoteLane != nullptr) {
852 // if the vehicles is frequently placed on a new edge, the route may
853 // consist only of a single edge. In this case the new edge may not be
854 // on the route so distAlongRoute will be double::max.
855 // In this case we still want a sensible speed value
856 const double distAlongRoute = veh->getDistanceToPosition(myRemotePos, &myRemoteLane->getEdge());
857 if (distAlongRoute != std::numeric_limits<double>::max()) {
858 dist = distAlongRoute;
859 }
860 }
861 //std::cout << SIMTIME << " veh=" << veh->getID() << " oldPos=" << veh->getPosition() << " traciPos=" << myRemoteXYPos << " dist=" << dist << "\n";
862 const double minSpeed = myConsiderMaxDeceleration ?
863 veh->getCarFollowModel().minNextSpeedEmergency(oldSpeed, veh) : 0;
864 const double maxSpeed = (myRemoteLane != nullptr
865 ? myRemoteLane->getVehicleMaxSpeed(veh)
866 : (veh->getLane() != nullptr
867 ? veh->getLane()->getVehicleMaxSpeed(veh)
868 : veh->getVehicleType().getMaxSpeed()));
869 return MIN2(maxSpeed, MAX2(minSpeed, DIST2SPEED(dist)));
870 }
871
872 double
implicitDeltaPosRemote(const MSVehicle * veh)873 MSVehicle::Influencer::implicitDeltaPosRemote(const MSVehicle* veh) {
874 double dist = 0;
875 if (myRemoteLane == nullptr) {
876 dist = veh->getPosition().distanceTo2D(myRemoteXYPos);
877 } else {
878 // if the vehicles is frequently placed on a new edge, the route may
879 // consist only of a single edge. In this case the new edge may not be
880 // on the route so getDistanceToPosition will return double::max.
881 // In this case we would rather not move the vehicle in executeMove
882 // (updateState) as it would result in emergency braking
883 dist = veh->getDistanceToPosition(myRemotePos, &myRemoteLane->getEdge());
884 }
885 if (DIST2SPEED(dist) > veh->getMaxSpeed()) {
886 return 0;
887 } else {
888 return dist;
889 }
890 }
891
892
893 SUMOAbstractRouter<MSEdge, SUMOVehicle>&
getRouterTT() const894 MSVehicle::Influencer::getRouterTT() const {
895 if (myRoutingMode == 1) {
896 return MSRoutingEngine::getRouterTT();
897 } else {
898 return MSNet::getInstance()->getRouterTT();
899 }
900 }
901
902
903 /* -------------------------------------------------------------------------
904 * Stop-methods
905 * ----------------------------------------------------------------------- */
906 double
getEndPos(const SUMOVehicle & veh) const907 MSVehicle::Stop::getEndPos(const SUMOVehicle& veh) const {
908 if (busstop != nullptr) {
909 return busstop->getLastFreePos(veh);
910 } else if (containerstop != nullptr) {
911 return containerstop->getLastFreePos(veh);
912 } else if (parkingarea != nullptr) {
913 return parkingarea->getLastFreePos(veh);
914 } else if (chargingStation != nullptr) {
915 return chargingStation->getLastFreePos(veh);
916 }
917 return pars.endPos;
918 }
919
920
921 std::string
getDescription() const922 MSVehicle::Stop::getDescription() const {
923 if (parkingarea != nullptr) {
924 return "parkingArea:" + parkingarea->getID();
925 } else if (containerstop != nullptr) {
926 return "containerStop:" + containerstop->getID();
927 } else if (busstop != nullptr) {
928 return "busStop:" + busstop->getID();
929 } else if (chargingStation != nullptr) {
930 return "chargingStation:" + chargingStation->getID();
931 } else {
932 return "lane:" + lane->getID() + " pos:" + toString(pars.endPos);
933 }
934 }
935
936
937 void
write(OutputDevice & dev) const938 MSVehicle::Stop::write(OutputDevice& dev) const {
939 // lots of duplication with SUMOVehicleParameter::Stop::write()
940 dev.openTag(SUMO_TAG_STOP);
941 if (busstop != nullptr) {
942 dev.writeAttr(SUMO_ATTR_BUS_STOP, busstop->getID());
943 }
944 if (containerstop != nullptr) {
945 dev.writeAttr(SUMO_ATTR_CONTAINER_STOP, containerstop->getID());
946 }
947 if (busstop == nullptr && containerstop == nullptr) {
948 dev.writeAttr(SUMO_ATTR_LANE, lane->getID());
949 dev.writeAttr(SUMO_ATTR_STARTPOS, pars.startPos);
950 dev.writeAttr(SUMO_ATTR_ENDPOS, pars.endPos);
951 }
952 if (duration >= 0) {
953 dev.writeAttr(SUMO_ATTR_DURATION, time2string(duration));
954 }
955 if (pars.until >= 0) {
956 dev.writeAttr(SUMO_ATTR_UNTIL, time2string(pars.until));
957 }
958 if (pars.triggered) {
959 dev.writeAttr(SUMO_ATTR_TRIGGERED, pars.triggered);
960 }
961 if (pars.containerTriggered) {
962 dev.writeAttr(SUMO_ATTR_CONTAINER_TRIGGERED, pars.containerTriggered);
963 }
964 if (pars.parking) {
965 dev.writeAttr(SUMO_ATTR_PARKING, pars.parking);
966 }
967 if (pars.awaitedPersons.size() > 0) {
968 dev.writeAttr(SUMO_ATTR_EXPECTED, joinToString(pars.awaitedPersons, " "));
969 }
970 if (pars.awaitedContainers.size() > 0) {
971 dev.writeAttr(SUMO_ATTR_EXPECTED_CONTAINERS, joinToString(pars.awaitedContainers, " "));
972 }
973 dev.closeTag();
974 }
975
976
977 /* -------------------------------------------------------------------------
978 * MSVehicle-methods
979 * ----------------------------------------------------------------------- */
MSVehicle(SUMOVehicleParameter * pars,const MSRoute * route,MSVehicleType * type,const double speedFactor)980 MSVehicle::MSVehicle(SUMOVehicleParameter* pars, const MSRoute* route,
981 MSVehicleType* type, const double speedFactor) :
982 MSBaseVehicle(pars, route, type, speedFactor),
983 myWaitingTime(0),
984 myWaitingTimeCollector(),
985 myTimeLoss(0),
986 myState(0, 0, 0, 0),
987 myDriverState(nullptr),
988 myActionStep(true),
989 myLastActionTime(0),
990 myLane(nullptr),
991 myLastBestLanesEdge(nullptr),
992 myLastBestLanesInternalLane(nullptr),
993 myAcceleration(0),
994 myNextTurn(0., LINKDIR_NODIR),
995 mySignals(0),
996 myAmOnNet(false),
997 myAmRegisteredAsWaitingForPerson(false),
998 myAmRegisteredAsWaitingForContainer(false),
999 myHaveToWaitOnNextLink(false),
1000 myAngle(0),
1001 myStopDist(std::numeric_limits<double>::max()),
1002 myCollisionImmunity(-1),
1003 myCachedPosition(Position::INVALID),
1004 myJunctionEntryTime(SUMOTime_MAX),
1005 myJunctionEntryTimeNeverYield(SUMOTime_MAX),
1006 myJunctionConflictEntryTime(SUMOTime_MAX),
1007 myEdgeWeights(nullptr),
1008 myInfluencer(nullptr) {
1009 if (!(*myCurrEdge)->isTazConnector()) {
1010 if (pars->departLaneProcedure == DEPART_LANE_GIVEN) {
1011 if ((*myCurrEdge)->getDepartLane(*this) == nullptr) {
1012 throw ProcessError("Invalid departlane definition for vehicle '" + pars->id + "'.");
1013 }
1014 } else {
1015 if ((*myCurrEdge)->allowedLanes(type->getVehicleClass()) == nullptr) {
1016 throw ProcessError("Vehicle '" + pars->id + "' is not allowed to depart on any lane of its first edge.");
1017 }
1018 }
1019 if (pars->departSpeedProcedure == DEPART_SPEED_GIVEN && pars->departSpeed > type->getMaxSpeed()) {
1020 throw ProcessError("Departure speed for vehicle '" + pars->id +
1021 "' is too high for the vehicle type '" + type->getID() + "'.");
1022 }
1023 }
1024 myLaneChangeModel = MSAbstractLaneChangeModel::build(type->getLaneChangeModel(), *this);
1025 myCFVariables = type->getCarFollowModel().createVehicleVariables();
1026 myDriverState = static_cast<MSDevice_DriverState*>(getDevice(typeid(MSDevice_DriverState)));
1027 myNextDriveItem = myLFLinkLanes.begin();
1028 }
1029
1030
~MSVehicle()1031 MSVehicle::~MSVehicle() {
1032 delete myEdgeWeights;
1033 for (std::vector<MSLane*>::iterator i = myFurtherLanes.begin(); i != myFurtherLanes.end(); ++i) {
1034 (*i)->resetPartialOccupation(this);
1035 }
1036 removeApproachingInformation(myLFLinkLanes);
1037 myLaneChangeModel->cleanupShadowLane();
1038 myLaneChangeModel->cleanupTargetLane();
1039 // still needed when calling resetPartialOccupation (getShadowLane) and when removing
1040 // approach information from parallel links
1041 delete myLaneChangeModel;
1042 myFurtherLanes.clear();
1043 myFurtherLanesPosLat.clear();
1044 //
1045 if (myType->isVehicleSpecific()) {
1046 MSNet::getInstance()->getVehicleControl().removeVType(myType);
1047 }
1048 delete myInfluencer;
1049 }
1050
1051
1052 void
onRemovalFromNet(const MSMoveReminder::Notification reason)1053 MSVehicle::onRemovalFromNet(const MSMoveReminder::Notification reason) {
1054 #ifdef DEBUG_ACTIONSTEPS
1055 if DEBUG_COND {
1056 std::cout << SIMTIME << " Removing vehicle '" << getID() << "' (reason: " << toString(reason) << ")" << std::endl;
1057 }
1058 #endif
1059 MSVehicleTransfer::getInstance()->remove(this);
1060 removeApproachingInformation(myLFLinkLanes);
1061 leaveLane(reason);
1062 }
1063
1064
1065 // ------------ interaction with the route
1066 bool
1067 MSVehicle::hasArrived() const {
1068 return (myCurrEdge == myRoute->end() - 1
1069 && (myStops.empty() || myStops.front().edge != myCurrEdge)
1070 && myState.myPos > myArrivalPos - POSITION_EPS
1071 && !isRemoteControlled());
1072 }
1073
1074
1075 bool
1076 MSVehicle::replaceRoute(const MSRoute* newRoute, const std::string& info, bool onInit, int offset, bool addRouteStops, bool removeStops) {
1077 const ConstMSEdgeVector& edges = newRoute->getEdges();
1078 // assert the vehicle may continue (must not be "teleported" or whatever to another position)
1079 if (!onInit && !newRoute->contains(*myCurrEdge)) {
1080 return false;
1081 }
1082
1083 // rebuild in-vehicle route information
1084 if (onInit) {
1085 myCurrEdge = newRoute->begin();
1086 } else {
1087 MSRouteIterator newCurrEdge = std::find(edges.begin() + offset, edges.end(), *myCurrEdge);;
1088 if (myLane->getEdge().isInternal() && (
1089 (newCurrEdge + 1) == edges.end() || (*(newCurrEdge + 1)) != &(myLane->getOutgoingViaLanes().front().first->getEdge()))) {
1090 return false;
1091 }
1092 myCurrEdge = newCurrEdge;
1093 }
1094 const bool stopsFromScratch = onInit && myRoute->getStops().empty();
1095 // check whether the old route may be deleted (is not used by anyone else)
1096 newRoute->addReference();
1097 myRoute->release();
1098 // assign new route
1099 myRoute = newRoute;
1100 // update arrival definition
1101 calculateArrivalParams();
1102 // save information that the vehicle was rerouted
1103 myNumberReroutes++;
1104 MSNet::getInstance()->informVehicleStateListener(this, MSNet::VEHICLE_STATE_NEWROUTE, info);
1105 // if we did not drive yet it may be best to simply reassign the stops from scratch
1106 if (stopsFromScratch) {
1107 myStops.clear();
1108 addStops(!MSGlobals::gCheckRoutes);
1109 } else {
1110 // recheck old stops
1111 MSRouteIterator searchStart = myCurrEdge;
1112 double lastPos = getPositionOnLane();
1113 if (myLane != nullptr && myLane->isInternal()
1114 && myStops.size() > 0 && !myStops.front().lane->isInternal()) {
1115 // searchStart is still incoming to the intersection so lastPos
1116 // relative to that edge must be adapted
1117 lastPos += (*myCurrEdge)->getLength();
1118 }
1119 #ifdef DEBUG_REPLACE_ROUTE
1120 if (DEBUG_COND) {
1121 std::cout << " replaceRoute on " << (*myCurrEdge)->getID() << " lane=" << myLane->getID() << "\n";
1122 }
1123 #endif
1124 for (std::list<Stop>::iterator iter = myStops.begin(); iter != myStops.end();) {
1125 double endPos = iter->getEndPos(*this);
1126 #ifdef DEBUG_REPLACE_ROUTE
1127 if (DEBUG_COND) {
1128 std::cout << " stopEdge=" << iter->lane->getEdge().getID() << " start=" << (searchStart - myCurrEdge) << " endPos=" << endPos << " lastPos=" << lastPos << "\n";
1129 }
1130 #endif
1131 if (*searchStart != &iter->lane->getEdge()
1132 || endPos < lastPos) {
1133 if (searchStart != edges.end() && !iter->reached) {
1134 searchStart++;
1135 }
1136 }
1137 lastPos = endPos;
1138
1139 iter->edge = std::find(searchStart, edges.end(), &iter->lane->getEdge());
1140 #ifdef DEBUG_REPLACE_ROUTE
1141 if (DEBUG_COND) {
1142 std::cout << " foundIndex=" << (iter->edge - myCurrEdge) << " end=" << (edges.end() - myCurrEdge) << "\n";
1143 }
1144 #endif
1145 if (iter->edge == edges.end()) {
1146 if (removeStops) {
1147 iter = myStops.erase(iter);
1148 continue;
1149 } else {
1150 assert(false);
1151 }
1152 } else {
1153 searchStart = iter->edge;
1154 }
1155 ++iter;
1156 }
1157 // add new stops
1158 if (addRouteStops) {
1159 for (std::vector<SUMOVehicleParameter::Stop>::const_iterator i = newRoute->getStops().begin(); i != newRoute->getStops().end(); ++i) {
1160 std::string error;
1161 addStop(*i, error);
1162 if (error != "") {
1163 WRITE_WARNING(error);
1164 }
1165 }
1166 }
1167 }
1168 // update best lanes (after stops were added)
1169 myLastBestLanesEdge = nullptr;
1170 myLastBestLanesInternalLane = nullptr;
1171 updateBestLanes(true, onInit ? (*myCurrEdge)->getLanes().front() : 0);
1172 assert(!removeStops || haveValidStopEdges());
1173 return true;
1174 }
1175
1176
1177 bool
1178 MSVehicle::willPass(const MSEdge* const edge) const {
1179 return std::find(myCurrEdge, myRoute->end(), edge) != myRoute->end();
1180 }
1181
1182
1183 int
1184 MSVehicle::getRoutePosition() const {
1185 return (int) std::distance(myRoute->begin(), myCurrEdge);
1186 }
1187
1188
1189 void
1190 MSVehicle::resetRoutePosition(int index) {
1191 myCurrEdge = myRoute->begin() + index;
1192 // !!! hack
1193 myArrivalPos = (*(myRoute->end() - 1))->getLanes()[0]->getLength();
1194 }
1195
1196
1197
1198 const MSEdgeWeightsStorage&
1199 MSVehicle::getWeightsStorage() const {
1200 return _getWeightsStorage();
1201 }
1202
1203
1204 MSEdgeWeightsStorage&
1205 MSVehicle::getWeightsStorage() {
1206 return _getWeightsStorage();
1207 }
1208
1209
1210 MSEdgeWeightsStorage&
1211 MSVehicle::_getWeightsStorage() const {
1212 if (myEdgeWeights == nullptr) {
1213 myEdgeWeights = new MSEdgeWeightsStorage();
1214 }
1215 return *myEdgeWeights;
1216 }
1217
1218
1219 // ------------ Interaction with move reminders
1220 void
1221 MSVehicle::workOnMoveReminders(double oldPos, double newPos, double newSpeed) {
1222 // This erasure-idiom works for all stl-sequence-containers
1223 // See Meyers: Effective STL, Item 9
1224 for (MoveReminderCont::iterator rem = myMoveReminders.begin(); rem != myMoveReminders.end();) {
1225 // XXX: calling notifyMove with newSpeed seems not the best choice. For the ballistic update, the average speed is calculated and used
1226 // although a higher order quadrature-formula might be more adequate.
1227 // For the euler case (where the speed is considered constant for each time step) it is conceivable that
1228 // the current calculations may lead to systematic errors for large time steps (compared to reality). Refs. #2579
1229 if (!rem->first->notifyMove(*this, oldPos + rem->second, newPos + rem->second, MAX2(0., newSpeed))) {
1230 #ifdef _DEBUG
1231 if (myTraceMoveReminders) {
1232 traceMoveReminder("notifyMove", rem->first, rem->second, false);
1233 }
1234 #endif
1235 rem = myMoveReminders.erase(rem);
1236 } else {
1237 #ifdef _DEBUG
1238 if (myTraceMoveReminders) {
1239 traceMoveReminder("notifyMove", rem->first, rem->second, true);
1240 }
1241 #endif
1242 ++rem;
1243 }
1244 }
1245 }
1246
1247
1248 // XXX: consider renaming...
1249 void
1250 MSVehicle::adaptLaneEntering2MoveReminder(const MSLane& enteredLane) {
1251 // save the old work reminders, patching the position information
1252 // add the information about the new offset to the old lane reminders
1253 const double oldLaneLength = myLane->getLength();
1254 for (MoveReminderCont::iterator rem = myMoveReminders.begin(); rem != myMoveReminders.end(); ++rem) {
1255 rem->second += oldLaneLength;
1256 #ifdef _DEBUG
1257 // if (rem->first==0) std::cout << "Null reminder (?!)" << std::endl;
1258 // std::cout << "Adapted MoveReminder on lane " << ((rem->first->getLane()==0) ? "NULL" : rem->first->getLane()->getID()) <<" position to " << rem->second << std::endl;
1259 if (myTraceMoveReminders) {
1260 traceMoveReminder("adaptedPos", rem->first, rem->second, true);
1261 }
1262 #endif
1263 }
1264 for (std::vector< MSMoveReminder* >::const_iterator rem = enteredLane.getMoveReminders().begin(); rem != enteredLane.getMoveReminders().end(); ++rem) {
1265 addReminder(*rem);
1266 }
1267 }
1268
1269
1270 // ------------ Other getter methods
1271 double
1272 MSVehicle::getSlope() const {
1273 if (myLane == nullptr) {
1274 return 0;
1275 }
1276 const double lp = getPositionOnLane();
1277 const double gp = myLane->interpolateLanePosToGeometryPos(lp);
1278 return myLane->getShape().slopeDegreeAtOffset(gp);
1279 }
1280
1281
1282 Position
1283 MSVehicle::getPosition(const double offset) const {
1284 if (myLane == nullptr) {
1285 // when called in the context of GUI-Drawing, the simulation step is already incremented
1286 if (myInfluencer != nullptr && myInfluencer->isRemoteAffected(MSNet::getInstance()->getCurrentTimeStep())) {
1287 return myCachedPosition;
1288 } else {
1289 return Position::INVALID;
1290 }
1291 }
1292 if (isParking()) {
1293 if (myStops.begin()->parkingarea != nullptr) {
1294 return myStops.begin()->parkingarea->getVehiclePosition(*this);
1295 } else {
1296 // position beside the road
1297 PositionVector shp = myLane->getEdge().getLanes()[0]->getShape();
1298 shp.move2side(SUMO_const_laneWidth * (MSNet::getInstance()->lefthand() ? -1 : 1));
1299 return shp.positionAtOffset(myLane->interpolateLanePosToGeometryPos(getPositionOnLane() + offset));
1300 }
1301 }
1302 const bool changingLanes = getLaneChangeModel().isChangingLanes();
1303 const double posLat = (MSNet::getInstance()->lefthand() ? 1 : -1) * getLateralPositionOnLane();
1304 if (offset == 0. && !changingLanes) {
1305 if (myCachedPosition == Position::INVALID) {
1306 myCachedPosition = validatePosition(myLane->geometryPositionAtOffset(myState.myPos, posLat));
1307 }
1308 return myCachedPosition;
1309 }
1310 Position result = validatePosition(myLane->geometryPositionAtOffset(getPositionOnLane() + offset, posLat), offset);
1311 return result;
1312 }
1313
1314
1315 Position
1316 MSVehicle::getPositionAlongBestLanes(double offset) const {
1317 assert(MSGlobals::gUsingInternalLanes);
1318 const std::vector<MSLane*>& bestLanes = getBestLanesContinuation();
1319 auto nextBestLane = bestLanes.begin();
1320 double pos = myState.myPos;
1321 const MSLane* lane = getLane();
1322 assert(lane != 0);
1323 bool success = true;
1324
1325 while (offset > 0) {
1326 // take into account lengths along internal lanes
1327 while (lane->isInternal() && offset > 0) {
1328 if (offset > lane->getLength() - pos) {
1329 offset -= lane->getLength() - pos;
1330 lane = lane->getLinkCont()[0]->getViaLaneOrLane();
1331 pos = 0.;
1332 if (lane == nullptr) {
1333 success = false;
1334 offset = 0.;
1335 }
1336 } else {
1337 pos += offset;
1338 offset = 0;
1339 }
1340 }
1341 // set nextBestLane to next non-internal lane
1342 while (nextBestLane != bestLanes.end() && *nextBestLane == nullptr) {
1343 ++nextBestLane;
1344 }
1345 if (offset > 0) {
1346 assert(!lane->isInternal());
1347 assert(lane == *nextBestLane);
1348 if (offset > lane->getLength() - pos) {
1349 offset -= lane->getLength() - pos;
1350 ++nextBestLane;
1351 assert(nextBestLane == bestLanes.end() || *nextBestLane != 0);
1352 if (nextBestLane == bestLanes.end()) {
1353 success = false;
1354 offset = 0.;
1355 } else {
1356 MSLink* link = lane->getLinkTo(*nextBestLane);
1357 assert(link != 0);
1358 lane = link->getViaLaneOrLane();
1359 pos = 0.;
1360 }
1361 } else {
1362 pos += offset;
1363 offset = 0;
1364 }
1365 }
1366
1367 }
1368
1369 if (success) {
1370 return lane->geometryPositionAtOffset(pos, -getLateralPositionOnLane());
1371 } else {
1372 return Position::INVALID;
1373 }
1374 }
1375
1376
1377 Position
1378 MSVehicle::validatePosition(Position result, double offset) const {
1379 int furtherIndex = 0;
1380 double lastLength = getPositionOnLane();
1381 while (result == Position::INVALID) {
1382 if (furtherIndex >= (int)myFurtherLanes.size()) {
1383 //WRITE_WARNING("Could not compute position for vehicle '" + getID() + "', time=" + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
1384 break;
1385 }
1386 //std::cout << SIMTIME << " veh=" << getID() << " lane=" << myLane->getID() << " pos=" << getPositionOnLane() << " posLat=" << getLateralPositionOnLane() << " offset=" << offset << " result=" << result << " i=" << furtherIndex << " further=" << myFurtherLanes.size() << "\n";
1387 MSLane* further = myFurtherLanes[furtherIndex];
1388 offset += lastLength;
1389 result = further->geometryPositionAtOffset(further->getLength() + offset, -getLateralPositionOnLane());
1390 lastLength = further->getLength();
1391 furtherIndex++;
1392 //std::cout << SIMTIME << " newResult=" << result << "\n";
1393 }
1394 return result;
1395 }
1396
1397
1398 const MSEdge*
1399 MSVehicle::getRerouteOrigin() const {
1400 // too close to the next junction, so avoid an emergency brake here
1401 if (myLane != nullptr && (myCurrEdge + 1) != myRoute->end() &&
1402 myState.myPos > myLane->getLength() - getCarFollowModel().brakeGap(myState.mySpeed, getCarFollowModel().getMaxDecel(), 0.)) {
1403 return *(myCurrEdge + 1);
1404 }
1405 if (myLane != nullptr) {
1406 return myLane->getNextNormal();
1407 }
1408 return *myCurrEdge;
1409 }
1410
1411 void
1412 MSVehicle::setAngle(double angle, bool straightenFurther) {
1413 #ifdef DEBUG_FURTHER
1414 if (DEBUG_COND) {
1415 std::cout << SIMTIME << " veh '" << getID() << " setAngle(" << angle << ") straightenFurther=" << straightenFurther << std::endl;
1416 }
1417 #endif
1418 myAngle = angle;
1419 MSLane* next = myLane;
1420 if (straightenFurther && myFurtherLanesPosLat.size() > 0) {
1421 for (int i = 0; i < (int)myFurtherLanes.size(); i++) {
1422 MSLane* further = myFurtherLanes[i];
1423 if (further->getLinkTo(next) != nullptr) {
1424 myFurtherLanesPosLat[i] = getLateralPositionOnLane();
1425 next = further;
1426 } else {
1427 break;
1428 }
1429 }
1430 }
1431 }
1432
1433
1434 void
1435 MSVehicle::setActionStepLength(double actionStepLength, bool resetOffset) {
1436 SUMOTime actionStepLengthMillisecs = SUMOVehicleParserHelper::processActionStepLength(actionStepLength);
1437 SUMOTime previousActionStepLength = getActionStepLength();
1438 const bool newActionStepLength = actionStepLengthMillisecs != previousActionStepLength;
1439 if (newActionStepLength) {
1440 getSingularType().setActionStepLength(actionStepLengthMillisecs, resetOffset);
1441 if (!resetOffset) {
1442 updateActionOffset(previousActionStepLength, actionStepLengthMillisecs);
1443 }
1444 }
1445 if (resetOffset) {
1446 resetActionOffset();
1447 }
1448 }
1449
1450
1451 double
1452 MSVehicle::computeAngle() const {
1453 Position p1;
1454 const double posLat = -myState.myPosLat; // @todo get rid of the '-'
1455 if (isParking()) {
1456 if (myStops.begin()->parkingarea != nullptr) {
1457 return myStops.begin()->parkingarea->getVehicleAngle(*this);
1458 } else {
1459 return myLane->getShape().rotationAtOffset(myLane->interpolateLanePosToGeometryPos(getPositionOnLane()));
1460 }
1461 }
1462 if (getLaneChangeModel().isChangingLanes()) {
1463 // cannot use getPosition() because it already includes the offset to the side and thus messes up the angle
1464 p1 = myLane->geometryPositionAtOffset(myState.myPos, posLat);
1465 } else {
1466 p1 = getPosition();
1467 }
1468
1469 Position p2 = getBackPosition();
1470 if (p2 == Position::INVALID) {
1471 // Handle special case of vehicle's back reaching out of the network
1472 if (myFurtherLanes.size() > 0) {
1473 p2 = myFurtherLanes.back()->geometryPositionAtOffset(0, -myFurtherLanesPosLat.back());
1474 if (p2 == Position::INVALID) {
1475 // unsuitable lane geometry
1476 p2 = myLane->geometryPositionAtOffset(0, posLat);
1477 }
1478 } else {
1479 p2 = myLane->geometryPositionAtOffset(0, posLat);
1480 }
1481 }
1482 double result = (p1 != p2 ? p2.angleTo2D(p1) :
1483 myLane->getShape().rotationAtOffset(myLane->interpolateLanePosToGeometryPos(getPositionOnLane())));
1484 if (getLaneChangeModel().isChangingLanes()) {
1485 result += DEG2RAD(getLaneChangeModel().getAngleOffset());
1486 }
1487 #ifdef DEBUG_FURTHER
1488 if (DEBUG_COND) {
1489 std::cout << SIMTIME << " computeAngle veh=" << getID() << " p1=" << p1 << " p2=" << p2 << " angle=" << RAD2DEG(result) << " naviDegree=" << GeomHelper::naviDegree(result) << "\n";
1490 }
1491 #endif
1492 return result;
1493 }
1494
1495
1496 const Position
1497 MSVehicle::getBackPosition() const {
1498 const double posLat = -myState.myPosLat; // @todo get rid of the '-'
1499 if (myState.myPos >= myType->getLength()) {
1500 // vehicle is fully on the new lane
1501 return myLane->geometryPositionAtOffset(myState.myPos - myType->getLength(), posLat);
1502 } else {
1503 if (getLaneChangeModel().isChangingLanes() && myFurtherLanes.size() > 0 && getLaneChangeModel().getShadowLane(myFurtherLanes.back()) == nullptr) {
1504 // special case where the target lane has no predecessor
1505 #ifdef DEBUG_FURTHER
1506 if (DEBUG_COND) {
1507 std::cout << " getBackPosition veh=" << getID() << " specialCase using myLane=" << myLane->getID() << " pos=0 posLat=" << myState.myPosLat << " result=" << myLane->geometryPositionAtOffset(0, posLat) << "\n";
1508 }
1509 #endif
1510 return myLane->geometryPositionAtOffset(0, posLat);
1511 } else {
1512 #ifdef DEBUG_FURTHER
1513 if (DEBUG_COND) {
1514 std::cout << " getBackPosition veh=" << getID() << " myLane=" << myLane->getID() << " further=" << toString(myFurtherLanes) << " myFurtherLanesPosLat=" << toString(myFurtherLanesPosLat) << "\n";
1515 }
1516 #endif
1517 return myFurtherLanes.size() > 0 && !getLaneChangeModel().isChangingLanes()
1518 ? myFurtherLanes.back()->geometryPositionAtOffset(getBackPositionOnLane(myFurtherLanes.back()), -myFurtherLanesPosLat.back())
1519 : myLane->geometryPositionAtOffset(0, posLat);
1520 }
1521 }
1522 }
1523
1524 // ------------
1525 bool
1526 MSVehicle::addStop(const SUMOVehicleParameter::Stop& stopPar, std::string& errorMsg, SUMOTime untilOffset, bool collision,
1527 MSRouteIterator* searchStart) {
1528 Stop stop(stopPar);
1529 stop.lane = MSLane::dictionary(stopPar.lane);
1530 if (!stop.lane->allowsVehicleClass(myType->getVehicleClass())) {
1531 errorMsg = "Vehicle '" + myParameter->id + "' is not allowed to stop on lane '" + stopPar.lane + "'.";
1532 return false;
1533 }
1534 stop.busstop = MSNet::getInstance()->getStoppingPlace(stopPar.busstop, SUMO_TAG_BUS_STOP);
1535 stop.containerstop = MSNet::getInstance()->getStoppingPlace(stopPar.containerstop, SUMO_TAG_CONTAINER_STOP);
1536 stop.parkingarea = static_cast<MSParkingArea*>(MSNet::getInstance()->getStoppingPlace(stopPar.parkingarea, SUMO_TAG_PARKING_AREA));
1537 stop.chargingStation = MSNet::getInstance()->getStoppingPlace(stopPar.chargingStation, SUMO_TAG_CHARGING_STATION);
1538 stop.duration = stopPar.duration;
1539 stop.triggered = stopPar.triggered;
1540 stop.containerTriggered = stopPar.containerTriggered;
1541 stop.timeToBoardNextPerson = 0;
1542 stop.timeToLoadNextContainer = 0;
1543 if (stopPar.until != -1) {
1544 // !!! it would be much cleaner to invent a constructor for stops which takes "until" as an argument
1545 const_cast<SUMOVehicleParameter::Stop&>(stop.pars).until += untilOffset;
1546 }
1547 stop.collision = collision;
1548 stop.reached = false;
1549 stop.numExpectedPerson = (int)stopPar.awaitedPersons.size();
1550 stop.numExpectedContainer = (int)stopPar.awaitedContainers.size();
1551 std::string stopType = "stop";
1552 std::string stopID = "";
1553 if (stop.busstop != nullptr) {
1554 stopType = "busStop";
1555 stopID = stop.busstop->getID();
1556 } else if (stop.containerstop != nullptr) {
1557 stopType = "containerStop";
1558 stopID = stop.containerstop->getID();
1559 } else if (stop.chargingStation != nullptr) {
1560 stopType = "chargingStation";
1561 stopID = stop.chargingStation->getID();
1562 } else if (stop.parkingarea != nullptr) {
1563 stopType = "parkingArea";
1564 stopID = stop.parkingarea->getID();
1565 }
1566 const std::string errorMsgStart = stopID == "" ? stopType : stopType + " '" + stopID + "'";
1567
1568 if (stop.pars.startPos < 0 || stop.pars.endPos > stop.lane->getLength()) {
1569 errorMsg = errorMsgStart + " for vehicle '" + myParameter->id + "' on lane '" + stopPar.lane + "' has an invalid position.";
1570 return false;
1571 }
1572 if (stopType != "stop" && stopType != "parkingArea" && myType->getLength() / 2. > stop.pars.endPos - stop.pars.startPos
1573 && MSNet::getInstance()->warnOnce(stopType + ":" + stopID)) {
1574 errorMsg = errorMsgStart + " on lane '" + stopPar.lane + "' is too short for vehicle '" + myParameter->id + "'.";
1575 }
1576 // if stop is on an internal edge the normal edge before the intersection is used
1577 const MSEdge* stopEdge = stop.lane->getEdge().getNormalBefore();
1578 if (searchStart == nullptr) {
1579 searchStart = &myCurrEdge;
1580 }
1581 stop.edge = std::find(*searchStart, myRoute->end(), stopEdge);
1582 MSRouteIterator prevStopEdge = myCurrEdge;
1583 MSEdge* prevEdge = nullptr;
1584 double prevStopPos = myState.myPos;
1585 // where to insert the stop
1586 std::list<Stop>::iterator iter = myStops.begin();
1587 if (stopPar.index == STOP_INDEX_END || stopPar.index >= static_cast<int>(myStops.size())) {
1588 if (myStops.size() > 0) {
1589 prevStopEdge = myStops.back().edge;
1590 prevEdge = &myStops.back().lane->getEdge();
1591 prevStopPos = myStops.back().pars.endPos;
1592 iter = myStops.end();
1593 stop.edge = std::find(prevStopEdge, myRoute->end(), stopEdge);
1594 if (prevStopEdge == stop.edge // laneEdge check is insufficient for looped routes
1595 && prevEdge == &stop.lane->getEdge() // route iterator check insufficient for internal lane stops
1596 && prevStopPos > stop.pars.endPos) {
1597 stop.edge = std::find(prevStopEdge + 1, myRoute->end(), stopEdge);
1598 }
1599 }
1600 } else {
1601 if (stopPar.index == STOP_INDEX_FIT) {
1602 while (iter != myStops.end() && (iter->edge < stop.edge ||
1603 (iter->pars.endPos < stop.pars.endPos && iter->edge == stop.edge))) {
1604 prevStopEdge = iter->edge;
1605 prevStopPos = iter->pars.endPos;
1606 ++iter;
1607 }
1608 } else {
1609 int index = stopPar.index;
1610 while (index > 0) {
1611 prevStopEdge = iter->edge;
1612 prevStopPos = iter->pars.endPos;
1613 ++iter;
1614 --index;
1615 }
1616 stop.edge = std::find(prevStopEdge, myRoute->end(), stopEdge);
1617 }
1618 }
1619 const bool sameEdgeAsLastStop = prevStopEdge == stop.edge && prevEdge == &stop.lane->getEdge();
1620 if (stop.edge == myRoute->end() || prevStopEdge > stop.edge ||
1621 (sameEdgeAsLastStop && prevStopPos > stop.pars.endPos && !collision)
1622 || (stop.lane->getEdge().isInternal() && stop.lane->getNextNormal() != *(stop.edge + 1))) {
1623 if (stop.edge != myRoute->end()) {
1624 // check if the edge occurs again later in the route
1625 MSRouteIterator next = stop.edge + 1;
1626 return addStop(stopPar, errorMsg, untilOffset, collision, &next);
1627 }
1628 errorMsg = errorMsgStart + " for vehicle '" + myParameter->id + "' on lane '" + stopPar.lane + "' is not downstream the current route.";
1629 //std::cout << " could not add stop " << errorMsgStart << " prevStops=" << myStops.size() << " searchStart=" << (*searchStart - myRoute->begin()) << " route=" << toString(myRoute->getEdges()) << "\n";
1630 return false;
1631 }
1632 // David.C:
1633 //if (!stop.parking && (myCurrEdge == stop.edge && myState.myPos > stop.endPos - getCarFollowModel().brakeGap(myState.mySpeed))) {
1634 const double endPosOffset = stop.lane->getEdge().isInternal() ? (*stop.edge)->getLength() : 0;
1635 const double distToStop = stop.pars.endPos + endPosOffset - myState.myPos;
1636 if (myCurrEdge == stop.edge && distToStop < getCarFollowModel().brakeGap(myState.mySpeed)) {
1637 if (collision) {
1638 if (distToStop < getCarFollowModel().brakeGap(myState.mySpeed, getCarFollowModel().getEmergencyDecel(), 0)) {
1639 double vNew = getCarFollowModel().maximumSafeStopSpeed(distToStop, getSpeed(), false, 0);
1640 //std::cout << SIMTIME << " veh=" << getID() << " v=" << myState.mySpeed << " distToStop=" << distToStop
1641 // << " vMinNex=" << getCarFollowModel().minNextSpeed(getSpeed(), this)
1642 // << " bg1=" << getCarFollowModel().brakeGap(myState.mySpeed)
1643 // << " bg2=" << getCarFollowModel().brakeGap(myState.mySpeed, getCarFollowModel().getEmergencyDecel(), 0)
1644 // << " vNew=" << vNew
1645 // << "\n";
1646 myState.mySpeed = MIN2(myState.mySpeed, vNew + ACCEL2SPEED(getCarFollowModel().getEmergencyDecel()));
1647 myState.myPos = MIN2(myState.myPos, stop.pars.endPos);
1648 myCachedPosition = Position::INVALID;
1649 }
1650 } else {
1651 errorMsg = errorMsgStart + " for vehicle '" + myParameter->id + "' on lane '" + stopPar.lane + "' is too close to break.";
1652 return false;
1653 }
1654 }
1655 if (!hasDeparted() && myCurrEdge == stop.edge) {
1656 double pos = -1;
1657 if (myParameter->departPosProcedure == DEPART_POS_GIVEN) {
1658 pos = myParameter->departPos;
1659 if (pos < 0.) {
1660 pos += (*myCurrEdge)->getLength();
1661 }
1662 }
1663 if (myParameter->departPosProcedure == DEPART_POS_BASE || myParameter->departPosProcedure == DEPART_POS_DEFAULT) {
1664 pos = MIN2(stop.pars.endPos + endPosOffset, basePos(*myCurrEdge));
1665 }
1666 if (pos > stop.pars.endPos + endPosOffset) {
1667 if (stop.edge != myRoute->end()) {
1668 // check if the edge occurs again later in the route
1669 MSRouteIterator next = stop.edge + 1;
1670 return addStop(stopPar, errorMsg, untilOffset, collision, &next);
1671 }
1672 errorMsg = errorMsgStart + " for vehicle '" + myParameter->id + "' on lane '" + stopPar.lane + "' is before departPos.";
1673 return false;
1674 }
1675 }
1676 if (iter != myStops.begin()) {
1677 std::list<Stop>::iterator iter2 = iter;
1678 iter2--;
1679 if (stop.pars.until >= 0 && iter2->pars.until > stop.pars.until) {
1680 errorMsg = errorMsgStart + " for vehicle '" + myParameter->id + "' on lane '" + stopPar.lane + "' ends earlier than previous stop.";
1681 }
1682 }
1683 myStops.insert(iter, stop);
1684 //std::cout << " added stop " << errorMsgStart << " totalStops=" << myStops.size() << " searchStart=" << (*searchStart - myRoute->begin())
1685 // << " routeIndex=" << (stop.edge - myRoute->begin())
1686 // << " route=" << toString(myRoute->getEdges()) << "\n";
1687 return true;
1688 }
1689
1690
1691 bool
1692 MSVehicle::replaceParkingArea(MSParkingArea* parkingArea, std::string& errorMsg) {
1693 // Check if there is a parking area to be replaced
1694 if (parkingArea == 0) {
1695 errorMsg = "new parkingArea is NULL";
1696 return false;
1697 }
1698 if (myStops.size() == 0) {
1699 errorMsg = "vehicle has no stops";
1700 return false;
1701 }
1702 if (myStops.front().parkingarea == 0) {
1703 errorMsg = "first stop is not at parkingArea";
1704 return false;
1705 }
1706 Stop& first = myStops.front();
1707 SUMOVehicleParameter::Stop& stopPar = const_cast<SUMOVehicleParameter::Stop&>(first.pars);
1708 // merge subsequent duplicate stops equals to parking area
1709 for (std::list<Stop>::iterator iter = ++myStops.begin(); iter != myStops.end();) {
1710 if (iter->parkingarea == parkingArea) {
1711 stopPar.duration += iter->duration;
1712 myStops.erase(iter++);
1713 } else {
1714 break;
1715 }
1716 }
1717 stopPar.lane = parkingArea->getLane().getID();
1718 stopPar.parkingarea = parkingArea->getID();
1719 stopPar.startPos = parkingArea->getBeginLanePosition();
1720 stopPar.endPos = parkingArea->getEndLanePosition();
1721 first.edge = myRoute->end(); // will be patched in replaceRoute
1722 first.lane = &parkingArea->getLane();
1723 first.parkingarea = parkingArea;
1724 return true;
1725 }
1726
1727
1728 MSParkingArea*
1729 MSVehicle::getNextParkingArea() {
1730 MSParkingArea* nextParkingArea = nullptr;
1731 if (!myStops.empty()) {
1732 SUMOVehicleParameter::Stop stopPar;
1733 Stop stop = myStops.front();
1734 if (!stop.reached && stop.parkingarea != nullptr) {
1735 nextParkingArea = stop.parkingarea;
1736 }
1737 }
1738 return nextParkingArea;
1739 }
1740
1741
1742 MSParkingArea*
1743 MSVehicle::getCurrentParkingArea() {
1744 MSParkingArea* currentParkingArea = nullptr;
1745 if (isParking()) {
1746 currentParkingArea = myStops.begin()->parkingarea;
1747 }
1748 return currentParkingArea;
1749 }
1750
1751
1752 bool
1753 MSVehicle::isStopped() const {
1754 return !myStops.empty() && myStops.begin()->reached /*&& myState.mySpeed < SUMO_const_haltingSpeed @todo #1864#*/;
1755 }
1756
1757
1758 bool
1759 MSVehicle::willStop() const {
1760 return !isStopped() && !myStops.empty() && myLane != nullptr && &myStops.front().lane->getEdge() == &myLane->getEdge();
1761 }
1762
1763 bool
1764 MSVehicle::isStoppedOnLane() const {
1765 return isStopped() && myStops.front().lane == myLane;
1766 }
1767
1768 bool
1769 MSVehicle::keepStopping(bool afterProcessing) const {
1770 if (isStopped()) {
1771 // after calling processNextStop, DELTA_T has already been subtracted from the duration
1772 return myStops.front().duration + (afterProcessing ? DELTA_T : 0) > 0 || isStoppedTriggered() || myStops.front().collision;
1773 } else {
1774 return false;
1775 }
1776 }
1777
1778
1779 SUMOTime
1780 MSVehicle::remainingStopDuration() const {
1781 if (isStopped()) {
1782 return myStops.front().duration;
1783 }
1784 return 0;
1785 }
1786
1787
1788 SUMOTime
1789 MSVehicle::collisionStopTime() const {
1790 return (myStops.empty() || !myStops.front().collision) ? myCollisionImmunity : MAX2((SUMOTime)0, myStops.front().duration);
1791 }
1792
1793
1794 bool
1795 MSVehicle::isParking() const {
1796 return isStopped() && myStops.begin()->pars.parking && (
1797 myStops.begin()->parkingarea == nullptr || !myStops.begin()->parkingarea->parkOnRoad());
1798 }
1799
1800
1801 bool
1802 MSVehicle::isStoppedTriggered() const {
1803 return isStopped() && (myStops.begin()->triggered || myStops.begin()->containerTriggered);
1804 }
1805
1806
1807 bool
1808 MSVehicle::isStoppedInRange(double pos) const {
1809 return isStopped() && myStops.begin()->pars.startPos <= pos && myStops.begin()->pars.endPos >= pos;
1810 }
1811
1812
1813 double
1814 MSVehicle::processNextStop(double currentVelocity) {
1815 if (myStops.empty()) {
1816 // no stops; pass
1817 return currentVelocity;
1818 }
1819
1820 #ifdef DEBUG_STOPS
1821 if (DEBUG_COND) {
1822 std::cout << "\nPROCESS_NEXT_STOP\n" << SIMTIME << " vehicle '" << getID() << "'" << std::endl;
1823 }
1824 #endif
1825
1826 Stop& stop = myStops.front();
1827 const SUMOTime time = MSNet::getInstance()->getCurrentTimeStep();
1828 if (stop.reached) {
1829
1830 #ifdef DEBUG_STOPS
1831 if (DEBUG_COND) {
1832 std::cout << SIMTIME << " vehicle '" << getID() << "' reached stop.\n"
1833 << "Remaining duration: " << STEPS2TIME(stop.duration) << std::endl;
1834 }
1835 #endif
1836 // ok, we have already reached the next stop
1837 // any waiting persons may board now
1838 MSNet* const net = MSNet::getInstance();
1839 const bool boarded = net->hasPersons() && net->getPersonControl().boardAnyWaiting(&myLane->getEdge(), this,
1840 stop.pars, stop.timeToBoardNextPerson, stop.duration) && stop.numExpectedPerson == 0;
1841 // load containers
1842 const bool loaded = net->hasContainers() && net->getContainerControl().loadAnyWaiting(&myLane->getEdge(), this,
1843 stop.pars, stop.timeToLoadNextContainer, stop.duration) && stop.numExpectedContainer == 0;
1844 if (boarded) {
1845 if (stop.busstop != nullptr) {
1846 const std::vector<MSTransportable*>& persons = myPersonDevice->getTransportables();
1847 for (std::vector<MSTransportable*>::const_iterator i = persons.begin(); i != persons.end(); ++i) {
1848 stop.busstop->removeTransportable(*i);
1849 }
1850 }
1851 // the triggering condition has been fulfilled. Maybe we want to wait a bit longer for additional riders (car pooling)
1852 stop.triggered = false;
1853 if (myAmRegisteredAsWaitingForPerson) {
1854 MSNet::getInstance()->getVehicleControl().unregisterOneWaiting(true);
1855 myAmRegisteredAsWaitingForPerson = false;
1856 #ifdef DEBUG_STOPS
1857 if (DEBUG_COND) {
1858 std::cout << SIMTIME << " vehicle '" << getID() << "' unregisters as waiting for person." << std::endl;
1859 }
1860 #endif
1861 }
1862 }
1863 if (loaded) {
1864 if (stop.containerstop != nullptr) {
1865 const std::vector<MSTransportable*>& containers = myContainerDevice->getTransportables();
1866 for (std::vector<MSTransportable*>::const_iterator i = containers.begin(); i != containers.end(); ++i) {
1867 stop.containerstop->removeTransportable(*i);
1868 }
1869 }
1870 // the triggering condition has been fulfilled
1871 stop.containerTriggered = false;
1872 if (myAmRegisteredAsWaitingForContainer) {
1873 MSNet::getInstance()->getVehicleControl().unregisterOneWaiting(false);
1874 myAmRegisteredAsWaitingForContainer = false;
1875 #ifdef DEBUG_STOPS
1876 if (DEBUG_COND) {
1877 std::cout << SIMTIME << " vehicle '" << getID() << "' unregisters as waiting for container." << std::endl;
1878 }
1879 #endif
1880 }
1881 }
1882 if (!keepStopping() && isOnRoad()) {
1883 #ifdef DEBUG_STOPS
1884 if (DEBUG_COND) {
1885 std::cout << SIMTIME << " vehicle '" << getID() << "' resumes from stopping." << std::endl;
1886 }
1887 #endif
1888 resumeFromStopping();
1889 } else {
1890 if (stop.triggered && !myAmRegisteredAsWaitingForPerson) {
1891 if (getVehicleType().getPersonCapacity() == getPersonNumber()) {
1892 WRITE_WARNING("Vehicle '" + getID() + "' ignores triggered stop on lane '" + stop.lane->getID() + "' due to capacity constraints.");
1893 stop.triggered = false;
1894 }
1895 // we can only register after waiting for one step. otherwise we might falsely signal a deadlock
1896 MSNet::getInstance()->getVehicleControl().registerOneWaiting(true);
1897 myAmRegisteredAsWaitingForPerson = true;
1898 #ifdef DEBUG_STOPS
1899 if (DEBUG_COND) {
1900 std::cout << SIMTIME << " vehicle '" << getID() << "' registers as waiting for person." << std::endl;
1901 }
1902 #endif
1903 }
1904 if (stop.containerTriggered && !myAmRegisteredAsWaitingForContainer) {
1905 if (getVehicleType().getContainerCapacity() == getContainerNumber()) {
1906 WRITE_WARNING("Vehicle '" + getID() + "' ignores container triggered stop on lane '" + stop.lane->getID() + "' due to capacity constraints.");
1907 stop.containerTriggered = false;
1908 }
1909 // we can only register after waiting for one step. otherwise we might falsely signal a deadlock
1910 MSNet::getInstance()->getVehicleControl().registerOneWaiting(false);
1911 myAmRegisteredAsWaitingForContainer = true;
1912 #ifdef DEBUG_STOPS
1913 if (DEBUG_COND) {
1914 std::cout << SIMTIME << " vehicle '" << getID() << "' registers as waiting for container." << std::endl;
1915 }
1916 #endif
1917 }
1918 // we have to wait some more time
1919 stop.duration -= getActionStepLength();
1920
1921 if (MSGlobals::gSemiImplicitEulerUpdate) {
1922 // euler
1923 return 0;
1924 } else {
1925 // ballistic:
1926 return getSpeed() - getCarFollowModel().getMaxDecel();
1927 }
1928 }
1929 } else {
1930
1931 #ifdef DEBUG_STOPS
1932 if (DEBUG_COND) {
1933 std::cout << SIMTIME << " vehicle '" << getID() << "' hasn't reached next stop." << std::endl;
1934 }
1935 #endif
1936
1937 // is the next stop on the current lane?
1938 if (stop.edge == myCurrEdge) {
1939 // get the stopping position
1940 bool useStoppingPlace = stop.busstop != nullptr || stop.containerstop != nullptr || stop.parkingarea != nullptr;
1941 bool fitsOnStoppingPlace = true;
1942 if (stop.busstop != nullptr) {
1943 fitsOnStoppingPlace &= stop.busstop->fits(myState.myPos, *this);
1944 }
1945 if (stop.containerstop != nullptr) {
1946 fitsOnStoppingPlace &= stop.containerstop->fits(myState.myPos, *this);
1947 }
1948 // if the stop is a parking area we check if there is a free position on the area
1949 if (stop.parkingarea != nullptr) {
1950 fitsOnStoppingPlace &= myState.myPos > stop.parkingarea->getBeginLanePosition();
1951 if (stop.parkingarea->getOccupancy() >= stop.parkingarea->getCapacity()) {
1952 fitsOnStoppingPlace = false;
1953 // trigger potential parkingZoneReroute
1954 for (std::vector< MSMoveReminder* >::const_iterator rem = myLane->getMoveReminders().begin(); rem != myLane->getMoveReminders().end(); ++rem) {
1955 addReminder(*rem);
1956 }
1957 MSParkingArea* oldParkingArea = stop.parkingarea;
1958 activateReminders(MSMoveReminder::NOTIFICATION_PARKING_REROUTE, myLane);
1959 if (myStops.empty() || myStops.front().parkingarea != oldParkingArea) {
1960 // rerouted, keep driving
1961 return currentVelocity;
1962 }
1963 }
1964 }
1965 const double targetPos = (myLFLinkLanes.empty()
1966 ? stop.getEndPos(*this) // loading simulation state
1967 : myState.myPos + myLFLinkLanes.front().myDistance); // avoid concurrent read/write to stoppingPlace during execute move;
1968 const double reachedThreshold = (useStoppingPlace ? targetPos - STOPPING_PLACE_OFFSET : stop.pars.startPos) - NUMERICAL_EPS;
1969 #ifdef DEBUG_STOPS
1970 if (DEBUG_COND) {
1971 std::cout << " pos=" << myState.pos() << " speed=" << currentVelocity << " targetPos=" << targetPos << " fits=" << fitsOnStoppingPlace << " reachedThresh=" << reachedThreshold << "\n";
1972 }
1973 #endif
1974 if (myState.pos() >= reachedThreshold && fitsOnStoppingPlace && currentVelocity <= SUMO_const_haltingSpeed && myLane == stop.lane) {
1975 // ok, we may stop (have reached the stop)
1976 stop.reached = true;
1977 #ifdef DEBUG_STOPS
1978 if (DEBUG_COND) {
1979 std::cout << SIMTIME << " vehicle '" << getID() << "' reached next stop." << std::endl;
1980 }
1981 #endif
1982 if (MSStopOut::active()) {
1983 MSStopOut::getInstance()->stopStarted(this, getPersonNumber(), getContainerNumber(), time);
1984 }
1985 MSNet::getInstance()->getVehicleControl().addWaiting(&myLane->getEdge(), this);
1986 MSNet::getInstance()->informVehicleStateListener(this, MSNet::VEHICLE_STATE_STARTING_STOP);
1987 // compute stopping time
1988 if (stop.pars.until >= 0) {
1989 if (stop.duration == -1) {
1990 stop.duration = stop.pars.until - time;
1991 } else {
1992 stop.duration = MAX2(stop.duration, stop.pars.until - time);
1993 }
1994 }
1995 if (stop.busstop != nullptr) {
1996 // let the bus stop know the vehicle
1997 stop.busstop->enter(this, myState.pos() + getVehicleType().getMinGap(), myState.pos() - myType->getLength());
1998 }
1999 if (stop.containerstop != nullptr) {
2000 // let the container stop know the vehicle
2001 stop.containerstop->enter(this, myState.pos() + getVehicleType().getMinGap(), myState.pos() - myType->getLength());
2002 }
2003 if (stop.parkingarea != nullptr) {
2004 // let the parking area know the vehicle
2005 stop.parkingarea->enter(this, myState.pos() + getVehicleType().getMinGap(), myState.pos() - myType->getLength());
2006 }
2007
2008 if (stop.pars.tripId != "") {
2009 ((SUMOVehicleParameter&)getParameter()).setParameter("tripId", stop.pars.tripId);
2010 }
2011 }
2012 }
2013 }
2014 return currentVelocity;
2015 }
2016
2017
2018 const ConstMSEdgeVector
2019 MSVehicle::getStopEdges(double& firstPos, double& lastPos) const {
2020 assert(haveValidStopEdges());
2021 ConstMSEdgeVector result;
2022 const Stop* prev = nullptr;
2023 for (const Stop& stop : myStops) {
2024 if (stop.reached) {
2025 continue;
2026 }
2027 const double stopPos = stop.getEndPos(*this);
2028 if (prev == nullptr
2029 || prev->edge != stop.edge
2030 || prev->getEndPos(*this) > stopPos) {
2031 result.push_back(*stop.edge);
2032 }
2033 prev = &stop;
2034 if (firstPos < 0) {
2035 firstPos = stopPos;
2036 }
2037 lastPos = stopPos;
2038 }
2039 //std::cout << "getStopEdges veh=" << getID() << " result=" << toString(result) << "\n";
2040 return result;
2041 }
2042
2043
2044 std::vector<std::pair<int, double> >
2045 MSVehicle::getStopIndices() const {
2046 std::vector<std::pair<int, double> > result;
2047 for (std::list<Stop>::const_iterator iter = myStops.begin(); iter != myStops.end(); ++iter) {
2048 result.push_back(std::make_pair(
2049 (int)(iter->edge - myRoute->begin()),
2050 iter->getEndPos(*this)));
2051 }
2052 return result;
2053 }
2054
2055 bool
2056 MSVehicle::stopsAt(MSStoppingPlace* stop) const {
2057 if (stop == nullptr) {
2058 return false;
2059 }
2060 for (const Stop& s : myStops) {
2061 if (s.busstop == stop
2062 || s.containerstop == stop
2063 || s.parkingarea == stop
2064 || s.chargingStation == stop) {
2065 return true;
2066 }
2067 }
2068 return false;
2069 }
2070
2071 double
2072 MSVehicle::getBrakeGap() const {
2073 return getCarFollowModel().brakeGap(getSpeed());
2074 }
2075
2076
2077 double
2078 MSVehicle::basePos(const MSEdge* edge) const {
2079 double result = MIN2(getVehicleType().getLength() + POSITION_EPS, edge->getLength());
2080 if (hasStops()
2081 && myStops.front().edge == myRoute->begin()
2082 && (&myStops.front().lane->getEdge()) == *myStops.front().edge) {
2083 result = MIN2(result, MAX2(0.0, myStops.front().getEndPos(*this)));
2084 }
2085 return result;
2086 }
2087
2088
2089 bool
2090 MSVehicle::checkActionStep(const SUMOTime t) {
2091 myActionStep = isActionStep(t);
2092 if (myActionStep) {
2093 myLastActionTime = t;
2094 }
2095 return myActionStep;
2096 }
2097
2098
2099 void
2100 MSVehicle::resetActionOffset(const SUMOTime timeUntilNextAction) {
2101 myLastActionTime = MSNet::getInstance()->getCurrentTimeStep() + timeUntilNextAction;
2102 }
2103
2104
2105 void
2106 MSVehicle::updateActionOffset(const SUMOTime oldActionStepLength, const SUMOTime newActionStepLength) {
2107 SUMOTime now = MSNet::getInstance()->getCurrentTimeStep();
2108 SUMOTime timeSinceLastAction = now - myLastActionTime;
2109 if (timeSinceLastAction == 0) {
2110 // Action was scheduled now, may be delayed be new action step length
2111 timeSinceLastAction = oldActionStepLength;
2112 }
2113 if (timeSinceLastAction >= newActionStepLength) {
2114 // Action point required in this step
2115 myLastActionTime = now;
2116 } else {
2117 SUMOTime timeUntilNextAction = newActionStepLength - timeSinceLastAction;
2118 resetActionOffset(timeUntilNextAction);
2119 }
2120 }
2121
2122
2123
2124 void
2125 MSVehicle::planMove(const SUMOTime t, const MSLeaderInfo& ahead, const double lengthsInFront) {
2126 #ifdef DEBUG_PLAN_MOVE
2127 if (DEBUG_COND) {
2128 std::cout
2129 << "\nPLAN_MOVE\n"
2130 << SIMTIME
2131 << std::setprecision(gPrecision)
2132 << " veh=" << getID()
2133 << " lane=" << myLane->getID()
2134 << " pos=" << getPositionOnLane()
2135 << " posLat=" << getLateralPositionOnLane()
2136 << " speed=" << getSpeed()
2137 << "\n";
2138 }
2139 #endif
2140 // Update the driver state
2141 if (hasDriverState()) {
2142 myDriverState->update();
2143 setActionStepLength(myDriverState->getDriverState()->getActionStepLength(), false);
2144 }
2145
2146 if (!checkActionStep(t)) {
2147 #ifdef DEBUG_ACTIONSTEPS
2148 if DEBUG_COND {
2149 std::cout << STEPS2TIME(t) << " vehicle '" << getID() << "' skips action." << std::endl;
2150 }
2151 #endif
2152 // During non-action passed drive items still need to be removed
2153 // @todo rather work with updating myCurrentDriveItem (refs #3714)
2154 removePassedDriveItems();
2155 return;
2156 } else {
2157 #ifdef DEBUG_ACTIONSTEPS
2158 if DEBUG_COND {
2159 std::cout << STEPS2TIME(t) << " vehicle = '" << getID() << "' takes action." << std::endl;
2160 }
2161 #endif
2162
2163 myLFLinkLanesPrev = myLFLinkLanes;
2164 planMoveInternal(t, ahead, myLFLinkLanes, myStopDist, myNextTurn);
2165 #ifdef DEBUG_PLAN_MOVE
2166 if (DEBUG_COND) {
2167 DriveItemVector::iterator i;
2168 for (i = myLFLinkLanes.begin(); i != myLFLinkLanes.end(); ++i) {
2169 std::cout
2170 << " vPass=" << (*i).myVLinkPass
2171 << " vWait=" << (*i).myVLinkWait
2172 << " linkLane=" << ((*i).myLink == 0 ? "NULL" : (*i).myLink->getViaLaneOrLane()->getID())
2173 << " request=" << (*i).mySetRequest
2174 << "\n";
2175 }
2176 }
2177 #endif
2178 checkRewindLinkLanes(lengthsInFront, myLFLinkLanes);
2179 myNextDriveItem = myLFLinkLanes.begin();
2180 }
2181 getLaneChangeModel().resetChanged();
2182 }
2183
2184 void
2185 MSVehicle::planMoveInternal(const SUMOTime t, MSLeaderInfo ahead, DriveItemVector& lfLinks, double& myStopDist, std::pair<double, LinkDirection>& myNextTurn) const {
2186 lfLinks.clear();
2187 myStopDist = std::numeric_limits<double>::max();
2188 //
2189 const MSCFModel& cfModel = getCarFollowModel();
2190 const double vehicleLength = getVehicleType().getLength();
2191 const double maxV = cfModel.maxNextSpeed(myState.mySpeed, this);
2192 const bool opposite = getLaneChangeModel().isOpposite();
2193 double laneMaxV = myLane->getVehicleMaxSpeed(this);
2194 const double vMinComfortable = cfModel.minNextSpeed(getSpeed(), this);
2195 double lateralShift = 0;
2196 if (isRailway((SVCPermissions)getVehicleType().getVehicleClass())) {
2197 // speed limits must hold for the whole length of the train
2198 for (MSLane* l : myFurtherLanes) {
2199 laneMaxV = MIN2(laneMaxV, l->getVehicleMaxSpeed(this));
2200 }
2201 }
2202 // speed limits are not emergencies (e.g. when the limit changes suddenly due to TraCI or a variableSpeedSignal)
2203 laneMaxV = MAX2(laneMaxV, vMinComfortable);
2204 if (myInfluencer && !myInfluencer->considerSafeVelocity()) {
2205 laneMaxV = std::numeric_limits<double>::max();
2206 }
2207 // v is the initial maximum velocity of this vehicle in this step
2208 double v = MIN2(maxV, laneMaxV);
2209 if (myInfluencer != nullptr) {
2210 const double vMin = MAX2(0., cfModel.minNextSpeed(myState.mySpeed, this));
2211 v = myInfluencer->influenceSpeed(MSNet::getInstance()->getCurrentTimeStep(), v, v, vMin, maxV);
2212 v = myInfluencer->gapControlSpeed(MSNet::getInstance()->getCurrentTimeStep(), this, v, v, vMin, maxV);
2213 }
2214 // all links within dist are taken into account (potentially)
2215 const double dist = SPEED2DIST(maxV) + cfModel.brakeGap(maxV);
2216
2217 const std::vector<MSLane*>& bestLaneConts = getBestLanesContinuation();
2218 #ifdef DEBUG_PLAN_MOVE
2219 if (DEBUG_COND) {
2220 std::cout << " dist=" << dist << " bestLaneConts=" << toString(bestLaneConts) << "\n";
2221 }
2222 #endif
2223 assert(bestLaneConts.size() > 0);
2224 bool hadNonInternal = false;
2225 // the distance already "seen"; in the following always up to the end of the current "lane"
2226 double seen = opposite ? myState.myPos : myLane->getLength() - myState.myPos;
2227 myNextTurn.first = seen;
2228 myNextTurn.second = LINKDIR_NODIR;
2229 bool encounteredTurn = (MSGlobals::gLateralResolution <= 0); // next turn is only needed for sublane
2230 double seenNonInternal = 0;
2231 double seenInternal = myLane->isInternal() ? seen : 0;
2232 double vLinkPass = MIN2(cfModel.estimateSpeedAfterDistance(seen, v, cfModel.getMaxAccel()), laneMaxV); // upper bound
2233 int view = 0;
2234 DriveProcessItem* lastLink = nullptr;
2235 bool slowedDownForMinor = false; // whether the vehicle already had to slow down on approach to a minor link
2236 // iterator over subsequent lanes and fill lfLinks until stopping distance or stopped
2237 const MSLane* lane = opposite ? myLane->getOpposite() : myLane;
2238 assert(lane != 0);
2239 const MSLane* leaderLane = myLane;
2240 #ifdef _MSC_VER
2241 #pragma warning(push)
2242 #pragma warning(disable: 4127) // do not warn about constant conditional expression
2243 #endif
2244 while (true) {
2245 #ifdef _MSC_VER
2246 #pragma warning(pop)
2247 #endif
2248 // check leader on lane
2249 // leader is given for the first edge only
2250 if (opposite &&
2251 (leaderLane->getVehicleNumberWithPartials() > 1
2252 || (leaderLane != myLane && leaderLane->getVehicleNumber() > 0))) {
2253 // find opposite-driving leader that must be respected on the currently looked at lane
2254 // XXX make sure to look no further than leaderLane
2255 CLeaderDist leader = leaderLane->getOppositeLeader(this, getPositionOnLane(), true);
2256 ahead.clear();
2257 if (leader.first != 0 && leader.first->getLane() == leaderLane && leader.first->getLaneChangeModel().isOpposite()) {
2258 ahead.addLeader(leader.first, true);
2259 }
2260 }
2261 adaptToLeaders(ahead, lateralShift, seen, lastLink, leaderLane, v, vLinkPass);
2262 #ifdef DEBUG_PLAN_MOVE
2263 if (DEBUG_COND) {
2264 std::cout << "\nv = " << v << "\n";
2265
2266 }
2267 #endif
2268 // XXX efficiently adapt to shadow leaders using neighAhead by iteration over the whole edge in parallel (lanechanger-style)
2269 if (getLaneChangeModel().getShadowLane() != nullptr) {
2270 // also slow down for leaders on the shadowLane relative to the current lane
2271 const MSLane* shadowLane = getLaneChangeModel().getShadowLane(lane);
2272 if (shadowLane != nullptr) {
2273 const double latOffset = getLane()->getRightSideOnEdge() - getLaneChangeModel().getShadowLane()->getRightSideOnEdge();
2274 adaptToLeaders(shadowLane->getLastVehicleInformation(this, latOffset, lane->getLength() - seen),
2275 latOffset,
2276 seen, lastLink, shadowLane, v, vLinkPass);
2277 }
2278 }
2279 // adapt to pedestrians on the same lane
2280 if (lane->getEdge().getPersons().size() > 0 && MSPModel::getModel()->hasPedestrians(lane)) {
2281 const double relativePos = lane->getLength() - seen;
2282 #ifdef DEBUG_PLAN_MOVE
2283 if (DEBUG_COND) {
2284 std::cout << SIMTIME << " adapt to pedestrians on lane=" << lane->getID() << " relPos=" << relativePos << "\n";
2285 }
2286 #endif
2287 PersonDist leader = MSPModel::getModel()->nextBlocking(lane, relativePos,
2288 getRightSideOnLane(), getRightSideOnLane() + getVehicleType().getWidth(), ceil(getSpeed() / cfModel.getMaxDecel()));
2289 if (leader.first != 0) {
2290 const double stopSpeed = cfModel.stopSpeed(this, getSpeed(), leader.second - getVehicleType().getMinGap());
2291 v = MIN2(v, stopSpeed);
2292 #ifdef DEBUG_PLAN_MOVE
2293 if (DEBUG_COND) {
2294 std::cout << SIMTIME << " pedLeader=" << leader.first->getID() << " dist=" << leader.second << " v=" << v << "\n";
2295 }
2296 #endif
2297 }
2298 }
2299
2300 // process stops
2301 if (!myStops.empty() && &myStops.begin()->lane->getEdge() == &lane->getEdge() && !myStops.begin()->reached
2302 // ignore stops that occur later in a looped route
2303 && myStops.front().edge == myCurrEdge + view) {
2304 // we are approaching a stop on the edge; must not drive further
2305 const Stop& stop = *myStops.begin();
2306 double endPos = stop.getEndPos(*this) + NUMERICAL_EPS;
2307 if (stop.parkingarea != nullptr) {
2308 // leave enough space so parking vehicles can exit
2309 endPos = stop.parkingarea->getLastFreePosWithReservation(t, *this);
2310 }
2311 myStopDist = seen + endPos - lane->getLength();
2312 // regular stops are not emergencies
2313 const double stopSpeed = MAX2(cfModel.stopSpeed(this, getSpeed(), myStopDist), vMinComfortable);
2314 if (lastLink != nullptr) {
2315 lastLink->adaptLeaveSpeed(cfModel.stopSpeed(this, vLinkPass, endPos));
2316 }
2317 v = MIN2(v, stopSpeed);
2318 if (lane->isInternal()) {
2319 MSLinkCont::const_iterator exitLink = MSLane::succLinkSec(*this, view + 1, *lane, bestLaneConts);
2320 assert(!lane->isLinkEnd(exitLink));
2321 bool dummySetRequest;
2322 double dummyVLinkWait;
2323 checkLinkLeaderCurrentAndParallel(*exitLink, lane, seen, lastLink, v, vLinkPass, dummyVLinkWait, dummySetRequest);
2324 }
2325 lfLinks.push_back(DriveProcessItem(v, myStopDist));
2326
2327 #ifdef DEBUG_PLAN_MOVE
2328 if (DEBUG_COND) {
2329 std::cout << "\n" << SIMTIME << " next stop: distance = " << myStopDist << " requires stopSpeed = " << stopSpeed << "\n";
2330
2331 }
2332 #endif
2333
2334 break;
2335 }
2336
2337 // move to next lane
2338 // get the next link used
2339 MSLinkCont::const_iterator link = MSLane::succLinkSec(*this, view + 1, *lane, bestLaneConts);
2340
2341 // Check whether this is a turn (to save info about the next upcoming turn)
2342 if (!encounteredTurn) {
2343 if (!lane->isLinkEnd(link) && lane->getLinkCont().size() > 1) {
2344 LinkDirection linkDir = (*link)->getDirection();
2345 switch (linkDir) {
2346 case LINKDIR_STRAIGHT:
2347 case LINKDIR_NODIR:
2348 break;
2349 default:
2350 myNextTurn.first = seen;
2351 myNextTurn.second = linkDir;
2352 encounteredTurn = true;
2353 #ifdef DEBUG_NEXT_TURN
2354 if DEBUG_COND {
2355 std::cout << SIMTIME << " veh '" << getID() << "' nextTurn: " << toString(myNextTurn.second)
2356 << " at " << myNextTurn.first << "m." << std::endl;
2357 }
2358 #endif
2359 }
2360 }
2361 }
2362
2363 // check whether the vehicle is on its final edge
2364 if (myCurrEdge + view + 1 == myRoute->end()) {
2365 const double arrivalSpeed = (myParameter->arrivalSpeedProcedure == ARRIVAL_SPEED_GIVEN ?
2366 myParameter->arrivalSpeed : laneMaxV);
2367 // subtract the arrival speed from the remaining distance so we get one additional driving step with arrival speed
2368 // XXX: This does not work for ballistic update refs #2579
2369 const double distToArrival = seen + myArrivalPos - lane->getLength() - SPEED2DIST(arrivalSpeed);
2370 const double va = MAX2(NUMERICAL_EPS, cfModel.freeSpeed(this, getSpeed(), distToArrival, arrivalSpeed));
2371 v = MIN2(v, va);
2372 if (lastLink != nullptr) {
2373 lastLink->adaptLeaveSpeed(va);
2374 }
2375 lfLinks.push_back(DriveProcessItem(v, seen, lane->getEdge().isFringe() ? 1000 : 0));
2376 break;
2377 }
2378 // check whether the lane or the shadowLane is a dead end (allow some leeway on intersections)
2379 if (lane->isLinkEnd(link) ||
2380 ((*link)->getViaLane() == nullptr
2381 && getLateralOverlap() > POSITION_EPS
2382 // do not get stuck on narrow edges
2383 && getVehicleType().getWidth() <= lane->getEdge().getWidth()
2384 // this is the exit link of a junction. The normal edge should support the shadow
2385 && ((getLaneChangeModel().getShadowLane((*link)->getLane()) == nullptr)
2386 // the internal lane after an internal junction has no parallel lane. make sure there is no shadow before continuing
2387 || (lane->getEdge().isInternal() && lane->getIncomingLanes()[0].lane->getEdge().isInternal()))
2388 // ignore situations where the shadow lane is part of a double-connection with the current lane
2389 && (getLaneChangeModel().getShadowLane() == nullptr
2390 || getLaneChangeModel().getShadowLane()->getLinkCont().size() == 0
2391 || getLaneChangeModel().getShadowLane()->getLinkCont().front()->getLane() != (*link)->getLane())
2392 )) {
2393 double va = cfModel.stopSpeed(this, getSpeed(), seen);
2394 if (lastLink != nullptr) {
2395 lastLink->adaptLeaveSpeed(va);
2396 }
2397 if (getLaneChangeModel().getCommittedSpeed() > 0) {
2398 v = MIN2(getLaneChangeModel().getCommittedSpeed(), v);
2399 } else {
2400 v = MIN2(va, v);
2401 }
2402 #ifdef DEBUG_PLAN_MOVE
2403 if (DEBUG_COND) {
2404 std::cout << " braking for link end lane=" << lane->getID() << " seen=" << seen
2405 << " overlap=" << getLateralOverlap() << " va=" << va << " committed=" << getLaneChangeModel().getCommittedSpeed() << " v=" << v << "\n";
2406
2407 }
2408 #endif
2409 if (lane->isLinkEnd(link)) {
2410 lfLinks.push_back(DriveProcessItem(v, seen));
2411 break;
2412 }
2413 }
2414 lateralShift += (*link)->getLateralShift();
2415 const bool yellowOrRed = (*link)->haveRed() || (*link)->haveYellow();
2416 // We distinguish 3 cases when determining the point at which a vehicle stops:
2417 // - links that require stopping: here the vehicle needs to stop close to the stop line
2418 // to ensure it gets onto the junction in the next step. Otherwise the vehicle would 'forget'
2419 // that it already stopped and need to stop again. This is necessary pending implementation of #999
2420 // - red/yellow light: here the vehicle 'knows' that it will have priority eventually and does not need to stop on a precise spot
2421 // - other types of minor links: the vehicle needs to stop as close to the junction as necessary
2422 // to minimize the time window for passing the junction. If the
2423 // vehicle 'decides' to accelerate and cannot enter the junction in
2424 // the next step, new foes may appear and cause a collision (see #1096)
2425 // - major links: stopping point is irrelevant
2426 double laneStopOffset;
2427 const double majorStopOffset = MAX2(DIST_TO_STOPLINE_EXPECT_PRIORITY, lane->getStopOffset(this));
2428 const double minorStopOffset = lane->getStopOffset(this);
2429 const double brakeDist = cfModel.brakeGap(myState.mySpeed, cfModel.getMaxDecel(), 0);
2430 const bool canBrakeBeforeLaneEnd = seen >= brakeDist;
2431 const bool canBrakeBeforeStopLine = seen - lane->getStopOffset(this) >= brakeDist;
2432 if (yellowOrRed) {
2433 // Wait at red traffic light with full distance if possible
2434 if (canBrakeBeforeLaneEnd) {
2435 laneStopOffset = MIN2(majorStopOffset, seen - brakeDist);
2436 } else {
2437 laneStopOffset = majorStopOffset;
2438 }
2439 } else if ((*link)->havePriority()) {
2440 // On priority link, we should never stop below visibility distance
2441 laneStopOffset = MIN2((*link)->getFoeVisibilityDistance() - POSITION_EPS, majorStopOffset);
2442 } else {
2443 // On minor link, we should likewise never stop below visibility distance
2444 laneStopOffset = MIN2((*link)->getFoeVisibilityDistance() - POSITION_EPS, minorStopOffset);
2445 }
2446 laneStopOffset = MAX2(POSITION_EPS, laneStopOffset);
2447 const double stopDist = MAX2(0., seen - laneStopOffset);
2448
2449 #ifdef DEBUG_PLAN_MOVE
2450 if DEBUG_COND {
2451 std::cout << SIMTIME << " veh=" << getID() << " effective stopOffset on lane '" << lane->getID()
2452 << "' is " << laneStopOffset << " (-> stopDist=" << stopDist << ")" << std::endl;
2453 }
2454 #endif
2455 // check for train direction reversal
2456 if (canBrakeBeforeLaneEnd && canReverse(laneMaxV)) {
2457 lfLinks.push_back(DriveProcessItem(*link, vMinComfortable, vMinComfortable, false, t, vMinComfortable, 0, 0, seen));
2458 }
2459
2460 // check whether we need to slow down in order to finish a continuous lane change
2461 if (getLaneChangeModel().isChangingLanes()) {
2462 if ( // slow down to finish lane change before a turn lane
2463 ((*link)->getDirection() == LINKDIR_LEFT || (*link)->getDirection() == LINKDIR_RIGHT) ||
2464 // slow down to finish lane change before the shadow lane ends
2465 (getLaneChangeModel().getShadowLane() != nullptr &&
2466 (*link)->getViaLaneOrLane()->getParallelLane(getLaneChangeModel().getShadowDirection()) == nullptr)) {
2467 // XXX maybe this is too harsh. Vehicles could cut some corners here
2468 const double timeRemaining = STEPS2TIME(getLaneChangeModel().remainingTime());
2469 assert(timeRemaining != 0);
2470 // XXX: Euler-logic (#860), but I couldn't identify problems from this yet (Leo). Refs. #2575
2471 const double va = MAX2(cfModel.stopSpeed(this, getSpeed(), seen - POSITION_EPS),
2472 (seen - POSITION_EPS) / timeRemaining);
2473 #ifdef DEBUG_PLAN_MOVE
2474 if (DEBUG_COND) {
2475 std::cout << SIMTIME << " veh=" << getID() << " slowing down to finish continuous change before"
2476 << " link=" << (*link)->getViaLaneOrLane()->getID()
2477 << " timeRemaining=" << timeRemaining
2478 << " v=" << v
2479 << " va=" << va
2480 << std::endl;
2481 }
2482 #endif
2483 v = MIN2(va, v);
2484 }
2485 }
2486
2487 // - always issue a request to leave the intersection we are currently on
2488 const bool leavingCurrentIntersection = myLane->getEdge().isInternal() && lastLink == nullptr;
2489 // - do not issue a request to enter an intersection after we already slowed down for an earlier one
2490 const bool abortRequestAfterMinor = slowedDownForMinor && (*link)->getInternalLaneBefore() == nullptr;
2491 // - even if red, if we cannot break we should issue a request
2492 bool setRequest = (v > NUMERICAL_EPS_SPEED && !abortRequestAfterMinor) || (leavingCurrentIntersection);
2493
2494 double vLinkWait = MIN2(v, cfModel.stopSpeed(this, getSpeed(), stopDist));
2495 #ifdef DEBUG_PLAN_MOVE
2496 if (DEBUG_COND) {
2497 std::cout
2498 << " stopDist=" << stopDist
2499 << " vLinkWait=" << vLinkWait
2500 << " brakeDist=" << brakeDist
2501 << " seen=" << seen
2502 << " leaveIntersection=" << leavingCurrentIntersection
2503 << " setRequest=" << setRequest
2504 //<< std::setprecision(16)
2505 //<< " v=" << v
2506 //<< " speedEps=" << NUMERICAL_EPS_SPEED
2507 //<< std::setprecision(gPrecision)
2508 << "\n";
2509 }
2510 #endif
2511
2512 // TODO: Consider option on the CFModel side to allow red/yellow light violation
2513
2514 if (yellowOrRed && canBrakeBeforeStopLine && !ignoreRed(*link, canBrakeBeforeStopLine)) {
2515 if (lane->isInternal()) {
2516 checkLinkLeaderCurrentAndParallel(*link, lane, seen, lastLink, v, vLinkPass, vLinkWait, setRequest);
2517 }
2518 // the vehicle is able to brake in front of a yellow/red traffic light
2519 lfLinks.push_back(DriveProcessItem(*link, vLinkWait, vLinkWait, false, t + TIME2STEPS(seen / MAX2(vLinkWait, NUMERICAL_EPS)), vLinkWait, 0, 0, seen));
2520 //lfLinks.push_back(DriveProcessItem(0, vLinkWait, vLinkWait, false, 0, 0, stopDist));
2521 break;
2522 }
2523
2524 if (ignoreRed(*link, canBrakeBeforeStopLine) && STEPS2TIME(t - (*link)->getLastStateChange()) > 2) {
2525 // restrict speed when ignoring a red light
2526 const double redSpeed = MIN2(v, getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_DRIVE_RED_SPEED, v));
2527 const double va = MAX2(redSpeed, cfModel.freeSpeed(this, getSpeed(), seen, redSpeed));
2528 v = MIN2(va, v);
2529 #ifdef DEBUG_PLAN_MOVE
2530 if (DEBUG_COND) std::cout
2531 << " ignoreRed spent=" << STEPS2TIME(t - (*link)->getLastStateChange())
2532 << " redSpeed=" << redSpeed
2533 << " va=" << va
2534 << " v=" << v
2535 << "\n";
2536 #endif
2537 }
2538
2539
2540 checkLinkLeaderCurrentAndParallel(*link, lane, seen, lastLink, v, vLinkPass, vLinkWait, setRequest);
2541
2542 if (lastLink != nullptr) {
2543 lastLink->adaptLeaveSpeed(laneMaxV);
2544 }
2545 double arrivalSpeed = vLinkPass;
2546 // vehicles should decelerate when approaching a minor link
2547 // - unless they are close enough to have clear visibility of all relevant foe lanes and may start to accelerate again
2548 // - and unless they are so close that stopping is impossible (i.e. when a green light turns to yellow when close to the junction)
2549
2550 // whether the vehicle/driver is close enough to the link to see all possible foes #2123
2551 double visibilityDistance = (*link)->getFoeVisibilityDistance();
2552 double determinedFoePresence = seen <= visibilityDistance;
2553 // // VARIANT: account for time needed to recognize whether relevant vehicles are on the foe lanes. (Leo)
2554 // double foeRecognitionTime = 0.0;
2555 // double determinedFoePresence = seen < visibilityDistance - myState.mySpeed*foeRecognitionTime;
2556
2557 #ifdef DEBUG_PLAN_MOVE
2558 if (DEBUG_COND) {
2559 std::cout << " approaching link=" << (*link)->getViaLaneOrLane()->getID() << " prio=" << (*link)->havePriority() << " seen=" << seen << " visibilityDistance=" << visibilityDistance << " brakeDist=" << brakeDist << "\n";
2560 }
2561 #endif
2562
2563 if (!(*link)->havePriority() && !determinedFoePresence && brakeDist < seen && !(*link)->lastWasContMajor()) {
2564 // vehicle decelerates just enough to be able to stop if necessary and then accelerates
2565 double maxSpeedAtVisibilityDist = cfModel.maximumSafeStopSpeed(visibilityDistance, myState.mySpeed, false, 0.);
2566 // XXX: estimateSpeedAfterDistance does not use euler-logic (thus returns a lower value than possible here...)
2567 double maxArrivalSpeed = cfModel.estimateSpeedAfterDistance(visibilityDistance, maxSpeedAtVisibilityDist, cfModel.getMaxAccel());
2568 arrivalSpeed = MIN2(vLinkPass, maxArrivalSpeed);
2569 slowedDownForMinor = true;
2570 #ifdef DEBUG_PLAN_MOVE
2571 if (DEBUG_COND) {
2572 std::cout << " slowedDownForMinor maxSpeedAtVisDist=" << maxSpeedAtVisibilityDist << " maxArrivalSpeed=" << maxArrivalSpeed << " arrivalSpeed=" << arrivalSpeed << "\n";
2573 }
2574 #endif
2575 }
2576
2577 SUMOTime arrivalTime;
2578 if (MSGlobals::gSemiImplicitEulerUpdate) {
2579 // @note intuitively it would make sense to compare arrivalSpeed with getSpeed() instead of v
2580 // however, due to the current position update rule (ticket #860) the vehicle moves with v in this step
2581 // subtract DELTA_T because t is the time at the end of this step and the movement is not carried out yet
2582 arrivalTime = t - DELTA_T + cfModel.getMinimalArrivalTime(seen, v, arrivalSpeed);
2583 } else {
2584 arrivalTime = t - DELTA_T + cfModel.getMinimalArrivalTime(seen, myState.mySpeed, arrivalSpeed);
2585 }
2586
2587 // compute arrival speed and arrival time if vehicle starts braking now
2588 // if stopping is possible, arrivalTime can be arbitrarily large. A small value keeps fractional times (impatience) meaningful
2589 double arrivalSpeedBraking = 0;
2590 SUMOTime arrivalTimeBraking = arrivalTime + TIME2STEPS(30);
2591 if (seen < cfModel.brakeGap(v)) { // XXX: should this use the current speed (at least for the ballistic case)? (Leo) Refs. #2575
2592 // vehicle cannot come to a complete stop in time
2593 if (MSGlobals::gSemiImplicitEulerUpdate) {
2594 arrivalSpeedBraking = cfModel.getMinimalArrivalSpeedEuler(seen, v);
2595 // due to discrete/continuous mismatch (when using Euler update) we have to ensure that braking actually helps
2596 arrivalSpeedBraking = MIN2(arrivalSpeedBraking, arrivalSpeed);
2597 } else {
2598 arrivalSpeedBraking = cfModel.getMinimalArrivalSpeed(seen, myState.mySpeed);
2599 }
2600 if (v + arrivalSpeedBraking <= 0.) {
2601 arrivalTimeBraking = std::numeric_limits<long long int>::max();
2602 } else {
2603 arrivalTimeBraking = MAX2(arrivalTime, t + TIME2STEPS(seen / ((v + arrivalSpeedBraking) * 0.5)));
2604 }
2605 }
2606 lfLinks.push_back(DriveProcessItem(*link, v, vLinkWait, setRequest,
2607 arrivalTime, arrivalSpeed,
2608 arrivalTimeBraking, arrivalSpeedBraking,
2609 seen,
2610 estimateLeaveSpeed(*link, arrivalSpeed)));
2611 if ((*link)->getViaLane() == nullptr) {
2612 hadNonInternal = true;
2613 ++view;
2614 }
2615 #ifdef DEBUG_PLAN_MOVE
2616 if (DEBUG_COND) {
2617 std::cout << " checkAbort setRequest=" << setRequest << " v=" << v << " seen=" << seen << " dist=" << dist
2618 << " seenNonInternal=" << seenNonInternal
2619 << " seenInternal=" << seenInternal << " length=" << vehicleLength << "\n";
2620 }
2621 #endif
2622 // we need to look ahead far enough to see available space for checkRewindLinkLanes
2623 if ((!setRequest || v <= 0 || seen > dist) && hadNonInternal && seenNonInternal > MAX2(vehicleLength * CRLL_LOOK_AHEAD, vehicleLength + seenInternal)) {
2624 break;
2625 }
2626 // get the following lane
2627 lane = (*link)->getViaLaneOrLane();
2628 laneMaxV = lane->getVehicleMaxSpeed(this);
2629 if (myInfluencer && !myInfluencer->considerSafeVelocity()) {
2630 laneMaxV = std::numeric_limits<double>::max();
2631 }
2632 // the link was passed
2633 // compute the velocity to use when the link is not blocked by other vehicles
2634 // the vehicle shall be not faster when reaching the next lane than allowed
2635 // speed limits are not emergencies (e.g. when the limit changes suddenly due to TraCI or a variableSpeedSignal)
2636 const double va = MAX2(cfModel.freeSpeed(this, getSpeed(), seen, laneMaxV), vMinComfortable);
2637 v = MIN2(va, v);
2638 if (lane->getEdge().isInternal()) {
2639 seenInternal += lane->getLength();
2640 } else {
2641 seenNonInternal += lane->getLength();
2642 }
2643 // do not restrict results to the current vehicle to allow caching for the current time step
2644 leaderLane = opposite ? lane->getOpposite() : lane;
2645 if (leaderLane == nullptr) {
2646 break;
2647 }
2648 ahead = opposite ? MSLeaderInfo(leaderLane) : leaderLane->getLastVehicleInformation(nullptr, 0);
2649 seen += lane->getLength();
2650 vLinkPass = MIN2(cfModel.estimateSpeedAfterDistance(lane->getLength(), v, cfModel.getMaxAccel()), laneMaxV); // upper bound
2651 lastLink = &lfLinks.back();
2652 }
2653
2654 //#ifdef DEBUG_PLAN_MOVE
2655 // if(DEBUG_COND){
2656 // std::cout << "planMoveInternal found safe speed v = " << v << std::endl;
2657 // }
2658 //#endif
2659
2660 }
2661
2662
2663 void
2664 MSVehicle::adaptToLeaders(const MSLeaderInfo& ahead, double latOffset,
2665 const double seen, DriveProcessItem* const lastLink,
2666 const MSLane* const lane, double& v, double& vLinkPass) const {
2667 int rightmost;
2668 int leftmost;
2669 ahead.getSubLanes(this, latOffset, rightmost, leftmost);
2670 #ifdef DEBUG_PLAN_MOVE
2671 if (DEBUG_COND) std::cout << SIMTIME
2672 << "\nADAPT_TO_LEADERS\nveh=" << getID()
2673 << " lane=" << lane->getID()
2674 << " latOffset=" << latOffset
2675 << " rm=" << rightmost
2676 << " lm=" << leftmost
2677 << " ahead=" << ahead.toString()
2678 << "\n";
2679 #endif
2680 /*
2681 if (getLaneChangeModel().getCommittedSpeed() > 0) {
2682 v = MIN2(v, getLaneChangeModel().getCommittedSpeed());
2683 vLinkPass = MIN2(vLinkPass, getLaneChangeModel().getCommittedSpeed());
2684 #ifdef DEBUG_PLAN_MOVE
2685 if (DEBUG_COND) std::cout << " hasCommitted=" << getLaneChangeModel().getCommittedSpeed() << "\n";
2686 #endif
2687 return;
2688 }
2689 */
2690 for (int sublane = rightmost; sublane <= leftmost; ++sublane) {
2691 const MSVehicle* pred = ahead[sublane];
2692 if (pred != nullptr && pred != this) {
2693 // @todo avoid multiple adaptations to the same leader
2694 const double predBack = pred->getBackPositionOnLane(lane);
2695 double gap = (lastLink == nullptr
2696 ? predBack - myState.myPos - getVehicleType().getMinGap()
2697 : predBack + seen - lane->getLength() - getVehicleType().getMinGap());
2698 if (getLaneChangeModel().isOpposite()) {
2699 gap *= -1;
2700 }
2701 #ifdef DEBUG_PLAN_MOVE
2702 if (DEBUG_COND) {
2703 std::cout << " pred=" << pred->getID() << " predLane=" << pred->getLane()->getID() << " predPos=" << pred->getPositionOnLane() << " gap=" << gap << " predBack=" << predBack << " seen=" << seen << " lane=" << lane->getID() << " myLane=" << myLane->getID() << "\n";
2704 }
2705 #endif
2706 adaptToLeader(std::make_pair(pred, gap), seen, lastLink, lane, v, vLinkPass);
2707 }
2708 }
2709 }
2710
2711
2712 void
2713 MSVehicle::adaptToLeader(const std::pair<const MSVehicle*, double> leaderInfo,
2714 const double seen, DriveProcessItem* const lastLink,
2715 const MSLane* const lane, double& v, double& vLinkPass,
2716 double distToCrossing) const {
2717 if (leaderInfo.first != 0) {
2718 const double vsafeLeader = getSafeFollowSpeed(leaderInfo, seen, lane, distToCrossing);
2719 if (lastLink != nullptr) {
2720 lastLink->adaptLeaveSpeed(vsafeLeader);
2721 }
2722 v = MIN2(v, vsafeLeader);
2723 vLinkPass = MIN2(vLinkPass, vsafeLeader);
2724
2725 #ifdef DEBUG_PLAN_MOVE
2726 if (DEBUG_COND) std::cout
2727 << SIMTIME
2728 //std::cout << std::setprecision(10);
2729 << " veh=" << getID()
2730 << " lead=" << leaderInfo.first->getID()
2731 << " leadSpeed=" << leaderInfo.first->getSpeed()
2732 << " gap=" << leaderInfo.second
2733 << " leadLane=" << leaderInfo.first->getLane()->getID()
2734 << " predPos=" << leaderInfo.first->getPositionOnLane()
2735 << " seen=" << seen
2736 << " lane=" << lane->getID()
2737 << " myLane=" << myLane->getID()
2738 << " dTC=" << distToCrossing
2739 << " v=" << v
2740 << " vSafeLeader=" << vsafeLeader
2741 << " vLinkPass=" << vLinkPass
2742 << "\n";
2743 #endif
2744 }
2745 }
2746
2747
2748 void
2749 MSVehicle::checkLinkLeaderCurrentAndParallel(const MSLink* link, const MSLane* lane, double seen,
2750 DriveProcessItem* const lastLink, double& v, double& vLinkPass, double& vLinkWait, bool& setRequest) const {
2751 if (MSGlobals::gUsingInternalLanes) {
2752 // we want to pass the link but need to check for foes on internal lanes
2753 checkLinkLeader(link, lane, seen, lastLink, v, vLinkPass, vLinkWait, setRequest);
2754 if (getLaneChangeModel().getShadowLane() != nullptr) {
2755 MSLink* parallelLink = link->getParallelLink(getLaneChangeModel().getShadowDirection());
2756 if (parallelLink != nullptr) {
2757 checkLinkLeader(parallelLink, lane, seen, lastLink, v, vLinkPass, vLinkWait, setRequest, true);
2758 }
2759 }
2760 }
2761
2762 }
2763
2764 void
2765 MSVehicle::checkLinkLeader(const MSLink* link, const MSLane* lane, double seen,
2766 DriveProcessItem* const lastLink, double& v, double& vLinkPass, double& vLinkWait, bool& setRequest,
2767 bool isShadowLink) const {
2768 #ifdef DEBUG_PLAN_MOVE_LEADERINFO
2769 if (DEBUG_COND) {
2770 gDebugFlag1 = true; // See MSLink::getLeaderInfo
2771 }
2772 #endif
2773 const MSLink::LinkLeaders linkLeaders = link->getLeaderInfo(this, seen, nullptr, isShadowLink);
2774 #ifdef DEBUG_PLAN_MOVE_LEADERINFO
2775 if (DEBUG_COND) {
2776 gDebugFlag1 = false; // See MSLink::getLeaderInfo
2777 }
2778 #endif
2779 for (MSLink::LinkLeaders::const_iterator it = linkLeaders.begin(); it != linkLeaders.end(); ++it) {
2780 // the vehicle to enter the junction first has priority
2781 const MSVehicle* leader = (*it).vehAndGap.first;
2782 if (leader == nullptr) {
2783 // leader is a pedestrian. Passing 'this' as a dummy.
2784 #ifdef DEBUG_PLAN_MOVE_LEADERINFO
2785 if (DEBUG_COND) {
2786 std::cout << SIMTIME << " veh=" << getID() << " is blocked on link to " << link->getViaLaneOrLane()->getID() << " by pedestrian. dist=" << it->distToCrossing << "\n";
2787 }
2788 #endif
2789 adaptToLeader(std::make_pair(this, -1), seen, lastLink, lane, v, vLinkPass, it->distToCrossing);
2790 } else if (isLeader(link, leader)) {
2791 if (MSGlobals::gLateralResolution > 0 &&
2792 // sibling link (XXX: could also be partial occupator where this check fails)
2793 &leader->getLane()->getEdge() == &lane->getEdge()) {
2794 // check for sublane obstruction (trivial for sibling link leaders)
2795 const MSLane* conflictLane = link->getInternalLaneBefore();
2796 MSLeaderInfo linkLeadersAhead = MSLeaderInfo(conflictLane);
2797 linkLeadersAhead.addLeader(leader, false, 0); // assume sibling lane has the same geometry as the leader lane
2798 const double latOffset = isShadowLink ? (getLane()->getRightSideOnEdge() - getLaneChangeModel().getShadowLane()->getRightSideOnEdge()) : 0;
2799 // leader is neither on lane nor conflictLane (the conflict is only established geometrically)
2800 adaptToLeaders(linkLeadersAhead, latOffset, seen, lastLink, leader->getLane(), v, vLinkPass);
2801 #ifdef DEBUG_PLAN_MOVE
2802 if (DEBUG_COND) {
2803 std::cout << SIMTIME << " veh=" << getID()
2804 << " siblingFoe link=" << link->getViaLaneOrLane()->getID()
2805 << " isShadowLink=" << isShadowLink
2806 << " lane=" << lane->getID()
2807 << " foe=" << leader->getID()
2808 << " foeLane=" << leader->getLane()->getID()
2809 << " latOffset=" << latOffset
2810 << " latOffsetFoe=" << leader->getLatOffset(lane)
2811 << " linkLeadersAhead=" << linkLeadersAhead.toString()
2812 << "\n";
2813 }
2814 #endif
2815 } else {
2816 #ifdef DEBUG_PLAN_MOVE
2817 if (DEBUG_COND) {
2818 std::cout << SIMTIME << " veh=" << getID() << " linkLeader=" << leader->getID()
2819 << " ET=" << myJunctionEntryTime << " lET=" << leader->myJunctionEntryTime
2820 << " ETN=" << myJunctionEntryTimeNeverYield << " lETN=" << leader->myJunctionEntryTimeNeverYield
2821 << " CET=" << myJunctionConflictEntryTime << " lCET=" << leader->myJunctionConflictEntryTime
2822 << "\n";
2823 }
2824 #endif
2825 adaptToLeader(it->vehAndGap, seen, lastLink, lane, v, vLinkPass, it->distToCrossing);
2826 }
2827 if (lastLink != nullptr) {
2828 // we are not yet on the junction with this linkLeader.
2829 // at least we can drive up to the previous link and stop there
2830 v = MAX2(v, lastLink->myVLinkWait);
2831 }
2832 // if blocked by a leader from the same or next lane we must yield our request
2833 // also, if blocked by a stopped or blocked leader
2834 if (v < SUMO_const_haltingSpeed
2835 //&& leader->getSpeed() < SUMO_const_haltingSpeed
2836 && (leader->getLane()->getLogicalPredecessorLane() == myLane->getLogicalPredecessorLane()
2837 || leader->getLane()->getLogicalPredecessorLane() == myLane
2838 || leader->isStopped()
2839 || leader->getWaitingTime() > TIME2STEPS(JUNCTION_BLOCKAGE_TIME))) {
2840 setRequest = false;
2841 #ifdef DEBUG_PLAN_MOVE_LEADERINFO
2842 if (DEBUG_COND) {
2843 std::cout << " aborting request\n";
2844 }
2845 #endif
2846 if (lastLink != nullptr && leader->getLane()->getLogicalPredecessorLane() == myLane) {
2847 // we are not yet on the junction so must abort that request as well
2848 // (or maybe we are already on the junction and the leader is a partial occupator beyond)
2849 lastLink->mySetRequest = false;
2850 #ifdef DEBUG_PLAN_MOVE_LEADERINFO
2851 if (DEBUG_COND) {
2852 std::cout << " aborting previous request\n";
2853 }
2854 #endif
2855 }
2856 }
2857 }
2858 #ifdef DEBUG_PLAN_MOVE_LEADERINFO
2859 else {
2860 if (DEBUG_COND) {
2861 std::cout << SIMTIME << " veh=" << getID() << " ignoring leader " << leader->getID()
2862 << " ET=" << myJunctionEntryTime << " lET=" << leader->myJunctionEntryTime
2863 << " ETN=" << myJunctionEntryTimeNeverYield << " lETN=" << leader->myJunctionEntryTimeNeverYield
2864 << " CET=" << myJunctionConflictEntryTime << " lCET=" << leader->myJunctionConflictEntryTime
2865 << "\n";
2866 }
2867 }
2868 #endif
2869 }
2870 // if this is the link between two internal lanes we may have to slow down for pedestrians
2871 vLinkWait = MIN2(vLinkWait, v);
2872 }
2873
2874
2875 double
2876 MSVehicle::getSafeFollowSpeed(const std::pair<const MSVehicle*, double> leaderInfo,
2877 const double seen, const MSLane* const lane, double distToCrossing) const {
2878 assert(leaderInfo.first != 0);
2879 const MSCFModel& cfModel = getCarFollowModel();
2880 double vsafeLeader = 0;
2881 if (!MSGlobals::gSemiImplicitEulerUpdate) {
2882 vsafeLeader = -std::numeric_limits<double>::max();
2883 }
2884 if (leaderInfo.second >= 0) {
2885 vsafeLeader = cfModel.followSpeed(this, getSpeed(), leaderInfo.second, leaderInfo.first->getSpeed(), leaderInfo.first->getCarFollowModel().getApparentDecel(), leaderInfo.first);
2886 } else {
2887 // the leading, in-lapping vehicle is occupying the complete next lane
2888 // stop before entering this lane
2889 vsafeLeader = cfModel.stopSpeed(this, getSpeed(), seen - lane->getLength() - POSITION_EPS);
2890 #ifdef DEBUG_PLAN_MOVE_LEADERINFO
2891 if (DEBUG_COND) {
2892 std::cout << SIMTIME << " veh=" << getID() << " stopping before junction: lane=" << lane->getID() << " seen=" << seen
2893 << " laneLength=" << lane->getLength()
2894 << " stopDist=" << seen - lane->getLength() - POSITION_EPS
2895 << " vsafeLeader=" << vsafeLeader
2896 << "\n";
2897 }
2898 #endif
2899 }
2900 if (distToCrossing >= 0) {
2901 // drive up to the crossing point with the current link leader
2902 vsafeLeader = MAX2(vsafeLeader, cfModel.stopSpeed(this, getSpeed(), distToCrossing - getVehicleType().getMinGap()));
2903 }
2904 return vsafeLeader;
2905 }
2906
2907 double
2908 MSVehicle::getDeltaPos(const double accel) const {
2909 double vNext = myState.mySpeed + ACCEL2SPEED(accel);
2910 if (MSGlobals::gSemiImplicitEulerUpdate) {
2911 // apply implicit Euler positional update
2912 return SPEED2DIST(MAX2(vNext, 0.));
2913 } else {
2914 // apply ballistic update
2915 if (vNext >= 0) {
2916 // assume constant acceleration during this time step
2917 return SPEED2DIST(myState.mySpeed + 0.5 * ACCEL2SPEED(accel));
2918 } else {
2919 // negative vNext indicates a stop within the middle of time step
2920 // The corresponding stop time is s = mySpeed/deceleration \in [0,dt], and the
2921 // covered distance is therefore deltaPos = mySpeed*s - 0.5*deceleration*s^2.
2922 // Here, deceleration = (myState.mySpeed - vNext)/dt is the constant deceleration
2923 // until the vehicle stops.
2924 return -SPEED2DIST(0.5 * myState.mySpeed * myState.mySpeed / ACCEL2SPEED(accel));
2925 }
2926 }
2927 }
2928
2929 void
2930 MSVehicle::processLinkApproaches(double& vSafe, double& vSafeMin, double& vSafeMinDist) {
2931
2932 // Speed limit due to zipper merging
2933 double vSafeZipper = std::numeric_limits<double>::max();
2934
2935 myHaveToWaitOnNextLink = false;
2936 bool canBrakeVSafeMin = false;
2937
2938 // Get safe velocities from DriveProcessItems.
2939 assert(myLFLinkLanes.size() != 0 || isRemoteControlled());
2940 for (const DriveProcessItem& dpi : myLFLinkLanes) {
2941 MSLink* const link = dpi.myLink;
2942
2943 #ifdef DEBUG_EXEC_MOVE
2944 if (DEBUG_COND) {
2945 std::cout
2946 << SIMTIME
2947 << " veh=" << getID()
2948 << " link=" << (link == 0 ? "NULL" : link->getViaLaneOrLane()->getID())
2949 << " req=" << dpi.mySetRequest
2950 << " vP=" << dpi.myVLinkPass
2951 << " vW=" << dpi.myVLinkWait
2952 << " d=" << dpi.myDistance
2953 << "\n";
2954 gDebugFlag1 = true; // See MSLink_DEBUG_OPENED
2955 }
2956 #endif
2957
2958 // the vehicle must change the lane on one of the next lanes (XXX: refs to code further below???, Leo)
2959 if (link != nullptr && dpi.mySetRequest) {
2960
2961 const LinkState ls = link->getState();
2962 // vehicles should brake when running onto a yellow light if the distance allows to halt in front
2963 const bool yellow = link->haveYellow();
2964 const bool canBrake = (dpi.myDistance > getCarFollowModel().brakeGap(myState.mySpeed, getCarFollowModel().getMaxDecel(), 0.)
2965 || (MSGlobals::gSemiImplicitEulerUpdate && myState.mySpeed < ACCEL2SPEED(getCarFollowModel().getMaxDecel())));
2966 assert(link->getLaneBefore() != nullptr);
2967 const bool beyondStopLine = dpi.myDistance < link->getLaneBefore()->getStopOffset(this);
2968 const bool ignoreRedLink = ignoreRed(link, canBrake) || beyondStopLine;
2969 if (yellow && canBrake && !ignoreRedLink) {
2970 vSafe = dpi.myVLinkWait;
2971 myHaveToWaitOnNextLink = true;
2972 #ifdef DEBUG_CHECKREWINDLINKLANES
2973 if (DEBUG_COND) {
2974 std::cout << SIMTIME << " veh=" << getID() << " haveToWait (yellow)\n";
2975 }
2976 #endif
2977 link->removeApproaching(this);
2978 break;
2979 }
2980 const bool influencerPrio = (myInfluencer != nullptr && !myInfluencer->getRespectJunctionPriority());
2981 std::vector<const SUMOVehicle*> collectFoes;
2982 bool opened = (yellow || influencerPrio
2983 || link->opened(dpi.myArrivalTime, dpi.myArrivalSpeed, dpi.getLeaveSpeed(),
2984 getVehicleType().getLength(),
2985 canBrake ? getImpatience() : 1,
2986 getCarFollowModel().getMaxDecel(),
2987 getWaitingTime(), getLateralPositionOnLane(),
2988 ls == LINKSTATE_ZIPPER ? &collectFoes : nullptr,
2989 ignoreRedLink, this));
2990 if (opened && getLaneChangeModel().getShadowLane() != nullptr) {
2991 MSLink* parallelLink = dpi.myLink->getParallelLink(getLaneChangeModel().getShadowDirection());
2992 if (parallelLink != nullptr) {
2993 const double shadowLatPos = getLateralPositionOnLane() - getLaneChangeModel().getShadowDirection() * 0.5 * (
2994 myLane->getWidth() + getLaneChangeModel().getShadowLane()->getWidth());
2995 opened = yellow || influencerPrio || (opened && parallelLink->opened(dpi.myArrivalTime, dpi.myArrivalSpeed, dpi.getLeaveSpeed(),
2996 getVehicleType().getLength(), getImpatience(),
2997 getCarFollowModel().getMaxDecel(),
2998 getWaitingTime(), shadowLatPos, nullptr,
2999 ignoreRedLink, this));
3000 #ifdef DEBUG_EXEC_MOVE
3001 if (DEBUG_COND) {
3002 std::cout << SIMTIME
3003 << " veh=" << getID()
3004 << " shadowLane=" << getLaneChangeModel().getShadowLane()->getID()
3005 << " shadowDir=" << getLaneChangeModel().getShadowDirection()
3006 << " parallelLink=" << (parallelLink == 0 ? "NULL" : parallelLink->getViaLaneOrLane()->getID())
3007 << " opened=" << opened
3008 << "\n";
3009 }
3010 #endif
3011 }
3012 }
3013 // vehicles should decelerate when approaching a minor link
3014 #ifdef DEBUG_EXEC_MOVE
3015 if (DEBUG_COND) {
3016 std::cout << SIMTIME
3017 << " opened=" << opened
3018 << " influencerPrio=" << influencerPrio
3019 << " linkPrio=" << link->havePriority()
3020 << " lastContMajor=" << link->lastWasContMajor()
3021 << " isCont=" << link->isCont()
3022 << " ignoreRed=" << ignoreRedLink
3023 << "\n";
3024 }
3025 #endif
3026 if (opened && !influencerPrio && !link->havePriority() && !link->lastWasContMajor() && !link->isCont() && !ignoreRedLink) {
3027 double visibilityDistance = link->getFoeVisibilityDistance();
3028 double determinedFoePresence = dpi.myDistance <= visibilityDistance;
3029 if (!determinedFoePresence && (canBrake || !yellow)) {
3030 vSafe = dpi.myVLinkWait;
3031 myHaveToWaitOnNextLink = true;
3032 #ifdef DEBUG_CHECKREWINDLINKLANES
3033 if (DEBUG_COND) {
3034 std::cout << SIMTIME << " veh=" << getID() << " haveToWait (minor)\n";
3035 }
3036 #endif
3037 if (ls == LINKSTATE_EQUAL) {
3038 link->removeApproaching(this);
3039 }
3040 break;
3041 } else {
3042 // past the point of no return. we need to drive fast enough
3043 // to make it across the link. However, minor slowdowns
3044 // should be permissible to follow leading traffic safely
3045 // basically, this code prevents dawdling
3046 // (it's harder to do this later using
3047 // SUMO_ATTR_JM_SIGMA_MINOR because we don't know whether the
3048 // vehicle is already too close to stop at that part of the code)
3049 //
3050 // XXX: There is a problem in subsecond simulation: If we cannot
3051 // make it across the minor link in one step, new traffic
3052 // could appear on a major foe link and cause a collision. Refs. #1845, #2123
3053 vSafeMinDist = dpi.myDistance; // distance that must be covered
3054 if (MSGlobals::gSemiImplicitEulerUpdate) {
3055 vSafeMin = MIN2((double) DIST2SPEED(vSafeMinDist + POSITION_EPS), dpi.myVLinkPass);
3056 } else {
3057 vSafeMin = MIN2((double) DIST2SPEED(2 * vSafeMinDist + NUMERICAL_EPS) - getSpeed(), dpi.myVLinkPass);
3058 }
3059 canBrakeVSafeMin = canBrake;
3060 #ifdef DEBUG_EXEC_MOVE
3061 if (DEBUG_COND) {
3062 std::cout << " vSafeMin=" << vSafeMin << " vSafeMinDist=" << vSafeMinDist << " canBrake=" << canBrake << "\n";
3063 }
3064 #endif
3065 }
3066 }
3067 // have waited; may pass if opened...
3068 if (opened) {
3069 vSafe = dpi.myVLinkPass;
3070 if (vSafe < getCarFollowModel().getMaxDecel() && vSafe <= dpi.myVLinkWait && vSafe < getCarFollowModel().maxNextSpeed(getSpeed(), this)) {
3071 // this vehicle is probably not gonna drive across the next junction (heuristic)
3072 myHaveToWaitOnNextLink = true;
3073 #ifdef DEBUG_CHECKREWINDLINKLANES
3074 if (DEBUG_COND) {
3075 std::cout << SIMTIME << " veh=" << getID() << " haveToWait (very slow)\n";
3076 }
3077 #endif
3078 }
3079 } else if (link->getState() == LINKSTATE_ZIPPER) {
3080 vSafeZipper = MIN2(vSafeZipper,
3081 link->getZipperSpeed(this, dpi.myDistance, dpi.myVLinkPass, dpi.myArrivalTime, &collectFoes));
3082 } else {
3083 vSafe = dpi.myVLinkWait;
3084 myHaveToWaitOnNextLink = true;
3085 #ifdef DEBUG_CHECKREWINDLINKLANES
3086 if (DEBUG_COND) {
3087 std::cout << SIMTIME << " veh=" << getID() << " haveToWait (closed)\n";
3088 }
3089 #endif
3090 if (ls == LINKSTATE_EQUAL) {
3091 link->removeApproaching(this);
3092 }
3093 #ifdef DEBUG_EXEC_MOVE
3094 if (DEBUG_COND) {
3095 std::cout << SIMTIME << " braking for closed link=" << link->getViaLaneOrLane()->getID() << "\n";
3096 }
3097 #endif
3098 break;
3099 }
3100 } else {
3101 if (link != nullptr && link->getInternalLaneBefore() != nullptr && myLane->isInternal() && link->getJunction() == myLane->getEdge().getToJunction()) {
3102 // blocked on the junction. yield request so other vehicles may
3103 // become junction leader
3104 #ifdef DEBUG_EXEC_MOVE
3105 if (DEBUG_COND) {
3106 std::cout << SIMTIME << " reseting junctionEntryTime at junction '" << link->getJunction()->getID() << "' beause of non-request exitLink\n";
3107 }
3108 #endif
3109 myJunctionEntryTime = SUMOTime_MAX;
3110 myJunctionConflictEntryTime = SUMOTime_MAX;
3111 }
3112 // we have: i->link == 0 || !i->setRequest
3113 vSafe = dpi.myVLinkWait;
3114 if (vSafe < getSpeed()) {
3115 myHaveToWaitOnNextLink = true;
3116 #ifdef DEBUG_CHECKREWINDLINKLANES
3117 if (DEBUG_COND) {
3118 std::cout << SIMTIME << " veh=" << getID() << " haveToWait (no request, braking)\n";
3119 }
3120 #endif
3121 } else if (vSafe < SUMO_const_haltingSpeed) {
3122 myHaveToWaitOnNextLink = true;
3123 #ifdef DEBUG_CHECKREWINDLINKLANES
3124 if (DEBUG_COND) {
3125 std::cout << SIMTIME << " veh=" << getID() << " haveToWait (no request, stopping)\n";
3126 }
3127 #endif
3128 }
3129 break;
3130 }
3131 }
3132
3133 //#ifdef DEBUG_EXEC_MOVE
3134 // if (DEBUG_COND) {
3135 // std::cout << "\nvCurrent = " << toString(getSpeed(), 24) << "" << std::endl;
3136 // std::cout << "vSafe = " << toString(vSafe, 24) << "" << std::endl;
3137 // std::cout << "vSafeMin = " << toString(vSafeMin, 24) << "" << std::endl;
3138 // std::cout << "vSafeMinDist = " << toString(vSafeMinDist, 24) << "" << std::endl;
3139 //
3140 // double gap = getLeader().second;
3141 // std::cout << "gap = " << toString(gap, 24) << std::endl;
3142 // std::cout << "vSafeStoppedLeader = " << toString(getCarFollowModel().stopSpeed(this, getSpeed(), gap), 24)
3143 // << "\n" << std::endl;
3144 // }
3145 //#endif
3146
3147 if ((MSGlobals::gSemiImplicitEulerUpdate && vSafe + NUMERICAL_EPS < vSafeMin)
3148 || (!MSGlobals::gSemiImplicitEulerUpdate && (vSafe + NUMERICAL_EPS < vSafeMin && vSafeMin != 0))) { // this might be good for the euler case as well
3149 // XXX: (Leo) This often called stopSpeed with vSafeMinDist==0 (for the ballistic update), since vSafe can become negative
3150 // For the Euler update the term '+ NUMERICAL_EPS' prevented a call here... Recheck, consider of -INVALID_SPEED instead of 0 to indicate absence of vSafeMin restrictions. Refs. #2577
3151 #ifdef DEBUG_EXEC_MOVE
3152 if (DEBUG_COND) {
3153 std::cout << "vSafeMin Problem? vSafe=" << vSafe << " vSafeMin=" << vSafeMin << " vSafeMinDist=" << vSafeMinDist << std::endl;
3154 }
3155 #endif
3156 if (canBrakeVSafeMin && vSafe < getSpeed()) {
3157 // cannot drive across a link so we need to stop before it
3158 vSafe = MIN2(vSafe, getCarFollowModel().stopSpeed(this, getSpeed(), vSafeMinDist));
3159 vSafeMin = 0;
3160 myHaveToWaitOnNextLink = true;
3161 #ifdef DEBUG_CHECKREWINDLINKLANES
3162 if (DEBUG_COND) {
3163 std::cout << SIMTIME << " veh=" << getID() << " haveToWait (vSafe=" << vSafe << " < vSafeMin=" << vSafeMin << ")\n";
3164 }
3165 #endif
3166 } else {
3167 // if the link is yellow or visibility distance is large
3168 // then we might not make it across the link in one step anyway..
3169 // Possibly, the lane after the intersection has a lower speed limit so
3170 // we really need to drive slower already
3171 // -> keep driving without dawdling
3172 vSafeMin = vSafe;
3173 }
3174 }
3175
3176 // vehicles inside a roundabout should maintain their requests
3177 if (myLane->getEdge().isRoundabout()) {
3178 myHaveToWaitOnNextLink = false;
3179 }
3180
3181 vSafe = MIN2(vSafe, vSafeZipper);
3182 }
3183
3184
3185 double
3186 MSVehicle::processTraCISpeedControl(double vSafe, double vNext) {
3187 if (myInfluencer != nullptr) {
3188 #ifdef DEBUG_TRACI
3189 if DEBUG_COND2(this) {
3190 std::cout << SIMTIME << " MSVehicle::processTraCISpeedControl() for vehicle '" << getID() << "'"
3191 << " vSafe=" << vSafe << " (init)vNext=" << vNext;
3192 }
3193 #endif
3194 if (myInfluencer->isRemoteControlled()) {
3195 vNext = myInfluencer->implicitSpeedRemote(this, myState.mySpeed);
3196 }
3197 const double vMax = getVehicleType().getCarFollowModel().maxNextSpeed(myState.mySpeed, this);
3198 double vMin = getVehicleType().getCarFollowModel().minNextSpeed(myState.mySpeed, this);
3199 if (MSGlobals::gSemiImplicitEulerUpdate) {
3200 vMin = MAX2(0., vMin);
3201 }
3202 vNext = myInfluencer->influenceSpeed(MSNet::getInstance()->getCurrentTimeStep(), vNext, vSafe, vMin, vMax);
3203 #ifdef DEBUG_TRACI
3204 if DEBUG_COND2(this) {
3205 std::cout << " (processed)vNext=" << vNext << std::endl;
3206 }
3207 #endif
3208 }
3209 return vNext;
3210 }
3211
3212
3213 void
3214 MSVehicle::removePassedDriveItems() {
3215 #ifdef DEBUG_ACTIONSTEPS
3216 if DEBUG_COND {
3217 std::cout << SIMTIME << " veh=" << getID() << " removePassedDriveItems()\n"
3218 << " Current items: ";
3219 for (auto& j : myLFLinkLanes) {
3220 if (j.myLink == 0) {
3221 std::cout << "\n Stop at distance " << j.myDistance;
3222 } else {
3223 const MSLane* to = j.myLink->getViaLaneOrLane();
3224 const MSLane* from = j.myLink->getLaneBefore();
3225 std::cout << "\n Link at distance " << j.myDistance << ": '"
3226 << (from == 0 ? "NONE" : from->getID()) << "' -> '" << (to == 0 ? "NONE" : to->getID()) << "'";
3227 }
3228 }
3229 std::cout << "\n myNextDriveItem: ";
3230 if (myLFLinkLanes.size() != 0) {
3231 if (myNextDriveItem->myLink == 0) {
3232 std::cout << "\n Stop at distance " << myNextDriveItem->myDistance;
3233 } else {
3234 const MSLane* to = myNextDriveItem->myLink->getViaLaneOrLane();
3235 const MSLane* from = myNextDriveItem->myLink->getLaneBefore();
3236 std::cout << "\n Link at distance " << myNextDriveItem->myDistance << ": '"
3237 << (from == 0 ? "NONE" : from->getID()) << "' -> '" << (to == 0 ? "NONE" : to->getID()) << "'";
3238 }
3239 }
3240 std::cout << std::endl;
3241 }
3242 #endif
3243 for (auto j = myLFLinkLanes.begin(); j != myNextDriveItem; ++j) {
3244 #ifdef DEBUG_ACTIONSTEPS
3245 if DEBUG_COND {
3246 std::cout << " Removing item: ";
3247 if (j->myLink == 0) {
3248 std::cout << "Stop at distance " << j->myDistance;
3249 } else {
3250 const MSLane* to = j->myLink->getViaLaneOrLane();
3251 const MSLane* from = j->myLink->getLaneBefore();
3252 std::cout << "Link at distance " << j->myDistance << ": '"
3253 << (from == 0 ? "NONE" : from->getID()) << "' -> '" << (to == 0 ? "NONE" : to->getID()) << "'";
3254 }
3255 std::cout << std::endl;
3256 }
3257 #endif
3258 if (j->myLink != nullptr) {
3259 j->myLink->removeApproaching(this);
3260 }
3261 }
3262 myLFLinkLanes.erase(myLFLinkLanes.begin(), myNextDriveItem);
3263 myNextDriveItem = myLFLinkLanes.begin();
3264 }
3265
3266
3267 void
3268 MSVehicle::updateDriveItems() {
3269 #ifdef DEBUG_ACTIONSTEPS
3270 if (DEBUG_COND) {
3271 std::cout << SIMTIME << " updateDriveItems(), veh='" << getID() << "' (lane: '" << getLane()->getID() << "')\nCurrent drive items:" << std::endl;
3272 DriveItemVector::iterator i;
3273 for (i = myLFLinkLanes.begin(); i != myLFLinkLanes.end(); ++i) {
3274 std::cout
3275 << " vPass=" << dpi.myVLinkPass
3276 << " vWait=" << dpi.myVLinkWait
3277 << " linkLane=" << (dpi.myLink == 0 ? "NULL" : dpi.myLink->getViaLaneOrLane()->getID())
3278 << " request=" << dpi.mySetRequest
3279 << "\n";
3280 }
3281 std::cout << " myNextDriveItem's linked lane: " << (myNextDriveItem->myLink == 0 ? "NULL" : myNextDriveItem->myLink->getViaLaneOrLane()->getID()) << std::endl;
3282 }
3283 #endif
3284 if (myLFLinkLanes.size() == 0) {
3285 // nothing to update
3286 return;
3287 }
3288 const MSLink* nextPlannedLink = nullptr;
3289 // auto i = myLFLinkLanes.begin();
3290 auto i = myNextDriveItem;
3291 while (i != myLFLinkLanes.end() && nextPlannedLink == nullptr) {
3292 nextPlannedLink = i->myLink;
3293 ++i;
3294 }
3295
3296 if (nextPlannedLink == nullptr) {
3297 // No link for upcoming item -> no need for an update
3298 #ifdef DEBUG_ACTIONSTEPS
3299 if (DEBUG_COND) {
3300 std::cout << "Found no link-related drive item." << std::endl;
3301 }
3302 #endif
3303 return;
3304 }
3305
3306 if (getLane() == nextPlannedLink->getLaneBefore()) {
3307 // Current lane approaches the stored next link, i.e. no LC happend and no update is required.
3308 #ifdef DEBUG_ACTIONSTEPS
3309 if (DEBUG_COND) {
3310 std::cout << "Continuing on planned lane sequence, no update required." << std::endl;
3311 }
3312 #endif
3313 return;
3314 }
3315 // Lane must have been changed, determine the change direction
3316 MSLink* parallelLink = nextPlannedLink->getParallelLink(1);
3317 if (parallelLink != nullptr && parallelLink->getLaneBefore() == getLane()) {
3318 // lcDir = 1;
3319 } else {
3320 parallelLink = nextPlannedLink->getParallelLink(-1);
3321 if (parallelLink != nullptr && parallelLink->getLaneBefore() == getLane()) {
3322 // lcDir = -1;
3323 } else {
3324 // If the vehicle's current lane is not the approaching lane for the next
3325 // drive process item's link, it is expected to lead to a parallel link,
3326 // XXX: What if the lc was an overtaking maneuver and there is no upcoming link?
3327 // Then a stop item should be scheduled! -> TODO!
3328 //assert(false);
3329 return;
3330 }
3331 }
3332 #ifdef DEBUG_ACTIONSTEPS
3333 if (DEBUG_COND) {
3334 std::cout << "Changed lane. Drive items will be updated along the current lane continuation." << std::endl;
3335 }
3336 #endif
3337 // Trace link sequence along current best lanes and transfer drive items to the corresponding links
3338 // DriveItemVector::iterator driveItemIt = myLFLinkLanes.begin();
3339 DriveItemVector::iterator driveItemIt = myNextDriveItem;
3340 // In the loop below, lane holds the currently considered lane on the vehicles continuation (including internal lanes)
3341 MSLane* lane = myLane;
3342 assert(myLane == parallelLink->getLaneBefore());
3343 // *lit is a pointer to the next lane in best continuations for the current lane (always non-internal)
3344 std::vector<MSLane*>::const_iterator bestLaneIt = getBestLanesContinuation().begin() + 1;
3345 // Pointer to the new link for the current drive process item
3346 MSLink* newLink = nullptr;
3347 while (driveItemIt != myLFLinkLanes.end()) {
3348 if (driveItemIt->myLink == nullptr) {
3349 // Items not related to a specific link are not updated
3350 // (XXX: when a stop item corresponded to a dead end, which is overcome by the LC that made
3351 // the update necessary, this may slow down the vehicle's continuation on the new lane...)
3352 ++driveItemIt;
3353 continue;
3354 }
3355 // Continuation links for current best lanes are less than for the former drive items (myLFLinkLanes)
3356 // We just remove the leftover link-items, as they cannot be mapped to new links.
3357 if (bestLaneIt == getBestLanesContinuation().end()) {
3358 #ifdef DEBUG_ACTIONSTEPS
3359 if (DEBUG_COND) {
3360 std::cout << "Reached end of the new continuation sequence. Erasing leftover link-items." << std::endl;
3361 }
3362 #endif
3363 while (driveItemIt != myLFLinkLanes.end()) {
3364 if (driveItemIt->myLink == nullptr) {
3365 ++driveItemIt;
3366 continue;
3367 } else {
3368 driveItemIt->myLink->removeApproaching(this);
3369 driveItemIt = myLFLinkLanes.erase(driveItemIt);
3370 }
3371 }
3372 break;
3373 }
3374 // Do the actual link-remapping for the item. And un/register approaching information on the corresponding links
3375 newLink = lane->getLinkTo(*bestLaneIt);
3376
3377 if (newLink == driveItemIt->myLink) {
3378 // new continuation merged into previous - stop update
3379 #ifdef DEBUG_ACTIONSTEPS
3380 if (DEBUG_COND) {
3381 std::cout << "Old and new continuation sequences merge at link\n"
3382 << "'" << newLink->getLaneBefore()->getID() << "'->'" << newLink->getViaLaneOrLane()->getID() << "'"
3383 << "\nNo update beyond merge required." << std::endl;
3384 }
3385 #endif
3386 break;
3387 }
3388
3389 #ifdef DEBUG_ACTIONSTEPS
3390 if (DEBUG_COND) {
3391 std::cout << "lane=" << lane->getID() << "\nUpdating link\n '" << driveItemIt->myLink->getLaneBefore()->getID() << "'->'" << driveItemIt->myLink->getViaLaneOrLane()->getID() << "'"
3392 << "==> " << "'" << newLink->getLaneBefore()->getID() << "'->'" << newLink->getViaLaneOrLane()->getID() << "'" << std::endl;
3393 }
3394 #endif
3395 newLink->setApproaching(this, driveItemIt->myLink->getApproaching(this));
3396 driveItemIt->myLink->removeApproaching(this);
3397 driveItemIt->myLink = newLink;
3398 lane = newLink->getViaLaneOrLane();
3399 ++driveItemIt;
3400 if (!lane->isInternal()) {
3401 ++bestLaneIt;
3402 }
3403 }
3404 #ifdef DEBUG_ACTIONSTEPS
3405 if (DEBUG_COND) {
3406 std::cout << "Updated drive items:" << std::endl;
3407 DriveItemVector::iterator i;
3408 for (i = myLFLinkLanes.begin(); i != myLFLinkLanes.end(); ++i) {
3409 std::cout
3410 << " vPass=" << dpi.myVLinkPass
3411 << " vWait=" << dpi.myVLinkWait
3412 << " linkLane=" << (dpi.myLink == 0 ? "NULL" : dpi.myLink->getViaLaneOrLane()->getID())
3413 << " request=" << dpi.mySetRequest
3414 << "\n";
3415 }
3416 }
3417 #endif
3418 }
3419
3420
3421 void
3422 MSVehicle::setBrakingSignals(double vNext) {
3423 // To avoid casual blinking brake lights at high speeds due to dawdling of the
3424 // leading vehicle, we don't show brake lights when the deceleration could be caused
3425 // by frictional forces and air resistance (i.e. proportional to v^2, coefficient could be adapted further)
3426 double pseudoFriction = (0.05 + 0.005 * getSpeed()) * getSpeed();
3427 bool brakelightsOn = vNext < getSpeed() - ACCEL2SPEED(pseudoFriction);
3428
3429 if (vNext <= SUMO_const_haltingSpeed) {
3430 brakelightsOn = true;
3431 }
3432 if (brakelightsOn && !isStopped()) {
3433 switchOnSignal(VEH_SIGNAL_BRAKELIGHT);
3434 } else {
3435 switchOffSignal(VEH_SIGNAL_BRAKELIGHT);
3436 }
3437 }
3438
3439
3440 void
3441 MSVehicle::updateWaitingTime(double vNext) {
3442 if (vNext <= SUMO_const_haltingSpeed && !isStopped()) {
3443 myWaitingTime += DELTA_T;
3444 myWaitingTimeCollector.passTime(DELTA_T, true);
3445 } else {
3446 myWaitingTime = 0;
3447 myWaitingTimeCollector.passTime(DELTA_T, false);
3448 }
3449 }
3450
3451
3452 void
3453 MSVehicle::updateTimeLoss(double vNext) {
3454 // update time loss (depends on the updated edge)
3455 if (!isStopped()) {
3456 const double vmax = myLane->getVehicleMaxSpeed(this);
3457 if (vmax > 0) {
3458 myTimeLoss += TS * (vmax - vNext) / vmax;
3459 }
3460 }
3461 }
3462
3463
3464 bool
3465 MSVehicle::canReverse(double speedThreshold) const {
3466 #ifdef DEBUG_REVERSE_BIDI
3467 if (DEBUG_COND) std::cout << SIMTIME << " canReverse lane=" << myLane->getID()
3468 << " pos=" << myState.myPos
3469 << " speed=" << std::setprecision(6) << getPreviousSpeed() << std::setprecision(gPrecision)
3470 << " isRail=" << ((getVClass() & SVC_RAIL_CLASSES) != 0)
3471 << " speedOk=" << (getPreviousSpeed() <= speedThreshold)
3472 << " posOK=" << (myState.myPos <= myLane->getLength())
3473 << " normal=" << !myLane->isInternal()
3474 << " routeOK=" << ((myCurrEdge + 1) != myRoute->end())
3475 << " bidi=" << (myLane->getEdge().getBidiEdge() == *(myCurrEdge + 1))
3476 << " stopOk=" << (myStops.empty() || myStops.front().edge != myCurrEdge)
3477 << "\n";
3478 #endif
3479 if ((getVClass() & SVC_RAIL_CLASSES) != 0
3480 && getPreviousSpeed() <= speedThreshold
3481 && myState.myPos <= myLane->getLength()
3482 && !myLane->isInternal()
3483 && (myCurrEdge + 1) != myRoute->end()
3484 && myLane->getEdge().getBidiEdge() == *(myCurrEdge + 1)
3485 // ensure there are no further stops on this edge
3486 && (myStops.empty() || myStops.front().edge != myCurrEdge)
3487 ) {
3488 //if (isSelected()) std::cout << " check1 passed\n";
3489 // ensure that the vehicle is fully on bidi edges that allow reversal
3490 if ((int)(myRoute->end() - myCurrEdge) <= (int)myFurtherLanes.size()) {
3491 return false;
3492 }
3493 //if (isSelected()) std::cout << " check2 passed\n";
3494 // ensure that the turn-around connection exists from the current edge to it's bidi-edge
3495 const MSEdgeVector& succ = myLane->getEdge().getSuccessors();
3496 if (std::find(succ.begin(), succ.end(), myLane->getEdge().getBidiEdge()) == succ.end()) {
3497 return false;
3498 }
3499
3500 //if (isSelected()) std::cout << " check3 passed\n";
3501 int view = 2;
3502 for (MSLane* further : myFurtherLanes) {
3503 if (!further->getEdge().isInternal()) {
3504 if (further->getEdge().getBidiEdge() != *(myCurrEdge + view)
3505 || (!myStops.empty() && myStops.front().edge == (myCurrEdge + view))) {
3506 return false;
3507 }
3508 view++;
3509 }
3510 }
3511 return true;
3512 }
3513 return false;
3514 }
3515
3516
3517 void
3518 MSVehicle::processLaneAdvances(std::vector<MSLane*>& passedLanes, bool& moved, std::string& emergencyReason) {
3519 for (std::vector<MSLane*>::reverse_iterator i = myFurtherLanes.rbegin(); i != myFurtherLanes.rend(); ++i) {
3520 passedLanes.push_back(*i);
3521 }
3522 if (passedLanes.size() == 0 || passedLanes.back() != myLane) {
3523 passedLanes.push_back(myLane);
3524 }
3525 // let trains reverse direction
3526 const bool reverseTrain = canReverse();
3527 if (reverseTrain) {
3528 myState.myPos += 2 * (myLane->getLength() - myState.myPos) + myType->getLength();
3529 myState.mySpeed = 0;
3530 }
3531 // move on lane(s)
3532 if (myState.myPos > myLane->getLength()) {
3533 // The vehicle has moved at least to the next lane (maybe it passed even more than one)
3534 if (myCurrEdge != myRoute->end() - 1) {
3535 MSLane* approachedLane = myLane;
3536 // move the vehicle forward
3537 myNextDriveItem = myLFLinkLanes.begin();
3538 while (myNextDriveItem != myLFLinkLanes.end() && approachedLane != nullptr && myState.myPos > approachedLane->getLength()) {
3539 const MSLink* link = myNextDriveItem->myLink;
3540 const double linkDist = myNextDriveItem->myDistance;
3541 ++myNextDriveItem;
3542 // check whether the vehicle was allowed to enter lane
3543 // otherwise it is decelerated and we do not need to test for it's
3544 // approach on the following lanes when a lane changing is performed
3545 // proceed to the next lane
3546 if (approachedLane->mustCheckJunctionCollisions()) {
3547 // vehicle moves past approachedLane within a single step, collision checking must still be done
3548 MSNet::getInstance()->getEdgeControl().checkCollisionForInactive(approachedLane);
3549 }
3550 if (link != nullptr) {
3551 approachedLane = link->getViaLaneOrLane();
3552 if (myInfluencer == nullptr || myInfluencer->getEmergencyBrakeRedLight()) {
3553 bool beyondStopLine = linkDist < link->getLaneBefore()->getStopOffset(this);
3554 if (link->haveRed() && !ignoreRed(link, false) && !beyondStopLine) {
3555 emergencyReason = " because of a red traffic light";
3556 break;
3557 }
3558 }
3559 } else if (myState.myPos < myLane->getLength() + NUMERICAL_EPS) {
3560 // avoid warning due to numerical instability
3561 approachedLane = myLane;
3562 myState.myPos = myLane->getLength();
3563 } else if (reverseTrain) {
3564 approachedLane = (*(myCurrEdge + 1))->getLanes()[0];
3565 if (MSGlobals::gUsingInternalLanes) {
3566 link = MSLinkContHelper::getConnectingLink(*myLane, *approachedLane);
3567 assert(link != 0);
3568 while (link->getViaLane() != nullptr) {
3569 link = link->getViaLane()->getLinkCont()[0];
3570 }
3571 }
3572 --myNextDriveItem;
3573 } else {
3574 emergencyReason = " because there is no connection to the next edge";
3575 approachedLane = nullptr;
3576 break;
3577 }
3578 if (approachedLane != myLane && approachedLane != nullptr) {
3579 leaveLane(MSMoveReminder::NOTIFICATION_JUNCTION, approachedLane);
3580 myState.myPos -= myLane->getLength();
3581 assert(myState.myPos > 0);
3582 enterLaneAtMove(approachedLane);
3583 if (link->isEntryLink()) {
3584 myJunctionEntryTime = MSNet::getInstance()->getCurrentTimeStep();
3585 myJunctionEntryTimeNeverYield = myJunctionEntryTime;
3586 }
3587 if (link->isConflictEntryLink()) {
3588 myJunctionConflictEntryTime = MSNet::getInstance()->getCurrentTimeStep();
3589 }
3590 if (link->isExitLink()) {
3591 // passed junction, reset for approaching the next one
3592 myJunctionEntryTime = SUMOTime_MAX;
3593 myJunctionEntryTimeNeverYield = SUMOTime_MAX;
3594 myJunctionConflictEntryTime = SUMOTime_MAX;
3595 }
3596 #ifdef DEBUG_PLAN_MOVE_LEADERINFO
3597 if (DEBUG_COND) {
3598 std::cout << "Update junctionTimes link=" << link->getViaLaneOrLane()->getID()
3599 << " entry=" << link->isEntryLink() << " conflict=" << link->isConflictEntryLink() << " exit=" << link->isExitLink()
3600 << " ET=" << myJunctionEntryTime
3601 << " ETN=" << myJunctionEntryTimeNeverYield
3602 << " CET=" << myJunctionConflictEntryTime
3603 << "\n";
3604 }
3605 #endif
3606 if (hasArrived()) {
3607 break;
3608 }
3609 if (getLaneChangeModel().isChangingLanes()) {
3610 if (link->getDirection() == LINKDIR_LEFT || link->getDirection() == LINKDIR_RIGHT) {
3611 // abort lane change
3612 WRITE_WARNING("Vehicle '" + getID() + "' could not finish continuous lane change (turn lane) time=" +
3613 time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
3614 getLaneChangeModel().endLaneChangeManeuver();
3615 }
3616 }
3617 moved = true;
3618 if (approachedLane->getEdge().isVaporizing()) {
3619 leaveLane(MSMoveReminder::NOTIFICATION_VAPORIZED);
3620 break;
3621 }
3622 passedLanes.push_back(approachedLane);
3623 }
3624 }
3625 // NOTE: Passed drive items will be erased in the next simstep's planMove()
3626
3627 #ifdef DEBUG_ACTIONSTEPS
3628 if (DEBUG_COND && myNextDriveItem != myLFLinkLanes.begin()) {
3629 std::cout << "Updated drive items:" << std::endl;
3630 for (DriveItemVector::iterator i = myLFLinkLanes.begin(); i != myLFLinkLanes.end(); ++i) {
3631 std::cout
3632 << " vPass=" << (*i).myVLinkPass
3633 << " vWait=" << (*i).myVLinkWait
3634 << " linkLane=" << ((*i).myLink == 0 ? "NULL" : (*i).myLink->getViaLaneOrLane()->getID())
3635 << " request=" << (*i).mySetRequest
3636 << "\n";
3637 }
3638 }
3639 #endif
3640 } else if (!hasArrived() && myState.myPos < myLane->getLength() + NUMERICAL_EPS) {
3641 // avoid warning due to numerical instability when stopping at the end of the route
3642 myState.myPos = myLane->getLength();
3643 }
3644
3645 }
3646 }
3647
3648
3649
3650 bool
3651 MSVehicle::executeMove() {
3652 #ifdef DEBUG_EXEC_MOVE
3653 if (DEBUG_COND) {
3654 std::cout << "\nEXECUTE_MOVE\n"
3655 << SIMTIME
3656 << " veh=" << getID()
3657 << " speed=" << getSpeed() // toString(getSpeed(), 24)
3658 << std::endl;
3659 }
3660 #endif
3661
3662
3663 // Maximum safe velocity
3664 double vSafe = std::numeric_limits<double>::max();
3665 // Minimum safe velocity (lower bound).
3666 double vSafeMin = -std::numeric_limits<double>::max();
3667 // The distance to a link, which should either be crossed this step
3668 // or in front of which we need to stop.
3669 double vSafeMinDist = 0;
3670
3671 if (myActionStep) {
3672 // Actuate control (i.e. choose bounds for safe speed in current simstep (euler), resp. after current sim step (ballistic))
3673 processLinkApproaches(vSafe, vSafeMin, vSafeMinDist);
3674 #ifdef DEBUG_ACTIONSTEPS
3675 if DEBUG_COND {
3676 std::cout << SIMTIME << " vehicle '" << getID() << "'\n"
3677 " vsafe from processLinkApproaches(): vsafe " << vSafe << std::endl;
3678 }
3679 #endif
3680 } else {
3681 // Continue with current acceleration
3682 vSafe = getSpeed() + ACCEL2SPEED(myAcceleration);
3683 #ifdef DEBUG_ACTIONSTEPS
3684 if DEBUG_COND {
3685 std::cout << SIMTIME << " vehicle '" << getID() << "' skips processLinkApproaches()\n"
3686 " continues with constant accel " << myAcceleration << "...\n"
3687 << "speed: " << getSpeed() << " -> " << vSafe << std::endl;
3688 }
3689 #endif
3690 }
3691
3692
3693 //#ifdef DEBUG_EXEC_MOVE
3694 // if (DEBUG_COND) {
3695 // std::cout << "vSafe = " << toString(vSafe,12) << "\n" << std::endl;
3696 // }
3697 //#endif
3698
3699 // Determine vNext = speed after current sim step (ballistic), resp. in current simstep (euler)
3700 // Call to finalizeSpeed applies speed reduction due to dawdling / lane changing but ensures minimum safe speed
3701 double vNext = vSafe;
3702 if (myActionStep) {
3703 vNext = getCarFollowModel().finalizeSpeed(this, vSafe);
3704 if (vNext > 0) {
3705 vNext = MAX2(vNext, vSafeMin);
3706 }
3707 }
3708 // (Leo) to avoid tiny oscillations (< 1e-10) of vNext in a standing vehicle column (observed for ballistic update), we cap off vNext
3709 // (We assure to do this only for vNext<<NUMERICAL_EPS since otherwise this would nullify the workaround for #2995
3710 if (fabs(vNext) < NUMERICAL_EPS_SPEED) {
3711 vNext = 0.;
3712 }
3713 #ifdef DEBUG_EXEC_MOVE
3714 if (DEBUG_COND) {
3715 std::cout << SIMTIME << " finalizeSpeed vSafe=" << vSafe << " vSafeMin=" << (vSafeMin == -std::numeric_limits<double>::max() ? "-Inf" : toString(vSafeMin))
3716 << " vNext=" << vNext << " (i.e. accel=" << SPEED2ACCEL(vNext - getSpeed()) << ")" << std::endl;
3717 }
3718 #endif
3719
3720 // vNext may be higher than vSafe without implying a bug:
3721 // - when approaching a green light that suddenly switches to yellow
3722 // - when using unregulated junctions
3723 // - when using tau < step-size
3724 // - when using unsafe car following models
3725 // - when using TraCI and some speedMode / laneChangeMode settings
3726 //if (vNext > vSafe + NUMERICAL_EPS) {
3727 // WRITE_WARNING("vehicle '" + getID() + "' cannot brake hard enough to reach safe speed "
3728 // + toString(vSafe, 4) + ", moving at " + toString(vNext, 4) + " instead. time="
3729 // + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
3730 //}
3731
3732 if (MSGlobals::gSemiImplicitEulerUpdate) {
3733 vNext = MAX2(vNext, 0.);
3734 } else {
3735 // (Leo) Ballistic: negative vNext can be used to indicate a stop within next step.
3736 }
3737
3738 // Check for speed advices from the traci client
3739 vNext = processTraCISpeedControl(vSafe, vNext);
3740
3741 setBrakingSignals(vNext);
3742 updateWaitingTime(vNext);
3743
3744 // update position and speed
3745 updateState(vNext);
3746
3747 // Lanes, which the vehicle touched at some moment of the executed simstep
3748 std::vector<MSLane*> passedLanes;
3749 // Whether the vehicle did move to another lane
3750 bool moved = false;
3751 // Reason for a possible emergency stop
3752 std::string emergencyReason = " for unknown reasons";
3753 processLaneAdvances(passedLanes, moved, emergencyReason);
3754
3755 updateTimeLoss(vNext);
3756 myCollisionImmunity = MAX2((SUMOTime) - 1, myCollisionImmunity - DELTA_T);
3757
3758 if (!hasArrived() && !myLane->getEdge().isVaporizing()) {
3759 if (myState.myPos > myLane->getLength()) {
3760 WRITE_WARNING("Vehicle '" + getID() + "' performs emergency stop at the end of lane '" + myLane->getID()
3761 + "'" + emergencyReason
3762 + " (decel=" + toString(myAcceleration - myState.mySpeed)
3763 + ", offset=" + toString(myState.myPos - myLane->getLength())
3764 + "), time=" + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
3765 MSNet::getInstance()->getVehicleControl().registerEmergencyStop();
3766 MSNet::getInstance()->informVehicleStateListener(this, MSNet::VEHICLE_STATE_EMERGENCYSTOP);
3767 myState.myPos = myLane->getLength();
3768 myState.mySpeed = 0;
3769 myAcceleration = 0;
3770 // reset drive items and link approaches
3771 removeApproachingInformation(myLFLinkLanes);
3772 myLFLinkLanes.clear();
3773 myNextDriveItem = myLFLinkLanes.begin();
3774 }
3775 const MSLane* oldBackLane = getBackLane();
3776 if (getLaneChangeModel().isOpposite()) {
3777 passedLanes.clear(); // ignore back occupation
3778 }
3779 #ifdef DEBUG_ACTIONSTEPS
3780 if DEBUG_COND {
3781 std::cout << SIMTIME << " veh '" << getID() << "' updates further lanes." << std::endl;
3782 }
3783 #endif
3784 myState.myBackPos = updateFurtherLanes(myFurtherLanes, myFurtherLanesPosLat, passedLanes);
3785 // bestLanes need to be updated before lane changing starts. NOTE: This call is also a presumption for updateDriveItems()
3786 updateBestLanes();
3787 if (moved || oldBackLane != getBackLane()) {
3788 if (getLaneChangeModel().getShadowLane() != nullptr || getLateralOverlap() > POSITION_EPS) {
3789 // shadow lane must be updated if the front or back lane changed
3790 // either if we already have a shadowLane or if there is lateral overlap
3791 getLaneChangeModel().updateShadowLane();
3792 }
3793 if (MSGlobals::gLateralResolution > 0) {
3794 // The vehicles target lane must be also be updated if the front or back lane changed
3795 getLaneChangeModel().updateTargetLane();
3796 }
3797 }
3798 setBlinkerInformation(); // needs updated bestLanes
3799 //change the blue light only for emergency vehicles SUMOVehicleClass
3800 if (myType->getVehicleClass() == SVC_EMERGENCY) {
3801 setEmergencyBlueLight(MSNet::getInstance()->getCurrentTimeStep());
3802 }
3803 // State needs to be reset for all vehicles before the next call to MSEdgeControl::changeLanes
3804 if (myActionStep) {
3805 // check (#2681): Can this be skipped?
3806 getLaneChangeModel().prepareStep();
3807 } else {
3808 #ifdef DEBUG_ACTIONSTEPS
3809 if DEBUG_COND {
3810 std::cout << SIMTIME << " veh '" << getID() << "' skips LCM->prepareStep()." << std::endl;
3811 }
3812 #endif
3813 }
3814 myAngle = computeAngle();
3815 }
3816
3817 #ifdef DEBUG_EXEC_MOVE
3818 if (DEBUG_COND) {
3819 std::cout << SIMTIME << " executeMove finished veh=" << getID() << " lane=" << myLane->getID() << " myPos=" << getPositionOnLane() << " myPosLat=" << getLateralPositionOnLane() << "\n";
3820 gDebugFlag1 = false; // See MSLink_DEBUG_OPENED
3821 }
3822 #endif
3823 if (getLaneChangeModel().isOpposite()) {
3824 // transform back to the opposite-direction lane
3825 if (myLane->getOpposite() == nullptr) {
3826 WRITE_WARNING("Unexpected end of opposite lane for vehicle '" + getID() + "' at lane '" + myLane->getID() + "', time=" +
3827 time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
3828 getLaneChangeModel().changedToOpposite();
3829 } else {
3830 myState.myPos = myLane->getOppositePos(myState.myPos);
3831 myLane = myLane->getOpposite();
3832 myCachedPosition = Position::INVALID;
3833 }
3834 }
3835 workOnMoveReminders(myState.myPos - myState.myLastCoveredDist, myState.myPos, myState.mySpeed);
3836 return moved;
3837 }
3838
3839
3840 void
3841 MSVehicle::updateState(double vNext) {
3842 // update position and speed
3843 double deltaPos; // positional change
3844 if (MSGlobals::gSemiImplicitEulerUpdate) {
3845 // euler
3846 deltaPos = SPEED2DIST(vNext);
3847 } else {
3848 // ballistic
3849 deltaPos = getDeltaPos(SPEED2ACCEL(vNext - myState.mySpeed));
3850 }
3851
3852 // the *mean* acceleration during the next step (probably most appropriate for emission calculation)
3853 // NOTE: for the ballistic update vNext may be negative, indicating a stop.
3854 myAcceleration = SPEED2ACCEL(MAX2(vNext, 0.) - myState.mySpeed);
3855
3856 #ifdef DEBUG_EXEC_MOVE
3857 if (DEBUG_COND) {
3858 std::cout << SIMTIME << " updateState() for veh '" << getID() << "': deltaPos=" << deltaPos
3859 << " pos=" << myState.myPos << " newPos=" << myState.myPos + deltaPos << std::endl;
3860 }
3861 #endif
3862 double decelPlus = -myAcceleration - getCarFollowModel().getMaxDecel() - NUMERICAL_EPS;
3863 if (decelPlus > 0) {
3864 const double previousAcceleration = SPEED2ACCEL(myState.mySpeed - myState.myPreviousSpeed);
3865 if (myAcceleration + NUMERICAL_EPS < previousAcceleration) {
3866 // vehicle brakes beyond wished maximum deceleration (only warn at the start of the braking manoeuvre)
3867 decelPlus += 2 * NUMERICAL_EPS;
3868 const double emergencyFraction = decelPlus / MAX2(NUMERICAL_EPS, getCarFollowModel().getEmergencyDecel() - getCarFollowModel().getMaxDecel());
3869 if (emergencyFraction >= MSGlobals::gEmergencyDecelWarningThreshold) {
3870 WRITE_WARNING("Vehicle '" + getID()
3871 + "' performs emergency braking with decel=" + toString(myAcceleration)
3872 + " wished=" + toString(getCarFollowModel().getMaxDecel())
3873 + " severity=" + toString(emergencyFraction)
3874 //+ " decelPlus=" + toString(decelPlus)
3875 //+ " prevAccel=" + toString(previousAcceleration)
3876 //+ " reserve=" + toString(MAX2(NUMERICAL_EPS, getCarFollowModel().getEmergencyDecel() - getCarFollowModel().getMaxDecel()))
3877 + ", time=" + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
3878 }
3879 }
3880 }
3881
3882 myState.myPreviousSpeed = myState.mySpeed;
3883 myState.mySpeed = MAX2(vNext, 0.);
3884
3885 if (myInfluencer != nullptr && myInfluencer->isRemoteControlled()) {
3886 deltaPos = myInfluencer->implicitDeltaPosRemote(this);
3887 }
3888
3889 if (getLaneChangeModel().isOpposite()) {
3890 // transform to the forward-direction lane, move and then transform back
3891 myState.myPos = myLane->getOppositePos(myState.myPos);
3892 myLane = myLane->getOpposite();
3893 }
3894 myState.myPos += deltaPos;
3895 myState.myLastCoveredDist = deltaPos;
3896 myNextTurn.first -= deltaPos;
3897
3898 myCachedPosition = Position::INVALID;
3899 }
3900
3901
3902 const MSLane*
3903 MSVehicle::getBackLane() const {
3904 if (myFurtherLanes.size() > 0) {
3905 return myFurtherLanes.back();
3906 } else {
3907 return myLane;
3908 }
3909 }
3910
3911
3912 double
3913 MSVehicle::updateFurtherLanes(std::vector<MSLane*>& furtherLanes, std::vector<double>& furtherLanesPosLat,
3914 const std::vector<MSLane*>& passedLanes) {
3915 #ifdef DEBUG_SETFURTHER
3916 if (DEBUG_COND) std::cout << SIMTIME << " veh=" << getID()
3917 << " updateFurtherLanes oldFurther=" << toString(furtherLanes)
3918 << " oldFurtherPosLat=" << toString(furtherLanesPosLat)
3919 << " passed=" << toString(passedLanes)
3920 << "\n";
3921 #endif
3922 for (std::vector<MSLane*>::iterator i = furtherLanes.begin(); i != furtherLanes.end(); ++i) {
3923 (*i)->resetPartialOccupation(this);
3924 }
3925
3926 std::vector<MSLane*> newFurther;
3927 std::vector<double> newFurtherPosLat;
3928 double backPosOnPreviousLane = myState.myPos - getLength();
3929 bool widthShift = myFurtherLanesPosLat.size() > myFurtherLanes.size();
3930 if (passedLanes.size() > 1) {
3931 // There are candidates for further lanes. (passedLanes[-1] is the current lane, or current shadow lane in context of updateShadowLanes())
3932 std::vector<MSLane*>::const_iterator fi = furtherLanes.begin();
3933 std::vector<double>::const_iterator fpi = furtherLanesPosLat.begin();
3934 for (auto pi = passedLanes.rbegin() + 1; pi != passedLanes.rend() && backPosOnPreviousLane < 0; ++pi) {
3935 // As long as vehicle back reaches into passed lane, add it to the further lanes
3936 newFurther.push_back(*pi);
3937 backPosOnPreviousLane += (*pi)->setPartialOccupation(this);
3938 if (fi != furtherLanes.end() && *pi == *fi) {
3939 // Lateral position on this lane is already known. Assume constant and use old value.
3940 newFurtherPosLat.push_back(*fpi);
3941 ++fi;
3942 ++fpi;
3943 } else {
3944 // The lane *pi was not in furtherLanes before.
3945 // If it is downstream, we assume as lateral position the current position
3946 // If it is a new lane upstream (can appear as shadow further in case of LC-maneuvering, e.g.)
3947 // we assign the last known lateral position.
3948 if (newFurtherPosLat.size() == 0) {
3949 if (widthShift) {
3950 newFurtherPosLat.push_back(myFurtherLanesPosLat.back());
3951 } else {
3952 newFurtherPosLat.push_back(myState.myPosLat);
3953 }
3954 } else {
3955 newFurtherPosLat.push_back(newFurtherPosLat.back());
3956 }
3957 }
3958 #ifdef DEBUG_SETFURTHER
3959 if (DEBUG_COND) {
3960 std::cout << SIMTIME << " updateFurtherLanes \n"
3961 << " further lane '" << (*pi)->getID() << "' backPosOnPreviousLane=" << backPosOnPreviousLane
3962 << std::endl;
3963 }
3964 #endif
3965 }
3966 furtherLanes = newFurther;
3967 furtherLanesPosLat = newFurtherPosLat;
3968 } else {
3969 furtherLanes.clear();
3970 furtherLanesPosLat.clear();
3971 }
3972 #ifdef DEBUG_SETFURTHER
3973 if (DEBUG_COND) std::cout
3974 << " newFurther=" << toString(furtherLanes)
3975 << " newFurtherPosLat=" << toString(furtherLanesPosLat)
3976 << " newBackPos=" << backPosOnPreviousLane
3977 << "\n";
3978 #endif
3979 return backPosOnPreviousLane;
3980 }
3981
3982
3983 double
3984 MSVehicle::getBackPositionOnLane(const MSLane* lane) const {
3985 #ifdef DEBUG_FURTHER
3986 if (DEBUG_COND) {
3987 std::cout << SIMTIME
3988 << " getBackPositionOnLane veh=" << getID()
3989 << " lane=" << Named::getIDSecure(lane)
3990 << " myLane=" << myLane->getID()
3991 << " further=" << toString(myFurtherLanes)
3992 << " furtherPosLat=" << toString(myFurtherLanesPosLat)
3993 << "\n shadowLane=" << Named::getIDSecure(getLaneChangeModel().getShadowLane())
3994 << " shadowFurther=" << toString(getLaneChangeModel().getShadowFurtherLanes())
3995 << " shadowFurtherPosLat=" << toString(getLaneChangeModel().getShadowFurtherLanesPosLat())
3996 << "\n targetLane=" << Named::getIDSecure(getLaneChangeModel().getTargetLane())
3997 << " furtherTargets=" << toString(getLaneChangeModel().getFurtherTargetLanes())
3998 << std::endl;
3999 }
4000 #endif
4001 if (lane == myLane
4002 || lane == getLaneChangeModel().getShadowLane()
4003 || lane == getLaneChangeModel().getTargetLane()) {
4004 if (getLaneChangeModel().isOpposite()) {
4005 return myState.myPos + myType->getLength();
4006 } else {
4007 return myState.myPos - myType->getLength();
4008 }
4009 } else if ((myFurtherLanes.size() > 0 && lane == myFurtherLanes.back())
4010 || (getLaneChangeModel().getShadowFurtherLanes().size() > 0 && lane == getLaneChangeModel().getShadowFurtherLanes().back())
4011 || (getLaneChangeModel().getFurtherTargetLanes().size() > 0 && lane == getLaneChangeModel().getFurtherTargetLanes().back())) {
4012 return myState.myBackPos;
4013 } else {
4014 //if (DEBUG_COND) std::cout << SIMTIME << " veh=" << getID() << " myFurtherLanes=" << toString(myFurtherLanes) << "\n";
4015 double leftLength = myType->getLength() - myState.myPos;
4016
4017 std::vector<MSLane*>::const_iterator i = myFurtherLanes.begin();
4018 while (leftLength > 0 && i != myFurtherLanes.end()) {
4019 leftLength -= (*i)->getLength();
4020 //if (DEBUG_COND) std::cout << " comparing i=" << (*i)->getID() << " lane=" << lane->getID() << "\n";
4021 if (*i == lane) {
4022 return -leftLength;
4023 }
4024 ++i;
4025 }
4026 //if (DEBUG_COND) std::cout << SIMTIME << " veh=" << getID() << " myShadowFurtherLanes=" << toString(getLaneChangeModel().getShadowFurtherLanes()) << "\n";
4027 leftLength = myType->getLength() - myState.myPos;
4028 i = getLaneChangeModel().getShadowFurtherLanes().begin();
4029 while (leftLength > 0 && i != getLaneChangeModel().getShadowFurtherLanes().end()) {
4030 leftLength -= (*i)->getLength();
4031 //if (DEBUG_COND) std::cout << " comparing i=" << (*i)->getID() << " lane=" << lane->getID() << "\n";
4032 if (*i == lane) {
4033 return -leftLength;
4034 }
4035 ++i;
4036 }
4037 //if (DEBUG_COND) std::cout << SIMTIME << " veh=" << getID() << " myFurtherTargetLanes=" << toString(getLaneChangeModel().getFurtherTargetLanes()) << "\n";
4038 leftLength = myType->getLength() - myState.myPos;
4039 i = getFurtherLanes().begin();
4040 const std::vector<MSLane*> furtherTargetLanes = getLaneChangeModel().getFurtherTargetLanes();
4041 auto j = furtherTargetLanes.begin();
4042 while (leftLength > 0 && j != furtherTargetLanes.end()) {
4043 leftLength -= (*i)->getLength();
4044 // if (DEBUG_COND) std::cout << " comparing i=" << (*i)->getID() << " lane=" << lane->getID() << "\n";
4045 if (*j == lane) {
4046 return -leftLength;
4047 }
4048 ++i;
4049 ++j;
4050 }
4051 WRITE_WARNING("Request backPos of vehicle '" + getID() + "' for invalid lane '" + Named::getIDSecure(lane)
4052 + "' time=" + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".")
4053 #ifdef _MSC_VER
4054 #pragma warning(push)
4055 #pragma warning(disable: 4127) // do not warn about constant conditional expression
4056 #endif
4057 SOFT_ASSERT(false);
4058 #ifdef _MSC_VER
4059 #pragma warning(pop)
4060 #endif
4061 return myState.myBackPos;
4062 }
4063 }
4064
4065
4066 double
4067 MSVehicle::getPositionOnLane(const MSLane* lane) const {
4068 return getBackPositionOnLane(lane) + myType->getLength();
4069 }
4070
4071
4072 bool
4073 MSVehicle::isFrontOnLane(const MSLane* lane) const {
4074 return lane == myLane || lane == getLaneChangeModel().getShadowLane();
4075 }
4076
4077
4078 double
4079 MSVehicle::getSpaceTillLastStanding(const MSLane* l, bool& foundStopped) const {
4080 double lengths = 0;
4081 const MSLane::VehCont& vehs = l->getVehiclesSecure();
4082 for (MSLane::VehCont::const_iterator i = vehs.begin(); i != vehs.end(); ++i) {
4083 const MSVehicle* last = *i;
4084 if (last->getSpeed() < SUMO_const_haltingSpeed && !last->getLane()->getEdge().isRoundabout()
4085 && last != this
4086 // @todo recheck
4087 && last->isFrontOnLane(l)) {
4088 foundStopped = true;
4089 const double lastBrakeGap = last->getCarFollowModel().brakeGap(last->getSpeed());
4090 const double ret = last->getBackPositionOnLane() + lastBrakeGap - lengths;
4091 l->releaseVehicles();
4092 return ret;
4093 }
4094 lengths += (*i)->getVehicleType().getLengthWithGap();
4095 }
4096 l->releaseVehicles();
4097 return l->getLength() - lengths;
4098 }
4099
4100
4101 void
4102 MSVehicle::checkRewindLinkLanes(const double lengthsInFront, DriveItemVector& lfLinks) const {
4103 if (MSGlobals::gUsingInternalLanes && !myLane->getEdge().isRoundabout() && !getLaneChangeModel().isOpposite()) {
4104 double seenSpace = -lengthsInFront;
4105 #ifdef DEBUG_CHECKREWINDLINKLANES
4106 if (DEBUG_COND) {
4107 std::cout << "\nCHECK_REWIND_LINKLANES\n" << " veh=" << getID() << " lengthsInFront=" << lengthsInFront << "\n";
4108 };
4109 #endif
4110 bool foundStopped = false;
4111 // compute available space until a stopped vehicle is found
4112 // this is the sum of non-interal lane length minus in-between vehicle lenghts
4113 for (int i = 0; i < (int)lfLinks.size(); ++i) {
4114 // skip unset links
4115 DriveProcessItem& item = lfLinks[i];
4116 #ifdef DEBUG_CHECKREWINDLINKLANES
4117 if (DEBUG_COND) std::cout << SIMTIME
4118 << " link=" << (item.myLink == 0 ? "NULL" : item.myLink->getViaLaneOrLane()->getID())
4119 << " foundStopped=" << foundStopped;
4120 #endif
4121 if (item.myLink == nullptr || foundStopped) {
4122 if (!foundStopped) {
4123 item.availableSpace += seenSpace;
4124 } else {
4125 item.availableSpace = seenSpace;
4126 }
4127 #ifdef DEBUG_CHECKREWINDLINKLANES
4128 if (DEBUG_COND) {
4129 std::cout << " avail=" << item.availableSpace << "\n";
4130 }
4131 #endif
4132 continue;
4133 }
4134 // get the next lane, determine whether it is an internal lane
4135 const MSLane* approachedLane = item.myLink->getViaLane();
4136 if (approachedLane != nullptr) {
4137 if (keepClear(item.myLink)) {
4138 seenSpace = seenSpace - approachedLane->getBruttoVehLenSum();
4139 if (approachedLane == myLane) {
4140 seenSpace += getVehicleType().getLengthWithGap();
4141 }
4142 } else {
4143 seenSpace = seenSpace + getSpaceTillLastStanding(approachedLane, foundStopped);// - approachedLane->getBruttoVehLenSum() + approachedLane->getLength();
4144 }
4145 item.availableSpace = seenSpace;
4146 #ifdef DEBUG_CHECKREWINDLINKLANES
4147 if (DEBUG_COND) std::cout
4148 << " approached=" << approachedLane->getID()
4149 << " approachedBrutto=" << approachedLane->getBruttoVehLenSum()
4150 << " avail=" << item.availableSpace
4151 << " seenSpace=" << seenSpace
4152 << " hadStoppedVehicle=" << item.hadStoppedVehicle
4153 << " lengthsInFront=" << lengthsInFront
4154 << "\n";
4155 #endif
4156 continue;
4157 }
4158 approachedLane = item.myLink->getLane();
4159 const MSVehicle* last = approachedLane->getLastAnyVehicle();
4160 if (last == nullptr || last == this) {
4161 if (approachedLane->getLength() > getVehicleType().getLength()
4162 || keepClear(item.myLink)) {
4163 seenSpace += approachedLane->getLength();
4164 }
4165 item.availableSpace = seenSpace;
4166 #ifdef DEBUG_CHECKREWINDLINKLANES
4167 if (DEBUG_COND) {
4168 std::cout << " last=" << Named::getIDSecure(last) << " laneLength=" << approachedLane->getLength() << " avail=" << item.availableSpace << "\n";
4169 }
4170 #endif
4171 } else {
4172 bool foundStopped2 = false;
4173 const double spaceTillLastStanding = getSpaceTillLastStanding(approachedLane, foundStopped2);
4174 seenSpace += spaceTillLastStanding;
4175 if (foundStopped2) {
4176 foundStopped = true;
4177 item.hadStoppedVehicle = true;
4178 }
4179 item.availableSpace = seenSpace;
4180 if (last->myHaveToWaitOnNextLink || last->isStopped()) {
4181 foundStopped = true;
4182 item.hadStoppedVehicle = true;
4183 }
4184 #ifdef DEBUG_CHECKREWINDLINKLANES
4185 if (DEBUG_COND) std::cout
4186 << " approached=" << approachedLane->getID()
4187 << " last=" << last->getID()
4188 << " lastHasToWait=" << last->myHaveToWaitOnNextLink
4189 << " lastBrakeLight=" << last->signalSet(VEH_SIGNAL_BRAKELIGHT)
4190 << " lastBrakeGap=" << last->getCarFollowModel().brakeGap(last->getSpeed())
4191 << " lastGap=" << (last->getBackPositionOnLane(approachedLane) + last->getCarFollowModel().brakeGap(last->getSpeed()) - last->getSpeed() * last->getCarFollowModel().getHeadwayTime()
4192 // gap of last up to the next intersection
4193 - last->getVehicleType().getMinGap())
4194 << " stls=" << spaceTillLastStanding
4195 << " avail=" << item.availableSpace
4196 << " seenSpace=" << seenSpace
4197 << " foundStopped=" << foundStopped
4198 << " foundStopped2=" << foundStopped2
4199 << "\n";
4200 #endif
4201 }
4202 }
4203
4204 // check which links allow continuation and add pass available to the previous item
4205 for (int i = ((int)lfLinks.size() - 1); i > 0; --i) {
4206 DriveProcessItem& item = lfLinks[i - 1];
4207 DriveProcessItem& nextItem = lfLinks[i];
4208 const bool canLeaveJunction = item.myLink->getViaLane() == nullptr || nextItem.myLink == nullptr || nextItem.mySetRequest;
4209 const bool opened = item.myLink != nullptr && canLeaveJunction && (
4210 item.myLink->havePriority()
4211 || i == 1 // the upcoming link (item 0) is checked in executeMove anyway. No need to use outdata approachData here
4212 || (myInfluencer != nullptr && !myInfluencer->getRespectJunctionPriority())
4213 || item.myLink->opened(item.myArrivalTime, item.myArrivalSpeed,
4214 item.getLeaveSpeed(), getVehicleType().getLength(),
4215 getImpatience(), getCarFollowModel().getMaxDecel(), getWaitingTime(), getLateralPositionOnLane()));
4216 bool allowsContinuation = (item.myLink == nullptr || item.myLink->isCont() || opened) && !item.hadStoppedVehicle;
4217 #ifdef DEBUG_CHECKREWINDLINKLANES
4218 if (DEBUG_COND) std::cout
4219 << " link=" << (item.myLink == 0 ? "NULL" : item.myLink->getViaLaneOrLane()->getID())
4220 << " canLeave=" << canLeaveJunction
4221 << " opened=" << opened
4222 << " allowsContinuation=" << allowsContinuation
4223 << " foundStopped=" << foundStopped
4224 << "\n";
4225 #endif
4226 if (!opened && item.myLink != nullptr) {
4227 foundStopped = true;
4228 if (i > 1) {
4229 DriveProcessItem& item2 = lfLinks[i - 2];
4230 if (item2.myLink != nullptr && item2.myLink->isCont()) {
4231 allowsContinuation = true;
4232 }
4233 }
4234 }
4235 if (allowsContinuation) {
4236 item.availableSpace = nextItem.availableSpace;
4237 #ifdef DEBUG_CHECKREWINDLINKLANES
4238 if (DEBUG_COND) std::cout
4239 << " link=" << (item.myLink == nullptr ? "NULL" : item.myLink->getViaLaneOrLane()->getID())
4240 << " copy nextAvail=" << nextItem.availableSpace
4241 << "\n";
4242 #endif
4243 }
4244 }
4245
4246 // find removalBegin
4247 int removalBegin = -1;
4248 for (int i = 0; foundStopped && i < (int)lfLinks.size() && removalBegin < 0; ++i) {
4249 // skip unset links
4250 const DriveProcessItem& item = lfLinks[i];
4251 if (item.myLink == nullptr) {
4252 continue;
4253 }
4254 /*
4255 double impatienceCorrection = MAX2(0., double(double(myWaitingTime)));
4256 if (seenSpace<getVehicleType().getLengthWithGap()-impatienceCorrection/10.&&nextSeenNonInternal!=0) {
4257 removalBegin = lastLinkToInternal;
4258 }
4259 */
4260
4261 const double leftSpace = item.availableSpace - getVehicleType().getLengthWithGap();
4262 #ifdef DEBUG_CHECKREWINDLINKLANES
4263 if (DEBUG_COND) std::cout
4264 << SIMTIME
4265 << " veh=" << getID()
4266 << " link=" << (item.myLink == 0 ? "NULL" : item.myLink->getViaLaneOrLane()->getID())
4267 << " avail=" << item.availableSpace
4268 << " leftSpace=" << leftSpace
4269 << "\n";
4270 #endif
4271 if (leftSpace < 0/* && item.myLink->willHaveBlockedFoe()*/) {
4272 double impatienceCorrection = 0;
4273 /*
4274 if(item.myLink->getState()==LINKSTATE_MINOR) {
4275 impatienceCorrection = MAX2(0., STEPS2TIME(myWaitingTime));
4276 }
4277 */
4278 // may ignore keepClear rules
4279 if (leftSpace < -impatienceCorrection / 10. && keepClear(item.myLink)) {
4280 removalBegin = i;
4281 }
4282 //removalBegin = i;
4283 }
4284 }
4285 // abort requests
4286 if (removalBegin != -1 && !(removalBegin == 0 && myLane->getEdge().isInternal())) {
4287 while (removalBegin < (int)(lfLinks.size())) {
4288 const double brakeGap = getCarFollowModel().brakeGap(myState.mySpeed, getCarFollowModel().getMaxDecel(), 0.);
4289 lfLinks[removalBegin].myVLinkPass = lfLinks[removalBegin].myVLinkWait;
4290 if (lfLinks[removalBegin].myDistance >= brakeGap || (lfLinks[removalBegin].myDistance > 0 && myState.mySpeed < ACCEL2SPEED(getCarFollowModel().getMaxDecel()))) {
4291 lfLinks[removalBegin].mySetRequest = false;
4292 }
4293 ++removalBegin;
4294 }
4295 }
4296 }
4297 }
4298
4299
4300 void
4301 MSVehicle::setApproachingForAllLinks(const SUMOTime t) {
4302 if (!checkActionStep(t)) {
4303 return;
4304 }
4305 removeApproachingInformation(myLFLinkLanesPrev);
4306 for (DriveProcessItem& dpi : myLFLinkLanes) {
4307 if (dpi.myLink != nullptr) {
4308 if (dpi.myLink->getState() == LINKSTATE_ALLWAY_STOP) {
4309 dpi.myArrivalTime += (SUMOTime)RandHelper::rand((int)2, getRNG()); // tie braker
4310 }
4311 dpi.myLink->setApproaching(this, dpi.myArrivalTime, dpi.myArrivalSpeed, dpi.getLeaveSpeed(),
4312 dpi.mySetRequest, dpi.myArrivalTimeBraking, dpi.myArrivalSpeedBraking, getWaitingTime(), dpi.myDistance);
4313 }
4314 }
4315 if (getLaneChangeModel().getShadowLane() != nullptr) {
4316 // register on all shadow links
4317 for (const DriveProcessItem& dpi : myLFLinkLanes) {
4318 if (dpi.myLink != nullptr) {
4319 MSLink* parallelLink = dpi.myLink->getParallelLink(getLaneChangeModel().getShadowDirection());
4320 if (parallelLink != nullptr) {
4321 parallelLink->setApproaching(this, dpi.myArrivalTime, dpi.myArrivalSpeed, dpi.getLeaveSpeed(),
4322 dpi.mySetRequest, dpi.myArrivalTimeBraking, dpi.myArrivalSpeedBraking, getWaitingTime(), dpi.myDistance);
4323 getLaneChangeModel().setShadowApproachingInformation(parallelLink);
4324 }
4325 }
4326 }
4327 }
4328 #ifdef DEBUG_PLAN_MOVE
4329 if (DEBUG_COND) {
4330 std::cout << SIMTIME
4331 << " veh=" << getID()
4332 << " after checkRewindLinkLanes\n";
4333 for (DriveProcessItem& dpi : myLFLinkLanes) {
4334 std::cout
4335 << " vPass=" << dpi.myVLinkPass
4336 << " vWait=" << dpi.myVLinkWait
4337 << " linkLane=" << (dpi.myLink == 0 ? "NULL" : dpi.myLink->getViaLaneOrLane()->getID())
4338 << " request=" << dpi.mySetRequest
4339 << " atime=" << dpi.myArrivalTime
4340 << " atimeB=" << dpi.myArrivalTimeBraking
4341 << "\n";
4342 }
4343 }
4344 #endif
4345 }
4346
4347 void
4348 MSVehicle::activateReminders(const MSMoveReminder::Notification reason, const MSLane* enteredLane) {
4349 for (MoveReminderCont::iterator rem = myMoveReminders.begin(); rem != myMoveReminders.end();) {
4350 // skip the reminder if it is a lane reminder but not for my lane (indicated by rem->second > 0.)
4351 if (rem->first->getLane() != nullptr && rem->second > 0.) {
4352 #ifdef _DEBUG
4353 if (myTraceMoveReminders) {
4354 traceMoveReminder("notifyEnter_skipped", rem->first, rem->second, true);
4355 }
4356 #endif
4357 ++rem;
4358 } else {
4359 if (rem->first->notifyEnter(*this, reason, enteredLane)) {
4360 #ifdef _DEBUG
4361 if (myTraceMoveReminders) {
4362 traceMoveReminder("notifyEnter", rem->first, rem->second, true);
4363 }
4364 #endif
4365 ++rem;
4366 } else {
4367 #ifdef _DEBUG
4368 if (myTraceMoveReminders) {
4369 traceMoveReminder("notifyEnter", rem->first, rem->second, false);
4370 }
4371 // std::cout << SIMTIME << " Vehicle '" << getID() << "' erases MoveReminder (with offset " << rem->second << ")" << std::endl;
4372 #endif
4373 rem = myMoveReminders.erase(rem);
4374 }
4375 }
4376 }
4377 }
4378
4379
4380 bool
4381 MSVehicle::enterLaneAtMove(MSLane* enteredLane, bool onTeleporting) {
4382 myAmOnNet = !onTeleporting;
4383 // vaporizing edge?
4384 /*
4385 if (enteredLane->getEdge().isVaporizing()) {
4386 // yep, let's do the vaporization...
4387 myLane = enteredLane;
4388 return true;
4389 }
4390 */
4391 // Adjust MoveReminder offset to the next lane
4392 adaptLaneEntering2MoveReminder(*enteredLane);
4393 // set the entered lane as the current lane
4394 MSLane* oldLane = myLane;
4395 myLane = enteredLane;
4396 myLastBestLanesEdge = nullptr;
4397
4398 // internal edges are not a part of the route...
4399 if (!enteredLane->getEdge().isInternal()) {
4400 ++myCurrEdge;
4401 assert(haveValidStopEdges());
4402 }
4403 if (myInfluencer != nullptr) {
4404 myInfluencer->adaptLaneTimeLine(myLane->getIndex() - oldLane->getIndex());
4405 }
4406 if (!onTeleporting) {
4407 activateReminders(MSMoveReminder::NOTIFICATION_JUNCTION, enteredLane);
4408 if (MSGlobals::gLateralResolution > 0) {
4409 // transform lateral position when the lane width changes
4410 assert(oldLane != 0);
4411 MSLink* link = oldLane->getLinkTo(myLane);
4412 if (link) {
4413 myFurtherLanesPosLat.push_back(myState.myPosLat);
4414 myState.myPosLat += link->getLateralShift();
4415 }
4416 }
4417
4418 } else {
4419 // normal move() isn't called so reset position here. must be done
4420 // before calling reminders
4421 myState.myPos = 0;
4422 myCachedPosition = Position::INVALID;
4423 activateReminders(MSMoveReminder::NOTIFICATION_TELEPORT, enteredLane);
4424 }
4425 // update via
4426 if (myParameter->via.size() > 0 && myLane->getEdge().getID() == myParameter->via.front()) {
4427 myParameter->via.erase(myParameter->via.begin());
4428 }
4429 return hasArrived();
4430 }
4431
4432
4433 void
4434 MSVehicle::enterLaneAtLaneChange(MSLane* enteredLane) {
4435 myAmOnNet = true;
4436 myLane = enteredLane;
4437 myCachedPosition = Position::INVALID;
4438 // need to update myCurrentLaneInBestLanes
4439 updateBestLanes();
4440 // switch to and activate the new lane's reminders
4441 // keep OldLaneReminders
4442 for (std::vector< MSMoveReminder* >::const_iterator rem = enteredLane->getMoveReminders().begin(); rem != enteredLane->getMoveReminders().end(); ++rem) {
4443 addReminder(*rem);
4444 }
4445 activateReminders(MSMoveReminder::NOTIFICATION_LANE_CHANGE, enteredLane);
4446 MSLane* lane = myLane;
4447 double leftLength = getVehicleType().getLength() - myState.myPos;
4448 for (int i = 0; i < (int)myFurtherLanes.size(); i++) {
4449 if (lane != nullptr) {
4450 lane = lane->getLogicalPredecessorLane(myFurtherLanes[i]->getEdge());
4451 }
4452 if (lane != nullptr) {
4453 #ifdef DEBUG_SETFURTHER
4454 if (DEBUG_COND) {
4455 std::cout << SIMTIME << " enterLaneAtLaneChange \n";
4456 }
4457 #endif
4458 myFurtherLanes[i]->resetPartialOccupation(this);
4459 myFurtherLanes[i] = lane;
4460 myFurtherLanesPosLat[i] = myState.myPosLat;
4461 #ifdef DEBUG_SETFURTHER
4462 if (DEBUG_COND) {
4463 std::cout << SIMTIME << " enterLaneAtLaneChange \n";
4464 }
4465 #endif
4466 leftLength -= (lane)->setPartialOccupation(this);
4467 } else {
4468 // keep the old values, but ensure there is no shadow
4469 if (myLaneChangeModel->isChangingLanes()) {
4470 myLaneChangeModel->setNoShadowPartialOccupator(myFurtherLanes[i]);
4471 }
4472 }
4473 }
4474 #ifdef DEBUG_SETFURTHER
4475 if (DEBUG_COND) {
4476 std::cout << SIMTIME << " enterLaneAtLaneChange new furtherLanes=" << toString(myFurtherLanes)
4477 << " furterLanesPosLat=" << toString(myFurtherLanesPosLat) << "\n";
4478 }
4479 #endif
4480 myAngle = computeAngle();
4481 }
4482
4483
4484 void
4485 MSVehicle::enterLaneAtInsertion(MSLane* enteredLane, double pos, double speed, double posLat, MSMoveReminder::Notification notification) {
4486 myState = State(pos, speed, posLat, pos - getVehicleType().getLength());
4487 if (myDeparture == NOT_YET_DEPARTED) {
4488 onDepart();
4489 }
4490 myCachedPosition = Position::INVALID;
4491 assert(myState.myPos >= 0);
4492 assert(myState.mySpeed >= 0);
4493 myLane = enteredLane;
4494 myAmOnNet = true;
4495 // schedule action for the next timestep
4496 myLastActionTime = MSNet::getInstance()->getCurrentTimeStep() + DELTA_T;
4497 if (notification != MSMoveReminder::NOTIFICATION_TELEPORT) {
4498 // set and activate the new lane's reminders, teleports already did that at enterLaneAtMove
4499 for (std::vector< MSMoveReminder* >::const_iterator rem = enteredLane->getMoveReminders().begin(); rem != enteredLane->getMoveReminders().end(); ++rem) {
4500 addReminder(*rem);
4501 }
4502 activateReminders(notification, enteredLane);
4503 }
4504 // build the list of lanes the vehicle is lapping into
4505 if (!myLaneChangeModel->isOpposite()) {
4506 double leftLength = myType->getLength() - pos;
4507 MSLane* clane = enteredLane;
4508 while (leftLength > 0) {
4509 clane = clane->getLogicalPredecessorLane();
4510 if (clane == nullptr || clane == myLane || clane == myLane->getBidiLane()) {
4511 break;
4512 }
4513 myFurtherLanes.push_back(clane);
4514 myFurtherLanesPosLat.push_back(myState.myPosLat);
4515 leftLength -= (clane)->setPartialOccupation(this);
4516 }
4517 myState.myBackPos = -leftLength;
4518 } else {
4519 // clear partial occupation
4520 for (std::vector<MSLane*>::iterator i = myFurtherLanes.begin(); i != myFurtherLanes.end(); ++i) {
4521 #ifdef DEBUG_FURTHER
4522 if (DEBUG_COND) {
4523 std::cout << SIMTIME << " enterLaneAtInsertion \n";
4524 }
4525 #endif
4526 (*i)->resetPartialOccupation(this);
4527 }
4528 myFurtherLanes.clear();
4529 myFurtherLanesPosLat.clear();
4530 }
4531 if (MSGlobals::gLateralResolution > 0) {
4532 getLaneChangeModel().updateShadowLane();
4533 getLaneChangeModel().updateTargetLane();
4534 } else if (MSGlobals::gLaneChangeDuration > 0) {
4535 getLaneChangeModel().updateShadowLane();
4536 }
4537 myAngle = computeAngle();
4538 if (getLaneChangeModel().isOpposite()) {
4539 myAngle += M_PI;
4540 }
4541 }
4542
4543
4544 void
4545 MSVehicle::leaveLane(const MSMoveReminder::Notification reason, const MSLane* approachedLane) {
4546 for (MoveReminderCont::iterator rem = myMoveReminders.begin(); rem != myMoveReminders.end();) {
4547 if (rem->first->notifyLeave(*this, myState.myPos + rem->second, reason, approachedLane)) {
4548 #ifdef _DEBUG
4549 if (myTraceMoveReminders) {
4550 traceMoveReminder("notifyLeave", rem->first, rem->second, true);
4551 }
4552 #endif
4553 ++rem;
4554 } else {
4555 #ifdef _DEBUG
4556 if (myTraceMoveReminders) {
4557 traceMoveReminder("notifyLeave", rem->first, rem->second, false);
4558 }
4559 #endif
4560 rem = myMoveReminders.erase(rem);
4561 }
4562 }
4563 if (reason != MSMoveReminder::NOTIFICATION_JUNCTION && reason != MSMoveReminder::NOTIFICATION_LANE_CHANGE) {
4564 // @note. In case of lane change, myFurtherLanes and partial occupation
4565 // are handled in enterLaneAtLaneChange()
4566 for (std::vector<MSLane*>::iterator i = myFurtherLanes.begin(); i != myFurtherLanes.end(); ++i) {
4567 #ifdef DEBUG_FURTHER
4568 if (DEBUG_COND) {
4569 std::cout << SIMTIME << " leaveLane \n";
4570 }
4571 #endif
4572 (*i)->resetPartialOccupation(this);
4573 }
4574 myFurtherLanes.clear();
4575 myFurtherLanesPosLat.clear();
4576 }
4577 if (reason >= MSMoveReminder::NOTIFICATION_TELEPORT) {
4578 myAmOnNet = false;
4579 myWaitingTime = 0;
4580 }
4581 if (reason != MSMoveReminder::NOTIFICATION_PARKING && resumeFromStopping()) {
4582 WRITE_WARNING("Vehicle '" + getID() + "' aborts stop.");
4583 }
4584 if (reason != MSMoveReminder::NOTIFICATION_PARKING && reason != MSMoveReminder::NOTIFICATION_LANE_CHANGE) {
4585 while (!myStops.empty() && myStops.front().edge == myCurrEdge && &myStops.front().lane->getEdge() == &myLane->getEdge()) {
4586 WRITE_WARNING("Vehicle '" + getID() + "' skips stop on lane '" + myStops.front().lane->getID()
4587 + "' time=" + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".")
4588 myStops.pop_front();
4589 }
4590 }
4591 }
4592
4593
4594 MSAbstractLaneChangeModel&
4595 MSVehicle::getLaneChangeModel() {
4596 return *myLaneChangeModel;
4597 }
4598
4599
4600 const MSAbstractLaneChangeModel&
4601 MSVehicle::getLaneChangeModel() const {
4602 return *myLaneChangeModel;
4603 }
4604
4605
4606 const std::vector<MSVehicle::LaneQ>&
4607 MSVehicle::getBestLanes() const {
4608 return *myBestLanes.begin();
4609 }
4610
4611
4612 void
4613 MSVehicle::updateBestLanes(bool forceRebuild, const MSLane* startLane) {
4614 #ifdef DEBUG_BESTLANES
4615 if (DEBUG_COND) {
4616 std::cout << SIMTIME << " updateBestLanes veh=" << getID() << " force=" << forceRebuild << " startLane1=" << Named::getIDSecure(startLane) << " myLane=" << Named::getIDSecure(myLane) << "\n";
4617 }
4618 #endif
4619 if (startLane == nullptr) {
4620 startLane = myLane;
4621 }
4622 assert(startLane != 0);
4623 if (getLaneChangeModel().isOpposite()) {
4624 // depending on the calling context, startLane might be the forward lane
4625 // or the reverse-direction lane. In the latter case we need to
4626 // transform it to the forward lane.
4627 bool startLaneIsOpposite = (startLane->isInternal()
4628 ? & (startLane->getLinkCont()[0]->getLane()->getEdge()) != *(myCurrEdge + 1)
4629 : &startLane->getEdge() != *myCurrEdge);
4630 if (startLaneIsOpposite) {
4631 startLane = startLane->getOpposite();
4632 assert(startLane != 0);
4633 }
4634 }
4635 if (myBestLanes.size() > 0 && !forceRebuild && myLastBestLanesEdge == &startLane->getEdge()) {
4636 updateOccupancyAndCurrentBestLane(startLane);
4637 #ifdef DEBUG_BESTLANES
4638 if (DEBUG_COND) {
4639 std::cout << " only updateOccupancyAndCurrentBestLane\n";
4640 }
4641 #endif
4642 return;
4643 }
4644 if (startLane->getEdge().isInternal()) {
4645 if (myBestLanes.size() == 0 || forceRebuild) {
4646 // rebuilt from previous non-internal lane (may backtrack twice if behind an internal junction)
4647 updateBestLanes(true, startLane->getLogicalPredecessorLane());
4648 }
4649 if (myLastBestLanesInternalLane == startLane && !forceRebuild) {
4650 #ifdef DEBUG_BESTLANES
4651 if (DEBUG_COND) {
4652 std::cout << " nothing to do on internal\n";
4653 }
4654 #endif
4655 return;
4656 }
4657 // adapt best lanes to fit the current internal edge:
4658 // keep the entries that are reachable from this edge
4659 const MSEdge* nextEdge = startLane->getNextNormal();
4660 assert(!nextEdge->isInternal());
4661 for (std::vector<std::vector<LaneQ> >::iterator it = myBestLanes.begin(); it != myBestLanes.end();) {
4662 std::vector<LaneQ>& lanes = *it;
4663 assert(lanes.size() > 0);
4664 if (&(lanes[0].lane->getEdge()) == nextEdge) {
4665 // keep those lanes which are successors of internal lanes from the edge of startLane
4666 std::vector<LaneQ> oldLanes = lanes;
4667 lanes.clear();
4668 const std::vector<MSLane*>& sourceLanes = startLane->getEdge().getLanes();
4669 for (std::vector<MSLane*>::const_iterator it_source = sourceLanes.begin(); it_source != sourceLanes.end(); ++it_source) {
4670 for (std::vector<LaneQ>::iterator it_lane = oldLanes.begin(); it_lane != oldLanes.end(); ++it_lane) {
4671 if ((*it_source)->getLinkCont()[0]->getLane() == (*it_lane).lane) {
4672 lanes.push_back(*it_lane);
4673 break;
4674 }
4675 }
4676 }
4677 assert(lanes.size() == startLane->getEdge().getLanes().size());
4678 // patch invalid bestLaneOffset and updated myCurrentLaneInBestLanes
4679 for (int i = 0; i < (int)lanes.size(); ++i) {
4680 if (i + lanes[i].bestLaneOffset < 0) {
4681 lanes[i].bestLaneOffset = -i;
4682 }
4683 if (i + lanes[i].bestLaneOffset >= (int)lanes.size()) {
4684 lanes[i].bestLaneOffset = (int)lanes.size() - i - 1;
4685 }
4686 assert(i + lanes[i].bestLaneOffset >= 0);
4687 assert(i + lanes[i].bestLaneOffset < (int)lanes.size());
4688 if (lanes[i].bestContinuations[0] != 0) {
4689 // patch length of bestContinuation to match expectations (only once)
4690 lanes[i].bestContinuations.insert(lanes[i].bestContinuations.begin(), (MSLane*)nullptr);
4691 }
4692 if (startLane->getLinkCont()[0]->getLane() == lanes[i].lane) {
4693 myCurrentLaneInBestLanes = lanes.begin() + i;
4694 }
4695 assert(&(lanes[i].lane->getEdge()) == nextEdge);
4696 }
4697 myLastBestLanesInternalLane = startLane;
4698 updateOccupancyAndCurrentBestLane(startLane);
4699 #ifdef DEBUG_BESTLANES
4700 if (DEBUG_COND) {
4701 std::cout << " updated for internal\n";
4702 }
4703 #endif
4704 return;
4705 } else {
4706 // remove passed edges
4707 it = myBestLanes.erase(it);
4708 }
4709 }
4710 assert(false); // should always find the next edge
4711 }
4712 // start rebuilding
4713 myLastBestLanesEdge = &startLane->getEdge();
4714 myBestLanes.clear();
4715
4716 // get information about the next stop
4717 MSRouteIterator nextStopEdge = myRoute->end();
4718 const MSLane* nextStopLane = nullptr;
4719 double nextStopPos = 0;
4720 if (!myStops.empty()) {
4721 const Stop& nextStop = myStops.front();
4722 nextStopLane = nextStop.lane;
4723 nextStopEdge = nextStop.edge;
4724 nextStopPos = nextStop.pars.startPos;
4725 }
4726 if (myParameter->arrivalLaneProcedure == ARRIVAL_LANE_GIVEN && nextStopEdge == myRoute->end()) {
4727 nextStopEdge = (myRoute->end() - 1);
4728 nextStopLane = (*nextStopEdge)->getLanes()[myArrivalLane];
4729 nextStopPos = myArrivalPos;
4730 }
4731 if (nextStopEdge != myRoute->end()) {
4732 // make sure that the "wrong" lanes get a penalty. (penalty needs to be
4733 // large enough to overcome a magic threshold in MSLCM_DK2004.cpp:383)
4734 nextStopPos = MAX2(POSITION_EPS, MIN2((double)nextStopPos, (double)(nextStopLane->getLength() - 2 * POSITION_EPS)));
4735 if (nextStopLane->isInternal()) {
4736 // switch to the correct lane before entering the intersection
4737 nextStopPos = (*nextStopEdge)->getLength();
4738 }
4739 }
4740
4741 // go forward along the next lanes;
4742 int seen = 0;
4743 double seenLength = 0;
4744 bool progress = true;
4745 for (MSRouteIterator ce = myCurrEdge; progress;) {
4746 std::vector<LaneQ> currentLanes;
4747 const std::vector<MSLane*>* allowed = nullptr;
4748 const MSEdge* nextEdge = nullptr;
4749 if (ce != myRoute->end() && ce + 1 != myRoute->end()) {
4750 nextEdge = *(ce + 1);
4751 allowed = (*ce)->allowedLanes(*nextEdge, myType->getVehicleClass());
4752 }
4753 const std::vector<MSLane*>& lanes = (*ce)->getLanes();
4754 for (std::vector<MSLane*>::const_iterator i = lanes.begin(); i != lanes.end(); ++i) {
4755 LaneQ q;
4756 MSLane* cl = *i;
4757 q.lane = cl;
4758 q.bestContinuations.push_back(cl);
4759 q.bestLaneOffset = 0;
4760 q.length = cl->allowsVehicleClass(myType->getVehicleClass()) ? cl->getLength() : 0;
4761 q.currentLength = q.length;
4762 q.allowsContinuation = allowed == nullptr || std::find(allowed->begin(), allowed->end(), cl) != allowed->end();
4763 q.occupation = 0;
4764 q.nextOccupation = 0;
4765 currentLanes.push_back(q);
4766 }
4767 //
4768 if (nextStopEdge == ce
4769 // already past the stop edge
4770 && !(ce == myCurrEdge && myLane != nullptr && myLane->isInternal())) {
4771 if (!nextStopLane->isInternal()) {
4772 progress = false;
4773 }
4774 const MSLane* normalStopLane = nextStopLane->getNormalPredecessorLane();
4775 for (std::vector<LaneQ>::iterator q = currentLanes.begin(); q != currentLanes.end(); ++q) {
4776 if (nextStopLane != nullptr && normalStopLane != (*q).lane) {
4777 (*q).allowsContinuation = false;
4778 (*q).length = nextStopPos;
4779 (*q).currentLength = (*q).length;
4780 }
4781 }
4782 }
4783
4784 myBestLanes.push_back(currentLanes);
4785 ++seen;
4786 seenLength += currentLanes[0].lane->getLength();
4787 ++ce;
4788 progress &= (seen <= 4 || seenLength < 3000); // motorway
4789 progress &= (seen <= 8 || seenLength < 200); // urban
4790 progress &= ce != myRoute->end();
4791 /*
4792 if(progress) {
4793 progress &= (currentLanes.size()!=1||(*ce)->getLanes().size()!=1);
4794 }
4795 */
4796 }
4797
4798 // we are examining the last lane explicitly
4799 if (myBestLanes.size() != 0) {
4800 double bestLength = -1;
4801 int bestThisIndex = 0;
4802 int index = 0;
4803 std::vector<LaneQ>& last = myBestLanes.back();
4804 for (std::vector<LaneQ>::iterator j = last.begin(); j != last.end(); ++j, ++index) {
4805 if ((*j).length > bestLength) {
4806 bestLength = (*j).length;
4807 bestThisIndex = index;
4808 }
4809 }
4810 index = 0;
4811 for (std::vector<LaneQ>::iterator j = last.begin(); j != last.end(); ++j, ++index) {
4812 if ((*j).length < bestLength) {
4813 (*j).bestLaneOffset = bestThisIndex - index;
4814 }
4815 }
4816 }
4817 #ifdef DEBUG_BESTLANES
4818 if (DEBUG_COND) {
4819 std::cout << " last edge:\n";
4820 std::vector<LaneQ>& laneQs = myBestLanes.back();
4821 for (std::vector<LaneQ>::iterator j = laneQs.begin(); j != laneQs.end(); ++j) {
4822 std::cout << " lane=" << (*j).lane->getID() << " length=" << (*j).length << " bestOffset=" << (*j).bestLaneOffset << "\n";
4823 }
4824 }
4825 #endif
4826 // go backward through the lanes
4827 // track back best lane and compute the best prior lane(s)
4828 for (std::vector<std::vector<LaneQ> >::reverse_iterator i = myBestLanes.rbegin() + 1; i != myBestLanes.rend(); ++i) {
4829 std::vector<LaneQ>& nextLanes = (*(i - 1));
4830 std::vector<LaneQ>& clanes = (*i);
4831 MSEdge& cE = clanes[0].lane->getEdge();
4832 int index = 0;
4833 double bestConnectedLength = -1;
4834 double bestLength = -1;
4835 for (std::vector<LaneQ>::iterator j = nextLanes.begin(); j != nextLanes.end(); ++j, ++index) {
4836 if ((*j).lane->isApproachedFrom(&cE) && bestConnectedLength < (*j).length) {
4837 bestConnectedLength = (*j).length;
4838 }
4839 if (bestLength < (*j).length) {
4840 bestLength = (*j).length;
4841 }
4842 }
4843 // compute index of the best lane (highest length and least offset from the best next lane)
4844 int bestThisIndex = 0;
4845 if (bestConnectedLength > 0) {
4846 index = 0;
4847 for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
4848 LaneQ bestConnectedNext;
4849 bestConnectedNext.length = -1;
4850 if ((*j).allowsContinuation) {
4851 for (std::vector<LaneQ>::const_iterator m = nextLanes.begin(); m != nextLanes.end(); ++m) {
4852 if ((*m).lane->isApproachedFrom(&cE, (*j).lane)) {
4853 if (bestConnectedNext.length < (*m).length || (bestConnectedNext.length == (*m).length && abs(bestConnectedNext.bestLaneOffset) > abs((*m).bestLaneOffset))) {
4854 bestConnectedNext = *m;
4855 }
4856 }
4857 }
4858 if (bestConnectedNext.length == bestConnectedLength && abs(bestConnectedNext.bestLaneOffset) < 2) {
4859 (*j).length += bestLength;
4860 } else {
4861 (*j).length += bestConnectedNext.length;
4862 }
4863 (*j).bestLaneOffset = bestConnectedNext.bestLaneOffset;
4864 }
4865 copy(bestConnectedNext.bestContinuations.begin(), bestConnectedNext.bestContinuations.end(), back_inserter((*j).bestContinuations));
4866 if (clanes[bestThisIndex].length < (*j).length
4867 || (clanes[bestThisIndex].length == (*j).length && abs(clanes[bestThisIndex].bestLaneOffset) > abs((*j).bestLaneOffset))
4868 || (clanes[bestThisIndex].length == (*j).length && abs(clanes[bestThisIndex].bestLaneOffset) == abs((*j).bestLaneOffset) &&
4869 nextLinkPriority(clanes[bestThisIndex].bestContinuations) < nextLinkPriority((*j).bestContinuations))
4870 ) {
4871 bestThisIndex = index;
4872 }
4873 }
4874 #ifdef DEBUG_BESTLANES
4875 if (DEBUG_COND) {
4876 std::cout << " edge=" << cE.getID() << "\n";
4877 std::vector<LaneQ>& laneQs = clanes;
4878 for (std::vector<LaneQ>::iterator j = laneQs.begin(); j != laneQs.end(); ++j) {
4879 std::cout << " lane=" << (*j).lane->getID() << " length=" << (*j).length << " bestOffset=" << (*j).bestLaneOffset << "\n";
4880 }
4881 }
4882 #endif
4883
4884 } else {
4885 // only needed in case of disconnected routes
4886 int bestNextIndex = 0;
4887 int bestDistToNeeded = (int) clanes.size();
4888 index = 0;
4889 for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
4890 if ((*j).allowsContinuation) {
4891 int nextIndex = 0;
4892 for (std::vector<LaneQ>::const_iterator m = nextLanes.begin(); m != nextLanes.end(); ++m, ++nextIndex) {
4893 if ((*m).lane->isApproachedFrom(&cE, (*j).lane)) {
4894 if (bestDistToNeeded > abs((*m).bestLaneOffset)) {
4895 bestDistToNeeded = abs((*m).bestLaneOffset);
4896 bestThisIndex = index;
4897 bestNextIndex = nextIndex;
4898 }
4899 }
4900 }
4901 }
4902 }
4903 clanes[bestThisIndex].length += nextLanes[bestNextIndex].length;
4904 copy(nextLanes[bestNextIndex].bestContinuations.begin(), nextLanes[bestNextIndex].bestContinuations.end(), back_inserter(clanes[bestThisIndex].bestContinuations));
4905
4906 }
4907 // set bestLaneOffset for all lanes
4908 index = 0;
4909 for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
4910 if ((*j).length < clanes[bestThisIndex].length
4911 || ((*j).length == clanes[bestThisIndex].length && abs((*j).bestLaneOffset) > abs(clanes[bestThisIndex].bestLaneOffset))
4912 || (nextLinkPriority((*j).bestContinuations)) < nextLinkPriority(clanes[bestThisIndex].bestContinuations)
4913 ) {
4914 (*j).bestLaneOffset = bestThisIndex - index;
4915 if ((nextLinkPriority((*j).bestContinuations)) < nextLinkPriority(clanes[bestThisIndex].bestContinuations)) {
4916 // try to move away from the lower-priority lane before it ends
4917 (*j).length = (*j).currentLength;
4918 }
4919 } else {
4920 (*j).bestLaneOffset = 0;
4921 }
4922 }
4923 }
4924 updateOccupancyAndCurrentBestLane(startLane);
4925 #ifdef DEBUG_BESTLANES
4926 if (DEBUG_COND) {
4927 std::cout << SIMTIME << " veh=" << getID() << " bestCont=" << toString(getBestLanesContinuation()) << "\n";
4928 }
4929 #endif
4930 return;
4931 }
4932
4933
4934 int
4935 MSVehicle::nextLinkPriority(const std::vector<MSLane*>& conts) {
4936 if (conts.size() < 2) {
4937 return -1;
4938 } else {
4939 MSLink* link = MSLinkContHelper::getConnectingLink(*conts[0], *conts[1]);
4940 if (link != nullptr) {
4941 return link->havePriority() ? 1 : 0;
4942 } else {
4943 // disconnected route
4944 return -1;
4945 }
4946 }
4947 }
4948
4949
4950 void
4951 MSVehicle::updateOccupancyAndCurrentBestLane(const MSLane* startLane) {
4952 std::vector<LaneQ>& currLanes = *myBestLanes.begin();
4953 std::vector<LaneQ>::iterator i;
4954 for (i = currLanes.begin(); i != currLanes.end(); ++i) {
4955 double nextOccupation = 0;
4956 for (std::vector<MSLane*>::const_iterator j = (*i).bestContinuations.begin() + 1; j != (*i).bestContinuations.end(); ++j) {
4957 nextOccupation += (*j)->getBruttoVehLenSum();
4958 }
4959 (*i).nextOccupation = nextOccupation;
4960 #ifdef DEBUG_BESTLANES
4961 if (DEBUG_COND) {
4962 std::cout << " lane=" << (*i).lane->getID() << " nextOccupation=" << nextOccupation << "\n";
4963 }
4964 #endif
4965 if ((*i).lane == startLane) {
4966 myCurrentLaneInBestLanes = i;
4967 }
4968 }
4969 }
4970
4971
4972 const std::vector<MSLane*>&
4973 MSVehicle::getBestLanesContinuation() const {
4974 if (myBestLanes.empty() || myBestLanes[0].empty()) {
4975 return myEmptyLaneVector;
4976 }
4977 return (*myCurrentLaneInBestLanes).bestContinuations;
4978 }
4979
4980
4981 const std::vector<MSLane*>&
4982 MSVehicle::getBestLanesContinuation(const MSLane* const l) const {
4983 const MSLane* lane = l;
4984 // XXX: shouldn't this be a "while" to cover more than one internal lane? (Leo) Refs. #2575
4985 if (lane->getEdge().isInternal()) {
4986 // internal edges are not kept inside the bestLanes structure
4987 lane = lane->getLinkCont()[0]->getLane();
4988 }
4989 if (myBestLanes.size() == 0) {
4990 return myEmptyLaneVector;
4991 }
4992 for (std::vector<LaneQ>::const_iterator i = myBestLanes[0].begin(); i != myBestLanes[0].end(); ++i) {
4993 if ((*i).lane == lane) {
4994 return (*i).bestContinuations;
4995 }
4996 }
4997 return myEmptyLaneVector;
4998 }
4999
5000
5001 int
5002 MSVehicle::getBestLaneOffset() const {
5003 if (myBestLanes.empty() || myBestLanes[0].empty()) {
5004 return 0;
5005 } else {
5006 return (*myCurrentLaneInBestLanes).bestLaneOffset;
5007 }
5008 }
5009
5010
5011 void
5012 MSVehicle::adaptBestLanesOccupation(int laneIndex, double density) {
5013 std::vector<MSVehicle::LaneQ>& preb = myBestLanes.front();
5014 assert(laneIndex < (int)preb.size());
5015 preb[laneIndex].occupation = density + preb[laneIndex].nextOccupation;
5016 }
5017
5018
5019 void
5020 MSVehicle::fixPosition() {
5021 if (MSGlobals::gLaneChangeDuration > 0 && !getLaneChangeModel().isChangingLanes()) {
5022 myState.myPosLat = 0;
5023 }
5024 }
5025
5026
5027 double
5028 MSVehicle::getDistanceToPosition(double destPos, const MSEdge* destEdge) const {
5029 double distance = std::numeric_limits<double>::max();
5030 if (isOnRoad() && destEdge != nullptr) {
5031 if (myLane->isInternal()) {
5032 // vehicle is on inner junction edge
5033 distance = myLane->getLength() - getPositionOnLane();
5034 distance += myRoute->getDistanceBetween(0, destPos, *(myCurrEdge + 1), destEdge);
5035 } else {
5036 // vehicle is on a normal edge
5037 distance = myRoute->getDistanceBetween(getPositionOnLane(), destPos, *myCurrEdge, destEdge);
5038 }
5039 }
5040 return distance;
5041 }
5042
5043
5044 std::pair<const MSVehicle* const, double>
5045 MSVehicle::getLeader(double dist) const {
5046 if (myLane == nullptr) {
5047 return std::make_pair(static_cast<const MSVehicle*>(nullptr), -1);
5048 }
5049 if (dist == 0) {
5050 dist = getCarFollowModel().brakeGap(getSpeed()) + getVehicleType().getMinGap();
5051 }
5052 const MSVehicle* lead = nullptr;
5053 const MSLane* lane = myLane; // ensure lane does not change between getVehiclesSecure and releaseVehicles;
5054 const MSLane::VehCont& vehs = lane->getVehiclesSecure();
5055 // vehicle might be outside the road network
5056 MSLane::VehCont::const_iterator it = std::find(vehs.begin(), vehs.end(), this);
5057 if (it != vehs.end() && it + 1 != vehs.end()) {
5058 lead = *(it + 1);
5059 }
5060 if (lead != nullptr) {
5061 std::pair<const MSVehicle* const, double> result(
5062 lead, lead->getBackPositionOnLane(myLane) - getPositionOnLane() - getVehicleType().getMinGap());
5063 lane->releaseVehicles();
5064 return result;
5065 }
5066 const double seen = myLane->getLength() - getPositionOnLane();
5067 const std::vector<MSLane*>& bestLaneConts = getBestLanesContinuation(myLane);
5068 std::pair<const MSVehicle* const, double> result = myLane->getLeaderOnConsecutive(dist, seen, getSpeed(), *this, bestLaneConts);
5069 lane->releaseVehicles();
5070 return result;
5071 }
5072
5073
5074 double
5075 MSVehicle::getTimeGapOnLane() const {
5076 // calling getLeader with 0 would induce a dist calculation but we only want to look for the leaders on the current lane
5077 std::pair<const MSVehicle* const, double> leaderInfo = getLeader(-1);
5078 if (leaderInfo.first == nullptr || getSpeed() == 0) {
5079 return -1;
5080 }
5081 return (leaderInfo.second + getVehicleType().getMinGap()) / getSpeed();
5082 }
5083
5084
5085 double
5086 MSVehicle::getCO2Emissions() const {
5087 return PollutantsInterface::compute(myType->getEmissionClass(), PollutantsInterface::CO2, myState.speed(), myAcceleration, getSlope());
5088 }
5089
5090
5091 double
5092 MSVehicle::getCOEmissions() const {
5093 return PollutantsInterface::compute(myType->getEmissionClass(), PollutantsInterface::CO, myState.speed(), myAcceleration, getSlope());
5094 }
5095
5096
5097 double
5098 MSVehicle::getHCEmissions() const {
5099 return PollutantsInterface::compute(myType->getEmissionClass(), PollutantsInterface::HC, myState.speed(), myAcceleration, getSlope());
5100 }
5101
5102
5103 double
5104 MSVehicle::getNOxEmissions() const {
5105 return PollutantsInterface::compute(myType->getEmissionClass(), PollutantsInterface::NO_X, myState.speed(), myAcceleration, getSlope());
5106 }
5107
5108
5109 double
5110 MSVehicle::getPMxEmissions() const {
5111 return PollutantsInterface::compute(myType->getEmissionClass(), PollutantsInterface::PM_X, myState.speed(), myAcceleration, getSlope());
5112 }
5113
5114
5115 double
5116 MSVehicle::getFuelConsumption() const {
5117 return PollutantsInterface::compute(myType->getEmissionClass(), PollutantsInterface::FUEL, myState.speed(), myAcceleration, getSlope());
5118 }
5119
5120
5121 double
5122 MSVehicle::getElectricityConsumption() const {
5123 return PollutantsInterface::compute(myType->getEmissionClass(), PollutantsInterface::ELEC, myState.speed(), myAcceleration, getSlope());
5124 }
5125
5126
5127 double
5128 MSVehicle::getHarmonoise_NoiseEmissions() const {
5129 return HelpersHarmonoise::computeNoise(myType->getEmissionClass(), myState.speed(), myAcceleration);
5130 }
5131
5132
5133 void
5134 MSVehicle::addPerson(MSTransportable* person) {
5135 MSBaseVehicle::addPerson(person);
5136 if (myStops.size() > 0 && myStops.front().reached && myStops.front().triggered && myStops.front().numExpectedPerson > 0) {
5137 myStops.front().numExpectedPerson -= (int)myStops.front().pars.awaitedPersons.count(person->getID());
5138 }
5139 }
5140
5141 void
5142 MSVehicle::addContainer(MSTransportable* container) {
5143 MSBaseVehicle::addContainer(container);
5144 if (myStops.size() > 0 && myStops.front().reached && myStops.front().pars.containerTriggered && myStops.front().numExpectedContainer > 0) {
5145 myStops.front().numExpectedContainer -= (int)myStops.front().pars.awaitedContainers.count(container->getID());
5146 }
5147 }
5148
5149
5150 void
5151 MSVehicle::setBlinkerInformation() {
5152 switchOffSignal(VEH_SIGNAL_BLINKER_RIGHT | VEH_SIGNAL_BLINKER_LEFT);
5153 int state = getLaneChangeModel().getOwnState();
5154 // do not set blinker for sublane changes or when blocked from changing to the right
5155 const bool blinkerManoeuvre = (((state & LCA_SUBLANE) == 0) && (
5156 (state & LCA_KEEPRIGHT) == 0 || (state & LCA_BLOCKED) == 0));
5157 Signalling left = VEH_SIGNAL_BLINKER_LEFT;
5158 Signalling right = VEH_SIGNAL_BLINKER_RIGHT;
5159 if (MSNet::getInstance()->lefthand()) {
5160 // lane indices increase from left to right
5161 std::swap(left, right);
5162 }
5163 if ((state & LCA_LEFT) != 0 && blinkerManoeuvre) {
5164 switchOnSignal(left);
5165 } else if ((state & LCA_RIGHT) != 0 && blinkerManoeuvre) {
5166 switchOnSignal(right);
5167 } else if (getLaneChangeModel().isChangingLanes()) {
5168 if (getLaneChangeModel().getLaneChangeDirection() == 1) {
5169 switchOnSignal(left);
5170 } else {
5171 switchOnSignal(right);
5172 }
5173 } else {
5174 const MSLane* lane = getLane();
5175 MSLinkCont::const_iterator link = MSLane::succLinkSec(*this, 1, *lane, getBestLanesContinuation());
5176 if (link != lane->getLinkCont().end() && lane->getLength() - getPositionOnLane() < lane->getVehicleMaxSpeed(this) * (double) 7.) {
5177 switch ((*link)->getDirection()) {
5178 case LINKDIR_TURN:
5179 case LINKDIR_LEFT:
5180 case LINKDIR_PARTLEFT:
5181 switchOnSignal(VEH_SIGNAL_BLINKER_LEFT);
5182 break;
5183 case LINKDIR_RIGHT:
5184 case LINKDIR_PARTRIGHT:
5185 switchOnSignal(VEH_SIGNAL_BLINKER_RIGHT);
5186 break;
5187 default:
5188 break;
5189 }
5190 }
5191 }
5192 if (myStopDist < (myLane->getLength() - getPositionOnLane())
5193 // signal parking stop on the current lane when within braking distance (~2 seconds before braking)
5194 && myStops.begin()->pars.parking
5195 && myStopDist < getCarFollowModel().brakeGap(myLane->getVehicleMaxSpeed(this), getCarFollowModel().getMaxDecel(), 3)) {
5196 switchOnSignal(MSNet::getInstance()->lefthand() ? VEH_SIGNAL_BLINKER_LEFT : VEH_SIGNAL_BLINKER_RIGHT);
5197 }
5198 if (myInfluencer != nullptr && myInfluencer->getSignals() >= 0) {
5199 mySignals = myInfluencer->getSignals();
5200 myInfluencer->setSignals(-1); // overwrite computed signals only once
5201 }
5202 }
5203
5204 void
5205 MSVehicle::setEmergencyBlueLight(SUMOTime currentTime) {
5206
5207 //TODO look if timestep ist SIMSTEP
5208 if (currentTime % 1000 == 0) {
5209 if (signalSet(VEH_SIGNAL_EMERGENCY_BLUE)) {
5210 switchOffSignal(VEH_SIGNAL_EMERGENCY_BLUE);
5211 } else {
5212 switchOnSignal(VEH_SIGNAL_EMERGENCY_BLUE);
5213 }
5214 }
5215 }
5216
5217
5218 int
5219 MSVehicle::getLaneIndex() const {
5220 std::vector<MSLane*>::const_iterator laneP = std::find(myLane->getEdge().getLanes().begin(), myLane->getEdge().getLanes().end(), myLane);
5221 return (int) std::distance(myLane->getEdge().getLanes().begin(), laneP);
5222 }
5223
5224
5225 void
5226 MSVehicle::setTentativeLaneAndPosition(MSLane* lane, double pos, double posLat) {
5227 assert(lane != 0);
5228 myLane = lane;
5229 myState.myPos = pos;
5230 myState.myPosLat = posLat;
5231 myState.myBackPos = pos - getVehicleType().getLength();
5232 }
5233
5234
5235 double
5236 MSVehicle::getRightSideOnLane() const {
5237 return myState.myPosLat + 0.5 * myLane->getWidth() - 0.5 * getVehicleType().getWidth();
5238 }
5239
5240
5241 double
5242 MSVehicle::getRightSideOnEdge(const MSLane* lane) const {
5243 return getCenterOnEdge(lane) - 0.5 * getVehicleType().getWidth();
5244 }
5245
5246
5247 double
5248 MSVehicle::getCenterOnEdge(const MSLane* lane) const {
5249 if (lane == nullptr || &lane->getEdge() == &myLane->getEdge()) {
5250 return myLane->getRightSideOnEdge() + myState.myPosLat + 0.5 * myLane->getWidth();
5251 } else {
5252 assert(myFurtherLanes.size() == myFurtherLanesPosLat.size());
5253 for (int i = 0; i < (int)myFurtherLanes.size(); ++i) {
5254 if (myFurtherLanes[i] == lane) {
5255 #ifdef DEBUG_FURTHER
5256 if (DEBUG_COND) std::cout << " getCenterOnEdge veh=" << getID() << " lane=" << lane->getID() << " i=" << i << " furtherLat=" << myFurtherLanesPosLat[i]
5257 << " result=" << lane->getRightSideOnEdge() + myFurtherLanesPosLat[i] + 0.5 * lane->getWidth()
5258 << "\n";
5259 #endif
5260 return lane->getRightSideOnEdge() + myFurtherLanesPosLat[i] + 0.5 * lane->getWidth();
5261 }
5262 }
5263 //if (DEBUG_COND) std::cout << SIMTIME << " veh=" << getID() << " myShadowFurtherLanes=" << toString(getLaneChangeModel().getShadowFurtherLanes()) << "\n";
5264 const std::vector<MSLane*>& shadowFurther = getLaneChangeModel().getShadowFurtherLanes();
5265 for (int i = 0; i < (int)shadowFurther.size(); ++i) {
5266 //if (DEBUG_COND) std::cout << " comparing i=" << (*i)->getID() << " lane=" << lane->getID() << "\n";
5267 if (shadowFurther[i] == lane) {
5268 assert(getLaneChangeModel().getShadowLane() != 0);
5269 return (lane->getRightSideOnEdge() + getLaneChangeModel().getShadowFurtherLanesPosLat()[i] + 0.5 * lane->getWidth()
5270 + (myLane->getCenterOnEdge() - getLaneChangeModel().getShadowLane()->getCenterOnEdge()));
5271 }
5272 }
5273 assert(false);
5274 throw ProcessError("Request lateral pos of vehicle '" + getID() + "' for invalid lane '" + Named::getIDSecure(lane) + "'");
5275 }
5276 }
5277
5278
5279 double
5280 MSVehicle::getLatOffset(const MSLane* lane) const {
5281 assert(lane != 0);
5282 if (&lane->getEdge() == &myLane->getEdge()) {
5283 return myLane->getRightSideOnEdge() - lane->getRightSideOnEdge();
5284 } else {
5285 // Check whether the lane is a further lane for the vehicle
5286 for (int i = 0; i < (int)myFurtherLanes.size(); ++i) {
5287 if (myFurtherLanes[i] == lane) {
5288 #ifdef DEBUG_FURTHER
5289 if (DEBUG_COND) {
5290 std::cout << " getLatOffset veh=" << getID() << " lane=" << lane->getID() << " i=" << i << " posLat=" << myState.myPosLat << " furtherLat=" << myFurtherLanesPosLat[i] << "\n";
5291 }
5292 #endif
5293 return myFurtherLanesPosLat[i] - myState.myPosLat;
5294 }
5295 }
5296 #ifdef DEBUG_FURTHER
5297 if (DEBUG_COND) {
5298 std::cout << SIMTIME << " veh=" << getID() << " myShadowFurtherLanes=" << toString(getLaneChangeModel().getShadowFurtherLanes()) << "\n";
5299 }
5300 #endif
5301 // Check whether the lane is a "shadow further lane" for the vehicle
5302 const std::vector<MSLane*>& shadowFurther = getLaneChangeModel().getShadowFurtherLanes();
5303 for (int i = 0; i < (int)shadowFurther.size(); ++i) {
5304 if (shadowFurther[i] == lane) {
5305 #ifdef DEBUG_FURTHER
5306 if (DEBUG_COND) std::cout << " getLatOffset veh=" << getID()
5307 << " shadowLane=" << Named::getIDSecure(getLaneChangeModel().getShadowLane())
5308 << " lane=" << lane->getID()
5309 << " i=" << i
5310 << " posLat=" << myState.myPosLat
5311 << " shadowPosLat=" << getLatOffset(getLaneChangeModel().getShadowLane())
5312 << " shadowFurtherLat=" << getLaneChangeModel().getShadowFurtherLanesPosLat()[i]
5313 << "\n";
5314 #endif
5315 return getLatOffset(getLaneChangeModel().getShadowLane()) + getLaneChangeModel().getShadowFurtherLanesPosLat()[i] - myState.myPosLat;
5316 }
5317 }
5318 // Check whether the vehicle issued a maneuverReservation on the lane.
5319 assert(&getLaneChangeModel().getTargetLane()->getEdge() == &myLane->getEdge()); // should have been handled in (&lane->getEdge() == &myLane->getEdge())-block
5320 const std::vector<MSLane*>& furtherTargets = getLaneChangeModel().getFurtherTargetLanes();
5321 for (int i = 0; i < (int)myFurtherLanes.size(); ++i) {
5322 // Further target lanes are just neighboring lanes of the vehicle's further lanes, @see MSAbstractLaneChangeModel::updateTargetLane()
5323 MSLane* targetLane = furtherTargets[i];
5324 if (targetLane == lane) {
5325 const double targetDir = getLaneChangeModel().getManeuverDist() < 0 ? -1. : 1.;
5326 const double latOffset = myFurtherLanesPosLat[i] - myState.myPosLat + targetDir * 0.5 * (myFurtherLanes[i]->getWidth() + targetLane->getWidth());
5327 #ifdef DEBUG_TARGET_LANE
5328 if (DEBUG_COND) {
5329 std::cout << " getLatOffset veh=" << getID()
5330 << " wrt targetLane=" << Named::getIDSecure(getLaneChangeModel().getTargetLane())
5331 << "\n i=" << i
5332 << " posLat=" << myState.myPosLat
5333 << " furtherPosLat=" << myFurtherLanesPosLat[i]
5334 << " maneuverDist=" << getLaneChangeModel().getManeuverDist()
5335 << " targetDir=" << targetDir
5336 << " latOffset=" << latOffset
5337 << std::endl;
5338 }
5339 #endif
5340 return latOffset;
5341 }
5342 }
5343 assert(false);
5344 throw ProcessError("Request lateral offset of vehicle '" + getID() + "' for invalid lane '" + Named::getIDSecure(lane) + "'");
5345 }
5346 }
5347
5348
5349 double
5350 MSVehicle::lateralDistanceToLane(const int offset) const {
5351 // compute the distance when changing to the neighboring lane
5352 // (ensure we do not lap into the line behind neighLane since there might be unseen blockers)
5353 assert(offset == 0 || offset == 1 || offset == -1);
5354 assert(myLane != nullptr);
5355 assert(myLane->getParallelLane(offset) != nullptr);
5356 const double halfCurrentLaneWidth = 0.5 * myLane->getWidth();
5357 const double halfVehWidth = 0.5 * (getWidth() + NUMERICAL_EPS);
5358 const double latPos = getLateralPositionOnLane();
5359 double leftLimit = halfCurrentLaneWidth - halfVehWidth - latPos;
5360 double rightLimit = -halfCurrentLaneWidth + halfVehWidth - latPos;
5361 double latLaneDist = 0; // minimum distance to move the vehicle fully onto the new lane
5362 if (offset == 0) {
5363 if (latPos + halfVehWidth > halfCurrentLaneWidth) {
5364 // correct overlapping left
5365 latLaneDist = halfCurrentLaneWidth - latPos - halfVehWidth;
5366 } else if (latPos - halfVehWidth < - halfCurrentLaneWidth) {
5367 // correct overlapping left
5368 latLaneDist = halfCurrentLaneWidth - latPos + halfVehWidth;
5369 }
5370 } else if (offset == -1) {
5371 latLaneDist = rightLimit - (getWidth() + NUMERICAL_EPS);
5372 } else if (offset == 1) {
5373 latLaneDist = leftLimit + (getWidth() + NUMERICAL_EPS);
5374 }
5375 #ifdef DEBUG_ACTIONSTEPS
5376 if DEBUG_COND {
5377 std::cout << SIMTIME
5378 << " veh=" << getID()
5379 << " halfCurrentLaneWidth=" << halfCurrentLaneWidth
5380 << " halfVehWidth=" << halfVehWidth
5381 << " latPos=" << latPos
5382 << " latLaneDist=" << latLaneDist
5383 << " leftLimit=" << leftLimit
5384 << " rightLimit=" << rightLimit
5385 << "\n";
5386 }
5387 #endif
5388 return latLaneDist;
5389 }
5390
5391
5392 double
5393 MSVehicle::getLateralOverlap(double posLat) const {
5394 return (fabs(posLat) + 0.5 * getVehicleType().getWidth()
5395 - 0.5 * myLane->getWidth());
5396 }
5397
5398
5399 double
5400 MSVehicle::getLateralOverlap() const {
5401 return getLateralOverlap(getLateralPositionOnLane());
5402 }
5403
5404
5405 void
5406 MSVehicle::removeApproachingInformation(const DriveItemVector& lfLinks) const {
5407 for (const DriveProcessItem& dpi : lfLinks) {
5408 if (dpi.myLink != nullptr) {
5409 dpi.myLink->removeApproaching(this);
5410 }
5411 }
5412 // unregister on all shadow links
5413 getLaneChangeModel().removeShadowApproachingInformation();
5414 }
5415
5416
5417 bool
5418 MSVehicle::unsafeLinkAhead(const MSLane* lane) const {
5419 // the following links are unsafe:
5420 // - zipper links if they are close enough and have approaching vehicles in the relevant time range
5421 // - unprioritized links if the vehicle is currently approaching a prioritzed link and unable to stop in time
5422 double seen = myLane->getLength() - getPositionOnLane();
5423 const double dist = getCarFollowModel().brakeGap(getSpeed(), getCarFollowModel().getMaxDecel(), 0);
5424 if (seen < dist) {
5425 const std::vector<MSLane*>& bestLaneConts = getBestLanesContinuation(lane);
5426 int view = 1;
5427 MSLinkCont::const_iterator link = MSLane::succLinkSec(*this, view, *lane, bestLaneConts);
5428 DriveItemVector::const_iterator di = myLFLinkLanes.begin();
5429 while (!lane->isLinkEnd(link) && seen <= dist) {
5430 if (!lane->getEdge().isInternal()
5431 && (((*link)->getState() == LINKSTATE_ZIPPER && seen < MSLink::ZIPPER_ADAPT_DIST)
5432 || !(*link)->havePriority())) {
5433 // find the drive item corresponding to this link
5434 bool found = false;
5435 while (di != myLFLinkLanes.end() && !found) {
5436 if ((*di).myLink != nullptr) {
5437 const MSLane* diPredLane = (*di).myLink->getLaneBefore();
5438 if (diPredLane != nullptr) {
5439 if (&diPredLane->getEdge() == &lane->getEdge()) {
5440 found = true;
5441 }
5442 }
5443 }
5444 if (!found) {
5445 di++;
5446 }
5447 }
5448 if (found) {
5449 const SUMOTime leaveTime = (*link)->getLeaveTime((*di).myArrivalTime, (*di).myArrivalSpeed,
5450 (*di).getLeaveSpeed(), getVehicleType().getLength());
5451 if ((*link)->hasApproachingFoe((*di).myArrivalTime, leaveTime, (*di).myArrivalSpeed, getCarFollowModel().getMaxDecel())) {
5452 //std::cout << SIMTIME << " veh=" << getID() << " aborting changeTo=" << Named::getIDSecure(bestLaneConts.front()) << " linkState=" << toString((*link)->getState()) << " seen=" << seen << " dist=" << dist << "\n";
5453 return true;
5454 }
5455 }
5456 // no drive item is found if the vehicle aborts it's request within dist
5457 }
5458 lane = (*link)->getViaLaneOrLane();
5459 if (!lane->getEdge().isInternal()) {
5460 view++;
5461 }
5462 seen += lane->getLength();
5463 link = MSLane::succLinkSec(*this, view, *lane, bestLaneConts);
5464 }
5465 }
5466 return false;
5467 }
5468
5469
5470 PositionVector
5471 MSVehicle::getBoundingBox() const {
5472 PositionVector centerLine;
5473 centerLine.push_back(getPosition());
5474 for (MSLane* lane : myFurtherLanes) {
5475 centerLine.push_back(lane->getShape().back());
5476 }
5477 centerLine.push_back(getBackPosition());
5478 centerLine.move2side(0.5 * myType->getWidth());
5479 PositionVector result = centerLine;
5480 centerLine.move2side(-myType->getWidth());
5481 result.append(centerLine.reverse(), POSITION_EPS);
5482 return result;
5483 }
5484
5485
5486 PositionVector
5487 MSVehicle::getBoundingPoly() const {
5488 // XXX implement more types
5489 switch (myType->getGuiShape()) {
5490 case SVS_PASSENGER:
5491 case SVS_PASSENGER_SEDAN:
5492 case SVS_PASSENGER_HATCHBACK:
5493 case SVS_PASSENGER_WAGON:
5494 case SVS_PASSENGER_VAN: {
5495 PositionVector result;
5496 PositionVector centerLine;
5497 centerLine.push_back(getPosition());
5498 centerLine.push_back(getBackPosition());
5499 PositionVector line1 = centerLine;
5500 PositionVector line2 = centerLine;
5501 line1.move2side(0.3 * myType->getWidth());
5502 line2.move2side(0.5 * myType->getWidth());
5503 line2.scaleRelative(0.8);
5504 result.push_back(line1[0]);
5505 result.push_back(line2[0]);
5506 result.push_back(line2[1]);
5507 result.push_back(line1[1]);
5508 line1.move2side(-0.6 * myType->getWidth());
5509 line2.move2side(-1.0 * myType->getWidth());
5510 result.push_back(line1[1]);
5511 result.push_back(line2[1]);
5512 result.push_back(line2[0]);
5513 result.push_back(line1[0]);
5514 return result;
5515 }
5516 default:
5517 return getBoundingBox();
5518 }
5519 }
5520
5521
5522 bool
5523 MSVehicle::onFurtherEdge(const MSEdge* edge) const {
5524 for (std::vector<MSLane*>::const_iterator i = myFurtherLanes.begin(); i != myFurtherLanes.end(); ++i) {
5525 if (&(*i)->getEdge() == edge) {
5526 return true;
5527 }
5528 }
5529 return false;
5530 }
5531
5532 bool
5533 MSVehicle::rerouteParkingArea(const std::string& parkingAreaID, std::string& errorMsg) {
5534 // this function is based on MSTriggeredRerouter::rerouteParkingArea in order to keep
5535 // consistency in the behaviour.
5536
5537 // get vehicle params
5538 MSParkingArea* destParkArea = getNextParkingArea();
5539 const MSRoute& route = getRoute();
5540 const MSEdge* lastEdge = route.getLastEdge();
5541
5542 if (destParkArea == nullptr) {
5543 // not driving towards a parking area
5544 errorMsg = "Vehicle " + getID() + " is not driving to a parking area so it cannot be rerouted.";
5545 return false;
5546 }
5547
5548 // if the current route ends at the parking area, the new route will also and at the new area
5549 bool newDestination = (&destParkArea->getLane().getEdge() == route.getLastEdge()
5550 && getArrivalPos() >= destParkArea->getBeginLanePosition()
5551 && getArrivalPos() <= destParkArea->getEndLanePosition());
5552
5553 // retrieve info on the new parking area
5554 MSParkingArea* newParkingArea = (MSParkingArea*) MSNet::getInstance()->getStoppingPlace(
5555 parkingAreaID, SumoXMLTag::SUMO_TAG_PARKING_AREA);
5556
5557 if (newParkingArea == nullptr) {
5558 errorMsg = "Parking area ID " + toString(parkingAreaID) + " not found in the network.";
5559 return false;
5560 }
5561
5562 const MSEdge* newEdge = &(newParkingArea->getLane().getEdge());
5563 SUMOAbstractRouter<MSEdge, SUMOVehicle>& router = getInfluencer().getRouterTT();
5564
5565 // Compute the route from the current edge to the parking area edge
5566 ConstMSEdgeVector edgesToPark;
5567 router.compute(getEdge(), newEdge, this, MSNet::getInstance()->getCurrentTimeStep(), edgesToPark);
5568
5569 // Compute the route from the parking area edge to the end of the route
5570 ConstMSEdgeVector edgesFromPark;
5571 if (!newDestination) {
5572 router.compute(newEdge, lastEdge, this, MSNet::getInstance()->getCurrentTimeStep(), edgesFromPark);
5573 } else {
5574 // adapt plans of any riders
5575 for (MSTransportable* p : getPersons()) {
5576 p->rerouteParkingArea(getNextParkingArea(), newParkingArea);
5577 }
5578 }
5579
5580 // we have a new destination, let's replace the vehicle route
5581 ConstMSEdgeVector edges = edgesToPark;
5582 if (edgesFromPark.size() > 0) {
5583 edges.insert(edges.end(), edgesFromPark.begin() + 1, edgesFromPark.end());
5584 }
5585
5586 if (newDestination) {
5587 SUMOVehicleParameter* newParameter = new SUMOVehicleParameter();
5588 *newParameter = getParameter();
5589 newParameter->arrivalPosProcedure = ARRIVAL_POS_GIVEN;
5590 newParameter->arrivalPos = newParkingArea->getEndLanePosition();
5591 replaceParameter(newParameter);
5592 }
5593 const double routeCost = router.recomputeCosts(edges, this, MSNet::getInstance()->getCurrentTimeStep());
5594 ConstMSEdgeVector prevEdges(myCurrEdge, myRoute->end());
5595 const double savings = router.recomputeCosts(prevEdges, this, MSNet::getInstance()->getCurrentTimeStep());
5596 if (replaceParkingArea(newParkingArea, errorMsg)) {
5597 replaceRouteEdges(edges, routeCost, savings, "TraCI:" + toString(SUMO_TAG_PARKING_ZONE_REROUTE), false, false, false);
5598 } else {
5599 WRITE_WARNING("Vehicle '" + getID() + "' could not reroute to new parkingArea '" + newParkingArea->getID()
5600 + "' reason=" + errorMsg + ", time=" + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
5601 return false;
5602 }
5603 return true;
5604 }
5605
5606 bool
5607 MSVehicle::addTraciStop(MSLane* const lane, const double startPos, const double endPos, const SUMOTime duration, const SUMOTime until,
5608 const bool parking, const bool triggered, const bool containerTriggered, std::string& errorMsg) {
5609 //if the stop exists update the duration
5610 for (std::list<Stop>::iterator iter = myStops.begin(); iter != myStops.end(); iter++) {
5611 if (iter->lane == lane && fabs(iter->pars.endPos - endPos) < POSITION_EPS) {
5612 if (duration == 0 && !iter->reached) {
5613 myStops.erase(iter);
5614 // XXX also erase from myParameter->stops ?
5615 updateBestLanes(true);
5616 } else {
5617 iter->duration = duration;
5618 }
5619 return true;
5620 }
5621 }
5622 SUMOVehicleParameter::Stop newStop;
5623 newStop.lane = lane->getID();
5624 newStop.startPos = startPos;
5625 newStop.endPos = endPos;
5626 newStop.duration = duration;
5627 newStop.until = until;
5628 newStop.triggered = triggered;
5629 newStop.containerTriggered = containerTriggered;
5630 newStop.parking = parking;
5631 newStop.index = STOP_INDEX_FIT;
5632 newStop.parametersSet = STOP_START_SET | STOP_END_SET;
5633 if (triggered) {
5634 newStop.parametersSet |= STOP_TRIGGER_SET;
5635 }
5636 if (containerTriggered) {
5637 newStop.parametersSet |= STOP_CONTAINER_TRIGGER_SET;
5638 }
5639 if (parking) {
5640 newStop.parametersSet |= STOP_PARKING_SET;
5641 }
5642 const bool result = addStop(newStop, errorMsg);
5643 if (result) {
5644 /// XXX handle stops added out of order
5645 myParameter->stops.push_back(newStop);
5646 }
5647 if (myLane != nullptr) {
5648 updateBestLanes(true);
5649 }
5650 return result;
5651 }
5652
5653
5654 bool
5655 MSVehicle::addTraciStopAtStoppingPlace(const std::string& stopId, const SUMOTime duration, const SUMOTime until, const bool parking,
5656 const bool triggered, const bool containerTriggered, const SumoXMLTag stoppingPlaceType, std::string& errorMsg) {
5657 //if the stop exists update the duration
5658 for (std::list<Stop>::iterator iter = myStops.begin(); iter != myStops.end(); iter++) {
5659 Named* stop = nullptr;
5660 switch (stoppingPlaceType) {
5661 case SUMO_TAG_BUS_STOP:
5662 stop = iter->busstop;
5663 break;
5664 case SUMO_TAG_CONTAINER_STOP:
5665 stop = iter->containerstop;
5666 break;
5667 case SUMO_TAG_CHARGING_STATION:
5668 stop = iter->chargingStation;
5669 break;
5670 case SUMO_TAG_PARKING_AREA:
5671 stop = iter->parkingarea;
5672 break;
5673 default:
5674 throw ProcessError("Invalid Stopping place type '" + toString(stoppingPlaceType) + "'");
5675 }
5676 if (stop != nullptr && stop->getID() == stopId) {
5677 if (duration == 0 && !iter->reached) {
5678 myStops.erase(iter);
5679 } else {
5680 iter->duration = duration;
5681 }
5682 return true;
5683 }
5684 }
5685
5686 SUMOVehicleParameter::Stop newStop;
5687 switch (stoppingPlaceType) {
5688 case SUMO_TAG_BUS_STOP:
5689 newStop.busstop = stopId;
5690 break;
5691 case SUMO_TAG_CONTAINER_STOP:
5692 newStop.containerstop = stopId;
5693 break;
5694 case SUMO_TAG_CHARGING_STATION:
5695 newStop.chargingStation = stopId;
5696 break;
5697 case SUMO_TAG_PARKING_AREA:
5698 newStop.parkingarea = stopId;
5699 break;
5700 default:
5701 throw ProcessError("Invalid stopping place type '" + toString(stoppingPlaceType) + "'");
5702 }
5703 MSStoppingPlace* bs = MSNet::getInstance()->getStoppingPlace(stopId, stoppingPlaceType);
5704 if (bs == nullptr) {
5705 errorMsg = "The " + toString(stoppingPlaceType) + " '" + stopId + "' is not known for vehicle '" + getID() + "'";
5706 return false;
5707 }
5708 newStop.duration = duration;
5709 newStop.until = until;
5710 newStop.triggered = triggered;
5711 newStop.containerTriggered = containerTriggered;
5712 newStop.parking = parking;
5713 newStop.index = STOP_INDEX_FIT;
5714 newStop.lane = bs->getLane().getID();
5715 newStop.endPos = bs->getEndLanePosition();
5716 newStop.startPos = bs->getBeginLanePosition();
5717 if (triggered) {
5718 newStop.parametersSet |= STOP_TRIGGER_SET;
5719 }
5720 if (containerTriggered) {
5721 newStop.parametersSet |= STOP_CONTAINER_TRIGGER_SET;
5722 }
5723 if (parking) {
5724 newStop.parametersSet |= STOP_PARKING_SET;
5725 }
5726 const bool result = addStop(newStop, errorMsg);
5727 if (myLane != nullptr) {
5728 updateBestLanes(true);
5729 }
5730 return result;
5731 }
5732
5733 bool
5734 MSVehicle::resumeFromStopping() {
5735 if (isStopped()) {
5736 if (myAmRegisteredAsWaitingForPerson) {
5737 MSNet::getInstance()->getVehicleControl().unregisterOneWaiting(true);
5738 myAmRegisteredAsWaitingForPerson = false;
5739 }
5740 if (myAmRegisteredAsWaitingForContainer) {
5741 MSNet::getInstance()->getVehicleControl().unregisterOneWaiting(false);
5742 myAmRegisteredAsWaitingForContainer = false;
5743 }
5744 // we have waited long enough and fulfilled any passenger-requirements
5745 if (myStops.front().busstop != nullptr) {
5746 // inform bus stop about leaving it
5747 myStops.front().busstop->leaveFrom(this);
5748 }
5749 // we have waited long enough and fulfilled any container-requirements
5750 if (myStops.front().containerstop != nullptr) {
5751 // inform container stop about leaving it
5752 myStops.front().containerstop->leaveFrom(this);
5753 }
5754 if (myStops.front().parkingarea != nullptr) {
5755 // inform parking area about leaving it
5756 myStops.front().parkingarea->leaveFrom(this);
5757 }
5758 // the current stop is no longer valid
5759 MSNet::getInstance()->getVehicleControl().removeWaiting(&myLane->getEdge(), this);
5760 MSDevice_Vehroutes* vehroutes = static_cast<MSDevice_Vehroutes*>(getDevice(typeid(MSDevice_Vehroutes)));
5761 if (vehroutes != nullptr) {
5762 vehroutes->stopEnded(myStops.front().pars);
5763 }
5764 if (MSStopOut::active()) {
5765 MSStopOut::getInstance()->stopEnded(this, myStops.front().pars, myStops.front().lane->getID());
5766 }
5767 if (myStops.front().collision && MSLane::getCollisionAction() == MSLane::COLLISION_ACTION_WARN) {
5768 myCollisionImmunity = TIME2STEPS(5); // leave the conflict area
5769 }
5770 myStops.pop_front();
5771 // do not count the stopping time towards gridlock time.
5772 // Other outputs use an independent counter and are not affected.
5773 myWaitingTime = 0;
5774 // maybe the next stop is on the same edge; let's rebuild best lanes
5775 updateBestLanes(true);
5776 // continue as wished...
5777 MSNet::getInstance()->informVehicleStateListener(this, MSNet::VEHICLE_STATE_ENDING_STOP);
5778 return true;
5779 }
5780 return false;
5781 }
5782
5783
5784 MSVehicle::Stop&
5785 MSVehicle::getNextStop() {
5786 return myStops.front();
5787 }
5788
5789 std::list<MSVehicle::Stop>
5790 MSVehicle::getMyStops() {
5791 return myStops;
5792 }
5793
5794 MSVehicle::Influencer&
5795 MSVehicle::getInfluencer() {
5796 if (myInfluencer == nullptr) {
5797 myInfluencer = new Influencer();
5798 }
5799 return *myInfluencer;
5800 }
5801
5802
5803 const MSVehicle::Influencer*
5804 MSVehicle::getInfluencer() const {
5805 return myInfluencer;
5806 }
5807
5808
5809 double
5810 MSVehicle::getSpeedWithoutTraciInfluence() const {
5811 if (myInfluencer != nullptr && myInfluencer->getOriginalSpeed() != -1) {
5812 return myInfluencer->getOriginalSpeed();
5813 }
5814 return myState.mySpeed;
5815 }
5816
5817
5818 int
5819 MSVehicle::influenceChangeDecision(int state) {
5820 if (hasInfluencer()) {
5821 state = getInfluencer().influenceChangeDecision(
5822 MSNet::getInstance()->getCurrentTimeStep(),
5823 myLane->getEdge(),
5824 getLaneIndex(),
5825 state);
5826 }
5827 return state;
5828 }
5829
5830
5831 void
5832 MSVehicle::setRemoteState(Position xyPos) {
5833 myCachedPosition = xyPos;
5834 }
5835
5836
5837 bool
5838 MSVehicle::isRemoteControlled() const {
5839 return myInfluencer != nullptr && myInfluencer->isRemoteControlled();
5840 }
5841
5842
5843 bool
5844 MSVehicle::wasRemoteControlled(SUMOTime lookBack) const {
5845 return myInfluencer != nullptr && myInfluencer->getLastAccessTimeStep() + lookBack >= MSNet::getInstance()->getCurrentTimeStep();
5846 }
5847
5848
5849 bool
5850 MSVehicle::keepClear(const MSLink* link) const {
5851 if (link->hasFoes() && link->keepClear() /* && item.myLink->willHaveBlockedFoe()*/) {
5852 const double keepClearTime = getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_KEEPCLEAR_TIME, -1);
5853 //std::cout << SIMTIME << " veh=" << getID() << " keepClearTime=" << keepClearTime << " accWait=" << getAccumulatedWaitingSeconds() << " keepClear=" << (keepClearTime < 0 || getAccumulatedWaitingSeconds() < keepClearTime) << "\n";
5854 return keepClearTime < 0 || getAccumulatedWaitingSeconds() < keepClearTime;
5855 } else {
5856 return false;
5857 }
5858 }
5859
5860
5861 bool
5862 MSVehicle::ignoreRed(const MSLink* link, bool canBrake) const {
5863 if ((myInfluencer != nullptr && !myInfluencer->getEmergencyBrakeRedLight())) {
5864 return true;
5865 }
5866 const double ignoreRedTime = getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_DRIVE_AFTER_RED_TIME, -1);
5867 #ifdef DEBUG_IGNORE_RED
5868 if (DEBUG_COND) {
5869 std::cout << SIMTIME << " veh=" << getID() << " link=" << link->getViaLaneOrLane()->getID() << " state=" << toString(link->getState()) << "\n";
5870 }
5871 #endif
5872 if (ignoreRedTime < 0) {
5873 return false;
5874 } else if (link->haveYellow()) {
5875 // always drive at yellow when ignoring red
5876 return true;
5877 } else if (link->haveRed()) {
5878 assert(link->getTLLogic() != 0);
5879 const double redDuration = STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep() - link->getLastStateChange());
5880 #ifdef DEBUG_IGNORE_RED
5881 if (DEBUG_COND) {
5882 std::cout
5883 // << SIMTIME << " veh=" << getID() << " link=" << link->getViaLaneOrLane()->getID()
5884 << " ignoreRedTime=" << ignoreRedTime
5885 << " spentRed=" << redDuration
5886 << " canBrake=" << canBrake << "\n";
5887 }
5888 #endif
5889 // when activating ignoreRed behavior, vehicles will always drive if they cannot brake
5890 return !canBrake || ignoreRedTime > redDuration;
5891 } else {
5892 return false;
5893 }
5894 }
5895
5896
5897 bool
5898 MSVehicle::passingMinor() const {
5899 // either on an internal lane that was entered via minor link
5900 // or on approach to minor link below visibility distance
5901 if (myLane == nullptr) {
5902 return false;
5903 }
5904 if (myLane->getEdge().isInternal()) {
5905 return !myLane->getIncomingLanes().front().viaLink->havePriority();
5906 } else if (myLFLinkLanes.size() > 0 && myLFLinkLanes.front().myLink != nullptr) {
5907 MSLink* link = myLFLinkLanes.front().myLink;
5908 return !link->havePriority() && myLFLinkLanes.front().myDistance <= link->getFoeVisibilityDistance();
5909 }
5910 return false;
5911 }
5912
5913 bool
5914 MSVehicle::isLeader(const MSLink* link, const MSVehicle* veh) const {
5915 assert(link->fromInternalLane());
5916 if (veh == nullptr) {
5917 return false;
5918 }
5919 if (!myLane->isInternal() || myLane->getEdge().getToJunction() != link->getJunction()) {
5920 // if this vehicle is not yet on the junction, every vehicle is a leader
5921 return true;
5922 }
5923 const MSLane* foeLane = veh->getLane();
5924 if (foeLane->isInternal()) {
5925 if (foeLane->getEdge().getFromJunction() == link->getJunction()) {
5926 SUMOTime egoET = myJunctionConflictEntryTime;
5927 SUMOTime foeET = veh->myJunctionEntryTime;
5928 // check relationship between link and foeLane
5929 if (foeLane->getEdge().getNormalBefore() == link->getInternalLaneBefore()->getEdge().getNormalBefore()) {
5930 // we are entering the junction from the same edge
5931 egoET = myJunctionEntryTimeNeverYield;
5932 foeET = veh->myJunctionEntryTimeNeverYield;
5933 } else {
5934 const MSLink* foeLink = foeLane->getIncomingLanes()[0].viaLink;
5935 const MSJunctionLogic* logic = link->getJunction()->getLogic();
5936 assert(logic != nullptr);
5937 #ifdef DEBUG_PLAN_MOVE_LEADERINFO
5938 if (DEBUG_COND) {
5939 std::cout << SIMTIME
5940 << " foeLane=" << foeLane->getID()
5941 << " foeLink=" << foeLink->getViaLaneOrLane()->getID()
5942 << " linkIndex=" << link->getIndex()
5943 << " foeLinkIndex=" << foeLink->getIndex()
5944 << " response=" << logic->getResponseFor(link->getIndex()).test(foeLink->getIndex())
5945 << "\n";
5946 }
5947 #endif
5948 if (!logic->getResponseFor(link->getIndex()).test(foeLink->getIndex())) {
5949 // if we have right of way over the foe, entryTime does not matter
5950 foeET = veh->myJunctionConflictEntryTime;
5951 egoET = myJunctionEntryTime;
5952 }
5953 }
5954 if (egoET == foeET) {
5955 // try to use speed as tie braker
5956 if (getSpeed() == veh->getSpeed()) {
5957 // use ID as tie braker
5958 #ifdef DEBUG_PLAN_MOVE_LEADERINFO
5959 if (DEBUG_COND) {
5960 std::cout << SIMTIME << " veh=" << getID() << " equal ET " << egoET << " with foe " << veh->getID()
5961 << " foeIsLeaderByID=" << (getID() < veh->getID()) << "\n";
5962 }
5963 #endif
5964 return getID() < veh->getID();
5965 } else {
5966 #ifdef DEBUG_PLAN_MOVE_LEADERINFO
5967 if (DEBUG_COND) {
5968 std::cout << SIMTIME << " veh=" << getID() << " equal ET " << egoET << " with foe " << veh->getID()
5969 << " foeIsLeaderBySpeed=" << (getSpeed() < veh->getSpeed())
5970 << " v=" << getSpeed() << " foeV=" << veh->getSpeed()
5971 << "\n";
5972 }
5973 #endif
5974 return getSpeed() < veh->getSpeed();
5975 }
5976 } else {
5977 // leader was on the junction first
5978 #ifdef DEBUG_PLAN_MOVE_LEADERINFO
5979 if (DEBUG_COND) {
5980 std::cout << " egoET=" << egoET << " foeET=" << foeET << " isLeader=" << (egoET > foeET) << "\n";
5981 }
5982 #endif
5983 return egoET > foeET;
5984 }
5985 } else {
5986 // vehicle can only be partially on the junction. Must be a leader
5987 return true;
5988 }
5989 } else {
5990 // vehicle can only be partially on the junction. Must be a leader
5991 return true;
5992 }
5993 }
5994
5995 void
5996 MSVehicle::saveState(OutputDevice& out) {
5997 MSBaseVehicle::saveState(out);
5998 // here starts the vehicle internal part (see loading)
5999 std::vector<std::string> internals;
6000 internals.push_back(toString(myDeparture));
6001 internals.push_back(toString(distance(myRoute->begin(), myCurrEdge)));
6002 internals.push_back(toString(myDepartPos));
6003 internals.push_back(toString(myWaitingTime));
6004 internals.push_back(toString(myLastActionTime));
6005 out.writeAttr(SUMO_ATTR_STATE, internals);
6006 out.writeAttr(SUMO_ATTR_POSITION, myState.myPos);
6007 out.writeAttr(SUMO_ATTR_SPEED, myState.mySpeed);
6008 out.writeAttr(SUMO_ATTR_POSITION_LAT, myState.myPosLat);
6009 // save stops and parameters
6010 for (std::list<Stop>::const_iterator it = myStops.begin(); it != myStops.end(); ++it) {
6011 it->write(out);
6012 }
6013 myParameter->writeParams(out);
6014 for (MSVehicleDevice* const dev : myDevices) {
6015 dev->saveState(out);
6016 }
6017 out.closeTag();
6018 }
6019
6020 void
6021 MSVehicle::loadState(const SUMOSAXAttributes& attrs, const SUMOTime offset) {
6022 if (!attrs.hasAttribute(SUMO_ATTR_POSITION)) {
6023 throw ProcessError("Error: Invalid vehicles in state (may be a meso state)!");
6024 }
6025 int routeOffset;
6026 std::istringstream bis(attrs.getString(SUMO_ATTR_STATE));
6027 bis >> myDeparture;
6028 bis >> routeOffset;
6029 bis >> myDepartPos;
6030 bis >> myWaitingTime;
6031 bis >> myLastActionTime;
6032 if (hasDeparted()) {
6033 myCurrEdge += routeOffset;
6034 myDeparture -= offset;
6035 }
6036 myState.myPos = attrs.getFloat(SUMO_ATTR_POSITION);
6037 myState.mySpeed = attrs.getFloat(SUMO_ATTR_SPEED);
6038 myState.myPosLat = attrs.getFloat(SUMO_ATTR_POSITION_LAT);
6039
6040 // no need to reset myCachedPosition here since state loading happens directly after creation
6041 }
6042
6043 bool
6044 MSVehicle::haveValidStopEdges() const {
6045 MSRouteIterator start = myCurrEdge;
6046 const std::string err = "for vehicle '" + getID() + "' at time " + time2string(MSNet::getInstance()->getCurrentTimeStep());
6047 int i = 0;
6048 bool ok = true;
6049 double lastPos = getPositionOnLane();
6050 if (myLane != nullptr && myLane->isInternal()
6051 && myStops.size() > 0 && !myStops.front().lane->isInternal()) {
6052 // start edge is still incoming to the intersection so lastPos
6053 // relative to that edge must be adapted
6054 lastPos += (*myCurrEdge)->getLength();
6055 }
6056 for (const Stop& stop : myStops) {
6057 const double endPos = stop.getEndPos(*this);
6058 const MSEdge* const stopEdge = &stop.lane->getEdge();
6059 const MSRouteIterator it = std::find(start, myRoute->end(), stopEdge);
6060 const std::string prefix = "Stop " + toString(i) + " on edge '" + stopEdge->getID() + "' ";
6061 if (it == myRoute->end()) {
6062 WRITE_ERROR(prefix + "is not found after edge '" + (*start)->getID() + "' (" + toString(start - myCurrEdge) + " after current " + err);
6063 ok = false;
6064 } else {
6065 MSRouteIterator it2;
6066 for (it2 = myRoute->begin(); it2 != myRoute->end(); it2++) {
6067 if (it2 == stop.edge) {
6068 break;
6069 }
6070 }
6071 if (it2 == myRoute->end()) {
6072 WRITE_ERROR(prefix + "used invalid route index " + err);
6073 ok = false;
6074 } else if (it2 < start) {
6075 WRITE_ERROR(prefix + "used invalid (relative) route index " + toString(it2 - myCurrEdge) + " expected after " + toString(start - myCurrEdge) + " " + err);
6076 ok = false;
6077 } else {
6078 if (it != stop.edge && endPos >= lastPos) {
6079 WRITE_WARNING(prefix + "is used in " + toString(stop.edge - myCurrEdge) + " edges but first encounter is in "
6080 + toString(it - myCurrEdge) + " edges " + err);
6081 }
6082 start = stop.edge;
6083 }
6084 }
6085 lastPos = endPos;
6086 i++;
6087 }
6088 return ok;
6089 }
6090
6091 std::shared_ptr<MSSimpleDriverState>
6092 MSVehicle::getDriverState() const {
6093 return myDriverState->getDriverState();
6094 }
6095
6096 /****************************************************************************/
6097