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/Behavior.h b/scout/libscout/src/Behavior.h
index d2f560d..a665699 100644
--- a/scout/libscout/src/Behavior.h
+++ b/scout/libscout/src/Behavior.h
@@ -38,6 +38,7 @@
 #define _BEHAVIOR_H_
 
 #include <ros/ros.h>
+#include <std_msgs/String.h>
 
 #include "MotorControl.h"
 #include "HeadlightControl.h"
diff --git a/scout/libscout/src/BehaviorList.cpp b/scout/libscout/src/BehaviorList.cpp
index b4d9b95..0d51799 100644
--- a/scout/libscout/src/BehaviorList.cpp
+++ b/scout/libscout/src/BehaviorList.cpp
@@ -6,7 +6,7 @@ BehaviorList::BehaviorList(std::string scoutname)
   behavior_list.push_back((Behavior*)new draw_ccw_circle(scoutname));
   behavior_list.push_back((Behavior*)new Odometry(scoutname));
   behavior_list.push_back((Behavior*)new navigationMap(scoutname));
-  behavior_list.push_back((Behavior*)new sim_line(scoutname));
+  //behavior_list.push_back((Behavior*)new sim_line(scoutname));
   behavior_list.push_back((Behavior*)new Scheduler(scoutname));
   behavior_list.push_back((Behavior*)new WH_Robot(scoutname));
   return;
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();
 	}
diff --git a/scout/libscout/src/behaviors/Scheduler.h b/scout/libscout/src/behaviors/Scheduler.h
index 690cfc2..e83d174 100644
--- a/scout/libscout/src/behaviors/Scheduler.h
+++ b/scout/libscout/src/behaviors/Scheduler.h
@@ -9,7 +9,7 @@
 
 typedef struct{
   std::string name;
-  ros::Subscriber topic;
+  ros::Publisher topic;
 } Robot;
 
 class Scheduler : Behavior {
@@ -38,7 +38,7 @@ public:
 	
 	void run();
 
-  ros::Publisher sched_to_robot;
+  ros::Subscriber robot_to_sched;
     
 };
 #endif
diff --git a/scout/libscout/src/behaviors/WH_Robot.cpp b/scout/libscout/src/behaviors/WH_Robot.cpp
index d16ad91..cad9d6e 100644
--- a/scout/libscout/src/behaviors/WH_Robot.cpp
+++ b/scout/libscout/src/behaviors/WH_Robot.cpp
@@ -4,25 +4,14 @@
 /** @Brief: warehouse robot constructor **/
 WH_Robot::WH_Robot(std::string scoutname):Behavior(scoutname, "WH_Robot")
 {
-    ROS_INFO("WH_ROBOT: Creating a WH_Robot...");
     nav_map = new navigationMap(scoutname);
     curr_task = DEFAULT_TASK;
     name = scoutname;
     reg_failed = 1;
 
-    robot_to_sched = n.advertise<std_msgs::String>(name + "_topic", 1000);
-    sched_to_robot = n.subscribe("sched_to_robot", 1000, msg_callback);
-
-    while(reg_failed)
-    {
-      std_msgs::String msg;
-      std::stringstream ss;
-      ss << "REGISTER "<<name;
-      msg.data = ss.str();
-      robot_to_sched.publish(msg);
-    }
-
-    ROS_INFO("WH_Robot: I am a created WH robot");
+    robot_to_sched = node.advertise<std_msgs::String>("robot_to_sched", 1000);
+    sched_to_robot = node.subscribe(name + "_topic", 1000, &WH_Robot::robot_callback, this);
+    ROS_INFO("A WH_Robot has been created");
 }
 
 WH_Robot::~WH_Robot()
@@ -54,36 +43,50 @@ int WH_Robot::exec_task()
     }
 }
 
-void WH_Robot::msg_callback(const std_msgs::String::ConstPtr& msg)
+void WH_Robot::robot_callback(const std_msgs::String::ConstPtr& msg)
 {
   if(msg->data.compare(0, 11, "REG_SUCCESS") == 0)
   {
+    id = atoi(msg->data.substr(12).c_str());
     reg_failed = 0;
   }
   else if(msg->data.compare(0, 8, "SET_TASK") == 0)
   {
-    char* string = msg->data.c_str();
+    char* string = (char*)msg->data.c_str();
     char* data;
     data = strtok(string, " ");
-    int id = atoi(data);
+    int order_id = atoi(data);
     data = strtok(string, " ");
     Address source = atoi(data);
     data = strtok(string, " ");
     Address dest = atoi(data);
     Time start_time = ros::Time::now();
-    Path path = NULL;
+    Path path;
+    path.len = 0;
+    path.path = NULL;
     Duration est_time(0);
 
-    curr_task = new Order(id, source, dest, start_time, path, est_time);
+    curr_task = new Order(order_id, source, dest, start_time, path, est_time);
   }
   else
   {
-    ROS_INFO("I got a bad message: %s", msg->data);
+    ROS_INFO("I got a bad message: %s", msg->data.c_str());
   }
 }
 
 void WH_Robot::run ()
 {
+    ROS_INFO("I am a robot. Registering with scheduler...");
+    while(reg_failed)
+    {
+      std_msgs::String msg;
+      std::stringstream ss;
+      ss << "REGISTER "<<name;
+      msg.data = ss.str();
+      robot_to_sched.publish(msg);
+
+      spinOnce();
+    }
 
     while(ok())
     {
@@ -96,6 +99,7 @@ void WH_Robot::run ()
         msg.data = ss.str();
         robot_to_sched.publish(msg);
 
+        spinOnce();
 
         while(curr_task == DEFAULT_TASK)
           continue;
@@ -104,7 +108,7 @@ void WH_Robot::run ()
         {
 
 
-        ss << "SUCCESS "<<curr_task->id;
+        ss << "SUCCESS "<<curr_task->getid();
         msg.data = ss.str();
         robot_to_sched.publish(msg);
 
@@ -114,7 +118,7 @@ void WH_Robot::run ()
         {
  
 
-        ss << "FAILED "<<curr_task->id;
+        ss << "FAILED "<<curr_task->getid();
         msg.data = ss.str();
         robot_to_sched.publish(msg);
 
diff --git a/scout/libscout/src/behaviors/WH_Robot.h b/scout/libscout/src/behaviors/WH_Robot.h
index e62e491..7898996 100644
--- a/scout/libscout/src/behaviors/WH_Robot.h
+++ b/scout/libscout/src/behaviors/WH_Robot.h
@@ -23,7 +23,7 @@ class WH_Robot : Behavior{
         Duration get_worst_case_time(State start_state, State target_state);
         int exec_task();
 
-        void msg_callback(const std_msgs::String::ConstPtr& msg);
+        void robot_callback(const std_msgs::String::ConstPtr& msg);
 
     public:
         WH_Robot(std::string scoutname);
