guarding insertion against comitted approach refs #7578

eclipse-sumo · Sep 6, 2024 · 77845cb · 77845cb
1 parent 2d71bec
commit 77845cb
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 3 deletions.
diff --git a/src/microsim/traffic_lights/MSDriveWay.cpp b/src/microsim/traffic_lights/MSDriveWay.cpp
@@ -283,7 +283,7 @@ MSDriveWay::conflictLinkApproached() const {
 
 
 bool
-MSDriveWay::hasLinkConflict(const Approaching& veh, MSLink* foeLink) const {
+MSDriveWay::hasLinkConflict(const Approaching& veh, const MSLink* foeLink) const {
 #ifdef DEBUG_SIGNALSTATE_PRIORITY
     if (gDebugFlag4) {
         std::cout << "   checkLinkConflict foeLink=" << getTLLinkID(foeLink) << "\n";
@@ -499,6 +499,20 @@ MSDriveWay::foeDriveWayOccupied(bool store, const SUMOVehicle* ego, MSEdgeVector
                 /// @todo: if foe occupies more than one edge we should add all of them to the occupied vector
             }
             return true;
+        } else if (foeDW != this && isDepartDriveway() && !foeDW->isDepartDriveway()) {
+            if (foeDW->myOrigin->getApproaching().size() > 0) {
+                Approaching foeA = foeDW->myOrigin->getClosest();
+                const SUMOVehicle* foe = foeA.first;
+                if (foeA.second.dist < foe->getBrakeGap(true)) {
+                    MSRouteIterator firstIt = std::find(foe->getCurrentRouteEdge(), foe->getRoute().end(), foeDW->myRoute.front());
+                    if (firstIt != foe->getRoute().end()) {
+                        if (foeDW->match(foe->getRoute(), firstIt)) {
+                            //std::cout << SIMTIME << " " << getID() << " blocked by " << foeDW->getID() << " (approached by " << foe->getID() << ")\n";
+                            return true;
+                        }
+                    }
+                }
+            }
         }
     }
     return false;

diff --git a/src/microsim/traffic_lights/MSDriveWay.h b/src/microsim/traffic_lights/MSDriveWay.h
@@ -226,7 +226,7 @@ class MSDriveWay : public MSMoveReminder, public Named {
     bool deadlockLaneOccupied(const SUMOVehicle* ego, bool store = true) const;
 
     /// @brief Whether the approaching vehicle is prevent from driving by another vehicle approaching the given link
-    bool hasLinkConflict(const Approaching& closest, MSLink* foeLink) const;
+    bool hasLinkConflict(const Approaching& closest, const MSLink* foeLink) const;
 
     /// @brief Wether this driveway (route) overlaps with the given one
     bool overlap(const MSDriveWay& other) const;
@@ -279,7 +279,7 @@ class MSDriveWay : public MSMoveReminder, public Named {
     /// @brief add symmetical conflict link for foes when building a new driveway
     void addConflictLink(const MSLink* link);
 
-    bool isDepartDriveway() {
+    bool isDepartDriveway() const {
         return myOrigin == nullptr;
     };