speed dependent waypoint completion #14685

eclipse-sumo · Apr 15, 2024 · 63eb0db · 63eb0db
1 parent e9933ca
commit 63eb0db
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 10 deletions.
diff --git a/src/microsim/transportables/MSPModel_JuPedSim.cpp b/src/microsim/transportables/MSPModel_JuPedSim.cpp
@@ -349,8 +349,10 @@ MSPModel_JuPedSim::execute(SUMOTime time) {
                 JPS_CollisionFreeSpeedModelState_SetV0(modelState, newMaxSpeed);
             }
         }
-
-        if (newPosition.distanceTo2D(state->getNextWaypoint().first) < 2 * state->getNextWaypoint().second) {
+        // In the worst case during one SUMO step the person touches the waypoint radius and walks immediately into a different direction,
+        // but at some simstep it should have a maximum distance of v * delta_t / 2 to the waypoint circle.
+        const double slack = person->getMaxSpeed() * TS / 2. + POSITION_EPS;
+        if (newPosition.distanceTo2D(state->getNextWaypoint().first) < state->getNextWaypoint().second + slack) {
             // If near the last waypoint, remove the agent.
             if (state->advanceNextWaypoint()) {
                 // TODO this only works if the final stage is actually a walk

diff --git a/src/microsim/transportables/MSPModel_JuPedSim.h b/src/microsim/transportables/MSPModel_JuPedSim.h
@@ -145,7 +145,6 @@ class MSPModel_JuPedSim : public MSPModel {
         double myLanePosition;
         /// @brief whether the pedestrian is waiting to start its walk
         bool myWaitingToEnter;
-        int myNumStages;
     };
 
     /// @brief The network on which the simulation runs.
@@ -226,5 +225,5 @@ class MSPModel_JuPedSim : public MSPModel {
     void preparePolygonForDrawing(const GEOSGeometry* polygon, const std::string& polygonId, const RGBColor& color);
     static const GEOSGeometry* getLargestComponent(const GEOSGeometry* polygon, int& nbrComponents, double& maxArea, double& totalArea);
     static JPS_Geometry buildJPSGeometryFromGEOSGeometry(const GEOSGeometry* polygon);
-    static void dumpGeometry(const GEOSGeometry* polygon, const std::string& filename, bool useGeoCoordinates=false);
+    static void dumpGeometry(const GEOSGeometry* polygon, const std::string& filename, bool useGeoCoordinates = false);
 };
diff --git a/src/microsim/transportables/MSPerson.cpp b/src/microsim/transportables/MSPerson.cpp
@@ -56,7 +56,7 @@
 * MSPerson::MSPersonStage_Access - methods
 * ----------------------------------------------------------------------- */
 MSPerson::MSPersonStage_Access::MSPersonStage_Access(const MSEdge* destination, MSStoppingPlace* toStop,
-        const double arrivalPos, const double arrivalPosLat, const double dist, const bool isExit, 
+        const double arrivalPos, const double arrivalPosLat, const double dist, const bool isExit,
         const Position& startPos, const Position& endPos) :
     MSStage(MSStageType::ACCESS, destination, toStop, arrivalPos, arrivalPosLat),
     myDist(dist), myAmExit(isExit) {
@@ -186,9 +186,8 @@ MSPerson::checkAccess(const MSStage* const prior, const bool waitAtStop) {
                         }
                     }
                     newStage = new MSPersonStage_Access(accessEdge, prevStop, arrivalPos, 0.0, access->length, true,
-                                                    trainExit, platformEntry);
-                }
-                else {
+                                                        trainExit, platformEntry);
+                } else {
                     const double startPos = prior->getStageType() == MSStageType::TRIP ? prior->getEdgePos(0) : prior->getArrivalPos();
                     const double startPosLat = prior->getStageType() == MSStageType::TRIP ? prior->getEdgePosLat(0) : prior->getArrivalPosLat();
                     // The start and end attributes of the access stage are equal in this case, but we need to compute the arrival position relatively
@@ -253,11 +252,15 @@ MSPerson::reroute(const ConstMSEdgeVector& newEdges, double departPos, int first
     //    << " departPos=" << getEdgePos()
     //    << " arrivalPos=" <<  getNextStage(nextIndex - 1)->getArrivalPos()
     //    << "\n";
+    MSStage* toBeReplaced = getNextStage(nextIndex - 1);
+    if (newEdges.back() != toBeReplaced->getDestination()) {
+
+    }
     MSStageWalking* newStage = new MSStageWalking(getID(), newEdges,
-            getNextStage(nextIndex - 1)->getDestinationStop(), -1,
+            toBeReplaced->getDestinationStop(), -1,
             -1,
             departPos,
-            getNextStage(nextIndex - 1)->getArrivalPos(),
+            toBeReplaced->getArrivalPos(),
             MSPModel::UNSPECIFIED_POS_LAT);
     appendStage(newStage, nextIndex);
     // remove stages in reverse order so that proceed will only be called at the last removal