From 996d8585efcfd7d734e66b96d4b7e86d7ee5bb06 Mon Sep 17 00:00:00 2001
From: Billy Okal <okal.billy@googlemail.com>
Date: Tue, 18 Mar 2014 16:14:39 +0100
Subject: [PATCH] finally --working queues--, joining logic could still use
 some tuning and some parameters consilidated, but basic experiments can
 hopefully be done now

---
 .../include/pedsim_simulator/waitingqueue.h   |  1 +
 pedsim_simulator/rviz/scene1.rviz             | 14 ++++-----
 pedsim_simulator/scenarios/scene1_queues.xml  |  4 +--
 pedsim_simulator/src/scene.cpp                |  2 +-
 pedsim_simulator/src/waitingqueue.cpp         | 29 ++++++++++++++++---
 5 files changed, 36 insertions(+), 14 deletions(-)

diff --git a/pedsim_simulator/include/pedsim_simulator/waitingqueue.h b/pedsim_simulator/include/pedsim_simulator/waitingqueue.h
index f4032cc4..693731e9 100644
--- a/pedsim_simulator/include/pedsim_simulator/waitingqueue.h
+++ b/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();
 };
 
 
diff --git a/pedsim_simulator/rviz/scene1.rviz b/pedsim_simulator/rviz/scene1.rviz
index e951b284..22a238a3 100644
--- a/pedsim_simulator/rviz/scene1.rviz
+++ b/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:
diff --git a/pedsim_simulator/scenarios/scene1_queues.xml b/pedsim_simulator/scenarios/scene1_queues.xml
index 59be2c9b..69d63466 100644
--- a/pedsim_simulator/scenarios/scene1_queues.xml
+++ b/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">
diff --git a/pedsim_simulator/src/scene.cpp b/pedsim_simulator/src/scene.cpp
index 2c7c1ee7..a0407151 100644
--- a/pedsim_simulator/src/scene.cpp
+++ b/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 );
                     }
diff --git a/pedsim_simulator/src/waitingqueue.cpp b/pedsim_simulator/src/waitingqueue.cpp
index f8614655..6e5a0cbc 100644
--- a/pedsim_simulator/src/waitingqueue.cpp
+++ b/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();
 }
\ No newline at end of file