notifyMove for persons, fixing rerouter radius #15426

eclipse-sumo · Jan 13, 2025 · 513197c · 513197c
1 parent 95dad55
commit 513197c
Show file tree

Hide file tree

Showing 6 changed files with 45 additions and 32 deletions.
diff --git a/src/microsim/transportables/MSPModel_JuPedSim.cpp b/src/microsim/transportables/MSPModel_JuPedSim.cpp
@@ -390,6 +390,7 @@ MSPModel_JuPedSim::remove(MSTransportableStateAdapter* state) {
     if (pstate->getLane() != nullptr) {
         auto& peds = myActiveLanes[pstate->getLane()];
         peds.erase(std::find(peds.begin(), peds.end(), pstate));
+        pstate->setLane(nullptr);
     }
     if (pstate->getStage() != nullptr) {
         pstate->getStage()->setPState(nullptr);  // we need to remove the old state reference to avoid double deletion
@@ -426,8 +427,8 @@ MSPModel_JuPedSim::execute(SUMOTime time) {
             continue;
         }
 
-        MSPerson* person = state->getPerson();
-        MSStageWalking* stage = dynamic_cast<MSStageWalking*>(person->getCurrentStage());
+        MSPerson* const person = state->getPerson();
+        MSStageWalking* const stage = dynamic_cast<MSStageWalking*>(person->getCurrentStage());
         if (stage == nullptr) {
             // It seems we kept the state for another stage but the new stage is not a walk.
             // So let's remove the state because after the new stage we will be elsewhere and need to be reinserted for JuPedSim anyway.
@@ -450,6 +451,7 @@ MSPModel_JuPedSim::execute(SUMOTime time) {
         Position newPosition(position.x, position.y);
         ConstMSEdgeVector route = stage->getEdges();
         const int routeIndex = (int)(stage->getRouteStep() - stage->getRoute().begin());
+        const double oldLanePos = state->getEdgePos(time);
         ConstMSEdgeVector forwardRoute = ConstMSEdgeVector(route.begin() + routeIndex, route.end());
         double bestDistance = std::numeric_limits<double>::max();
         MSLane* candidateLane = nullptr;
@@ -462,9 +464,7 @@ MSPModel_JuPedSim::execute(SUMOTime time) {
             if (candidateLane != state->getLane()) {
                 if (state->getLane() != nullptr) {
                     auto& peds = myActiveLanes[state->getLane()];
-                    if (!peds.empty()) {
-                        peds.erase(std::find(peds.begin(), peds.end(), state));
-                    }
+                    peds.erase(std::find(peds.begin(), peds.end(), state));
                 }
                 myActiveLanes[candidateLane].push_back(state);
                 state->setLane(candidateLane);
@@ -530,6 +530,7 @@ MSPModel_JuPedSim::execute(SUMOTime time) {
                 JPS_WaitingSetProxy_SetWaitingSetState(proxy, open ? JPS_WaitingSet_Inactive : JPS_WaitingSet_Active);
             }
         }
+        stage->activateMoveReminders(person, oldLanePos, state->getEdgePos(time), state->getSpeed(*stage));
         // 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;
@@ -1300,13 +1301,13 @@ MSPModel_JuPedSim::PState::~PState() {
 }
 
 
-void MSPModel_JuPedSim::PState::setPosition(double x, double y) {
+void MSPModel_JuPedSim::PState::setPosition(const double x, const double y, const double z) {
     if (myRemoteXYPos != Position::INVALID) {
-        mySpeed = myRemoteXYPos.distanceTo2D(Position(x, y)) / STEPS2TIME(DELTA_T);
+        mySpeed = myRemoteXYPos.distanceTo2D(Position(x, y, z)) / STEPS2TIME(DELTA_T);
     } else {
         mySpeed = 0.;
     }
-    myRemoteXYPos.set(x, y);
+    myRemoteXYPos.set(x, y, z);
 }
 
 

diff --git a/src/microsim/transportables/MSPModel_JuPedSim.h b/src/microsim/transportables/MSPModel_JuPedSim.h
@@ -95,7 +95,7 @@ class MSPModel_JuPedSim : public MSPModel_Interacting {
         inline Position getPosition(const MSStageMoving&, SUMOTime) const override {
             return myRemoteXYPos;
         }
-        void setPosition(double x, double y);
+        void setPosition(const double x, const double y, const double z = 0.);
 
         inline void setAngle(double angle) {
             myAngle = angle;

diff --git a/src/microsim/transportables/MSStageWalking.cpp b/src/microsim/transportables/MSStageWalking.cpp
@@ -393,16 +393,6 @@ MSStageWalking::moveToNextEdge(MSTransportable* person, SUMOTime currentTime, in
 }
 
 
-void
-MSStageWalking::activateLeaveReminders(MSTransportable* person, const MSLane* lane, double lastPos, SUMOTime t, bool arrived) {
-    MSMoveReminder::Notification notification = arrived ? MSMoveReminder::NOTIFICATION_ARRIVED : MSMoveReminder::NOTIFICATION_JUNCTION;
-    for (MSMoveReminder* const rem : myMoveReminders) {
-        rem->updateDetector(*person, 0.0, lane->getLength(), myLastEdgeEntryTime, t, t, true);
-        rem->notifyLeave(*person, lastPos, notification);
-    }
-}
-
-
 void
 MSStageWalking::activateEntryReminders(MSTransportable* person, const bool isDepart) {
     const MSLane* const nextLane = getSidewalk<MSEdge, MSLane>(getEdge());
@@ -434,6 +424,28 @@ MSStageWalking::activateEntryReminders(MSTransportable* person, const bool isDep
 }
 
 
+void
+MSStageWalking::activateMoveReminders(MSTransportable* person, double oldPos, double newPos, double newSpeed) {
+    for (std::vector<MSMoveReminder*>::iterator rem = myMoveReminders.begin(); rem != myMoveReminders.end();) {
+        if ((*rem)->notifyMove(*person, oldPos, newPos, newSpeed)) {
+            ++rem;
+        } else {
+            rem = myMoveReminders.erase(rem);
+        }
+    }
+}
+
+
+void
+MSStageWalking::activateLeaveReminders(MSTransportable* person, const MSLane* lane, double lastPos, SUMOTime t, bool arrived) {
+    MSMoveReminder::Notification notification = arrived ? MSMoveReminder::NOTIFICATION_ARRIVED : MSMoveReminder::NOTIFICATION_JUNCTION;
+    for (MSMoveReminder* const rem : myMoveReminders) {
+        rem->updateDetector(*person, 0.0, lane->getLength(), myLastEdgeEntryTime, t, t, true);
+        rem->notifyLeave(*person, lastPos, notification);
+    }
+}
+
+
 int
 MSStageWalking::getRoutePosition() const {
     return (int)(myRouteStep - myRoute.begin());

diff --git a/src/microsim/transportables/MSStageWalking.h b/src/microsim/transportables/MSStageWalking.h
@@ -117,6 +117,8 @@ class MSStageWalking : public MSStageMoving {
 
     void activateEntryReminders(MSTransportable* person, const bool isDepart = false);
 
+    void activateMoveReminders(MSTransportable* person, double oldPos, double newPos, double newSpeed);
+
     void activateLeaveReminders(MSTransportable* person, const MSLane* lane, double lastPos, SUMOTime t, bool arrived);
 
     /// @brief accessors to be used by MSPModel

diff --git a/src/microsim/trigger/MSTriggeredRerouter.cpp b/src/microsim/trigger/MSTriggeredRerouter.cpp
@@ -389,7 +389,7 @@ MSTriggeredRerouter::getCurrentReroute(SUMOTime time) const {
 
 bool
 MSTriggeredRerouter::notifyEnter(SUMOTrafficObject& tObject, MSMoveReminder::Notification reason, const MSLane* /* enteredLane */) {
-    if (myAmOptional) {
+    if (myAmOptional || myRadius != std::numeric_limits<double>::max()) {
         return true;
     }
     return triggerRouting(tObject, reason);

diff --git a/src/netload/NLTriggerBuilder.cpp b/src/netload/NLTriggerBuilder.cpp
@@ -723,20 +723,18 @@ NLTriggerBuilder::parseAndBuildRerouter(MSNet& net, const SUMOSAXAttributes& att
     const bool optional = attrs.getOpt<bool>(SUMO_ATTR_OPTIONAL, id.c_str(), ok, false);
     const SUMOTime timeThreshold = TIME2STEPS(attrs.getOpt<double>(SUMO_ATTR_HALTING_TIME_THRESHOLD, id.c_str(), ok, 0));
     const std::string vTypes = attrs.getOpt<std::string>(SUMO_ATTR_VTYPES, id.c_str(), ok, "");
-    const std::string pos = attrs.getOpt<std::string>(SUMO_ATTR_POSITION, id.c_str(), ok, "");
+    const std::string pos = attrs.getOpt<std::string>(SUMO_ATTR_POSITION, id.c_str(), ok, "0");
     const double radius = attrs.getOpt<double>(SUMO_ATTR_RADIUS, id.c_str(), ok, std::numeric_limits<double>::max());
     Position p = Position::INVALID;
-    if (pos != "") {
-        const std::vector<std::string> posSplit = StringTokenizer(pos, ",").getVector();
-        if (posSplit.size() == 1) {
-            p = edges.front()->getLanes()[0]->geometryPositionAtOffset(StringUtils::toDouble(posSplit[0]));
-        } else if (posSplit.size() == 2) {
-            p = Position(StringUtils::toDouble(posSplit[0]), StringUtils::toDouble(posSplit[1]));
-        } else if (posSplit.size() == 3) {
-            p = Position(StringUtils::toDouble(posSplit[0]), StringUtils::toDouble(posSplit[1]), StringUtils::toDouble(posSplit[2]));
-        } else {
-            throw InvalidArgument("Invalid position for rerouter '" + id + "'.");
-        }
+    const std::vector<std::string> posSplit = StringTokenizer(pos, ",").getVector();
+    if (posSplit.size() == 1) {
+        p = edges.front()->getLanes()[0]->geometryPositionAtOffset(StringUtils::toDouble(posSplit[0]));
+    } else if (posSplit.size() == 2) {
+        p = Position(StringUtils::toDouble(posSplit[0]), StringUtils::toDouble(posSplit[1]));
+    } else if (posSplit.size() == 3) {
+        p = Position(StringUtils::toDouble(posSplit[0]), StringUtils::toDouble(posSplit[1]), StringUtils::toDouble(posSplit[2]));
+    } else {
+        throw InvalidArgument("Invalid position for rerouter '" + id + "'.");
     }
     if (!ok) {
         throw InvalidArgument("Could not parse rerouter '" + id + "'.");