From 513197ce4d085337dcd961e767393f042b5c797d Mon Sep 17 00:00:00 2001 From: Michael Behrisch Date: Mon, 13 Jan 2025 16:35:36 +0100 Subject: [PATCH] notifyMove for persons, fixing rerouter radius #15426 --- .../transportables/MSPModel_JuPedSim.cpp | 17 +++++----- .../transportables/MSPModel_JuPedSim.h | 2 +- .../transportables/MSStageWalking.cpp | 32 +++++++++++++------ src/microsim/transportables/MSStageWalking.h | 2 ++ src/microsim/trigger/MSTriggeredRerouter.cpp | 2 +- src/netload/NLTriggerBuilder.cpp | 22 ++++++------- 6 files changed, 45 insertions(+), 32 deletions(-) diff --git a/src/microsim/transportables/MSPModel_JuPedSim.cpp b/src/microsim/transportables/MSPModel_JuPedSim.cpp index d77c302765d7..96c38adb0843 100644 --- 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(person->getCurrentStage()); + MSPerson* const person = state->getPerson(); + MSStageWalking* const stage = dynamic_cast(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::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 index f442c05d6fc7..ce1e38b65285 100644 --- 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 index a060c5decc82..7b90b5d67ff5 100644 --- 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(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::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 index 4f7c9e018171..064350868a0b 100644 --- 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 index e8b50f6bab39..6e0b2809e981 100644 --- 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::max()) { return true; } return triggerRouting(tObject, reason); diff --git a/src/netload/NLTriggerBuilder.cpp b/src/netload/NLTriggerBuilder.cpp index 3dd1c4cc5a5c..bd36e5d1e381 100644 --- 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(SUMO_ATTR_OPTIONAL, id.c_str(), ok, false); const SUMOTime timeThreshold = TIME2STEPS(attrs.getOpt(SUMO_ATTR_HALTING_TIME_THRESHOLD, id.c_str(), ok, 0)); const std::string vTypes = attrs.getOpt(SUMO_ATTR_VTYPES, id.c_str(), ok, ""); - const std::string pos = attrs.getOpt(SUMO_ATTR_POSITION, id.c_str(), ok, ""); + const std::string pos = attrs.getOpt(SUMO_ATTR_POSITION, id.c_str(), ok, "0"); const double radius = attrs.getOpt(SUMO_ATTR_RADIUS, id.c_str(), ok, std::numeric_limits::max()); Position p = Position::INVALID; - if (pos != "") { - const std::vector 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 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 + "'.");