jupedsim now uses the sumo route #14198, refactoring to distinguish b…

…etween saved state and pedestrian state #12

src/guisim/GUIPerson.cpp

@@ -326,7 +326,7 @@ GUIPerson::drawAction_drawWalkingareaPath(const GUIVisualizationSettings& s) con
     MSStageWalking* stage = dynamic_cast<MSStageWalking*>(getCurrentStage());
     if (stage != nullptr) {
         setColor(s);
-        MSPModel_Striping::PState* stripingState = dynamic_cast<MSPModel_Striping::PState*>(stage->getState());
+        MSPModel_Striping::PState* stripingState = dynamic_cast<MSPModel_Striping::PState*>(stage->getPState());
         if (stripingState != nullptr) {
             const MSPModel_Striping::WalkingAreaPath* waPath = stripingState->myWalkingAreaPath;
             if (waPath != nullptr) {

src/libsumo/Person.cpp

@@ -822,7 +822,7 @@ Person::moveTo(const std::string& personID, const std::string& laneID, double po
         case MSStageType::WALKING: {
             MSStageWalking* s = dynamic_cast<MSStageWalking*>(p->getCurrentStage());
             assert(s != 0);
-            s->getState()->moveTo(p, l, pos, posLat, SIMSTEP);
+            s->getPState()->moveTo(p, l, pos, posLat, SIMSTEP);
             break;
         }
         default:

src/microsim/transportables/MSPModel_JuPedSim.cpp

@@ -145,11 +145,6 @@ MSPModel_JuPedSim::addWaypoint(JPS_JourneyDescription journey, JPS_StageId& pred
 MSTransportableStateAdapter*
 MSPModel_JuPedSim::add(MSTransportable* person, MSStageMoving* stage, SUMOTime now) {
     assert(person->getCurrentStageType() == MSStageType::WALKING);
-    for (PState* const pstate : myPedestrianStates) {  // TODO transform myPedestrianStates into a map for faster lookup
-        if (pstate->getPerson() == person) {
-            return pstate;
-        }
-    }
     Position departurePosition = Position::INVALID;
     const MSLane* const departureLane = getSidewalk<MSEdge, MSLane>(stage->getRoute().front());
     // First real stage, stage 0 is waiting.
@@ -181,46 +176,28 @@ MSPModel_JuPedSim::add(MSTransportable* person, MSStageMoving* stage, SUMOTime n
         departurePosition = departureLane->getShape().positionAtOffset(stage->getDepartPos(), -departureRelativePositionY); // Minus sign is here for legacy reasons.
     }
 
+    PositionVector waypoints;
+    for (const MSEdge* const e : stage->getEdges()) {
+        waypoints.push_back(getSidewalk<MSEdge, MSLane>(e)->getShape().positionAtOffset(e->getLength() / 2.));
+    }
+    waypoints.pop_back();  // arrival waypoint comes later
+    if (!waypoints.empty()) {
+        waypoints.erase(waypoints.begin());  // departure edge also does not need a waypoint
+    }
+    const MSLane* const arrivalLane = getSidewalk<MSEdge, MSLane>(stage->getDestination());
+    const Position arrivalPosition = arrivalLane->getShape().positionAtOffset(stage->getArrivalPos());
+    waypoints.push_back(arrivalPosition);
+
     JPS_JourneyDescription journey = JPS_JourneyDescription_Create();
     JPS_StageId startingStage = 0;
     JPS_StageId predecessor = 0;
-
-    int stageOffset = 1;
-    PositionVector waypoints;
-    while (person->getNumRemainingStages() > stageOffset) {
-        const MSStage* const next = person->getNextStage(stageOffset);
-        if (!next->isWalk()) {
-            break;
-        }
-        const MSStage* const prev = person->getNextStage(stageOffset - 1);
-        if (prev->getDestination() != next->getEdge()) {
-            // she is probably using an access
-            break;
-        }
-        double prevArrivalPos = prev->getArrivalPos();
-        if (prev->getDestinationStop() != nullptr) {
-            prevArrivalPos = prev->getDestinationStop()->getAccessPos(prev->getDestination());
-        }
-        waypoints.push_back(getSidewalk<MSEdge, MSLane>(prev->getDestination())->getShape().positionAtOffset(prevArrivalPos));
-        if (!addWaypoint(journey, predecessor, waypoints.back(), person->getID())) {
+    for (const Position& p : waypoints) {
+        if (!addWaypoint(journey, predecessor, p, person->getID())) {
             return nullptr;
         }
         if (startingStage == 0) {
             startingStage = predecessor;
         }
-        stageOffset++;
-    }
-
-    const MSStage* const arrivalStage = person->getNextStage(stageOffset - 1);
-    const MSLane* const arrivalLane = getSidewalk<MSEdge, MSLane>(arrivalStage->getDestination());
-    const Position arrivalPosition = arrivalLane->getShape().positionAtOffset(arrivalStage->getArrivalPos());
-    waypoints.push_back(arrivalPosition);
-
-    if (!addWaypoint(journey, predecessor, arrivalPosition, person->getID())) {
-        return nullptr;
-    }
-    if (startingStage == 0) {
-        startingStage = predecessor;
     }
     JPS_ErrorMessage message = nullptr;
     JPS_JourneyId journeyId = JPS_Simulation_AddJourney(myJPSSimulation, journey, &message);
@@ -230,23 +207,42 @@ MSPModel_JuPedSim::add(MSTransportable* person, MSStageMoving* stage, SUMOTime n
         return nullptr;
     }
 
-    PState* state = new PState(static_cast<MSPerson*>(person), stage, journey, journeyId, startingStage, waypoints);
-    state->setLanePosition(stage->getDepartPos());
-    state->setPreviousPosition(departurePosition);
-    state->setPosition(departurePosition.x(), departurePosition.y());
-    state->setAngle(departureLane->getShape().rotationAtOffset(stage->getDepartPos()));
-    myPedestrianStates.push_back(state);
-    myNumActivePedestrians++;
-    tryPedestrianInsertion(state, state->getPosition(*state->getStage(), now));
-
+    PState* state = nullptr;
+    for (PState* const pstate : myPedestrianStates) {  // TODO transform myPedestrianStates into a map for faster lookup
+        if (pstate->getPerson() == person) {
+            state = pstate;
+            break;
+        }
+    }
+    if (state == nullptr) {
+        state = new PState(static_cast<MSPerson*>(person), stage, journey, journeyId, startingStage, waypoints);
+        state->setLanePosition(stage->getDepartPos());
+        state->setPreviousPosition(departurePosition);
+        state->setPosition(departurePosition.x(), departurePosition.y());
+        state->setAngle(departureLane->getShape().rotationAtOffset(stage->getDepartPos()));
+        myPedestrianStates.push_back(state);
+        myNumActivePedestrians++;
+    } else {
+        state->getStage()->setPState(nullptr);  // we need to remove the old state reference to avoid double deletion
+        state->reinit(stage, journey, journeyId, startingStage, waypoints);
+    }
+    if (state->isWaitingToEnter()) {
+        tryPedestrianInsertion(state, state->getPosition(*state->getStage(), now));
+    } else {
+        JPS_Simulation_SwitchAgentJourney(myJPSSimulation, state->getAgentId(), journeyId, startingStage, &message);
+        if (message != nullptr) {
+            WRITE_WARNINGF(TL("Error while switching to a new journey for person '%': %"), person->getID(), JPS_ErrorMessage_GetMessage(message));
+            JPS_ErrorMessage_Free(message);
+            return nullptr;
+        }
+    }
     return state;
 }
 
 
 void
-MSPModel_JuPedSim::remove(MSTransportableStateAdapter* /* state */) {
-    // This function is called only when using TraCI.
-    // Not sure what to do here.
+MSPModel_JuPedSim::remove(MSTransportableStateAdapter* state) {
+    static_cast<PState*>(state)->setStage(nullptr);
 }
 
 
@@ -314,16 +310,20 @@ MSPModel_JuPedSim::execute(SUMOTime time) {
         }
 
         if (newPosition.distanceTo2D(state->getNextWaypoint()) < 2 * myExitTolerance) {
-            while (!stage->moveToNextEdge(person, time, 1, nullptr));
             // If near the last waypoint, remove the agent.
             if (state->advanceNextWaypoint()) {
-                registerArrived();
-                JPS_Simulation_MarkAgentForRemoval(myJPSSimulation, state->getAgentId(), nullptr);
-                stateIt = myPedestrianStates.erase(stateIt);
+                // TODO this only works if the final stage is actually a walk
+                const bool finalStage = person->getNumRemainingStages() == 1;
+                while (!stage->moveToNextEdge(person, time, 1, nullptr));
+                if (finalStage) {
+                    registerArrived();
+                    JPS_Simulation_MarkAgentForRemoval(myJPSSimulation, state->getAgentId(), nullptr);
+                    stateIt = myPedestrianStates.erase(stateIt);
+                    continue;
+                }
             }
-        } else {
-            ++stateIt;
         }
+        ++stateIt;
     }
 
     JPS_ErrorMessage_Free(message);
@@ -819,6 +819,17 @@ MSPModel_JuPedSim::PState::PState(MSPerson* person, MSStageMoving* stage,
 }
 
 
+void
+MSPModel_JuPedSim::PState::reinit(MSStageMoving* stage, JPS_JourneyDescription journey, JPS_JourneyId journeyId, JPS_StageId stageId,
+                                  const PositionVector& waypoints) {
+    myStage = stage;
+    myJourney = journey;
+    myJourneyId = journeyId;
+    myStageId = stageId;
+    myWaypoints = waypoints;
+}
+
+
 MSPModel_JuPedSim::PState::~PState() {
     JPS_JourneyDescription_Free(myJourney);
 }
@@ -849,12 +860,18 @@ void MSPModel_JuPedSim::PState::setAngle(double angle) {
 }
 
 
-MSStageMoving* MSPModel_JuPedSim::PState::getStage() {
+MSStageMoving* MSPModel_JuPedSim::PState::getStage() const {
     return myStage;
 }
 
 
-MSPerson* MSPModel_JuPedSim::PState::getPerson() {
+void
+MSPModel_JuPedSim::PState::setStage(MSStageMoving* const stage) {
+    myStage = stage;
+}
+
+
+MSPerson* MSPModel_JuPedSim::PState::getPerson() const {
     return myPerson;
 }
 

src/microsim/transportables/MSPModel_JuPedSim.h

@@ -78,6 +78,8 @@ class MSPModel_JuPedSim : public MSPModel {
         PState(MSPerson* person, MSStageMoving* stage, JPS_JourneyDescription journey, JPS_JourneyId journeyId, JPS_StageId stageId, const PositionVector& waypoints);
         ~PState() override;
 
+        void reinit(MSStageMoving* stage, JPS_JourneyDescription journey, JPS_JourneyId journeyId, JPS_StageId stageId, const PositionVector& waypoints);
+
         Position getPosition(const MSStageMoving& stage, SUMOTime now) const override;
         void setPosition(double x, double y);
 
@@ -87,8 +89,10 @@ class MSPModel_JuPedSim : public MSPModel {
         double getAngle(const MSStageMoving& stage, SUMOTime now) const override;
         void setAngle(double angle);
 
-        MSStageMoving* getStage();
-        MSPerson* getPerson();
+        MSStageMoving* getStage() const;
+        void setStage(MSStageMoving* const stage);
+
+        MSPerson* getPerson() const;
 
         void setLanePosition(double lanePosition);
         double getEdgePos(const MSStageMoving& stage, SUMOTime now) const override;

src/microsim/transportables/MSPModel_NonInteracting.cpp

@@ -102,13 +102,13 @@ MSPModel_NonInteracting::MoveToNextEdge::execute(SUMOTime currentTime) {
         return 0; // descheduled
     }
     const MSEdge* old = myParent.getEdge();
-    const bool arrived = myParent.moveToNextEdge(myTransportable, currentTime, myParent.getState()->getDirection(myParent, currentTime));
+    const bool arrived = myParent.moveToNextEdge(myTransportable, currentTime, myParent.getPState()->getDirection(myParent, currentTime));
     if (arrived) {
         myModel->registerArrived();
         return 0;
     }
     myParent.activateEntryReminders(myTransportable);
-    return static_cast<PState*>(myParent.getState())->computeDuration(old, myParent, currentTime);
+    return static_cast<PState*>(myParent.getPState())->computeDuration(old, myParent, currentTime);
 }
 
 

src/microsim/transportables/MSPerson.cpp

@@ -222,7 +222,7 @@ MSPerson::getNextEdgePtr() const {
     if (getCurrentStageType() == MSStageType::WALKING) {
         MSStageWalking* walkingStage =  dynamic_cast<MSStageWalking*>(*myStep);
         assert(walkingStage != nullptr);
-        return walkingStage->getState()->getNextEdge(*walkingStage);
+        return walkingStage->getPState()->getNextEdge(*walkingStage);
     }
     return nullptr;
 }
@@ -322,8 +322,9 @@ MSPerson::Influencer::postProcessRemoteControl(MSPerson* p) {
         case MSStageType::WALKING: {
             MSStageWalking* s = dynamic_cast<MSStageWalking*>(p->getCurrentStage());
             assert(s != nullptr);
-            s->getState()->moveToXY(p, myRemoteXYPos, myRemoteLane, myRemotePos, myRemotePosLat, myRemoteAngle, myRemoteEdgeOffset, myRemoteRoute,
-                                    MSNet::getInstance()->getCurrentTimeStep());
+            s->getPState()->moveToXY(p, myRemoteXYPos, myRemoteLane, myRemotePos, myRemotePosLat,
+                                     myRemoteAngle, myRemoteEdgeOffset, myRemoteRoute,
+                                     MSNet::getInstance()->getCurrentTimeStep());
         }
         break;
         default:

src/microsim/transportables/MSStageMoving.cpp

@@ -36,8 +36,8 @@
 * MSStageMoving - methods
 * ----------------------------------------------------------------------- */
 MSStageMoving::~MSStageMoving() {
-    if (myState != nullptr && myState->isFinished()) {
-        delete myState;
+    if (myPState != nullptr && myPState->isFinished()) {
+        delete myPState;
     }
 }
 
@@ -63,38 +63,38 @@ MSStageMoving::getEdges() const {
 
 double
 MSStageMoving::getEdgePos(SUMOTime now) const {
-    return myState == nullptr ? myDepartPos : myState->getEdgePos(*this, now);
+    return myPState == nullptr ? myDepartPos : myPState->getEdgePos(*this, now);
 }
 
 int
 MSStageMoving::getDirection() const {
-    return myState == nullptr ? MSPModel::UNDEFINED_DIRECTION : myState->getDirection(*this, MSNet::getInstance()->getCurrentTimeStep());
+    return myPState == nullptr ? MSPModel::UNDEFINED_DIRECTION : myPState->getDirection(*this, MSNet::getInstance()->getCurrentTimeStep());
 }
 
 
 Position
 MSStageMoving::getPosition(SUMOTime now) const {
-    return myState == nullptr ? Position::INVALID : myState->getPosition(*this, now);
+    return myPState == nullptr ? Position::INVALID : myPState->getPosition(*this, now);
 }
 
 double
 MSStageMoving::getAngle(SUMOTime now) const {
-    return myState == nullptr ? 0. : myState->getAngle(*this, now);
+    return myPState == nullptr ? 0. : myPState->getAngle(*this, now);
 }
 
 SUMOTime
 MSStageMoving::getWaitingTime(SUMOTime now) const {
-    return myState == nullptr ? 0 : myState->getWaitingTime(*this, now);
+    return myPState == nullptr ? 0 : myPState->getWaitingTime(*this, now);
 }
 
 double
 MSStageMoving::getSpeed() const {
-    return myState == nullptr ? 0. : myState->getSpeed(*this);
+    return myPState == nullptr ? 0. : myPState->getSpeed(*this);
 }
 
 const MSLane*
 MSStageMoving::getLane() const {
-    return myState == nullptr ? nullptr : myState->getLane();
+    return myPState == nullptr ? nullptr : myPState->getLane();
 }
 
 void

src/microsim/transportables/MSStageMoving.h

@@ -37,7 +37,7 @@ class MSStageMoving : public MSStage {
     MSStageMoving(const std::vector<const MSEdge*>& route, const std::string& routeID, MSStoppingPlace* toStop, const double speed,
                   const double departPos, const double arrivalPos, const double departPosLat, const int departLane, MSStageType type) :
         MSStage(route.back(), toStop, arrivalPos, type),
-        myState(nullptr), myRoute(route), myRouteID(routeID), myRouteStep(myRoute.begin()),
+        myPState(nullptr), myRoute(route), myRouteID(routeID), myRouteStep(myRoute.begin()),
         mySpeed(speed), myDepartPos(departPos),
         myDepartPosLat(departPosLat), myDepartLane(departLane) {}
 
@@ -46,8 +46,12 @@ class MSStageMoving : public MSStage {
 
     virtual const MSEdge* getNextRouteEdge() const = 0;
 
-    virtual MSTransportableStateAdapter* getState() const {
-        return myState;
+    inline MSTransportableStateAdapter* getPState() const {
+        return myPState;
+    }
+
+    inline void setPState(MSTransportableStateAdapter* pstate) {
+        myPState = pstate;
     }
 
     /// Returns the current edge
@@ -131,7 +135,7 @@ class MSStageMoving : public MSStage {
 
 protected:
     /// @brief state that is to be manipulated by MSPModel
-    MSTransportableStateAdapter* myState;
+    MSTransportableStateAdapter* myPState;
 
     /// @brief The route of the container
     std::vector<const MSEdge*> myRoute;

src/microsim/transportables/MSStageTranship.cpp

@@ -77,10 +77,10 @@ MSStageTranship::proceed(MSNet* net, MSTransportable* transportable, SUMOTime no
     myRouteStep = myRoute.end() - 1;
     myDepartPos = previous->getEdgePos(now);
     if (transportable->isPerson()) {
-        myState = net->getPersonControl().getNonInteractingModel()->add(transportable, this, now);
+        myPState = net->getPersonControl().getNonInteractingModel()->add(transportable, this, now);
         (*myRouteStep)->addTransportable(transportable);
     } else {
-        myState = net->getContainerControl().getNonInteractingModel()->add(transportable, this, now);
+        myPState = net->getContainerControl().getNonInteractingModel()->add(transportable, this, now);
         (*myRouteStep)->addTransportable(transportable);
     }
 }