commit 7ac5e9bc2e048962b82aab3a995270bab56d816a
Author: Priya <pdeo@Phlegethon.(none)>
Date:   Mon Apr 9 23:08:56 2012 -0400

    ROS scheduler/whrobot behavior needs to be debugged

diff --git a/scout/libscout/src/behaviors/Scheduler.cpp b/scout/libscout/src/behaviors/Scheduler.cpp
index 05f793f..ec12926 100644
--- a/scout/libscout/src/behaviors/Scheduler.cpp
+++ b/scout/libscout/src/behaviors/Scheduler.cpp
@@ -6,25 +6,16 @@ using namespace std;
 /** @Brief: Initialize data structures for the scheduler. */
 Scheduler::Scheduler(std::string scoutname):Behavior(scoutname, "Scheduler")
 {
-
-  ROS_INFO("Scheduler: Creating order PQ...");
-
   unassignedOrders = new PQWrapper(NUM_TASKS);
 
-  ROS_INFO("Scheduler: PQ orders initialized. ");
-
-  sched_to_robot = n.advertise("sched_to_robot", 1000);
+  robot_to_sched = node.subscribe("robot_to_sched", 1000, &Scheduler::msg_callback, this);
 
 	create_orders();
-  ROS_INFO("Scheduler: I am initialized!");
 }
 
 /** @Brief: Free allocatetd memory. */
 Scheduler::~Scheduler()
 {
-  while(robots.size() != 0)
-    delete robots.at(0);
-   
   delete unassignedOrders;
 }
 
@@ -120,12 +111,13 @@ Order Scheduler::get_next_item()
 
 void Scheduler::msg_callback(const std_msgs::String::ConstPtr& msg)
 {
+  ROS_INFO("I got a message: %s", msg->data.c_str());
   if(msg->data.compare(0, 6, "FAILED") == 0)
   {
     int order_id = atoi(msg->data.substr(7).c_str());
-    for(int i=0; i<assignedOrders.size(); i++)
+    for(unsigned int i=0; i<assignedOrders.size(); i++)
     {
-      if(assignedOrders[i].id == order_id)
+      if(assignedOrders[i].getid() == order_id)
       {
         task_failed(assignedOrders[i]);
         break;
@@ -135,9 +127,9 @@ void Scheduler::msg_callback(const std_msgs::String::ConstPtr& msg)
   else if(msg->data.compare(0, 7, "SUCCESS") == 0)
   {
     int order_id = atoi(msg->data.substr(8).c_str());
-    for(int i=0; i<assignedOrders.size(); i++)
+    for(unsigned int i=0; i<assignedOrders.size(); i++)
     {
-      if(assignedOrders[i].id == order_id)
+      if(assignedOrders[i].getid() == order_id)
       {
         task_complete(assignedOrders[i]);
         break;
@@ -151,22 +143,30 @@ void Scheduler::msg_callback(const std_msgs::String::ConstPtr& msg)
   }
   else if(msg->data.compare(0, 8, "REGISTER") == 0)
   {
+    string robot_name = msg->data.substr(9);
+    for(unsigned int i=0; i<robots.size(); i++)
+    {
+      if(robots[i].name.compare(robot_name) == 0)
+        return;
+    }
+
     int id = robots.size() +1;
     Robot new_robot;
-    new_robot.name = msg->data.substr(9);
-    new_robot.topic = n.subscribe(new_robot.name + "_topic", 1000, msg_callback);
+    new_robot.name = robot_name;
+    new_robot.topic = node.advertise<std_msgs::String>(new_robot.name + "_topic", 1000);
     robots.push_back(new_robot);
 
+    ROS_INFO("Registration a robot %s", new_robot.name.c_str());
     std_msgs::String msg;
     std::stringstream ss;
-    ss<<"REG_SUCCESS";
+    ss<<"REG_SUCCESS "<<id;
     msg.data = ss.str();
-    sched_to_robot.publish(msg);
-
+    new_robot.topic.publish(msg);
+    ROS_INFO("Registration a success");
   }
   else
   {
-    ROS_INFO("I got a bad message: %s", msg->data);
+    ROS_INFO("I got a bad message: %s", msg->data.c_str());
   }
 
   return;
@@ -176,21 +176,26 @@ void Scheduler::msg_callback(const std_msgs::String::ConstPtr& msg)
     this function calls the waiting_dance() function. */
 void Scheduler::run()
 {
-  while(robots.size() < 3);
+  ROS_INFO("I am a scheduler!. Now waiting for robots");
+  while(robots.size() < 3)
+    spinOnce();
 
 	while (ok())
 	{
     ROS_INFO("Scheduler running");
+    spinOnce();
 		while(waitingRobots.empty() || unassignedOrders->arraySize()==0) 
       waiting_dance();
 
 
     Order next = get_next_item();
+    Robot r = waitingRobots.front();
+
     std_msgs::String msg;
     std::stringstream ss;
-    ss<<"SET_TASK "<<next.id<<" "<<next.get_source()<<" "<<next.get_dest;
+    ss<<"SET_TASK "<<next.getid()<<" "<<next.get_source()<<" "<<next.get_dest();
     msg.data = ss.str();
-    sched_to_robot.publish(msg);
+    r.topic.publish(msg);
 
 		waitingRobots.pop();
 	}
