SUMO - Simulation of Urban MObility
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
MSLink.cpp
Go to the documentation of this file.
1 /****************************************************************************/
10 // A connnection between lanes
11 /****************************************************************************/
12 // SUMO, Simulation of Urban MObility; see http://sumo-sim.org/
13 // Copyright (C) 2001-2014 DLR (http://www.dlr.de/) and contributors
14 /****************************************************************************/
15 //
16 // This file is part of SUMO.
17 // SUMO is free software: you can redistribute it and/or modify
18 // it under the terms of the GNU General Public License as published by
19 // the Free Software Foundation, either version 3 of the License, or
20 // (at your option) any later version.
21 //
22 /****************************************************************************/
23 
24 // ===========================================================================
25 // included modules
26 // ===========================================================================
27 #ifdef _MSC_VER
28 #include <windows_config.h>
29 #else
30 #include <config.h>
31 #endif
32 
33 #include <iostream>
34 #include <algorithm>
35 #include <limits>
37 #include "MSNet.h"
38 #include "MSLink.h"
39 #include "MSLane.h"
40 #include "MSPerson.h"
41 #include "MSEdge.h"
42 #include "MSGlobals.h"
43 #include "MSVehicle.h"
44 #include "MSPModel.h"
45 
46 #ifdef CHECK_MEMORY_LEAKS
47 #include <foreign/nvwa/debug_new.h>
48 #endif // CHECK_MEMORY_LEAKS
49 
50 //#define MSLink_DEBUG_CROSSING_POINTS
51 
52 // ===========================================================================
53 // static member variables
54 // ===========================================================================
56 
57 
58 // ===========================================================================
59 // member method definitions
60 // ===========================================================================
61 #ifndef HAVE_INTERNAL_LANES
62 MSLink::MSLink(MSLane* succLane, LinkDirection dir, LinkState state, SUMOReal length) :
63  myLane(succLane),
64  myIndex(-1),
65  myState(state),
66  myDirection(dir),
67  myLength(length),
68  myHasFoes(false),
69  myAmCont(false)
70 #else
71 MSLink::MSLink(MSLane* succLane, MSLane* via, LinkDirection dir, LinkState state, SUMOReal length) :
72  myLane(succLane),
73  myIndex(-1),
74  myState(state),
75  myDirection(dir),
76  myLength(length),
77  myHasFoes(false),
78  myAmCont(false),
79  myJunctionInlane(via)
80 #endif
81 {}
82 
83 
85 
86 
87 void
88 MSLink::setRequestInformation(int index, bool hasFoes, bool isCont,
89  const std::vector<MSLink*>& foeLinks,
90  const std::vector<MSLane*>& foeLanes,
91  MSLane* internalLaneBefore) {
92  myIndex = index;
94  myAmCont = isCont;
95  myFoeLinks = foeLinks;
96  myFoeLanes = foeLanes;
97 #ifdef MSLink_DEBUG_CROSSING_POINTS
98  std::cout << " link " << myIndex << " to " << getViaLaneOrLane()->getID() << " has foes: " << toString(foeLanes) << "\n";
99 #endif
100 #ifdef HAVE_INTERNAL_LANES
101  MSLane* lane = 0;
102  if (internalLaneBefore != 0) {
103  // this is an exit link. compute crossing points with all foeLanes
104  lane = internalLaneBefore;
105  //} else if (myLane->getEdge().isCrossing()) {
106  // // this is the link to a pedestrian crossing. compute crossing points with all foeLanes
107  // // @note not currently used by pedestrians
108  // lane = myLane;
109  }
110  if (lane != 0) {
111  for (std::vector<MSLane*>::const_iterator it_lane = myFoeLanes.begin(); it_lane != myFoeLanes.end(); ++it_lane) {
112  if (myLane == (*it_lane)->getLinkCont()[0]->getLane() && !lane->getLinkCont()[0]->getViaLaneOrLane()->getEdge().isInternal()) {
113  //if (myLane == (*it_lane)->getLinkCont()[0]->getLane()) {
114  // this foeLane has the same target and merges at the end (lane exits the junction)
115  myLengthsBehindCrossing.push_back(std::make_pair(0, 0)); // dummy value, never used
116 #ifdef MSLink_DEBUG_CROSSING_POINTS
117  std::cout
118  << " " << lane->getID()
119  << " merges with " << (*it_lane)->getID()
120  << " nextLane " << lane->getLinkCont()[0]->getViaLaneOrLane()->getID()
121  << " dist1=" << myLengthsBehindCrossing.back().first
122  << " dist2=" << myLengthsBehindCrossing.back().second
123  << "\n";
124 #endif
125  } else {
126  std::vector<SUMOReal> intersections1 = lane->getShape().intersectsAtLengths2D((*it_lane)->getShape());
127  //std::cout << " number of intersections1=" << intersections1.size() << "\n";
128  if (intersections1.size() == 0) {
129  intersections1.push_back(10000.0); // disregard this foe (using maxdouble leads to nasty problems down the line)
130  } else if (intersections1.size() > 1) {
131  std::sort(intersections1.begin(), intersections1.end());
132  }
133  std::vector<SUMOReal> intersections2 = (*it_lane)->getShape().intersectsAtLengths2D(lane->getShape());
134  //std::cout << " number of intersections2=" << intersections2.size() << "\n";
135  if (intersections2.size() == 0) {
136  intersections2.push_back(0);
137  } else if (intersections2.size() > 1) {
138  std::sort(intersections2.begin(), intersections2.end());
139  }
140  myLengthsBehindCrossing.push_back(std::make_pair(
141  lane->getLength() - intersections1.back(),
142  (*it_lane)->getLength() - intersections2.back()));
143 #ifdef MSLink_DEBUG_CROSSING_POINTS
144  std::cout
145  << " intersection of " << lane->getID()
146  << " totalLength=" << lane->getLength()
147  << " with " << (*it_lane)->getID()
148  << " totalLength=" << (*it_lane)->getLength()
149  << " dist1=" << myLengthsBehindCrossing.back().first
150  << " dist2=" << myLengthsBehindCrossing.back().second
151  << "\n";
152 #endif
153  }
154  }
155  }
156 #else
157  UNUSED_PARAMETER(internalLaneBefore);
158 #endif
159 }
160 
161 
162 void
163 MSLink::setApproaching(const SUMOVehicle* approaching, const SUMOTime arrivalTime, const SUMOReal arrivalSpeed, const SUMOReal leaveSpeed,
164  const bool setRequest, const SUMOTime arrivalTimeBraking, const SUMOReal arrivalSpeedBraking, const SUMOTime waitingTime) {
165  const SUMOTime leaveTime = getLeaveTime(arrivalTime, arrivalSpeed, leaveSpeed, approaching->getVehicleType().getLength());
166  myApproachingVehicles.insert(std::make_pair(approaching,
167  ApproachingVehicleInformation(arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, setRequest,
168  arrivalTimeBraking, arrivalSpeedBraking, waitingTime)));
169 }
170 
171 
172 void
174  myBlockedFoeLinks.insert(link);
175 }
176 
177 
178 
179 bool
181  for (std::set<MSLink*>::const_iterator i = myBlockedFoeLinks.begin(); i != myBlockedFoeLinks.end(); ++i) {
182  if ((*i)->isBlockingAnyone()) {
183  return true;
184  }
185  }
186  return false;
187 }
188 
189 
190 void
192  myApproachingVehicles.erase(veh);
193 }
194 
195 
198  std::map<const SUMOVehicle*, ApproachingVehicleInformation>::const_iterator i = myApproachingVehicles.find(veh);
199  if (i != myApproachingVehicles.end()) {
200  return i->second;
201  } else {
202  return ApproachingVehicleInformation(-1000, -1000, 0, 0, false, -1000, 0, 0);
203  }
204 }
205 
206 
207 SUMOTime
208 MSLink::getLeaveTime(const SUMOTime arrivalTime, const SUMOReal arrivalSpeed,
209  const SUMOReal leaveSpeed, const SUMOReal vehicleLength) const {
210  return arrivalTime + TIME2STEPS((getLength() + vehicleLength) / MAX2((SUMOReal)0.5 * (arrivalSpeed + leaveSpeed), NUMERICAL_EPS));
211 }
212 
213 
214 bool
215 MSLink::opened(SUMOTime arrivalTime, SUMOReal arrivalSpeed, SUMOReal leaveSpeed, SUMOReal vehicleLength,
216  SUMOReal impatience, SUMOReal decel, SUMOTime waitingTime,
217  std::vector<const SUMOVehicle*>* collectFoes) const {
218  if (myState == LINKSTATE_TL_RED) {
219  return false;
220  }
222  return true;
223  }
224  if ((myState == LINKSTATE_STOP || myState == LINKSTATE_ALLWAY_STOP) && waitingTime == 0) {
225  return false;
226  }
227  const SUMOTime leaveTime = getLeaveTime(arrivalTime, arrivalSpeed, leaveSpeed, vehicleLength);
228  for (std::vector<MSLink*>::const_iterator i = myFoeLinks.begin(); i != myFoeLinks.end(); ++i) {
229 #ifdef HAVE_INTERNAL
231  if ((*i)->getState() == LINKSTATE_TL_RED) {
232  continue;
233  }
234  }
235 #endif
236  if ((*i)->blockedAtTime(arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, myLane == (*i)->getLane(),
237  impatience, decel, waitingTime, collectFoes)) {
238  return false;
239  }
240  }
241  return true;
242 }
243 
244 
245 bool
246 MSLink::blockedAtTime(SUMOTime arrivalTime, SUMOTime leaveTime, SUMOReal arrivalSpeed, SUMOReal leaveSpeed,
247  bool sameTargetLane, SUMOReal impatience, SUMOReal decel, SUMOTime waitingTime,
248  std::vector<const SUMOVehicle*>* collectFoes) const {
249  for (std::map<const SUMOVehicle*, ApproachingVehicleInformation>::const_iterator i = myApproachingVehicles.begin(); i != myApproachingVehicles.end(); ++i) {
250  if (!i->second.willPass) {
251  continue;
252  }
254  assert(waitingTime > 0);
255  if (waitingTime > i->second.waitingTime) {
256  continue;
257  }
258  if (waitingTime == i->second.waitingTime && arrivalTime < i->second.arrivalTime) {
259  continue;
260  }
261  }
262  const SUMOTime foeArrivalTime = (SUMOTime)((1.0 - impatience) * i->second.arrivalTime + impatience * i->second.arrivalTimeBraking);
263  if (i->second.leavingTime < arrivalTime) {
264  // ego wants to be follower
265  if (sameTargetLane && (arrivalTime - i->second.leavingTime < myLookaheadTime
266  || unsafeMergeSpeeds(i->second.leaveSpeed, arrivalSpeed,
267  i->first->getVehicleType().getCarFollowModel().getMaxDecel(), decel))) {
268  if (collectFoes == 0) {
269  return true;
270  } else {
271  collectFoes->push_back(i->first);
272  }
273  }
274  } else if (foeArrivalTime > leaveTime) {
275  // ego wants to be leader.
276  if (sameTargetLane && (foeArrivalTime - leaveTime < myLookaheadTime
277  || unsafeMergeSpeeds(leaveSpeed, i->second.arrivalSpeedBraking,
278  decel, i->first->getVehicleType().getCarFollowModel().getMaxDecel()))) {
279  if (collectFoes == 0) {
280  return true;
281  } else {
282  collectFoes->push_back(i->first);
283  }
284  }
285  } else {
286  // even without considering safeHeadwayTime there is already a conflict
287  if (collectFoes == 0) {
288  return true;
289  } else {
290  collectFoes->push_back(i->first);
291  }
292  }
293  }
294  return false;
295 }
296 
297 
298 bool
300  MSVehicle* veh = lane->getLastVehicle();
301  SUMOReal distLeft = 0;
302  if (veh == 0) {
303  veh = lane->getPartialOccupator();
304  distLeft = lane->getLength() - lane->getPartialOccupatorEnd();
305  } else {
306  distLeft = lane->getLength() - veh->getPositionOnLane() + veh->getVehicleType().getLength();
307  }
308  if (veh == 0) {
309  return false;
310  } else {
311  assert(distLeft > 0);
312  // can we be sure that the vehicle leaves this lane in the next step?
313  bool result = distLeft > (veh->getSpeed() - veh->getCarFollowModel().getMaxDecel());
314  return result;
315  }
316 }
317 
318 
319 bool
320 MSLink::hasApproachingFoe(SUMOTime arrivalTime, SUMOTime leaveTime, SUMOReal speed, SUMOReal decel) const {
321  for (std::vector<MSLink*>::const_iterator i = myFoeLinks.begin(); i != myFoeLinks.end(); ++i) {
322  if ((*i)->blockedAtTime(arrivalTime, leaveTime, speed, speed, myLane == (*i)->getLane(), 0, decel, 0)) {
323  return true;
324  }
325  }
326  for (std::vector<MSLane*>::const_iterator i = myFoeLanes.begin(); i != myFoeLanes.end(); ++i) {
327  if ((*i)->getVehicleNumber() > 0 || (*i)->getPartialOccupator() != 0) {
328  return true;
329  }
330  }
331  return false;
332 }
333 
334 
337  return myDirection;
338 }
339 
340 
341 void
343  myState = state;
344 }
345 
346 
347 MSLane*
349  return myLane;
350 }
351 
352 
353 bool
355 #ifdef HAVE_INTERNAL_LANES
356  if (myJunctionInlane == 0 || myAmCont) {
357  return false;
358  } else {
359  MSLane* pred = myJunctionInlane->getLogicalPredecessorLane();
361  return false;
362  } else {
363  MSLane* pred2 = pred->getLogicalPredecessorLane();
364  assert(pred2 != 0);
365  MSLink* predLink = MSLinkContHelper::getConnectingLink(*pred2, *pred);
366  assert(predLink != 0);
367  return predLink->havePriority();
368  }
369  }
370 #else
371  return false;
372 #endif
373 }
374 
375 
376 void
377 MSLink::writeApproaching(OutputDevice& od, const std::string fromLaneID) const {
378  if (myApproachingVehicles.size() > 0) {
379  od.openTag("link");
380  od.writeAttr(SUMO_ATTR_FROM, fromLaneID);
381 #ifdef HAVE_INTERNAL_LANES
382  const std::string via = getViaLane() == 0 ? "" : getViaLane()->getID();
383 #else
384  const std::string via = "";
385 #endif
386  od.writeAttr(SUMO_ATTR_VIA, via);
387  od.writeAttr(SUMO_ATTR_TO, getLane() == 0 ? "" : getLane()->getID());
388  std::vector<std::pair<SUMOTime, const SUMOVehicle*> > toSort; // stabilize output
389  for (std::map<const SUMOVehicle*, ApproachingVehicleInformation>::const_iterator it = myApproachingVehicles.begin(); it != myApproachingVehicles.end(); ++it) {
390  toSort.push_back(std::make_pair(it->second.arrivalTime, it->first));
391  }
392  std::sort(toSort.begin(), toSort.end());
393  for (std::vector<std::pair<SUMOTime, const SUMOVehicle*> >::const_iterator it = toSort.begin(); it != toSort.end(); ++it) {
394  od.openTag("approaching");
395  const ApproachingVehicleInformation& avi = myApproachingVehicles.find(it->second)->second;
396  od.writeAttr(SUMO_ATTR_ID, it->second->getID());
397  od.writeAttr(SUMO_ATTR_IMPATIENCE, it->second->getImpatience());
398  od.writeAttr("arrivalTime", time2string(avi.arrivalTime));
399  od.writeAttr("arrivalTimeBraking", time2string(avi.arrivalTimeBraking));
400  od.writeAttr("leaveTime", time2string(avi.leavingTime));
401  od.writeAttr("arrivalSpeed", toString(avi.arrivalSpeed));
402  od.writeAttr("arrivalSpeedBraking", toString(avi.arrivalSpeedBraking));
403  od.writeAttr("leaveSpeed", toString(avi.leaveSpeed));
404  od.writeAttr("willPass", toString(avi.willPass));
405  od.closeTag();
406  }
407  od.closeTag();
408  }
409 }
410 
411 
412 #ifdef HAVE_INTERNAL_LANES
413 MSLane*
414 MSLink::getViaLane() const {
415  return myJunctionInlane;
416 }
417 
418 
420 MSLink::getLeaderInfo(SUMOReal dist, SUMOReal minGap, std::vector<const MSPerson*>* collectBlockers) const {
421  LinkLeaders result;
422  // this link needs to start at an internal lane (either an exit link or between two internal lanes)
424  (myJunctionInlane == 0 && getLane()->getEdge().getPurpose() != MSEdge::EDGEFUNCTION_INTERNAL)
425  || (myJunctionInlane != 0 && myJunctionInlane->getLogicalPredecessorLane()->getEdge().isInternal()))) {
426  //std::cout << " getLeaderInfo link=" << getViaLaneOrLane()->getID() << "\n";
427  // this is an exit link
428  for (size_t i = 0; i < myFoeLanes.size(); ++i) {
429  MSLane* foeLane = myFoeLanes[i];
430  // distance from the querying vehicle to the crossing point with foeLane
431  const SUMOReal distToCrossing = dist - myLengthsBehindCrossing[i].first;
432  //std::cout << " distToCrossing=" << distToCrossing << " foeLane=" << foeLane->getID() << "\n";
433  if (distToCrossing < 0) {
434  continue; // vehicle is behind the crossing point, continue with next foe lane
435  }
436  const SUMOReal foeDistToCrossing = foeLane->getLength() - myLengthsBehindCrossing[i].second;
437  // it is not sufficient to return the last vehicle on the foeLane because ego might be its leader
438  // therefore we return all vehicles on the lane
439  //
440  // special care must be taken for continuation lanes. (next lane is also internal)
441  // vehicles on these lanes should always block (gap = -1)
442  const bool contLane = (foeLane->getLinkCont()[0]->getViaLaneOrLane()->getEdge().getPurpose() == MSEdge::EDGEFUNCTION_INTERNAL);
443  const bool sameTarget = (myLane == foeLane->getLinkCont()[0]->getLane());
444  // vehicles on cont. lanes or on internal lanes with the same target as this link can never be ignored
445  const bool cannotIgnore = contLane || sameTarget;
446  const MSLane::VehCont& vehicles = foeLane->getVehiclesSecure();
447  foeLane->releaseVehicles();
448  for (MSLane::VehCont::const_iterator it_veh = vehicles.begin(); it_veh != vehicles.end(); ++it_veh) {
449  MSVehicle* leader = *it_veh;
450  if (cannotIgnore || leader->getWaitingTime() < MSGlobals::gIgnoreJunctionBlocker) {
451  // compute distance between vehicles on the the superimposition of both lanes
452  // where the crossing point is the common point
453  SUMOReal gap;
454  if (contLane) {
455  gap = -1; // always break for vehicles which are on a continuation lane
456  } else {
457  const SUMOReal leaderBack = leader->getPositionOnLane() - leader->getVehicleType().getLength();
458  const SUMOReal leaderBackDist = foeDistToCrossing - leaderBack;
459  //std::cout << " distToCrossing=" << distToCrossing << " leader backDist=" << leaderBackDist << "\n";
460  if (leaderBackDist < 0) {
461  // leader is completely past the crossing point
462  // or there is no crossing point
463  continue; // next vehicle
464  }
465  gap = distToCrossing - leaderBackDist - (sameTarget ? minGap : 0);
466  }
467  result.push_back(LinkLeader(leader, gap, cannotIgnore ? -1 : distToCrossing));
468  }
469 
470  }
471  MSVehicle* leader = foeLane->getPartialOccupator();
472  if (leader != 0) {
473  if (cannotIgnore || leader->getWaitingTime() < MSGlobals::gIgnoreJunctionBlocker) {
474  // compute distance between vehicles on the the superimposition of both lanes
475  // where the crossing point is the common point
476  SUMOReal gap;
477  if (contLane) {
478  gap = -1; // always break for vehicles which are on a continuation lane
479  } else {
480  const SUMOReal leaderBackDist = foeDistToCrossing - foeLane->getPartialOccupatorEnd();
481  //std::cout << " distToCrossing=" << distToCrossing << " leader (partialOccupator) backDist=" << leaderBackDist << "\n";
482  if (leaderBackDist < 0) {
483  // leader is completely past the crossing point
484  // or there is no crossing point
485  continue; // next lane
486  }
487  gap = distToCrossing - leaderBackDist - (sameTarget ? minGap : 0);
488  }
489  result.push_back(LinkLeader(leader, gap, sameTarget ? -1 : distToCrossing));
490  }
491  }
492  // check for crossing pedestrians (keep driving if already on top of the crossing
493  const SUMOReal distToPeds = distToCrossing - MSPModel::SAFETY_GAP - foeLane->getWidth() * 0.5;
494  if (distToPeds >= -MSPModel::SAFETY_GAP && MSPModel::getModel()->blockedAtDist(foeLane, foeDistToCrossing, collectBlockers)) {
495  result.push_back(LinkLeader((MSVehicle*)0, -1, distToPeds));
496  }
497  }
498  }
499  return result;
500 }
501 #endif
502 
503 
504 MSLane*
506 #ifdef HAVE_INTERNAL_LANES
507  if (myJunctionInlane != 0) {
508  return myJunctionInlane;
509  }
510 #endif
511  return myLane;
512 }
513 
514 
515 /****************************************************************************/
516