now not eating people leaving queues

Author: makokal

pedsim_simulator/3rdparty/libpedsim/ped_agent.h

@@ -78,7 +78,7 @@ namespace Ped {
         virtual Tvector obstacleForce() const;
         virtual Tvector lookaheadForce(Tvector desired) const;
         virtual Tvector myForce(Tvector desired);
-		virtual void updateState() {}
+		virtual void updateState(int event = 0) {}
 
         void setPosition(double px, double py, double pz);
         void setType(AgentType t) { this->type = t; };

pedsim_simulator/include/pedsim_simulator/agent.h

@@ -65,7 +65,10 @@ class Agent : public Ped::Tagent
     void setX(double xIn);
     void setY(double yIn);
     void setType(Ped::Tagent::AgentType t);
-	void updateState();
+	void updateState(int event = 0);
+	
+	StateMachine::State status() { return state_machine_->getCurrentState(); }
+	StateMachine::State prevStatus() { return state_machine_->getPreviousState(); }
 
 private:
 	StateMachinePtr state_machine_;

pedsim_simulator/include/pedsim_simulator/statemachine.h

@@ -55,11 +55,11 @@ class StateMachine
     /// \brief Agent events to transition between states
     enum Event
     {
-        START_WALKING = 0,
-        STOP_WALKING = 1,
-        JOIN_QUEUE = 2,
-        LEAVE_QUEUE = 3,
-        NO_EVENT = 4
+		NO_EVENT = 0,
+        START_WALKING = 1,
+        STOP_WALKING = 2,
+        JOIN_QUEUE = 3,
+        LEAVE_QUEUE = 4
     };
 
 
@@ -90,12 +90,15 @@ class StateMachine
         /// Walking transitions
         if ( current_state_ == WALKING )
         {
+			if ( previous_state_ == QUEUEING )
+				return;
+			
             if ( triggered_event == JOIN_QUEUE )
             {
                 previous_state_ = WALKING;
                 current_state_ = QUEUEING;
             }
-            else if ( triggered_event == START_WALKING )
+            else if ( triggered_event == STOP_WALKING )
             {
                 previous_state_ = WALKING;
                 current_state_ = IDLE;
@@ -120,6 +123,11 @@ class StateMachine
         return current_state_;
     }
 
+    State getPreviousState()
+    {
+        return previous_state_;
+    }
+    
     void reset()
     {
         current_state_ = IDLE;

pedsim_simulator/src/agent.cpp

@@ -82,13 +82,14 @@ Ped::Tvector Agent::myForce ( Ped::Tvector desired ) const
     return t;
 }
 
-void Agent::updateState()
+void Agent::updateState(int event)
 {
     if ( getteleop() == true )
         state_machine_->reset();
 
     // check state
-    state_machine_->doStateTransition ( StateMachine::NO_EVENT );
+	StateMachine::Event ev = static_cast<StateMachine::Event>(event);
+    state_machine_->doStateTransition ( ev );
 }
 
 /// \brief Move the agents in one time step

pedsim_simulator/src/scene.cpp

@@ -285,18 +285,16 @@ void Scene::updateQueues()
         /// Add agents into queues
         BOOST_FOREACH ( Agent* a, agents )
         {
-            if ( a->gettype() != Ped::Tagent::ROBOT )
+            if ( a->gettype() != Ped::Tagent::ROBOT && q->agentInQueue ( a ) == false )
             {
-                if ( q->agentInQueue ( a ) == false )
+                if ( a->prevStatus() != StateMachine::QUEUEING )
                 {
                     double d = distance (
                                    q->getX(), q->getY(),
                                    a->getx(), a->gety() );
 
                     if ( d < 4.5 && coinFlip() > 0.5 )
                     {
-                        // TODO - do state transition
-
                         q->enqueueAgent ( a );
                     }
                 }

pedsim_simulator/src/waitingqueue.cpp

@@ -87,7 +87,7 @@ void WaitingQueue::enqueueAgent ( Agent *a )
     Ped::Tvector qend = getQueueEnd();
     a->setPosition ( qend.x, qend.y );
 
-	a->updateState(); /// TODO - add event information here
+	a->updateState( StateMachine::JOIN_QUEUE ); 
 
     queueing_agents_.push_back ( a );
 }
@@ -144,8 +144,10 @@ void WaitingQueue::releaseAgent ( Agent *a )
 {
     // reset the factors for the social forces
     a->setMobile();
-    a->setPosition ( x_ + a->getRadius() + 10, y_ + a->getRadius() + 10 );
-	a->updateState(); /// TODO - add event information here
+    a->setPosition ( x_ + a->getRadius(), y_ + a->getRadius() );
+	a->updateState( StateMachine::LEAVE_QUEUE ); 
+	
+	queueing_agents_.pop_front();
 }