finally --working queues--, joining logic could still use some tuning and some parameters consilidated, but basic experiments can hopefully be done now

Author: makokal

pedsim_simulator/include/pedsim_simulator/waitingqueue.h

@@ -109,6 +109,7 @@ class WaitingQueue
     void updateQueue ( double px, double py );
     void releaseAgent ( Agent* a );
     Ped::Tvector getQueueEnd();
+	int getLastAgentId();
 };
 
 

pedsim_simulator/rviz/scene1.rviz

@@ -6,7 +6,7 @@ Panels:
       Expanded:
         - /Grid1/Offset1
       Splitter Ratio: 0.5
-    Tree Height: 817
+    Tree Height: 356
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -125,22 +125,22 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/XYOrbit
-      Distance: 75.9384
+      Distance: 44.2378
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.06
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 76.7738
-        Y: 10.0154
-        Z: 2.86065e-05
+        X: 70.6832
+        Y: 20.6308
+        Z: 1.2394e-05
       Name: Current View
       Near Clip Distance: 0.01
-      Pitch: 1.2304
+      Pitch: 0.615401
       Target Frame: <Fixed Frame>
       Value: XYOrbit (rviz)
-      Yaw: 4.70855
+      Yaw: 1.55354
     Saved: ~
 Window Geometry:
   Displays:

pedsim_simulator/scenarios/scene1_queues.xml

@@ -28,8 +28,8 @@
     <waypoint id="robot_goal" x="120" y="40" r="5"/>
     <waypoint id="robot_start" x="35" y="40" r="5"/>
 
-    <queue id="infodesk" x="50" y="20" direction="0"/>
-    <queue id="checkpoint" x="90" y="15" direction="180"/>
+    <queue id="infodesk" x="50" y="25" direction="0"/>
+    <queue id="checkpoint" x="90" y="10" direction="3.142"/>
     <!--Attractions-->
     <!--Agents-->
     <agent x="60" y="20" n="1" dx="0" dy="0" type="0">

pedsim_simulator/src/scene.cpp

@@ -293,7 +293,7 @@ void Scene::updateQueues()
                                    q->getX(), q->getY(),
                                    a->getx(), a->gety() );
 
-                    if ( d < 4.5 && coinFlip() > 0.5 )
+                    if ( d < 2.5 && coinFlip() > 0.5 )
                     {
                         q->enqueueAgent ( a );
                     }

pedsim_simulator/src/waitingqueue.cpp

@@ -77,16 +77,29 @@ WaitingQueue::~WaitingQueue()
 
 void WaitingQueue::enqueueAgent ( Agent *a )
 {
-    if ( queueing_agents_.size() == 15 ) //TEMP limit queue size
+    if ( queueing_agents_.size() == 20 ) //TEMP limit queue size
         return;
 
     // make the agent stop
     a->setStationary();
 
     // set position to the end of the queue
+	// NOTE - old way with setting positions
     Ped::Tvector qend = getQueueEnd();
     a->setPosition ( qend.x, qend.y );
 
+	// NOTE - new way with follow id
+// 	if ( queueing_agents_.size() == 0 )
+// 	{
+// 		Ped::Tvector qend = getQueueEnd();
+// 		a->setPosition ( qend.x, qend.y );
+// 	}
+// 	else
+// 	{
+// 		int follow_id = getLastAgentId();
+// 		a->setFollow(follow_id);
+// 	}
+	
 	a->updateState( StateMachine::JOIN_QUEUE ); 
 
     queueing_agents_.push_back ( a );
@@ -101,7 +114,7 @@ void WaitingQueue::serveAgent()
         releaseAgent ( lucky_one );
 
         // update queue
-        // updateQueue(lucky_one->getx(), lucky_one->gety());
+        updateQueue(lucky_one->getx(), lucky_one->gety());
 
         time_passed_ = 0;
     }
@@ -157,8 +170,8 @@ Ped::Tvector WaitingQueue::getQueueEnd()
     if ( queueing_agents_.size() == 0 )
     {
         return Ped::Tvector (
-                   x_ + ( buffer * cos ( theta_ ) ),
-                   y_ + ( buffer * sin ( theta_ ) ),
+                   x_ + ( buffer+1 * cos ( theta_ ) ),
+                   y_ + ( buffer+1 * sin ( theta_ ) ),
                    0.0 );
     }
     else
@@ -169,4 +182,12 @@ Ped::Tvector WaitingQueue::getQueueEnd()
                    last_one->gety() + ( buffer * sin ( theta_ ) ),
                    0.0 );
     }
+}
+
+int WaitingQueue::getLastAgentId()
+{
+	if ( queueing_agents_.size() == 0 )
+		return -1;
+	else
+		return queueing_agents_.back()->getid();
 }