considering out-of-bounds vehicles in planMove. refs #10458

eclipse-sumo · Apr 8, 2022 · deb7156 · deb7156
1 parent 8cf1791
commit deb7156
Show file tree

Hide file tree

Showing 2 changed files with 49 additions and 0 deletions.
diff --git a/src/microsim/MSVehicle.cpp b/src/microsim/MSVehicle.cpp
@@ -2155,6 +2155,30 @@ MSVehicle::planMoveInternal(const SUMOTime t, MSLeaderInfo ahead, DriveItemVecto
 #endif
             adaptToLeaderDistance(oppositeLeaders, 0, seen, lastLink, v, vLinkPass);
         } else {
+            if (MSGlobals::gLateralResolution > 0
+                    && (getLeftSideOnLane(lane) < 0 || getRightSideOnLane(lane) > lane->getWidth())) {
+                    // if ego is driving outside lane bounds we must consider
+                    // potential leaders that are also outside lane bounds
+                const bool outsideLeft = getLeftSideOnLane(lane) < 0;
+                if (outsideLeft) {
+                    ahead.setSublaneOffset(ceil(-getLeftSideOnLane(lane) / MSGlobals::gLateralResolution) * MSGlobals::gLateralResolution);
+                } else if (getRightSideOnLane(lane) > lane->getWidth()) {
+                    ahead.setSublaneOffset(-ceil((getRightSideOnLane(lane) - getWidth()) / MSGlobals::gLateralResolution) * MSGlobals::gLateralResolution);
+                }
+                for (const MSVehicle* cand : lane->getVehiclesSecure()) {
+                    if ((lane != myLane || cand->getPositionOnLane() > getPositionOnLane())
+                            && ((outsideLeft && cand->getLeftSideOnLane() < 0)
+                                || (!outsideLeft && cand->getRightSideOnEdge() > lane->getWidth()))) {
+                        ahead.addLeader(cand, true);
+#ifdef DEBUG_PLAN_MOVE
+                        if (DEBUG_COND) {
+                            std::cout << SIMTIME << " veh=" << getID() << " offset=" << ahead.getSublaneOffset() << " outsideLeft=" << outsideLeft << " outsideLeader=" << cand->getID() << "\n";
+                        }
+#endif
+                    }
+                }
+                lane->releaseVehicles();
+            }
             adaptToLeaders(ahead, lateralShift, seen, lastLink, leaderLane, v, vLinkPass);
         }
         if (lastLink != nullptr) {
@@ -2742,6 +2766,7 @@ MSVehicle::adaptToLeaders(const MSLeaderInfo& ahead, double latOffset,
                                   << " latOffset=" << latOffset
                                   << " rm=" << rightmost
                                   << " lm=" << leftmost
+                                  << " shift=" << ahead.getSublaneOffset()
                                   << " ahead=" << ahead.toString()
                                   << "\n";
 #endif
@@ -6021,6 +6046,18 @@ MSVehicle::getLeftSideOnLane() const {
 }
 
 
+double
+MSVehicle::getRightSideOnLane(const MSLane* lane) const {
+    return myState.myPosLat + 0.5 *lane->getWidth() - 0.5 * getVehicleType().getWidth();
+}
+
+
+double
+MSVehicle::getLeftSideOnLane(const MSLane* lane) const {
+    return myState.myPosLat + 0.5 * lane->getWidth() + 0.5 * getVehicleType().getWidth();
+}
+
+
 double
 MSVehicle::getRightSideOnEdge(const MSLane* lane) const {
     return getCenterOnEdge(lane) - 0.5 * getVehicleType().getWidth();

diff --git a/src/microsim/MSVehicle.h b/src/microsim/MSVehicle.h
@@ -432,6 +432,18 @@ class MSVehicle : public MSBaseVehicle {
      */
     double getLeftSideOnLane() const;
 
+    /** @brief Get the lateral position of the vehicles right side on the lane:
+     * @return The lateral position of the vehicle (in m distance between right
+     * side of vehicle and right side of the lane it is on
+     */
+    double getRightSideOnLane(const MSLane* lane) const;
+
+    /** @brief Get the lateral position of the vehicles left side on the lane:
+     * @return The lateral position of the vehicle (in m distance between left
+     * side of vehicle and right side of the lane it is on
+     */
+    double getLeftSideOnLane(const MSLane* lane) const;
+
     /** @brief Get the minimal lateral distance required to move fully onto the lane at given offset
      * @return The lateral distance to be covered to move the vehicle fully onto the lane (in m)
      */